cloudaboveGround

include/commontools.h

@@ -17,6 +17,7 @@
 
 #include "smoothing.h"
 
+
 struct PointXYZIRPYT
 {
     PCL_ADD_POINT4D

include/ringProjection.h

@@ -0,0 +1,179 @@
+//
+// Created by joe on 2019/9/26.
+//
+// Modified from LeGO-LOAM imageProjection.
+//
+
+#ifndef POINTCLOUDPROCESSING_RINGPROJECTION_H
+#define POINTCLOUDPROCESSING_RINGPROJECTION_H
+
+#include "commontools.h"
+#include <octomap/octomap.h>
+#include <opencv2/opencv.hpp>
+
+extern const int N_SCAN = 16;
+extern const int Horizon_SCAN = 1800;//360\0.2
+extern const float ang_res_x = 0.2;
+extern const float ang_res_y = 2.0;
+extern const float ang_bottom = 15.0+0.1;
+extern const int groundScanInd = 7;
+
+extern const float sensorMountAngle = 0.0;
+extern const float segmentTheta = 1.0472;
+extern const int segmentValidPointNum = 5;
+extern const int segmentValidLineNum = 3;
+extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
+extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;
+
+class RingProjector{
+
+private:
+    struct ringGrid{
+        int row, col;
+        std::vector<int> ptIndices;
+        double height;
+
+        ringGrid(){
+            row = -1;
+            col = -1;
+            height = -1;
+            std::vector<int>().swap(ptIndices);
+        }
+    };
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn;  //单帧点云
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr fullCloud;   // 距离图像对应的点云,intensity=row + (col/10000)
+    pcl::PointCloud<pcl::PointXYZI>::Ptr fullInfoCloud;  //投影得到的距离图像 16*1800
+    // （index为行列号，无坐标值，x为原始点云indice, intensity为range）
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr groundCloud; //地面点
+    pcl::PointCloud<pcl::PointXYZI>::Ptr segmentedCloud;  //代表较大物体的点，其中地面点抽稀过,与fullCloud点类型相同
+    pcl::PointCloud<pcl::PointXYZI>::Ptr segmentedCloudPure;  //所有代表较大物体的点（不含地面点）,intensity为标签
+    pcl::PointCloud<pcl::PointXYZI>::Ptr outlierCloud;  //地面点中因抽稀而滤掉的点以及散乱点
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloudaboveGround;  //位于地面上的点云，与非地面点不同
+
+    pcl::PointXYZI nanPoint;
+
+    cv::Mat rangeMat; //距离图像对应的距离矩阵
+    cv::Mat labelMat;  //距离图像对应的点的label, -1为地面点
+    cv::Mat groundMat; //距离图像对应的点是否为地面点 1则为地面点
+    int labelCount;
+
+    float startOrientation;
+    float endOrientation;
+    float orientationDiff;
+
+    std::vector<std::pair<uint8_t, uint8_t> > neighborIterator;
+
+    uint16_t *allPushedIndX;
+    uint16_t *allPushedIndY;
+
+    uint16_t *queueIndX;
+    uint16_t *queueIndY;
+
+    bool isOutdoor;
+
+    double angleResolu_, distResolu_;
+    int rowNum_, colNum_;
+    double *maxGroundrangeOfcol;
+
+    ringGrid *ringGrids_;
+
+public:
+    RingProjector(double angle, double dist){
+
+        angleResolu_ = angle;
+        distResolu_ = dist;
+        rowNum_ = 100.0 / dist;
+        colNum_ = 360.0 / angle;
+
+        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.intensity = -1;
+
+        allocateMemory();
+        resetParameters();
+    }
+
+    void allocateMemory(){
+
+        laserCloudIn.reset(new pcl::PointCloud<pcl::PointXYZI>());
+
+        fullCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
+        fullInfoCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
+
+        groundCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
+        segmentedCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
+        segmentedCloudPure.reset(new pcl::PointCloud<pcl::PointXYZI>());
+        outlierCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
+        cloudaboveGround.reset(new pcl::PointCloud<pcl::PointXYZI>());
+
+        fullCloud->points.resize(N_SCAN*Horizon_SCAN);
+        fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);
+
+        std::pair<int8_t, int8_t> neighbor;
+        neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
+        neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
+        neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
+        neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);
+
+        allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
+        allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];
+
+        queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
+        queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
+
+        maxGroundrangeOfcol = new double[Horizon_SCAN];//初始化为0？
+        ringGrids_ = new ringGrid[rowNum_*colNum_];
+    }
+    void resetParameters(){
+
+        laserCloudIn->clear();
+        groundCloud->clear();
+        segmentedCloud->clear();
+        segmentedCloudPure->clear();
+        outlierCloud->clear();
+        cloudaboveGround->clear();
+
+        rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
+        groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
+        labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
+        labelCount = 1;
+
+        std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
+        std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
+    }
+
+    ~RingProjector(){}
+
+
+    //1
+    void setInputCloud(const pcXYZIptr inCloud);
+
+    //2
+    void findStartEndAngle();
+
+    //3
+    void projectPointCloud();
+
+    //4
+    void groundRemoval();
+
+    //5
+    void cloudSegmentation();
+    void labelComponents(int row, int col);
+
+    //6
+    void publishCloud(pcXYZIptr outCloud);
+
+    void fullSegmentation(){
+
+        projectPointCloud();
+        groundRemoval();
+        cloudSegmentation();
+    }
+
+};
+#endif //POINTCLOUDPROCESSING_RINGPROJECTION_H

