Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization into laneline_decition

src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -232,7 +232,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
         mroadedge[i].mbvertex = true;
         mroadedge[i].mvertexstart = 2*i;
         mroadedge[i].mvertexend = 2*i + 1;
-        qDebug("road:%d vertex %d %d ",mroadedge[i].mroadid,mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
+   //     qDebug("road:%d vertex %d %d ",mroadedge[i].mroadid,mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
 //        std::cout<<"road id "<<mroadedge[i].mroadid<<" lenght "<<mroadedge[i].mfedgelen<<std::endl;
     }
 

src/decition/common/common/car_status.h

@@ -263,7 +263,7 @@ namespace iv {
         bool laneline_drive=false;  
         double camera_x_adjust; //camera2vehicle coordinate adjust at x-dim
         double camera_y_adjust; //camera2vehicle coordinate adjust at y-dim
-
+        double laneline_speed; // laneline driver speed
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/adc_tools/transfer.cpp

@@ -418,7 +418,7 @@ double iv::decition::GetL0InDegree(double dLIn)
 // 相机转车体
 iv::Point2D iv::decition::camera2vehicle(iv::Point2D point){
     iv::Point2D new_point;
-    new_point.x = point.x+ServiceCarStatus.msysparam.camera_x_adjust;
-    new_point.y = point.y+ServiceCarStatus.msysparam.camera_y_adjust;
+    new_point.x = point.x+ServiceCarStatus.camera_x_adjust;
+    new_point.y = point.y+ServiceCarStatus.camera_y_adjust;
     return new_point;
 }

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -373,9 +373,11 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.laneline_drive = false;
     }
     std::string camera_x_adjust = xp.GetParam("camera_x_adjust","0.0");
-    ServiceCarStatus.msysparam.camera_x_adjust = atof(camera_x_adjust.data());
+    ServiceCarStatus.camera_x_adjust = atof(camera_x_adjust.data());
     std::string camera_y_adjust = xp.GetParam("camera_y_adjust","0.0");
-    ServiceCarStatus.msysparam.camera_y_adjust = atof(camera_y_adjust.data());
+    ServiceCarStatus.camera_y_adjust = atof(camera_y_adjust.data());
+    std::string laneline_speed = xp.GetParam("laneline_speed","5.0");
+    ServiceCarStatus.laneline_speed = atof(laneline_speed.data());
 
 
 

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/decide_from_laneline.cpp

