change view_ndtmatching. add detection show.

src/tool/view_ndtmatching/main.cpp

@@ -19,6 +19,8 @@ bool gbShowPointCloud = true;
 bool gbShowXODRAsLine = false;
 double gfVehicleZOff = 0.0;
 
+bool gbShowCNN = true;
+
 
 void LoadCameraPos(QString strpath)
 {
@@ -36,6 +38,7 @@ void LoadCameraPos(QString strpath)
     gfcustom_view_y = xparam.GetParam("custom_view_y",0.0);
     gfcustom_view_z = xparam.GetParam("custom_view_z",0.0);
     gfcustom_yawrotate = xparam.GetParam("custom_yawrotate",0.0) *M_PI/180.0;
+    gbShowCNN = xparam.GetParam("ShowCNN",false);
 }
 
 int main(int argc, char *argv[])

src/tool/view_ndtmatching/mainwindow.cpp

@@ -45,6 +45,12 @@ static bool gbCarModelLoad = false;
 extern bool gbShowPointCloud;
 extern bool gbShowXODRAsLine;
 extern double gfVehicleZOff;
+extern bool gbShowCNN;
+
+static std::vector<std::string> gvectorobjviewname;
+static int64_t gnLastLidarUpdatems = 0;
+static iv::lidar::objectarray gobjarray;
+static std::mutex gmutexlidarobj;
 
 #include <cmath>
 
@@ -262,7 +268,7 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
         //   viewer.addCube(mpos_x+20 -0.9,mpos_x + 20+0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
         //  viewer.addCube(trans,quat,6.0,3.0,1.0,"car");
 
-        Eigen::AngleAxisf rotation_vector(gCurPose.z, Eigen::Vector3f(0, 0, 1));
+        Eigen::AngleAxisf rotation_vector(gCurPose.yaw, Eigen::Vector3f(0, 0, 1));
 
         viewer.addCube(Eigen::Vector3f(gCurPose.x,gCurPose.y,gCurPose.z-1.15),
                        Eigen::Quaternionf(rotation_vector), 4.6, 2.3, 1.5, "car");
@@ -300,6 +306,56 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
         }
     }
 
+
+    if(gbShowCNN)
+    {
+        unsigned int i;
+        for(i=0;i<gvectorobjviewname.size();i++)
+        {
+            viewer.removeShape(gvectorobjviewname[i]);
+        }
+        gvectorobjviewname.clear();
+        int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+        static int64_t nobjuptime = 0;
+        static iv::lidar::objectarray xlidarobjarray;
+
+        if(nobjuptime != gnLastLidarUpdatems)
+        {
+            gmutexlidarobj.lock();
+            xlidarobjarray.CopyFrom(gobjarray);
+            gmutexlidarobj.unlock();
+            nobjuptime = gnLastLidarUpdatems;
+        }
+
+        if(abs(nNow - nobjuptime) < 3000)
+        {
+            unsigned int j;
+            for(j=0;j<xlidarobjarray.obj_size();j++)
+            {
+                iv::lidar::lidarobject * pobj = xlidarobjarray.mutable_obj(j);
+                double fobjpos_x,fobjpos_y,fobjpos_z;
+                fobjpos_x = gCurPose.x + pobj->position().x() * cos(gCurPose.yaw) - pobj->position().y() * sin(gCurPose.yaw);
+                fobjpos_y = gCurPose.y + pobj->position().x() * sin(gCurPose.yaw) - pobj->position().y() * cos(gCurPose.yaw);
+                fobjpos_z = pobj->position().z();
+                double fobj_yaw = gCurPose.yaw + pobj->tyaw();
+                char strname[256];
+                snprintf(strname,256,"obj%d",j);
+                std::string strobjname = strname;
+
+                Eigen::AngleAxisf rotation_vector(fobj_yaw, Eigen::Vector3f(0, 0, 1));
+
+                viewer.addCube(Eigen::Vector3f(fobjpos_x,fobjpos_y,fobjpos_z),
+                               Eigen::Quaternionf(rotation_vector), pobj->dimensions().x(),pobj->dimensions().y(), pobj->dimensions().z(), strobjname);
+
+                gvectorobjviewname.push_back(strobjname);
+
+
+            }
+        }
+
+    }
+
     std::stringstream ss;
  //   viewer.setCameraPosition(mpos_x,0,30,mpos_x + 20,0,0,0,0,0);
  //   ss << "Once per viewer loop: " << count++;
@@ -316,6 +372,8 @@ MainWindow::MainWindow(QWidget *parent) :
 {
     ui->setupUi(this);
 
+    gvectorobjviewname.clear();
+
     mpLabelStatus = new QLabel("No NDT Pos",ui->statusbar);
     mpLabelStatus->setMinimumWidth(350);
 
@@ -356,6 +414,9 @@ MainWindow::MainWindow(QWidget *parent) :
     ModuleFun funfusiongpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpafusiongpsimu  = iv::modulecomm::RegisterRecvPlus("fusion_gpsimu",funfusiongpsimu);
 
+    ModuleFun funcnndetect =std::bind(&MainWindow::Updatelidarcnndetect,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpacnndetect  = iv::modulecomm::RegisterRecvPlus("lidar_track",funcnndetect);
+
     connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
 
     setWindowTitle("NDT Matching View & Relocation Enable");
@@ -367,6 +428,8 @@ MainWindow::~MainWindow()
 {
     mbRun = false;
     mpthreadpcd->join();
+    iv::modulecomm::Unregister(mpacnndetect);
+    iv::modulecomm::Unregister(mpafusiongpsimu);
     iv::modulecomm::Unregister(mpandtpos);
     iv::modulecomm::Unregister(mparelocate);
     iv::modulecomm::Unregister(mpapcdmap);
@@ -533,6 +596,23 @@ void MainWindow::on_comboBox_currentIndexChanged(int index)
     gnViewMode = index;
 }
 
+void MainWindow::Updatelidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::objectarray lidarobj;
+
+    if(nSize<1)return;
+    if(false == lidarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Updatelidarcnndetect parse fail."<<std::endl;
+        return;
+    }
+    gmutexlidarobj.lock();
+    gnLastLidarUpdatems = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    gobjarray.CopyFrom(lidarobj);
+    gmutexlidarobj.unlock();
+
+}
+
 void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 

src/tool/view_ndtmatching/mainwindow.h

@@ -16,6 +16,7 @@
 #include "ndtpos.pb.h"
 #include "relocate.pb.h"
 #include "pcdmap.pb.h"
+#include "objectarray.pb.h"
 
 #include <QLabel>
 
@@ -69,6 +70,8 @@ private:
 
     void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
+    void Updatelidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 
     void ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
 
@@ -86,6 +89,7 @@ private:
     void * mpapcdmap;
     void * mpagpsimu;
     void *mpafusiongpsimu;
+    void * mpacnndetect;
 
     pose mCurPose;
 
@@ -113,6 +117,9 @@ private:
 
 
 
+
+
+
 };
 
 #endif // MAINWINDOW_H

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -24,7 +24,9 @@ SOURCES += main.cpp \
     ../../common/common/math/gnss_coordinate_convert.cpp \
     ../../include/msgtype/gps.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
-    ../../include/msgtype/imu.pb.cc
+    ../../include/msgtype/imu.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc \
+    ../../include/msgtype/object.pb.cc
 
 QMAKE_LFLAGS += -no-pie
 
@@ -83,7 +85,9 @@ HEADERS += \
     ../../common/common/math/gnss_coordinate_convert.h \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
-    ../../include/msgtype/imu.pb.h
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
+    ../../include/msgtype/object.pb.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )