geely fusion update

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -737,7 +737,7 @@ int main(int argc, char *argv[])
     {
         std::cout<<"use trt model."<<std::endl;
         pPillars = new PointPillars(
-                    0.15,
+                    0.17,
                     0.10,
                     false,
                     pfe_trt_file,
@@ -746,13 +746,13 @@ int main(int argc, char *argv[])
                     );
     }
     std::cout<<"PointPillars Init OK."<<std::endl;
-    Eigen::AngleAxisd r_z ( 0.00654, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
-    Eigen::AngleAxisd r_y ( 0.13, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
-    Eigen::AngleAxisd r_x ( -0.0377, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
+    Eigen::AngleAxisd r_z ( 0.37/180.0*M_PI, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+    Eigen::AngleAxisd r_y ( 7.45/180.0*M_PI, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+    Eigen::AngleAxisd r_x ( -2.16/180.0*M_PI, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
     Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
     // 四元数-->>旋转矩阵
     rotation_matrix = q_zyx.toRotationMatrix();
-    trans_matrix << 0, 1.1, 0.35;//x,y,z
+    trans_matrix << 0, 1.04, 0.35;//x,y,z
     gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
     gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
     gpthread = new std::thread(statethread);

src/driver/driver_lidar_merge/driver_lidar_merge.pro

@@ -8,7 +8,7 @@ CONFIG -= app_bundle
 # depend on your compiler). Please consult the documentation of the
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
-
+QMAKE_LFLAGS += -no-pie
 
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.
@@ -18,11 +18,18 @@ DEFINES += QT_DEPRECATED_WARNINGS
 SOURCES += main.cpp \
     lidarmerge.cpp
 
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/eigen3
+
+LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
 
 HEADERS += \
     lidarmerge.h
 
 
+unix:LIBS += -lboost_thread -lboost_system
+
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
 }

src/driver/driver_lidar_merge/driver_lidar_merge.yaml

@@ -6,18 +6,61 @@ center:
   memname: lidarpc_center
   offset:
     x: 0.0
-    y: 0.0
-    z: 0.0
+    y: 1.04
+    z: 1.25
+  angle:
+    x: -2.16   # pitch -
+    y: 7.45    # roll  
+    z: 0.37    # yaw   +
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
 left:
   memname: lidarpc_left
   offset:
-    x: -0.64
-    y: 0.0
-    z: -0.13
+    x: -0.88
+    y: 1.08
+    z: 0.98
+  angle:
+    x: 45   # pitch -
+    y: -25   # roll  +
+    z: -71.8   # yaw   +
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
 right:
   memname: lidarpc_right
   offset:
-    x: 0.64
-    y: 0.0
-    z: -0.13
+    x: 0.88
+    y: 1.08
+    z: 0.98
+  angle:
+    x: 45  # pitch -
+    y: 25   # roll  +
+    z: 69.5   # yaw   +
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
+ignore: # if x>xmin&&x<xmax&&y>ymin&&y<ymax ignore
+  xmin: -0.9475 
+  ymin: -1.1
+  xmax: 0.9475
+  ymax: 3.67
+  zmin: -1 # if z<zmin ignore
+  zmax: 3    # if z>zmax ignore
 output: lidar_pc

src/driver/driver_lidar_merge/lidarmerge.cpp

@@ -2,6 +2,10 @@
 
 #include <QDateTime>
 
+#include <QMutex>
+
+extern QMutex gMutex;
+
 static int g_seq = 0;
 
 pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar)
@@ -16,82 +20,66 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvect
     point_cloud->header.seq =g_seq;
     g_seq++;
 
-    double tan_fmin = tan(-15.0*M_PI/180.0);
 
     int i,j;
     for(i=0;i<xvectorlidar.size();i++)
     {
+        Eigen::Matrix3d rotation_matrix;
+        Eigen::Vector3d trans_matrix;
+        Eigen::AngleAxisd r_z ( xvectorlidar[i].fang_z/180.0*M_PI, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw
+        Eigen::AngleAxisd r_y ( xvectorlidar[i].fang_y/180.0*M_PI, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll
+        Eigen::AngleAxisd r_x ( xvectorlidar[i].fang_x/180.0*M_PI, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch
+        Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序
+        // 四元数-->>旋转矩阵
+        rotation_matrix = q_zyx.toRotationMatrix();
+        trans_matrix << xvectorlidar[i].foff_x, xvectorlidar[i].foff_y, xvectorlidar[i].foff_z;//x,y,z
+
         int nsize;
-        if(xvectorlidar[i].bUpdate)
+
+        pcl::PointCloud<pcl::PointXYZI>::Ptr partpoint_cloud;
+        int64_t curtime = QDateTime::currentMSecsSinceEpoch();
+        if((xvectorlidar[i].bUpdate)||((curtime -xvectorlidar[i].mupdatetime)<150))
         {
-            nsize = xvectorlidar[i].mpoint_cloud->width;
+            gMutex.lock();
+            partpoint_cloud = xvectorlidar[i].mpoint_cloud;
+            gMutex.unlock();
+            nsize = partpoint_cloud->width;
             qDebug("size is %d ",nsize);
-
-            if(strncmp(xvectorlidar[i].strmemname,"lidarpc_center",255) == 0)
-            {
-                for(j=0;j<nsize;j++)
-                {
-                    pcl::PointXYZI point;
-                    point = xvectorlidar[i].mpoint_cloud->at(j);
-                    if((abs(point.x)<0.9)&&(abs(point.y)<2.3))
-                    {
-
-                    }
-                    else
-                    {
-                        point_cloud->push_back(point);
-                        ++point_cloud->width;
-                    }
-                }
-            }
-            else
+            for(j=0;j<nsize;j++)
             {
+                pcl::PointXYZI point;
+                point = partpoint_cloud->at(j);
+                Eigen::Vector3d new_point, old_point;
+                old_point<<point.x, point.y, point.z;
+                new_point = rotation_matrix * (old_point) + trans_matrix;
+                point.x = new_point[0];
+                point.y = new_point[1];
+                point.z = new_point[2];
+
+                if(point.x>xvectorlidar[i].fmax_x)continue;
+                if(point.y>xvectorlidar[i].fmax_y)continue;
+                if(point.z>xvectorlidar[i].fmax_z)continue;
+                if(point.z > xvectorlidar[i].fignore_zmax) continue;
+                if(point.z < xvectorlidar[i].fignore_zmin) continue;
+//                std::cout<<"  fignore  zmin  "<<xvectorlidar[i].fignore_zmin<<std::endl;
 
-//                double max = -1000.0;
-//                double min = 1000.0;
-                for(j=0;j<nsize;j++)
+                if(point.x<xvectorlidar[i].fmin_x)continue;
+                if(point.y<xvectorlidar[i].fmin_y)continue;
+                if(point.z<xvectorlidar[i].fmin_z)continue;
+
+                if((point.x>xvectorlidar[i].fignore_xmin)&&(point.x<xvectorlidar[i].fignore_xmax)&&(point.y>xvectorlidar[i].fignore_ymin)&&(point.y<xvectorlidar[i].fignore_ymax))
                 {
-                    pcl::PointXYZI point;
-                    point = xvectorlidar[i].mpoint_cloud->at(j);
-                    point.x = point.x + xvectorlidar[i].foff_x;
-                    point.y = point.y + xvectorlidar[i].foff_y;
-                    point.z = point.z + xvectorlidar[i].foff_z;
-
- //                   if((point.x<-1.0)&&(abs(point.y)<15.0))
-//                    if((point.x<-2.0)&&(point.y>-15.0)&&(point.y < 15.0))
-//                    {
-//                        if(point.z<min)min = point.z;
-//                        if(point.z>max)max = point.z;
-//                    }
-                    bool bUse = false;
-                    if(point.z<(tan_fmin*sqrt(point.x*point.x + point.y*point.y)))
-                    {
-                        bUse = true;
-                    }
-
-//                    bUse = true;
-                    if(((fabs(point.x)<0.95)&&(abs(point.y)<2.3))||(bUse == false))
-                    {
-
-                    }
-                    else
-                    {
-                        if((point.y>0.6)&&(point.y<1.8)&&(fabs(point.x)<1.3))
-                        {
-
-                        }
-                        else
-                        {
-                            point_cloud->push_back(point);
-                            ++point_cloud->width;
-                        }
-                    }
+                    continue;
                 }
- //               qDebug("min = %f max = %f",min,max);
+                if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
+
+                point_cloud->push_back(point);
+                ++point_cloud->width;
             }
+
+
         }
     }
     qDebug("merge size is %d ",point_cloud->width);
-
     return point_cloud;
 }

src/driver/driver_lidar_merge/lidarmerge.h

@@ -4,6 +4,8 @@
 #include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
 
+
+
 namespace  iv {
 struct lidar_data
 {
@@ -11,8 +13,25 @@ struct lidar_data
     double foff_x = 0.0;
     double foff_y = 0.0;
     double foff_z = 0.0;
+    double fang_x = 0.0;
+    double fang_y = 0.0;
+    double fang_z = 0.0;
     bool bUpdate = false;
-    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    double fmax_x = 0;
+    double fmax_y = 0;
+    double fmax_z = 0;
+    double fmin_x = 0;
+    double fmin_y = 0;
+    double fmin_z = 0;
+    double fignore_xmax = 0;
+    double fignore_ymax = 0;
+    double fignore_xmin = 0;
+    double fignore_ymin = 0;
+    double fignore_zmax = 0;
+    double fignore_zmin = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud = NULL;
+    int64_t mupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_old = NULL;
 };
 
 }

src/driver/driver_lidar_merge/main.cpp

@@ -3,6 +3,8 @@
 
 #include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
+//#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
 
 #include <iostream>
 #include <fstream>
@@ -62,6 +64,52 @@ void dec_yaml(const char * stryamlpath)
     std::string stroffset_x;
     std::string stroffset_y;
     std::string stroffset_z;
+    std::string strangle_x;
+    std::string strangle_y;
+    std::string strangle_z;
+    std::string strtem;
+
+    double fignore_xmax = 0;
+    double fignore_ymax = 0;
+    double fignore_xmin = 0;
+    double fignore_ymin = 0;
+    double fignore_zmax = 0;
+    double fignore_zmin = 0;
+
+    if(config["ignore"]["xmin"])
+    {
+        strtem = config["ignore"]["xmin"].as<std::string>();
+        fignore_xmin = atof(strtem.data());
+    }
+
+    if(config["ignore"]["ymin"])
+    {
+        strtem = config["ignore"]["ymin"].as<std::string>();
+        fignore_ymin = atof(strtem.data());
+    }
+
+    if(config["ignore"]["xmax"])
+    {
+        strtem = config["ignore"]["xmax"].as<std::string>();
+        fignore_xmax = atof(strtem.data());
+    }
+
+    if(config["ignore"]["ymax"])
+    {
+        strtem = config["ignore"]["ymax"].as<std::string>();
+        fignore_ymax = atof(strtem.data());
+    }
+    if(config["ignore"]["zmax"])
+    {
+        strtem = config["ignore"]["zmax"].as<std::string>();
+        fignore_zmax = atof(strtem.data());
+    }
+    if(config["ignore"]["zmin"])
+    {
+        strtem = config["ignore"]["zmin"].as<std::string>();
+        fignore_zmin = atof(strtem.data());
+    }
+
 
     for(i=0;i<nlidarsize;i++)
     {
@@ -73,15 +121,62 @@ void dec_yaml(const char * stryamlpath)
                 stroffset_x = config[veclidarname[i].data()]["offset"]["x"].as<std::string>();
                 stroffset_y = config[veclidarname[i].data()]["offset"]["y"].as<std::string>();
                 stroffset_z = config[veclidarname[i].data()]["offset"]["z"].as<std::string>();
+                strangle_x = config[veclidarname[i].data()]["angle"]["x"].as<std::string>();
+                strangle_y = config[veclidarname[i].data()]["angle"]["y"].as<std::string>();
+                strangle_z = config[veclidarname[i].data()]["angle"]["z"].as<std::string>();
 
                 iv::lidar_data xlidardata;
                 xlidardata.foff_x = atof(stroffset_x.data());
                 xlidardata.foff_y = atof(stroffset_y.data());
                 xlidardata.foff_z = atof(stroffset_z.data());
+                xlidardata.fang_x = atof(strangle_x.data());
+                xlidardata.fang_y = atof(strangle_y.data());
+                xlidardata.fang_z = atof(strangle_z.data());
+                if(config[veclidarname[i].data()]["maximum"]["x"])
+                {
+                    strtem = config[veclidarname[i].data()]["maximum"]["x"].as<std::string>();
+                    xlidardata.fmax_x = atof(strtem.data());
+                }
+                if(config[veclidarname[i].data()]["maximum"]["y"])
+                {
+                    strtem = config[veclidarname[i].data()]["maximum"]["y"].as<std::string>();
+                    xlidardata.fmax_y = atof(strtem.data());
+                }
+                if(config[veclidarname[i].data()]["maximum"]["z"])
+                {
+                    strtem = config[veclidarname[i].data()]["maximum"]["z"].as<std::string>();
+                    xlidardata.fmax_z = atof(strtem.data());
+                }
+                if(config[veclidarname[i].data()]["minimum"]["x"])
+                {
+                    strtem = config[veclidarname[i].data()]["minimum"]["x"].as<std::string>();
+                    xlidardata.fmin_x = atof(strtem.data());
+                }
+                if(config[veclidarname[i].data()]["minimum"]["y"])
+                {
+                    strtem = config[veclidarname[i].data()]["minimum"]["y"].as<std::string>();
+                    xlidardata.fmin_y = atof(strtem.data());
+                }
+                if(config[veclidarname[i].data()]["minimum"]["z"])
+                {
+                    strtem = config[veclidarname[i].data()]["minimum"]["z"].as<std::string>();
+                    xlidardata.fmin_z = atof(strtem.data());
+                }
+
+                xlidardata.fignore_xmax = fignore_xmax;
+                xlidardata.fignore_ymax = fignore_ymax;
+                xlidardata.fignore_xmin = fignore_xmin;
+                xlidardata.fignore_ymin = fignore_ymin;
+                xlidardata.fignore_zmax = fignore_zmax;
+                xlidardata.fignore_zmin = fignore_zmin;
+
+
                 strncpy(xlidardata.strmemname,strmemname.data(),255);
                 gvectorlidar.push_back(xlidardata);
                 qDebug("name is %s ",strmemname.data());
+
             }
+
         }
     }
 
@@ -138,7 +233,9 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
         if(strncmp(strmemname,gvectorlidar[i].strmemname,255) == 0)
         {
             gMutex.lock();
+            gvectorlidar[i].mpoint_cloud_old = gvectorlidar[i].mpoint_cloud;
             gvectorlidar[i].mpoint_cloud = point_cloud;
+            gvectorlidar[i].mupdatetime = QDateTime::currentMSecsSinceEpoch();
             gvectorlidar[i].bUpdate = true;
             gMutex.unlock();
         }
@@ -170,6 +267,9 @@ void merge()
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());
 
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
 
 
     gMutex.lock();
@@ -180,6 +280,11 @@ void merge()
     }
     gMutex.unlock();
     point_cloud = mergefunc(xvectorlidar);
+//    pcl::RadiusOutlierRemoval<pcl::PointXYZI> pcFilter;
+//    pcFilter.setInputCloud(point_cloud);
+//    pcFilter.setRadiusSearch(0.8);
+//    pcFilter.setMinNeighborsInRadius(5);
+//    pcFilter.filter(*cloud_filtered);
     sharepointcloud(point_cloud,gpaout);
 }
 
@@ -198,7 +303,7 @@ void mergethread()
                 break;
             }
         }