@@ -218,7 +218,7 @@ iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(G
     controlAng = limitAngle(realspeed, controlAng);
 
     transferGpsMode2(gpsMapLine);
-    dSpeed = min(dSpeed, 5.0);
+    dSpeed = min(dSpeed, ServiceCarStatus.laneline_speed);
 
     laneline_decition->speed = dSpeed;
     laneline_decition->wheel_angle = 0.0 - controlAng;

src/detection/detection_lidar_cnn_segmentation/cnn_segmentation.cpp

@@ -241,7 +241,7 @@ bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr
                               pcl::PointIndicesPtr & valid_idx_ptr)
 {
 
-    int64 npctime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    int64 npctime =  pc_ptr->header.stamp/1000;// std::chrono::system_clock::now().time_since_epoch().count()/1000000;
     if(use_gpu_)
     {
         caffe::Caffe::SetDevice(gpu_device_id_);

src/detection/detection_lidar_cnn_segmentation/main.cpp

@@ -321,10 +321,13 @@ void threadgetres()
         {
             iv::lidar::objectarray lidarobjvec;
             GetLidarObj(xcnnres.mobjvec,lidarobjvec);
+            lidarobjvec.set_timestamp(xcnnres.mcluster2ddata.mPointCloudUpdateTime);
             std::string out = lidarobjvec.SerializeAsString();
             //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
             iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
 
+
+
             int64 now =  std::chrono::system_clock::now().time_since_epoch().count()/1000000;
             std::cout<<" pc time : "<<xcnnres.mcluster2ddata.mPointCloudUpdateTime<<" cnn use: "<<(now - xcnnres.mcluster2ddata.mPointCloudUpdateTime)
                     <<" obj size: "<<xcnnres.mobjvec.size()<<std::endl;

src/detection/laneATT_trt/main.cpp

@@ -486,6 +486,9 @@ int main(int argc, char *argv[]) {
                     "ms" << std::endl;
         if(imshow_flag)
         {
+
+            cv::namedWindow("rst",cv::WINDOW_NORMAL);
+            cv::namedWindow("warp",cv::WINDOW_NORMAL);
             cv::imshow("rst",raw_image);
             cv::imshow("warp",img_warp );
             if (cv::waitKey(10) == 'q')

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1304,10 +1304,12 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,cons
         pp.x = x;
         pp.y = y;
         pp.hdg = hdg;
-        s = s+ fspace;
+
         pp.dis = pp.dis + fspace;;
         pp.mS = s_start + s;
         xvectorPP.push_back(pp);
+
+        s = s+ fspace;
     }
     return xvectorPP;
 }
@@ -1586,20 +1588,36 @@ struct lanewidthabcd
 
 double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
 {
+    if(nlane == 0)return 0;
     double frtn = 0;
 
+    std::vector<double> xvectorlanewidth;
 
-    int i;
-    int nlanecount = xvectorlwa.size();
-    for(i=0;i<nlanecount;i++)
+    if(nlane<0)
+        xvectorlanewidth = pRoad->GetLaneWidthVector(s,2);
+    else
+        xvectorlanewidth = pRoad->GetLaneWidthVector(s,1);
+
+    if(abs(nlane)<=xvectorlanewidth.size())
     {
-        if(nlane == xvectorlwa.at(i).nlane)
-        {
-            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
-            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
-            break;
-        }
+        return xvectorlanewidth[abs(nlane)-1];
+    }
+    else
+    {
+        std::cout<<" Warning: getwidth get lane width fault."<<std::endl;
+        return 0;
     }
+//    int i;
+//    int nlanecount = xvectorlwa.size();
+//    for(i=0;i<nlanecount;i++)
+//    {
+//        if(nlane == xvectorlwa.at(i).nlane)
+//        {
+//            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
+//            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
+//            break;
+//        }
+//    }
     return frtn;
 }
 
@@ -1700,27 +1718,34 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
 
 }
 
-double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+
+double GetRoadWidth(Road * pRoad,int nlane,double s)
 {
-    double fwidth = 0;
-    int i;
-    double a,b,c,d;
+    if(nlane<0)return pRoad->GetRightRoadWidth(s);
+    return pRoad->GetLeftRoadWidth(s);
+}
 
-    int nsize = xvectorLWA.size();
-    for(i=0;i<nsize;i++)
-    {
-        if(nlane*(xvectorLWA[i].nlane)>0)
-        {
-            a = xvectorLWA[i].A;
-            b = xvectorLWA[i].B;
-            c = xvectorLWA[i].C;
-            d = xvectorLWA[i].D;
-            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
-        }
-    }
-    return fwidth;
+//double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+//{
+//    double fwidth = 0;
+//    int i;
+//    double a,b,c,d;
 
-}
+//    int nsize = xvectorLWA.size();
+//    for(i=0;i<nsize;i++)
+//    {
+//        if(nlane*(xvectorLWA[i].nlane)>0)
+//        {
+//            a = xvectorLWA[i].A;
+//            b = xvectorLWA[i].B;
+//            c = xvectorLWA[i].C;
+//            d = xvectorLWA[i].D;
+//            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+//        }
+//    }
+//    return fwidth;
+
+//}
 
 int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
 {
@@ -2187,7 +2212,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.mfDisToLaneLeft = pp.mWidth/2.0;
             pp.lanmp = 0;
             pp.mfDisToRoadLeft = off1*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
             GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
             xvectorPP.push_back(pp);
@@ -2235,7 +2260,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.mfDisToLaneLeft = pp.mWidth/2.0;
             pp.lanmp = 0;
             pp.mfDisToRoadLeft = off1;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
             GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
             pp.hdg = pp.hdg + M_PI;
@@ -3004,26 +3029,26 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                         if(lr_start == 2)
                         {
                             PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            foff =  getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+   //                         std::cout<<"foff: "<<foff<<std::endl;
                             pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                             pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                             pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
                             pp.mfDisToLaneLeft = pp.mWidth/2.0;
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff *(-1.0);
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
                             pp.mLaneCur = abs(nlane);
                             pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                             pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
                             GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
                             xvectorPP.push_back(pp);
                             i++;
                         }
                         else
                         {
                             PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            foff = getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
                             pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                             pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                             pp.hdg = pp.hdg + M_PI;
@@ -3032,7 +3057,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                             pp.mfDisToLaneLeft = pp.mWidth/2.0;
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff;
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
                             pp.mLaneCur = abs(nlane);
                             pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                             pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
@@ -3053,7 +3078,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     xps.mbStartToMainChange = false;
                     xps.mbMainToEndChange = false;
          //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
-                    xPlan = xvectorPP;
+                    xPlan =  xvectorPP;
 
                     xvectorplans.push_back(xvectorPP);
                     xvectorroutedis.push_back(xvectorPP.size() * 0.1);

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -30,6 +30,7 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
     int nradar = radar_object_array.obj_size();
     match_idx.clear();
     radar_idx.clear();
+//    std::cout<<"radar size: "<<nradar<<std::endl;
     for(int i = 0; i<nlidar; i++)
     {
         match_index match;
@@ -67,8 +68,10 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
         match_idx.push_back(match);
 
     }
-//std::cout<<"  match_size   :   "<<match_idx.size()<<"    "<<match_idx[0].nradar<<std::endl;
-
+    for(int k = 0; k<match_idx.size(); k++)
+    {
+                    std::cout<<"    lidar   radar   "<<match_idx[k].nlidar<<"   "<<match_idx[k].nradar<<std::endl;
+}
     for(int k = 0; k<radar_object_array.obj_size(); k++)
     {
         std::vector<int> indexs;
@@ -118,6 +121,9 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             iv::fusion::VelXY *vel_relative_;
             vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
             vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
+            fusion_object.set_velocity_linear_x(-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x());
+//            std::cout<<"lidar: "<<-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()<<std::endl;
+
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -128,8 +134,10 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+//             std::cout<<"lidar: "<<centroid.y()<<" "<<fusion_object.type()<<std::endl;
+#ifdef DEBUG_SHOW
             std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
-
+#endif
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
             fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
@@ -137,10 +145,13 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-//            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
-//            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
-            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
-            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
+            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            fusion_object.set_velocity_linear_x(radar_object_array.obj(match_idx.at(i).nradar).vy());
+//            std::cout<<"lidar: "<<-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()<<"     radar: "<<radar_object_array.obj(match_idx.at(i).nradar).vy()<<std::endl;
+//            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
+//            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
+//            std::cout<<"radar: "<<radar_object_array.obj(match_idx.at(i).nradar).vx()<<","<<radar_object_array.obj(match_idx.at(i).nradar).vy()<<std::endl;
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -148,6 +159,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             iv::fusion::VelXY *vel_abs_;
             vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
             vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+
             vel_abs_ = fusion_object.mutable_vel_abs();
             vel_abs_->CopyFrom(vel_abs);
 
@@ -158,6 +170,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+//            std::cout<<"radar: "<<centroid.y()<<std::endl;
 #ifdef DEBUG_SHOW
             std::cout<<"radar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
 #endif
@@ -171,7 +184,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
-        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
+//        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
 
 
 
@@ -179,13 +192,15 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         pobj->CopyFrom(fusion_object);
     }
     for(int j = 0; j< radar_idx.size() ; j++){
-        break;
+//        break;
+        if(radar_object_array.obj(radar_idx[j]).y()<50) continue;
         iv::fusion::fusionobject fusion_obj;
         fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
         iv::fusion::VelXY vel_relative;
         iv::fusion::VelXY *vel_relative_;
         vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
         vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
+        fusion_obj.set_velocity_linear_x(radar_object_array.obj(radar_idx[j]).vy());
         vel_relative_ = fusion_obj.mutable_vel_relative();
         vel_relative_->CopyFrom(vel_relative);
 
@@ -196,7 +211,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         centroid.set_z(1.0);
         centroid_ = fusion_obj.mutable_centroid();
         centroid_->CopyFrom(centroid);
-
+//std::cout<<"radar>50: "<<centroid.y()<<std::endl;
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
         dimension.set_x(0.5);
@@ -261,8 +276,8 @@ 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() > 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)&&

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -1,6 +1,6 @@
 QT -= gui
 
-CONFIG += c++14 console
+CONFIG += c++14 #console
 #CONFIG += c++14
 CONFIG -= app_bundle
 

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.xml

@@ -0,0 +1,11 @@
+<xml>	
+	<node name="lidar_radar_fusion_cnn.xml">
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="uselast" value="false" />
+		<param name="LookTime" value="3000" />
+		<param name="LookProb" value="0.3" />
+		<param name="LidarOffX" value="0.0" />
+		<param name="LidarOffY" value="0.1" />
+		<param name="disthresh" value="1.0" />
+	</node>
+</xml>

src/fusion/lidar_radar_fusion_cnn/lidarbuffer.cpp

@@ -3,9 +3,13 @@
 #include <iostream>
 #include "math/gnss_coordinate_convert.h"
 
-lidarbuffer::lidarbuffer()
+lidarbuffer::lidarbuffer(int64_t nLookTime,double fProb_Thresh,double fLidarOffX, double fLidarOffY, double fDisThresh)
 {
-
+    mnLookTime = nLookTime;
+    mfProb_Thresh = fProb_Thresh;
+    mfLidarOffX = fLidarOffX;
+    mfLidarOffY = fLidarOffY;
+    mfDisThresh = fDisThresh;
 }
 
 void lidarbuffer::AddGPS(iv::gps::gpsimu & xgpsimu)
@@ -72,9 +76,11 @@ void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
 
     mmutex.lock();
     iv::lidarobj_rec xrec;
-    xrec.nTime = xobjarray.timestamp()/1000; //to ms
-    ChangePos(&xrec);
+    xrec.nTime = xobjarray.timestamp(); //to ms
+
     xrec.xobjarray.CopyFrom(xobjarray);
+    ChangePos(&xrec);
+
     mvectorlidarobj_rec.push_back(xrec);
     mmutex.unlock();
 }
@@ -90,6 +96,8 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
     unsigned int nnearindex = 0;
     int64_t ntimediff = mnLookTime;
 
+//    xrec->nTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000 - 100;
+
     unsigned int nsize = static_cast<unsigned int>( mvectorgps_rec.size());
     unsigned int i;
     for(i=1;i<nsize;i++)
@@ -111,13 +119,18 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
 
     double hdg = (90 - xgpsimu.heading())*M_PI/180.0;
 
-    double fsr = hdg + M_PI/2.0;
+    double fsr = hdg - M_PI/2.0;
     double x_sensor = mfLidarOffX * cos(fsr) - mfLidarOffY * sin(fsr);
     double y_sensor = mfLidarOffX * sin(fsr) + mfLidarOffY * cos(fsr);
 
+    xrec->sensor_hdg = hdg;
+    xrec->sensor_x = x_sensor;
+    xrec->sensor_y = y_sensor;
+
     double x_gps,y_gps;
     GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_gps,&y_gps);
 
+    nsize = xrec->xobjarray.obj_size();
     for(i=0;i<nsize;i++)
     {
         iv::lidar::lidarobject * pobj = xrec->xobjarray.mutable_obj(i);
@@ -132,4 +145,172 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
 
 
     }
+
+    xrec->sensor_x = xrec->sensor_x + x_gps;
+    xrec->sensor_y = xrec->sensor_y + y_gps;
+}
+
+int lidarbuffer::MergeLast(iv::lidar::objectarray & xselarray)
+{
+    int nsel = 0;
+    if(mvectorlidarobj_rec.size() == 0)return 0;
+    mmutex.lock();
+
+    int nsize = mvectorlidarobj_rec.size();
+    int i;
+    std::vector<std::vector<iv::stats_count>> xvectorcount_series;
+    for(i=0;i<nsize;i++)
+    {
+        unsigned int j;
+        unsigned int nobjsize = mvectorlidarobj_rec[i].xobjarray.obj_size();
+        std::vector<iv::stats_count> xvectorcount;
+        for(j=0;j<nobjsize;j++)
+        {
+            iv::stats_count sc;
+            sc.ncount = 1;
+            sc.nIndex_vector = i;
+            sc.nIndex_obj = j;
+            sc.nSameRef = -1;
+            xvectorcount.push_back(sc);
+        }
+        if(xvectorcount.size()>0) xvectorcount_series.push_back(xvectorcount);
+    }
+
+    std::vector<std::vector<iv::stats_count>> xvectorcount_same;
+
+
+    while(xvectorcount_series.size()>0)
+    {
+        if(xvectorcount_series[0].size() == 0)
+        {
+            xvectorcount_series.erase(xvectorcount_series.begin());
+            continue;
+        }
+        iv::stats_count scref = xvectorcount_series[0].at(0);
+        xvectorcount_series[0].erase(xvectorcount_series[0].begin());
+        std::vector<iv::stats_count> xvectorsa;
+        xvectorsa.push_back(scref);
+
+
+        iv::lidar::lidarobject * pobj = mvectorlidarobj_rec[scref.nIndex_vector].xobjarray.mutable_obj(scref.nIndex_obj);
+
+        for(i=1;i<static_cast<int>( xvectorcount_series.size());i++)
+        {
+            double fdismin = 1000.0;
+            int nindexmin = -1;
+            int j;
+            for(j=0;j<static_cast<int>( xvectorcount_series[i].size());j++)
+            {
+               iv::lidar::lidarobject * pobj2 = mvectorlidarobj_rec[xvectorcount_series[i].at(j).nIndex_vector].xobjarray.mutable_obj(xvectorcount_series[i].at(j).nIndex_obj);
+               double fdis =sqrt(pow(pobj->mutable_position()->x() -  pobj2->mutable_position()->x(),2)
+                                 +pow(pobj->mutable_position()->y() - pobj2->mutable_position()->y(),2));
+               if(fdis<fdismin)
+               {
+                   fdismin = fdis;
+                   nindexmin = j;
+               }
+            }
+            if(nindexmin>=0)
+            {
+                if(fdismin<mfDisThresh)
+                {
+                    xvectorsa.push_back(xvectorcount_series[i].at(nindexmin));
+                    xvectorcount_series[i].erase(xvectorcount_series[i].begin() + nindexmin);
+                }
+            }
+
+        }
+        xvectorcount_same.push_back(xvectorsa);
+
+    }
+
+    //Select Object
+    for(i=0;i<static_cast<int>( xvectorcount_same.size());i++)
+    {
+        double fprob = ((double)xvectorcount_same[i].size())/((double)nsize);
+        if(fprob >= mfProb_Thresh)
+        {
+            iv::lidar::lidarobject * pobj = xselarray.add_obj();
+            iv::stats_count sc = xvectorcount_same[i].at(xvectorcount_same[i].size() -1);
+            pobj->CopyFrom(*mvectorlidarobj_rec[sc.nIndex_vector].xobjarray.mutable_obj(sc.nIndex_obj));
+            nsel++;
+        }
+    }
+
+    mmutex.unlock();
+    return nsel;
+}
+
+int lidarbuffer::FilterLidarObj(iv::lidar::objectarray & xobjarray)
+{
+    int nAdd = 0;
+    iv::lidar::objectarray xlast;
+    MergeLast(xlast);
+
+    iv::lidarobj_rec xrec;
+    xrec.nTime = xobjarray.timestamp(); //to ms
+    xrec.xobjarray.CopyFrom(xobjarray);
+    mmutex.lock();
+    ChangePos(&xrec);
+    mmutex.unlock();
+
+    iv::lidar::objectarray xobjarraylast;
+
+    int i;
+    for(i=0;i<xlast.obj_size();i++)
+    {
+        iv::lidar::lidarobject * pobjlast = xlast.mutable_obj(i);
+        double fdismin = 1000.0;
+        int nindexmin = -1;
+        int j;
+        for(j=0;j<xrec.xobjarray.obj_size();j++)
+        {
+            iv::lidar::lidarobject * pobjnow = xrec.xobjarray.mutable_obj(j);
+            double fdis = sqrt(pow(pobjlast->mutable_position()->x() - pobjnow->mutable_position()->x(),2)
+                               +pow(pobjlast->mutable_position()->y() - pobjnow->mutable_position()->y(),2));
+            if(fdis<fdismin)
+            {
+                fdismin = fdis;
+                nindexmin = j;
+            }
+        }
+        if(fdismin< mfDisThresh)
+        {
+            //same
+        }
+        else
+        {
+            iv::lidar::lidarobject * pobj = xobjarraylast.add_obj();
+            pobj->CopyFrom(*pobjlast);
+            nAdd++;
+        }
+    }
+
+    if(nAdd>0)
+    {
+        int i;
+        for(i=0;i<xobjarraylast.obj_size();i++)
+        {
+            iv::lidar::lidarobject * pobj = xobjarraylast.mutable_obj(i);
+            double x_abs = pobj->mutable_position()->x();
+            double y_abs = pobj->mutable_position()->y();
+            double x_rel,y_rel;
+            x_rel = x_abs - xrec.sensor_x;
+            y_rel = y_abs - xrec.sensor_y;
+
+            double fsr = 0 + M_PI/2.0 - xrec.sensor_hdg;
+
+            double x_raw =   x_rel * cos(fsr) - y_rel * sin(fsr);
+            double y_raw =   x_rel * sin(fsr) + y_rel * cos(fsr);
+            pobj->mutable_position()->set_x(x_raw);
+            pobj->mutable_position()->set_y(y_raw);
+            std::cout<<" add: x:"<<x_raw<<" y: "<<y_raw<<std::endl;
+
+            iv::lidar::lidarobject * pnew =  xobjarray.add_obj();
+            pnew->CopyFrom(*pobj);
+
+        }
+    }
+
+    return nAdd;
 }