main.cpp

@@ -91,10 +91,9 @@ int main()
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr bigCurv(new pcl::PointCloud<pcl::PointXYZRGB>);
     pcl::PointCloud<pcl::Normal>::Ptr pcNormals (new pcl::PointCloud<pcl::Normal>);
     preprocessor.normalestimate(pc_filtered, pcNormals);///normal estimation
+
     for(int i=0 ; i<pcNormals->points.size() ; i++)
-    {
-        if(pcNormals->points[i].curvature * 100 > 25)
-        {
+        if(pcNormals->points[i].curvature * 100 > 25){
             pcl::PointXYZRGB ptRGB ;
             ptRGB.x = pc_filtered->points[i].x;
             ptRGB.y = pc_filtered->points[i].y;
@@ -106,10 +105,8 @@ int main()
 
             bigCurv->push_back(ptRGB);
 //            cout<<"curvature : "<<pcNormals->points[i].curvature * 100<<endl;
-
         }
 
-    }
 
     ///PCA
 //    pcl::PointCloud<pcl::PointXYZI>::Ptr keyCloud (new pcl::PointCloud<pcl::PointXYZI>);

src/CMakeLists.txt

@@ -79,6 +79,7 @@ target_link_libraries(lidarOdometryLib smooth ${PCL_LIBRARIES})
 add_library(pcapReader SHARED pcapReader.cpp)
 target_link_libraries(pcapReader ${PCL_LIBRARIES} PacketDriver PacketDecoder)
 
+add_library(ringProjection SHARED ringProjection.cpp)
 
 #//////////////////////////////////////////////////////////////////////////
 #add excutables
@@ -94,8 +95,8 @@ target_link_libraries(dynaObjTest
         ${OpenCV_LIBRARIES}
         polynomial_splines_traits
         cubicHermiteSE3curve
-        ${Sophus_LIBRARIES}
-        gtsam)
+        ringProjection
+        )
 
 add_executable(planeFactorTest planeFactorTest.cpp)
 target_link_libraries(planeFactorTest
@@ -118,4 +119,5 @@ add_executable(trjectoryIntepolateTest trjectoryIntepolateTest.cpp)
 target_link_libraries(trjectoryIntepolateTest
         smooth
         ${PCL_LIBRARIES}
+        ${Sophus_LIBRARIES}
         )

src/buildingFacadeExtraction.cpp

@@ -162,8 +162,8 @@ bool buildingFacadeExtractor::constructVoxels(pcXYZIptr inputCloud, float gridRe
 //    voxel *voxelArrptr = new voxel[voxelSize];
     std::vector<int> inVoxelpt;
 
-    for(int i=0 ; i<voxelSize ; i++)
-    {
+    for(int i=0 ; i<voxelSize ; i++){
+
         std::vector<int>().swap(inVoxelpt);
         pcOctree.voxelSearch(voxelCenterpts[i], inVoxelpt);
 

src/dynaObjTest.cpp

@@ -11,7 +11,6 @@
 
 #include <Eigen/Core>
 
-#include <octomap/octomap.h>
 #include <octomap/ColorOcTree.h>
 
 #include <pcl/common/transforms.h>
@@ -24,6 +23,7 @@
 #include "dbscanCLuster.h"
 #include "smoothing.h"
 #include "commontools.h"
+#include "ringProjection.h"
 
 using namespace std;
 //using namespace curves;
@@ -47,6 +47,7 @@ int main(int argc, char** argv) {
         pcXYZIptr transformedScan(new pcXYZI());
         pcXYZIptr transformedSeg(new pcXYZI());
         pcXYZIptr scan(new pcXYZI());
+        pcXYZIptr segedCloud(new pcXYZI());
 
         std::vector <tools::clusterTracker> trackersList;
         string scanFolder = "/home/joe/workspace/testData/veloScans/";
@@ -68,6 +69,7 @@ int main(int argc, char** argv) {
         pcl::PointXYZI predicpt;
         pcl::visualization::CloudViewer ccviewer("Viewer");
         bool init = false;
+        RingProjector ringProjector(2, 2);
 
         for (int i = 0; i < posesSize; i++) {
 
@@ -84,14 +86,20 @@ int main(int argc, char** argv) {
             vector <pcXYZI>().swap(vecTrackedallMeasurements) ;
 
             //read segmented cloud first
-            if (pcl::io::loadPCDFile<pcl::PointXYZI>(segFolder + to_string(pcRPYpose->points[i].time) + ".pcd",
+            if (pcl::io::loadPCDFile<pcl::PointXYZI>(scanFolder + to_string(pcRPYpose->points[i].time) + ".pcd",
                                                      *scan) != -1) {
                 cout << "  --->Scan " << to_string(pcRPYpose->points[i].time) << endl;
 
                 st = clock();
+                segedCloud->clear();
                 transformedScan->clear();
                 transformedSeg->clear();
 
+                RingProjector ringProjector(2, 2);
+                ringProjector.setInputCloud(scan);
+                ringProjector.fullSegmentation();
+                ringProjector.publishCloud(segedCloud);
+
                 PointTypePose vlp_pose;
                 vlp_pose = pcRPYpose->points[i];
 
@@ -105,17 +113,21 @@ int main(int argc, char** argv) {
 
                 Eigen::Vector3f t(vlp_pose.z, vlp_pose.x, vlp_pose.y);
                 Eigen::Quaternionf quan(vlp_pose.intensity, vlp_pose.yaw, vlp_pose.roll, vlp_pose.pitch);
-                pcl::transformPointCloud(*scan, *transformedSeg, t, quan);
+                pcl::transformPointCloud(*segedCloud, *transformedSeg, t, quan);
 
                 //打开相对应的vlpscan
-                if (pcl::io::loadPCDFile<pcl::PointXYZI>(scanFolder + to_string(pcRPYpose->points[i].time) + ".pcd",
-                                                         *scan) == -1)
-                    continue;
+//                if (pcl::io::loadPCDFile<pcl::PointXYZI>(scanFolder + to_string(pcRPYpose->points[i].time) + ".pcd",
+//                                                         *scan) == -1)
+//                    continue;
                 pcl::transformPointCloud(*scan, *transformedScan, t, quan);
 
 //            ccviewer.showCloud(transformedScan);
-//                ccviewer.showCloud(transformedSeg);
-
+                ccviewer.showCloud(segedCloud);
+                segedCloud->width = 1;
+                segedCloud->height = segedCloud->points.size();
+                pcl::io::savePCDFile("/home/joe/workspace/testData/cloudaboveground/"+
+                to_string(pcRPYpose->points[i].time)+".pcd", *segedCloud);
+                continue;
 
                 int clusterSize = tools::extractClusterEuclidean(transformedSeg,
                                                                  pcClusterCenters,

src/mainFunc.cpp

@@ -77,7 +77,7 @@ int main()
     }
 
 
-    //point feature based on curvature of scan line
+    //point feature based on the curvature of scan line
     {
         vector<pcXYZI> pcScans;
         std::vector<Eigen::MatrixXf> rangeImgs;