change lidar_radar_fusion_cnn, not complete.

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -32,7 +32,10 @@ HEADERS += \
     ../../include/msgtype/mobileye_lane.pb.h \
     ../../include/msgtype/mobileye_obs.pb.h \
     ../../include/msgtype/mobileye_tsr.pb.h \
-    Tracker/Tracking.hpp
+    Tracker/Tracking.hpp \
+    lidarbuffer.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../common/common/math/gnss_coordinate_convert.h
 
 
 SOURCES += \
@@ -52,7 +55,10 @@ SOURCES += \
     ../../include/msgtype/mobileye.pb.cc \
     ../../include/msgtype/mobileye_lane.pb.cc \
     ../../include/msgtype/mobileye_obs.pb.cc \
-    ../../include/msgtype/mobileye_tsr.pb.cc
+    ../../include/msgtype/mobileye_tsr.pb.cc \
+    lidarbuffer.cpp \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../common/common/math/gnss_coordinate_convert.cpp
 
 
 INCLUDEPATH += /usr/include/eigen3
@@ -66,6 +72,10 @@ INCLUDEPATH += $$PWD/../../include/msgtype
     error( "Couldn't find the ivprotobuf.pri file!" )
 }
 
+#!include(../../../include/ivopencv.pri ) {
+#    error( "Couldn't find the ivopencv.pri file!" )
+#}
+
 INCLUDEPATH += /usr/include/opencv4/
 LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 

src/fusion/lidar_radar_fusion_cnn/lidarbuffer.cpp

@@ -0,0 +1,135 @@
+#include "lidarbuffer.h"
+
+#include <iostream>
+#include "math/gnss_coordinate_convert.h"
+
+lidarbuffer::lidarbuffer()
+{
+
+}
+
+void lidarbuffer::AddGPS(iv::gps::gpsimu & xgpsimu)
+{
+    const int nMaxGPSFre = 100;
+    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxGPSFre*2/1000) ) )
+    {
+        std::cout<<" Warning. GPS Buffer Warning."<<std::endl;
+        mmutex.lock();
+        mvectorgps_rec.clear();;
+        mmutex.unlock();
+    }
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    mmutex.lock();
+    while(mvectorgps_rec.size()>0)
+    {
+        if(abs(nnow - mvectorgps_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
+        {
+            mvectorgps_rec.erase(mvectorgps_rec.begin());
+        }
+        else
+        {
+            break;
+        }
+    }
+    mmutex.unlock();
+
+    mmutex.lock();
+    iv::gps_rec xrec;
+    xrec.nTime = xgpsimu.msgtime();
+    xrec.xgpsimu.CopyFrom(xgpsimu);
+    mvectorgps_rec.push_back(xrec);
+    mmutex.unlock();
+}
+
+void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
+{
+    const int nMaxLidarFre = 100;
+    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxLidarFre*2/1000) ) )
+    {
+        std::cout<<" Warning. Lidar Buffer Warning."<<std::endl;
+        mmutex.lock();
+        mvectorlidarobj_rec.clear();;
+        mmutex.unlock();
+    }
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    mmutex.lock();
+    while(mvectorlidarobj_rec.size()>0)
+    {
+        if(abs(nnow - mvectorlidarobj_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
+        {
+            mvectorlidarobj_rec.erase(mvectorlidarobj_rec.begin());
+        }
+        else
+        {
+            break;
+        }
+    }
+    mmutex.unlock();
+
+    mmutex.lock();
+    iv::lidarobj_rec xrec;
+    xrec.nTime = xobjarray.timestamp()/1000; //to ms
+    ChangePos(&xrec);
+    xrec.xobjarray.CopyFrom(xobjarray);
+    mvectorlidarobj_rec.push_back(xrec);
+    mmutex.unlock();
+}
+
+void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
+{
+    if(mvectorgps_rec.size() == 0)
+    {
+        std::cout<<" warning no gps data. "<<std::endl;
+        return;
+    }
+
+    unsigned int nnearindex = 0;
+    int64_t ntimediff = mnLookTime;
+
+    unsigned int nsize = static_cast<unsigned int>( mvectorgps_rec.size());
+    unsigned int i;
+    for(i=1;i<nsize;i++)
+    {
+        if(abs(xrec->nTime - mvectorgps_rec[i].nTime)<abs(ntimediff))
+        {
+            ntimediff = xrec->nTime - mvectorgps_rec[i].nTime;
+            nnearindex = i;
+        }
+    }
+
+    if(abs(ntimediff) > 100)
+    {
+        std::cout<<"warning. lidar gps time diff more than 100ms"<<std::endl;
+    }
+
+    iv::gps::gpsimu xgpsimu;
+    xgpsimu.CopyFrom(mvectorgps_rec[nnearindex].xgpsimu);
+
+    double hdg = (90 - xgpsimu.heading())*M_PI/180.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);
+
+    double x_gps,y_gps;
+    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_gps,&y_gps);
+
+    for(i=0;i<nsize;i++)
+    {
+        iv::lidar::lidarobject * pobj = xrec->xobjarray.mutable_obj(i);
+        double x_raw = pobj->position().x();
+        double y_raw = pobj->position().y();
+
+        double x_abs = x_gps + x_sensor + x_raw * cos(fsr) - y_raw * sin(fsr);
+        double y_abs = y_gps + y_sensor + x_raw * sin(fsr) + y_raw * cos(fsr);
+
+        pobj->mutable_position()->set_x(x_abs);
+        pobj->mutable_position()->set_y(y_abs);
+
+
+    }
+}

src/fusion/lidar_radar_fusion_cnn/lidarbuffer.h

@@ -0,0 +1,54 @@
+#ifndef LIDARBUFFER_H
+#define LIDARBUFFER_H
+
+
+#include "objectarray.pb.h"
+#include "gpsimu.pb.h"
+
+#include <vector>
+#include <mutex>
+
+namespace  iv {
+
+struct  lidarobj_rec
+{
+    iv::lidar::objectarray xobjarray;
+    int64_t nTime;    //ms since epoch
+};
+
+struct gps_rec
+{
+    iv::gps::gpsimu xgpsimu;
+    int64_t nTime; //ms since epoch
+};
+
+}
+
+class lidarbuffer
+{
+public:
+    lidarbuffer();
+
+public:
+    void AddGPS(iv::gps::gpsimu & xgpsimu);
+    void AddLidarObj(iv::lidar::objectarray & xobjarray);
+
+private:
+    std::vector<iv::lidarobj_rec> mvectorlidarobj_rec;
+    std::vector<iv::gps_rec> mvectorgps_rec;
+
+private:
+    void ChangePos(iv::lidarobj_rec * xrec);
+
+private:
+    int64_t mnLookTime = 3000;  //3000 ms data
+    std::mutex mmutex;
+    double mfProb_Thresh = 0.3;
+
+    double mfLidarOffX = 0;  //Right
+    double mfLidarOffY = 0;  //Front
+
+
+};
+
+#endif // LIDARBUFFER_H