src/fusion/lidar_radar_fusion_cnn/lidarbuffer.h

@@ -14,12 +14,25 @@ struct  lidarobj_rec
 {
     iv::lidar::objectarray xobjarray;
     int64_t nTime;    //ms since epoch
+
+    double sensor_x;
+    double sensor_y;
+    double sensor_hdg;
 };
 
 struct gps_rec
 {
     iv::gps::gpsimu xgpsimu;
     int64_t nTime; //ms since epoch
+
+};
+
+struct stats_count
+{
+    int nIndex_vector;
+    int nIndex_obj;
+    int ncount;
+    int nSameRef;
 };
 
 }
@@ -27,11 +40,12 @@ struct gps_rec
 class lidarbuffer
 {
 public:
-    lidarbuffer();
+    lidarbuffer(int64_t nLookTime,double fProb_Thresh,double fLidarOffX, double fLidarOffY, double fDisThresh);
 
 public:
     void AddGPS(iv::gps::gpsimu & xgpsimu);
     void AddLidarObj(iv::lidar::objectarray & xobjarray);
+    int FilterLidarObj(iv::lidar::objectarray & xobjarray);
 
 private:
     std::vector<iv::lidarobj_rec> mvectorlidarobj_rec;
@@ -39,6 +53,8 @@ private:
 
 private:
     void ChangePos(iv::lidarobj_rec * xrec);
+    int MergeLast(iv::lidar::objectarray & xselarray);
+
 
 private:
     int64_t mnLookTime = 3000;  //3000 ms data
@@ -48,6 +64,8 @@ private:
     double mfLidarOffX = 0;  //Right
     double mfLidarOffY = 0;  //Front
 
+    double mfDisThresh = 1.0;
+
 
 };
 

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -6,6 +6,9 @@
 #include "objectarray.pb.h"
 #include "fusionobjectarray.pb.h"
 #include "fusionobject.pb.h"
