change laneAtt lane_decition. fusion.

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/main.cpp

@@ -325,6 +325,8 @@ void threadgetres()
             //   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/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/lidarbuffer.cpp

@@ -77,6 +77,7 @@ void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
     mmutex.lock();
     iv::lidarobj_rec xrec;
     xrec.nTime = xobjarray.timestamp()/1000; //to ms
+
     xrec.xobjarray.CopyFrom(xobjarray);
     ChangePos(&xrec);
 
@@ -95,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++)
@@ -127,6 +130,7 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
     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);
@@ -141,6 +145,9 @@ 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)
@@ -297,6 +304,7 @@ int lidarbuffer::FilterLidarObj(iv::lidar::objectarray & xobjarray)
             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;
 
         }
     }

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -2,11 +2,13 @@
 #include <QDateTime>
 #include <iostream>
 #include "modulecomm.h"
-#include "xmlparam.h"
 #include "radarobjectarray.pb.h"
 #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>
@@ -16,8 +18,6 @@
 #include "Tracking.hpp"
 #include "transformation.h"
 #include "mobileye.pb.h"
-#include "gpsimu.pb.h"
-#include "lidarbuffer.h"
 using namespace iv;
 using namespace fusion;
 
@@ -48,6 +48,7 @@ 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)
 {
 
@@ -75,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();
 }
@@ -98,9 +99,15 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
         gplidarbuffer->AddLidarObj(lidarobj);
     }
 
+
     //    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();
 }
@@ -140,9 +147,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++)
     //    {
@@ -172,20 +179,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;
 
@@ -271,12 +278,11 @@ int main(int argc, char *argv[])
         gbUseLastLidar = false;
     }
 
-    void *gpa;
-    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
-    gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
-    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
-
     void * pa = iv::modulecomm::RegisterRecv(strgpsmsgname.data(),ListenGPS);
 
+    void *gpa;
+    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;
 }