fix(fusion,lidargrid,groundfilter):fix yaw of obj often change

src/detection/detection_ground_filter/main.cpp

@@ -3,7 +3,7 @@
 #include "base_segmenter.hpp"
 #include "ground_plane_fitting_segmenter.hpp"
 #include <pcl/filters/radius_outlier_removal.h>
-void * gpoint_out= iv::modulecomm::RegisterSend("lidar_pc",20000000,1);
+void * gpoint_out= iv::modulecomm::RegisterSend("lidar_pc_out",20000000,1);
 std::unique_ptr<lidar::segmenter::BaseSegmenter> ground_remover_;
 std::unique_ptr<lidar::segmenter::BaseSegmenter> segmenter_;
 
@@ -99,7 +99,7 @@ int main(int argc, char *argv[])
     params.gpf_th_gnds = 0.3;
     ground_remover_ = std::unique_ptr<lidar::segmenter::BaseSegmenter>(
         new lidar::segmenter::GroundPlaneFittingSegmenter(params));
-    gpoint = iv::modulecomm::RegisterRecv("lidar_pc_in",ListenPointCloud);
+    gpoint = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
 
     return a.exec();
 }

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -142,7 +142,7 @@ static int g_seq = 0;
 static float skip_xmin = -0.4;
 static float skip_ymin = 0.0;
 static float skip_xmax = 0.4;
-static float skip_ymax = 0.3;
+static float skip_ymax = 0.5;
 
 static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::shared_ptr<iv::fusion::fusionobjectarray> lidar_obs)
 {
@@ -413,7 +413,7 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
                     nomal_y = i*SMALLGRID_SIZE +0.1 +VEC_MIN;
                 }
             }
-            if (abs(nomal_x)<0.4 && nomal_y<1.0 && nomal_y>-0.3) continue;
+            if (abs(nomal_x)<0.4 && nomal_y<0.3 && nomal_y>-1.0) continue;
             nomal_centroid.set_nomal_x(nomal_x);
             nomal_centroid.set_nomal_y(nomal_y);
             nomal_centroid.set_nomal_z(nomal_z);
@@ -702,8 +702,8 @@ int main(int argc, char *argv[])
 
     snprintf(gstr_sensorheight,255,"1.35");
     snprintf(gstr_vehicleheight,255,"1.5");
-    snprintf(gstr_inputmemname,255,"lidar_pc");
-    snprintf(gstr_outputmemname,255,"li_ra_fusion");
+    snprintf(gstr_inputmemname,255,"lidar_pc_out");
+    snprintf(gstr_outputmemname,255,"grid_fusion");
     snprintf(gstr_skipxmin,255,"-0.4");
     snprintf(gstr_skipxmax,255,"0.4");
     snprintf(gstr_skipymin,255,"0.0");

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -22,6 +22,54 @@ float ComputeDis(cv::Point2f po1, cv::Point2f po2)
     return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
 }
 
+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).dimensions().x()>0)&&
+                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
+        {
+            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
+            {
+                for(iy = 0; iy<(yp*2); iy++)
+                {
+                    iv::fusion::NomalXYZ nomal_centroid;
+                    iv::fusion::NomalXYZ *nomal_centroid_;
+                    float nomal_x = ix*0.2 - xp*0.2;
+                    float nomal_y = iy*0.2 - yp*0.2;
+                    float nomal_z = 1.0;
+                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
+                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
+                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
+                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
+                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <0.4 &&
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <0.3 ||
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <-1.0 ) continue;
+                    else{
+                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
+                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                        nomal_centroid_->CopyFrom(nomal_centroid);
+                    }
+                }
+            }
+
+    }
+}
+}
+
 void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
                         std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
 {
@@ -91,7 +139,7 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
 void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
 //    std::cout<< "    get   fusion  begin   "<<std::endl;
-    lidar_radar_fusion_object_array.Clear();
+//    lidar_radar_fusion_object_array.Clear();
     std::vector<match_index> match_idx;
     std::vector<int> radar_idx;
     Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
@@ -208,53 +256,11 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
         obj->CopyFrom(fusion_obj);
     }
-//    std::cout<<"   fusion   end    "<<std::endl;
+     ObsToNormal(lidar_radar_fusion_object_array);
+//             std::cout<<"   fusion   end    "<<std::endl;
 }
 
-void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
-{
-    int time_step = 0.3;
-//    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_yaw(0);
-        iv::fusion::VelXY vel_relative;
-        iv::fusion::VelXY *vel_relative_;
-        if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
-        {
-            fusion_obj.set_type(2);
-        }else{
-            fusion_obj.set_type(1);
-            vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
-            vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
-        }
-        vel_relative_ = fusion_obj.mutable_vel_relative();
-        vel_relative_->CopyFrom(vel_relative);
 
-        iv::fusion::PointXYZ centroid;
-        iv::fusion::PointXYZ *centroid_;
-        centroid.set_x(-(xobs_info.xobj(j).pos_y()));
-        centroid.set_y(xobs_info.xobj(j).pos_x());
-        centroid.set_z(1.0);
-        centroid_ = fusion_obj.mutable_centroid();
-        centroid_->CopyFrom(centroid);
-#ifdef DEBUG_SHOW
-//        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
-#endif
-
-        iv::fusion::Dimension dimension;
-        iv::fusion::Dimension *dimension_;
-        dimension.set_x(xobs_info.xobj(j).obswidth());
-        dimension.set_y(2.0);
-        dimension.set_z(1.0);
-        dimension_ = fusion_obj.mutable_dimensions();
-        dimension_->CopyFrom(dimension);
-        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
-        obj->CopyFrom(fusion_obj);
-    }
-
-}
 
 void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::radar::radarobjectarray& radar_info)
 {
@@ -293,53 +299,6 @@ void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array
 }
 }
 