+#include "gpsimu.pb.h"
+#include "lidarbuffer.h"
+#include "xmlparam.h"
 #include <QThread>
 #include <QString>
 #include <QMutex>
@@ -42,6 +45,9 @@ TrackerSettings settings;
 CTracker tracker(settings);
 bool m_isTrackerInitialized = false;
 
+static bool gbUseLastLidar = false;
+static lidarbuffer * gplidarbuffer;
+
 
 void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -70,7 +76,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     //        }
 
     //    }
-
+//std::cout<<"radar size:"<<nSize<<std::endl;
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
 }
@@ -85,9 +91,38 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
+
+//    static int a = 0;
+//    if(a != 0)
+//    {
+//        lidarobj.clear_obj();
+//    }
+//    a++;
+//    if(a == 2)a = 0;
+
+
+    std::cout<<" before obj size: "<<lidarobj.obj_size()<<std::endl;
+    if(gbUseLastLidar)
+    {
+        iv::lidar::objectarray lidarobjraw;
+        lidarobjraw.CopyFrom(lidarobj);
+        gplidarbuffer->FilterLidarObj(lidarobj);
+        gplidarbuffer->AddLidarObj(lidarobjraw);
+
+    }
+
+
+    std::cout<<" after obj size: "<<lidarobj.obj_size()<<std::endl;
+
+
     //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
