lidar_radar_fusion modify

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -99,7 +99,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
     {
         iv::fusion::fusionobject fusion_object;
         fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
-//        fusion_object.set_sensor_type(0);
+        fusion_object.set_sensor_type(0);
         fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
         fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
 
@@ -110,7 +110,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                    lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
             {
                 fusion_object.set_yaw(1.57);
-//                fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
             } else {
                 fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
             }
@@ -219,7 +218,7 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 //    lidar_radar_fusion_object_array.Clear();
     for(int j = 0; j< xobs_info.xobj_size() ; j++){
         iv::fusion::fusionobject fusion_obj;
-//        fusion_obj.set_sensor_type(1);
+        fusion_obj.set_sensor_type(1);
         fusion_obj.set_yaw(0);
         iv::fusion::VelXY vel_relative;
         iv::fusion::VelXY *vel_relative_;
@@ -262,9 +261,9 @@ void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
     for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
     {
-//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
-//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
-//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
+        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
 
         if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
                 (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -17,7 +17,6 @@
 #include "mobileye.pb.h"
 using namespace iv;
 using namespace fusion;
-ImmUkfPda giup;
 
 void *gfu = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
 static QMutex gMutex;
@@ -32,7 +31,6 @@ iv::mobileye::mobileye mobileye_info;
 iv::mobileye::obs obs_info;
 iv::mobileye::lane lane_info;
 iv::mobileye::tsr tsr_info;
-bool m_isTrackerInitialized = false;
 
 
 QTime gTime;
@@ -60,6 +58,18 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     }
     //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
     gMutex.lock();
+    //    for(int i=0;i<radarobj.obj_size();i++)
+    //    {
+    //        if(abs(radarobj.obj(i).x())<2 && abs(radarobj.obj(i).vy()) >10)
+    //        {
+    //            std::cout<<"x    y      vx    vy  "<<radarobj.obj(i).x()<<"          "
+    //                    <<radarobj.obj(i).y()<<"         "
+    //                    <<radarobj.obj(i).vx()<<"     "
+    //                    <<radarobj.obj(i).vy()<<"    "<<std::endl;
+
+    //        }
+
+    //    }
 
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
@@ -92,6 +102,14 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
+    //    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
+    //    for (int m_index = 0; m_index< mobileye.xobj_size(); m_index++)
+    //    {
+    //        std::cout<<" p_x  p_y  v_x  v_y   "<<mobileye.xobj(m_index).pos_y()<< "  "<<mobileye.xobj(m_index).pos_x()
+    //                <<"    "<<mobileye.xobj(m_index).obs_rel_vel_x() * tan(mobileye.xobj(m_index).obsang())
+    //               <<"   "<<mobileye.xobj(m_index).obs_rel_vel_x()<<"                                   \n"
+    //               << mobileye.xobj(m_index).obsang()<<std::endl;
+    //    }
 
     gMutex.lock();
     mobileye_info.CopyFrom(mobileye);
@@ -101,12 +119,59 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
 int ccccc =0;
 void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
 {
-
+    //    gMutex.lock();
+    //    Transformation::RadarPross(radarobjvec);
 
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-    giup.call(li_ra_fusion);
 
 
+    //    li_ra_fusion.Clear();
+    //      std::cout<<" RLFusion     begin    "<<std::endl;
+    AddMobileye(li_ra_fusion,mobileye_info);
+    //    std::cout<<" RLFusion     end      "<<std::endl;
+    mobileye_info.clear_xobj();
+    //        std::cout<< "     fusion size  "<<li_ra_fusion.obj_size()<<std::endl;
+    //    for(int i=0;i<li_ra_fusion.obj_size();i++)
+    //    {
+    //        if(abs(li_ra_fusion.obj(i).vel_relative().y())>10 )
+    //        {
+    //            std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.-obj(i).centroid().x()<<"          "
+    //                    <<li_ra_fusion.obj(i).centroid().y()<<"         "
+    //                    <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
+    //                    <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
+    //                    <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
+
+    //        }
+
+    //    }
+
+
+    //    int nsize =0;
+    //    for(int nradar = 0; nradar<radarobjvec.obj_size(); nradar++)
+    //    {
+    //        if(radarobjvec.obj(nradar).bvalid() == false) continue;
+    //        if(sqrt(abs(radarobjvec.obj(nradar).x()))< 4 && radarobjvec.obj(nradar).y()< 20 ) nsize = nsize +1;
+    //    }
+
+    //        std::cout<<"   fusion_obj_size     radar_obj_size   :   "<<li_ra_fusion.obj_size()<<"     "<<nsize<<std::endl;
+
+
+    //    gMutex.lock();
+    //    std::cout<<" track   begin"<<std::endl;
+    //---------------------------------------------  init tracker  -------------------------------------------------
+    if (!m_isTrackerInitialized)
+    {
+        m_isTrackerInitialized = InitTracker(tracker);
+        if (!m_isTrackerInitialized)
+        {
+            std::cerr << "Tracker initialize error!!!" << std::endl;
+        }
+    }
+    iv::fusion::fusionobjectarray trackedobjvec = Tracking(li_ra_fusion, tracker);
+    //    std::<<"track    end"<<std::endl;
+
+    //--------------------------------------------  end tracking  --------------------------------------------------
+    //    gMutex.unlock();
     iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
     ObsToNormal(out_fusion);
     string out;
@@ -131,6 +196,11 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     }else
     {
         out = out_fusion.SerializeAsString();
+        //        for (int k = 0; k<trackedobjvec.obj_size();k++){
+        //            std::cout<<" size   x    y    vy :   "<<trackedobjvec.obj_size()<<"   "
+        //                    <<trackedobjvec.obj(k).centroid().x()<< "   "<<trackedobjvec.obj(k).centroid().y()<<"    "
+        //                   <<trackedobjvec.obj(k).vel_relative().y()<<"   "<<trackedobjvec.obj(k).nomal_centroid_size() <<std::endl;
+        //        }
     }
     iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
     //    gMutex.unlock();

src/include/proto/fusionobject.proto

@@ -71,6 +71,7 @@ message fusionobject
   optional double acceleration_linear_y = 23;
   optional bool pose_reliable = 24;
   optional bool velocity_reliable = 25;
+  optional int32 sensor_type = 26;  //sensor type   0:lidar, 1:radar, 2:camera
 
 };