change lidar_radar_fusion_cnn, add look last.

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)
@@ -73,8 +77,9 @@ void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
     mmutex.lock();
     iv::lidarobj_rec xrec;
     xrec.nTime = xobjarray.timestamp()/1000; //to ms
-    ChangePos(&xrec);
     xrec.xobjarray.CopyFrom(xobjarray);
+    ChangePos(&xrec);
+
     mvectorlidarobj_rec.push_back(xrec);
     mmutex.unlock();
 }
@@ -115,6 +120,10 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
     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);
 
@@ -133,3 +142,164 @@ void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
 
     }
 }
+
+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()/1000; //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);
+
+        }
+    }
+
+    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

@@ -2,6 +2,7 @@
 #include <QDateTime>
 #include <iostream>
 #include "modulecomm.h"
+#include "xmlparam.h"
 #include "radarobjectarray.pb.h"
 #include "objectarray.pb.h"
 #include "fusionobjectarray.pb.h"
@@ -15,6 +16,8 @@
 #include "Tracking.hpp"
 #include "transformation.h"
 #include "mobileye.pb.h"
+#include "gpsimu.pb.h"
+#include "lidarbuffer.h"
 using namespace iv;
 using namespace fusion;
 
@@ -42,6 +45,8 @@ 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)
 {
@@ -85,6 +90,14 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
+
+
+    if(gbUseLastLidar)
+    {
+        gplidarbuffer->FilterLidarObj(lidarobj);
+        gplidarbuffer->AddLidarObj(lidarobj);
+    }
+
     //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
     //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
@@ -206,15 +219,64 @@ 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 *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);
+
     return a.exec();
 }