-    //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
+//        std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
+    for(int i = 0; i < lidarobj.obj_size();i++) {
+        if(abs(lidarobj.obj(i).centroid().x())<4){
+            std::cout<<lidarobj.obj(i).velocity_linear_x()<<std::endl;
+        }
+    }
     datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
@@ -127,9 +162,9 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     //    li_ra_fusion.Clear();
     //      std::cout<<" RLFusion     begin    "<<std::endl;
-    AddMobileye(li_ra_fusion,mobileye_info);
+//    AddMobileye(li_ra_fusion,mobileye_info);
     //    std::cout<<" RLFusion     end      "<<std::endl;
-    mobileye_info.clear_xobj();
+//    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++)
     //    {
@@ -159,20 +194,20 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     //    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);
+//    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;
+    iv::fusion::fusionobjectarray out_fusion = li_ra_fusion;
     ObsToNormal(out_fusion);
     string out;
 
@@ -206,15 +241,63 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     //    gMutex.unlock();
 }
 
+void ListenGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenGPS Parse error."<<std::endl;
+        return;
+    }
+
+    if(gbUseLastLidar)
+    {
+        gplidarbuffer->AddGPS(xgpsimu);
+    }
+}
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+    if(argc < 2)
+        strpath = strpath + "/lidar_radar_fusion_cnn.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+
+
     tracker.setSettings(settings);
 
+    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+    std::string strgpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
+    std::string struselast = xparam.GetParam("uselast","false");
+    std::string strlooktime = xparam.GetParam("LookTime","3000"); //3000ms
+    std::string strlookprob = xparam.GetParam("LookProb","0.3");
+    std::string strlidaroffx = xparam.GetParam("LidarOffX","0.0");
+    std::string strlidaroffy = xparam.GetParam("LidarOffY","0.0");
+    std::string strdisthresh = xparam.GetParam("disthresh","1.0");
+
+    if(struselast == "true")
+    {
+        gbUseLastLidar = true;
+        gplidarbuffer = new lidarbuffer(atoi(strlooktime.data()),atof(strlookprob.data()),
+                                        atof(strlidaroffx.data()),atof(strlidaroffy.data()),atof(strdisthresh.data()));
+    }
+    else
+    {
+        gbUseLastLidar = false;
+    }
+
+    void * pa = iv::modulecomm::RegisterRecv(strgpsmsgname.data(),ListenGPS);
+
     void *gpa;
-    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
-    gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("radar_front",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
     return a.exec();
 }

src/fusion/lidar_radar_fusion_cnn/transformation.cpp

@@ -18,14 +18,16 @@ float iv::fusion::Transformation::ComputeRadarSpeed(double vx, double vy)
 //radar to lidar
 Eigen::Matrix<float,3,1> iv::fusion::Transformation::RadarToLidar(Eigen::Matrix<float,3,1> radar_int_radar)
 {
-    Eigen::Matrix<float,3,3> radar_to_lidar_R;
-    radar_to_lidar_R << -0.07164891, -0.99734797, -0.01278487,
-                        0.99733994,  -0.07180872, 0.01251175,
-                        -0.01339663, -0.0118544,  0.99983999;
+//    Eigen::Matrix<float,3,3> radar_to_lidar_R;
+//    radar_to_lidar_R << -0.07164891, -0.99734797, -0.01278487,
+//                        0.99733994,  -0.07180872, 0.01251175,
+//                        -0.01339663, -0.0118544,  0.99983999;
     Eigen::Matrix<float,3,1> radar_in_lidar;
     Eigen::Matrix<float,3,1> radar_to_lidar_T;
-    radar_to_lidar_T << -0.35455075, -0.84757324, 0.26293475;
-    radar_in_lidar = radar_to_lidar_R*radar_int_radar + radar_to_lidar_T;
+    radar_to_lidar_T << 0, 3.67, 0;
+    radar_in_lidar = radar_int_radar + radar_to_lidar_T;
+//    radar_in_lidar = radar_to_lidar_R*radar_int_radar + radar_to_lidar_T;
+
     return radar_in_lidar;
 }