-        if(((QDateTime::currentMSecsSinceEpoch() - glasttime)>150) ||(bNOtAllOK == false))
+        if(((QDateTime::currentMSecsSinceEpoch() - glasttime)>=150) ||(bNOtAllOK == false))
         {
             merge();
             qDebug("time is %ld",glasttime);

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;
@@ -102,7 +105,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         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());
-        fusion_object.set_velocity_linear_x(-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x());
 
         if(match_idx.at(i).nradar == -1000)
         {
@@ -119,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);
 
@@ -139,10 +144,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);
 
@@ -150,6 +158,8 @@ 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());
+            std::cout<<"lidar: "<<-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()<<std::endl;
+
             vel_abs_ = fusion_object.mutable_vel_abs();
             vel_abs_->CopyFrom(vel_abs);
 
@@ -181,13 +191,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);
 

src/fusion/lidar_radar_fusion_cnn/fusion_probabilities.cpp

@@ -6,7 +6,7 @@ int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::rad
     Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
     radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
     radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
-//    std::cout<<" x    y   "<<lidarobject.centroid().x()<<"    "<<lidarobject.centroid().y()<<std::endl;
+//    std::cout<<" x    y   "<<radarPoint.x()<<radarPoint.x()<<"    "<<radarPoint.y()<<lidarobject.centroid().x()<<"    "<<lidarobject.centroid().y()<<std::endl;
     if(!((radarPoint.x()>=(lidarobject.centroid().x() - lidarobject.dimensions().x()/2.0 -2))&&
          (radarPoint.y()>= (lidarobject.centroid().y()-lidarobject.dimensions().y()/2.0 -2 ))
          &&(radarPoint.x()<=(lidarobject.centroid().x() + lidarobject.dimensions().x()/2.0 +2))&&

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -70,7 +70,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     //        }
 
     //    }
-
+//std::cout<<"radar size:"<<nSize<<std::endl;
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
 }
@@ -87,7 +87,12 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
     }
     //    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();
 }
@@ -213,7 +218,7 @@ int main(int argc, char *argv[])
     tracker.setSettings(settings);
 
     void *gpa;
-    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
+    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;
 }