-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).dimensions().x()>0)&&
-                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
-        {
-            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
-            if(xp == 0)xp=1;
-            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
-            if(yp == 0)yp=1;
-            int ix,iy;
-            for(ix = 0; ix<(xp*2); ix++)
-            {
-                for(iy = 0; iy<(yp*2); iy++)
-                {
-                    iv::fusion::NomalXYZ nomal_centroid;
-                    iv::fusion::NomalXYZ *nomal_centroid_;
-                    float nomal_x = ix*0.2 - xp*0.2;
-                    float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = 1.0;
-                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
-                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
-                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
-                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
-                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
-                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
-                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
-                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <1.0 ||
-                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <-1.0 ) continue;
-                    else{
-                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
-                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
-                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
-                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
-                        nomal_centroid_->CopyFrom(nomal_centroid);
-                    }
-                }
-            }
-
-    }
-}
-}
 
 }
 }

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -24,14 +24,12 @@ static QMutex gMutex;
 typedef  iv::radar::radarobjectarray RadarDataType;
 typedef  iv::lidar::objectarray LidarDataType;
 typedef  std::chrono::system_clock::time_point TimeType;
+typedef  iv::fusion::fusionobjectarray FusionDataType;
 
 iv::radar::radarobjectarray radar_front;
 iv::radar::radarobjectarray radar_back;
 iv::lidar::objectarray lidar_obj;
-iv::mobileye::mobileye mobileye_info;
-iv::mobileye::obs obs_info;
-iv::mobileye::lane lane_info;
-iv::mobileye::tsr tsr_info;
+FusionDataType fusion_grid_info;
 
 
 QTime gTime;
@@ -109,12 +107,14 @@ void Listenradarback(const char * strdata,const unsigned int nSize,const unsigne
     gMutex.unlock();
 }
 
-void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+void Listenfusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-    iv::mobileye::mobileye mobileye;
+    iv::fusion::fusionobjectarray fusion_grid;
+
+
 
     if(nSize<1)return;
-    if(false == mobileye.ParseFromArray(strdata,nSize))
+    if(false == fusion_grid.ParseFromArray(strdata,nSize))
     {
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
@@ -129,7 +129,8 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
     //    }
 
     gMutex.lock();
-    mobileye_info.CopyFrom(mobileye);
+//     std::cout<<"          is           ok       "<<fusion_grid.obj_size()<<std::endl;
+    fusion_grid_info.CopyFrom(fusion_grid);
     gMutex.unlock();
 }
 
@@ -138,8 +139,10 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 {
     //    gMutex.lock();
     //    Transformation::RadarPross(radarobjvec);
-
+    li_ra_fusion.Clear();
+    li_ra_fusion.CopyFrom(fusion_grid_info);
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    std::cout<<"     obj     size  "<<li_ra_fusion.obj_size()<<std::endl;
 
 
     //    li_ra_fusion.Clear();
@@ -147,27 +150,26 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 //    AddMobileye(li_ra_fusion,mobileye_info);
     //    std::cout<<" RLFusion     end      "<<std::endl;
 //    mobileye_info.clear_xobj();
-      AddRadarBack(li_ra_fusion,radar_back);
-      radar_back.clear_obj();
-    //---------------------------------------------  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);
+//      AddRadarBack(li_ra_fusion,radar_back)
+//      radar_back.clear_obj();
+//    //---------------------------------------------  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);
+//    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
     string out;
 
-    if(out_fusion.obj_size() == 0)
+    if(li_ra_fusion.obj_size() == 0)
     {
         //        std::cout<<"   fake   obj"<<std::endl;
 
@@ -181,12 +183,12 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
         fake_cen_ = fake_obj.mutable_centroid();
         fake_cen_ ->CopyFrom(fake_cen);
 
-        fake_obj_ = out_fusion.add_obj();
+        fake_obj_ = li_ra_fusion.add_obj();
         fake_obj_->CopyFrom(fake_obj);
-        out = out_fusion.SerializeAsString();
+        out = li_ra_fusion.SerializeAsString();
     }else
     {
-        out = out_fusion.SerializeAsString();
+        out = li_ra_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()<<"    "
@@ -207,6 +209,6 @@ int main(int argc, char *argv[])
     gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
     gpa = iv::modulecomm::RegisterRecv("radar_back",Listenradarback);
     gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
-    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
+    gpa = iv::modulecomm::RegisterRecv("grid_fusion",Listenfusion);
     return a.exec();
 }