change view_showxodrinvtk, add cube show xodr line.

src/tool/view_ndtmatching/mainwindow.cpp

@@ -129,7 +129,16 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 
 //   ShowXODR(viewer);
 
-   viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
+
+   Eigen::AngleAxisf rotation_vector(0, Eigen::Vector3f(0, 0, 1));
+
+   //绘制对象外接长方体
+   //参数为矩形的顶点，长宽高还有旋转角度以及长方体名称
+   //函数原型：addCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth, const std::string &id="cube", int viewport=0);
+   viewer.addCube(Eigen::Vector3f(0,0,-1.15),
+                  Eigen::Quaternionf(rotation_vector), 4.6, 2.3, 1.5, "car");
+
+ //  viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
 
 
 }
@@ -147,7 +156,12 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
     trans(0) = gCurPose.x;trans(1) = gCurPose.y;trans(2) = gCurPose.z;
 
  //   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");
+  //  viewer.addCube(trans,quat,6.0,3.0,1.0,"car");
+
+    Eigen::AngleAxisf rotation_vector(gCurPose.z, 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");
 
 
 

src/tool/view_showxodrinvtk/main.cpp

@@ -4,12 +4,14 @@
 
 #include <pcl/visualization/cloud_viewer.h>
 
+#include <pcl/io/ply_io.h>
+
 #include "showxodrinvtk.h"
 
 pcl::visualization::CloudViewer viewer1("Cloud Viewer");//创建viewer对象
 
 
-
+ pcl::PolygonMesh mesh;
 
 void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 {
@@ -19,7 +21,32 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
    double flon0,flat0;
    bool bxodr = false;
 
-   ShowXODRINVTK(viewer,flon0,flat0,bxodr,-1.7);
+   ShowXODRINVTK(viewer,flon0,flat0,bxodr,-1.7,1);
+
+
+//   viewer.addPolygonMesh();
+
+//   pcl::PolygonMesh xmesh;
+
+//   Eigen::AngleAxisf rotation_vector(0.6, Eigen::Vector3f(0, 0, 1));
+
+//       //绘制对象外接长方体
+//       //参数为矩形的顶点，长宽高还有旋转角度以及长方体名称
+//       //函数原型：addCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth, const std::string &id="cube", int viewport=0);
+//       viewer.addCube(Eigen::Vector3f(0,0,0),
+//           Eigen::Quaternionf(rotation_vector), 4.6, 2.3, 3.0, "car");
+
+
+
+
+
+//       if (pcl::io::loadPLYFile("/home/yuchuli/car.ply", mesh))
+//       {
+//           std::cout << "error"<<std::endl;
+//       }
+
+//   std::cout<<"mesh cloud size: "<<mesh.cloud.width<<" mesh polygon size: "<<mesh.polygons.size()<<std::endl;
+//              viewer.addPolygonMesh(mesh, "mesh");
 
    viewer.resetCamera();
 
@@ -47,6 +74,9 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+
+
+
     //This will only get called once
     viewer1.runOnVisualizationThreadOnce (viewerOneOff);
 
@@ -54,6 +84,7 @@ int main(int argc, char *argv[])
     viewer1.runOnVisualizationThread (viewerPsycho);
 
 
+
     while(!viewer1.wasStopped())
     {
         std::this_thread::sleep_for(std::chrono::milliseconds(10));

src/tool/view_showxodrinvtk/showxodrinvtk.cpp

@@ -24,16 +24,47 @@ void DrawLine(std::vector<iv::LanePoint> & xveclastlp,std::vector<iv::LanePoint>
             {
                 iv::LanePoint xLast = xveclastlp[j];
 
-                pcl::PointXYZ mass_center(xLast.mfX,xLast.mfY,fz);
-                pcl::PointXYZ kinectZ(xNow.mfX,xNow.mfY,fz);
-
-                char strgeoname[256];
-                snprintf(strgeoname,256,"xodr%d",gnLineIndex);
-                gnLineIndex++;
-                if(xNow.mnlane != 0)
-                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,strgeoname);
+                if(nmode == 0)
+                {
+
+                    pcl::PointXYZ mass_center(xLast.mfX,xLast.mfY,fz);
+                    pcl::PointXYZ kinectZ(xNow.mfX,xNow.mfY,fz);
+
+                    char strgeoname[256];
+                    snprintf(strgeoname,256,"xodr%d",gnLineIndex);
+                    gnLineIndex++;
+                    if(xNow.mnlane != 0)
+                        viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,strgeoname);
+                    else
+                        viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,strgeoname);
+
+                }
                 else
-                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,strgeoname);
+                {
+                    double fhdg = xLast.mfhdg;
+                    double flen = sqrt(pow(xNow.mfX - xLast.mfX,2) + pow(xNow.mfY - xLast.mfY,2) +pow(0,2));
+                    double fwidth = 0.15;
+                    double fheight = 0.01;
+
+                    double fx,fy;
+                    fx = (xNow.mfX + xLast.mfX)/2.0;
+                    fy = (xNow.mfY + xLast.mfY)/2.0;
+
+                    Eigen::AngleAxisf rotation_vector(fhdg, Eigen::Vector3f(0, 0, 1));
+
+
+                    char strgeoname[256];
+                    snprintf(strgeoname,256,"xodr%d",gnLineIndex);
+                    gnLineIndex++;
+
+                        viewer.addCube(Eigen::Vector3f(fx,fy,fz),
+                            Eigen::Quaternionf(rotation_vector), flen, fwidth, fheight, strgeoname);
+
+                   if(xNow.mnlane == 0)
+                   {
+                       viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, strgeoname);
+                   }
+                }
 
                 break;
             }