add detection_localization_LeGO_LOAM

src/detection/detection_localization_LeGO_LOAM/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/detection/detection_localization_LeGO_LOAM/detection_localization_LeGO_LOAM.pro

@@ -0,0 +1,33 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    imageProjection.cpp
+
+HEADERS += \
+    utility.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}

src/detection/detection_localization_LeGO_LOAM/imageProjection.cpp

@@ -0,0 +1,546 @@
+// Copyright 2013, Ji Zhang, Carnegie Mellon University
+// Further contributions copyright (c) 2016, Southwest Research Institute
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+//    this list of conditions and the following disclaimer.
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+// 3. Neither the name of the copyright holder nor the names of its
+//    contributors may be used to endorse or promote products derived from this
+//    software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// This is an implementation of the algorithm described in the following papers:
+//   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
+//     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
+//   T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain
+//      IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). October 2018.
+
+#include "utility.h"
+
+namespace iv {
+
+struct SegInfo
+{
+    int startRingIndex[N_SCAN];
+    int endRingIndex[N_SCAN];
+
+    float startOrientation;
+    float endOrientation;
+    float orientationDiff;
+
+    bool   segmentedCloudGroundFlag[N_SCAN*Horizon_SCAN]; // true - ground point, false - other points
+    unsigned int  segmentedCloudColInd[N_SCAN*Horizon_SCAN]; // point column index in range image
+    float segmentedCloudRange[N_SCAN*Horizon_SCAN]; // point range
+};
+}
+
+class ImageProjection{
+private:
+
+//    ros::NodeHandle nh;
+
+//    ros::Subscriber subLaserCloud;
+    
+//    ros::Publisher pubFullCloud;
+//    ros::Publisher pubFullInfoCloud;
+
+//    ros::Publisher pubGroundCloud;
+//    ros::Publisher pubSegmentedCloud;
+//    ros::Publisher pubSegmentedCloudPure;
+//    ros::Publisher pubSegmentedCloudInfo;
+//    ros::Publisher pubOutlierCloud;
+
+    pcl::PointCloud<PointType>::Ptr laserCloudIn;
+    pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;
+
+    pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
+    pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range
+
+    pcl::PointCloud<PointType>::Ptr groundCloud;
+    pcl::PointCloud<PointType>::Ptr segmentedCloud;
+    pcl::PointCloud<PointType>::Ptr segmentedCloudPure;
+    pcl::PointCloud<PointType>::Ptr outlierCloud;
+
+    PointType nanPoint; // fill in fullCloud at each iteration
+
+    cv::Mat rangeMat; // range matrix for range image
+    cv::Mat labelMat; // label matrix for segmentaiton marking
+    cv::Mat groundMat; // ground matrix for ground cloud marking
+    int labelCount;
+
+    float startOrientation;
+    float endOrientation;
+
+    iv::SegInfo segMsg;
+//    cloud_msgs::cloud_info segMsg; // info of segmented cloud
+//    std_msgs::Header cloudHeader;
+
+    std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
+
+    uint16_t *allPushedIndX; // array for tracking points of a segmented object
+    uint16_t *allPushedIndY;
+
+    uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
+    uint16_t *queueIndY;
+
+public:
+    ImageProjection()
+    {
+ //   ImageProjection():
+//        nh("~"){
+
+//        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
+
+//        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
+//        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
+
+//        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
+//        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
+//        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
+//        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
+//        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);
+
+        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<PointType>());
+        laserCloudInRing.reset(new pcl::PointCloud<PointXYZIR>());
+
+        fullCloud.reset(new pcl::PointCloud<PointType>());
+        fullInfoCloud.reset(new pcl::PointCloud<PointType>());
+
+        groundCloud.reset(new pcl::PointCloud<PointType>());
+        segmentedCloud.reset(new pcl::PointCloud<PointType>());
+        segmentedCloudPure.reset(new pcl::PointCloud<PointType>());
+        outlierCloud.reset(new pcl::PointCloud<PointType>());
+
+        fullCloud->points.resize(N_SCAN*Horizon_SCAN);
+        fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);
+
+//        segMsg.startRingIndex.assign(N_SCAN, 0);
+//        segMsg.endRingIndex.assign(N_SCAN, 0);
+
+//        segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);
+//        segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);
+//        segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);
+
+        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];
+    }
+
+    void resetParameters(){
+        laserCloudIn->clear();
+        groundCloud->clear();
+        segmentedCloud->clear();
+        segmentedCloudPure->clear();
+        outlierCloud->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);
+    }
+
+    ~ImageProjection(){}
+
+//    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
+
+//        cloudHeader = laserCloudMsg->header;
+//        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
+//        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
+//        // Remove Nan points
+//        std::vector<int> indices;
+//        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
+//        // have "ring" channel in the cloud
+//        if (useCloudRing == true){
+//            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
+//            if (laserCloudInRing->is_dense == false) {
+//                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
+//                ros::shutdown();
+//            }
+//        }
+//    }
+    
+//    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
+    void cloudHandler(){
+
+        // 1. Convert ros message to pcl point cloud
+//        copyPointCloud(laserCloudMsg);
+        // 2. Start and end angle of a scan
+ //       findStartEndAngle();
+        // 3. Range image projection
+        projectPointCloud();
+        // 4. Mark ground points
+        groundRemoval();
+        // 5. Point cloud segmentation
+        cloudSegmentation();
+        // 6. Publish all clouds
+ //       publishCloud();
+        // 7. Reset parameters for next iteration
+        resetParameters();
+    }
+
+//    void findStartEndAngle(){
+//        // start and end orientation of this cloud
+//        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
+//        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
+//                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
+//        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
+//            segMsg.endOrientation -= 2 * M_PI;
+//        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
+//            segMsg.endOrientation += 2 * M_PI;
+//        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
+//    }
+
+    void projectPointCloud(){
+        // range image projection
+        float verticalAngle, horizonAngle, range;
+        size_t rowIdn, columnIdn, index, cloudSize; 
+        PointType thisPoint;
+
+        cloudSize = laserCloudIn->points.size();
+
+        for (size_t i = 0; i < cloudSize; ++i){
+
+            thisPoint.x = laserCloudIn->points[i].x;
+            thisPoint.y = laserCloudIn->points[i].y;
+            thisPoint.z = laserCloudIn->points[i].z;
+            // find the row and column index in the iamge for this point
+            if (useCloudRing == true){
+                rowIdn = laserCloudInRing->points[i].ring;
+            }
+            else{
+                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
+                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
+            }
+            if (rowIdn < 0 || rowIdn >= N_SCAN)
+                continue;
+
+            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
+
+            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
+            if (columnIdn >= Horizon_SCAN)
+                columnIdn -= Horizon_SCAN;
+
+            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
+                continue;
+
+            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
+            if (range < sensorMinimumRange)
+                continue;
+            
+            rangeMat.at<float>(rowIdn, columnIdn) = range;
+
+            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
+
+            index = columnIdn  + rowIdn * Horizon_SCAN;
+            fullCloud->points[index] = thisPoint;
+            fullInfoCloud->points[index] = thisPoint;
+            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
+        }
+    }
+
+
+    void groundRemoval(){
+        size_t lowerInd, upperInd;
+        float diffX, diffY, diffZ, angle;
+        // groundMat
+        // -1, no valid info to check if ground of not
+        //  0, initial value, after validation, means not ground
+        //  1, ground
+        for (size_t j = 0; j < Horizon_SCAN; ++j){
+            for (size_t i = 0; i < groundScanInd; ++i){
+
+                lowerInd = j + ( i )*Horizon_SCAN;
+                upperInd = j + (i+1)*Horizon_SCAN;
+
+                if (fullCloud->points[lowerInd].intensity == -1 ||
+                    fullCloud->points[upperInd].intensity == -1){
+                    // no info to check, invalid points
+                    groundMat.at<int8_t>(i,j) = -1;
+                    continue;
+                }
+                    
+                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
+                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
+                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
+
+                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
+
+                if (abs(angle - sensorMountAngle) <= 10){
+                    groundMat.at<int8_t>(i,j) = 1;
+                    groundMat.at<int8_t>(i+1,j) = 1;
+                }
+            }
+        }
+        // extract ground cloud (groundMat == 1)
+        // mark entry that doesn't need to label (ground and invalid point) for segmentation
+        // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
+        for (size_t i = 0; i < N_SCAN; ++i){
+            for (size_t j = 0; j < Horizon_SCAN; ++j){
+                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
+                    labelMat.at<int>(i,j) = -1;
+                }
+            }
+        }
+//        if (pubGroundCloud.getNumSubscribers() != 0){
+//            for (size_t i = 0; i <= groundScanInd; ++i){
+//                for (size_t j = 0; j < Horizon_SCAN; ++j){
+//                    if (groundMat.at<int8_t>(i,j) == 1)
+//                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+//                }
+//            }
+//        }
+    }
+
+    void cloudSegmentation(){
+        // segmentation process
+        for (size_t i = 0; i < N_SCAN; ++i)
+            for (size_t j = 0; j < Horizon_SCAN; ++j)
+                if (labelMat.at<int>(i,j) == 0)
+                    labelComponents(i, j);
+
+        int sizeOfSegCloud = 0;
+        // extract segmented cloud for lidar odometry
+        for (size_t i = 0; i < N_SCAN; ++i) {
+
+            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;
+
+            for (size_t j = 0; j < Horizon_SCAN; ++j) {
+                if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
+                    // outliers that will not be used for optimization (always continue)
+                    if (labelMat.at<int>(i,j) == 999999){
+                        if (i > groundScanInd && j % 5 == 0){
+                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+                            continue;
+                        }else{
+                            continue;
+                        }
+                    }
+                    // majority of ground points are skipped
+                    if (groundMat.at<int8_t>(i,j) == 1){
+                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
+                            continue;
+                    }
+                    // mark ground points so they will not be considered as edge features later
+                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
+                    // mark the points' column index for marking occlusion later
+                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
+                    // save range info
+                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
+                    // save seg cloud
+                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+                    // size of seg cloud
+                    ++sizeOfSegCloud;
+                }
+            }
+
+            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
+        }
+        
+        // extract segmented cloud for visualization
+//        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
+//            for (size_t i = 0; i < N_SCAN; ++i){
+//                for (size_t j = 0; j < Horizon_SCAN; ++j){
+//                    if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
+//                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+//                        segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
+//                    }
+//                }
+//            }
+//        }
+    }
+
+    void labelComponents(int row, int col){
+        // use std::queue std::vector std::deque will slow the program down greatly
+        float d1, d2, alpha, angle;
+        int fromIndX, fromIndY, thisIndX, thisIndY; 
+        bool lineCountFlag[N_SCAN] = {false};
+
+        queueIndX[0] = row;
+        queueIndY[0] = col;
+        int queueSize = 1;
+        int queueStartInd = 0;
+        int queueEndInd = 1;
+
+        allPushedIndX[0] = row;
+        allPushedIndY[0] = col;
+        int allPushedIndSize = 1;
+        
+        while(queueSize > 0){
+            // Pop point
+            fromIndX = queueIndX[queueStartInd];
+            fromIndY = queueIndY[queueStartInd];
+            --queueSize;
+            ++queueStartInd;
+            // Mark popped point
+            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
+            // Loop through all the neighboring grids of popped grid
+            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
+                // new index
+                thisIndX = fromIndX + (*iter).first;
+                thisIndY = fromIndY + (*iter).second;
+                // index should be within the boundary
+                if (thisIndX < 0 || thisIndX >= N_SCAN)
+                    continue;
+                // at range image margin (left or right side)
+                if (thisIndY < 0)
+                    thisIndY = Horizon_SCAN - 1;
+                if (thisIndY >= Horizon_SCAN)
+                    thisIndY = 0;
+                // prevent infinite loop (caused by put already examined point back)
+                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
+                    continue;
+
+                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
+                              rangeMat.at<float>(thisIndX, thisIndY));
+                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
+                              rangeMat.at<float>(thisIndX, thisIndY));
+
+                if ((*iter).first == 0)
+                    alpha = segmentAlphaX;
+                else
+                    alpha = segmentAlphaY;
+
+                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
+
+                if (angle > segmentTheta){
+
+                    queueIndX[queueEndInd] = thisIndX;
+                    queueIndY[queueEndInd] = thisIndY;
+                    ++queueSize;
+                    ++queueEndInd;
+
+                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
+                    lineCountFlag[thisIndX] = true;
+
+                    allPushedIndX[allPushedIndSize] = thisIndX;
+                    allPushedIndY[allPushedIndSize] = thisIndY;
+                    ++allPushedIndSize;
+                }
+            }
+        }
+
+        // check if this segment is valid
+        bool feasibleSegment = false;
+        if (allPushedIndSize >= 30)
+            feasibleSegment = true;
+        else if (allPushedIndSize >= segmentValidPointNum){
+            int lineCount = 0;
+            for (size_t i = 0; i < N_SCAN; ++i)
+                if (lineCountFlag[i] == true)
+                    ++lineCount;
+            if (lineCount >= segmentValidLineNum)
+                feasibleSegment = true;            
+        }
+        // segment is valid, mark these points
+        if (feasibleSegment == true){
+            ++labelCount;
+        }else{ // segment is invalid, mark these points
+            for (size_t i = 0; i < allPushedIndSize; ++i){
+                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
+            }
+        }
+    }
+
+    
+//    void publishCloud(){
+//        // 1. Publish Seg Cloud Info
+//        segMsg.header = cloudHeader;
+//        pubSegmentedCloudInfo.publish(segMsg);
+//        // 2. Publish clouds
+//        sensor_msgs::PointCloud2 laserCloudTemp;
+
+//        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
+//        laserCloudTemp.header.stamp = cloudHeader.stamp;
+//        laserCloudTemp.header.frame_id = "base_link";
+//        pubOutlierCloud.publish(laserCloudTemp);
+//        // segmented cloud with ground
+//        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
+//        laserCloudTemp.header.stamp = cloudHeader.stamp;
+//        laserCloudTemp.header.frame_id = "base_link";
+//        pubSegmentedCloud.publish(laserCloudTemp);
+//        // projected full cloud
+//        if (pubFullCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*fullCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubFullCloud.publish(laserCloudTemp);
+//        }
+//        // original dense ground cloud
+//        if (pubGroundCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*groundCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubGroundCloud.publish(laserCloudTemp);
+//        }
+//        // segmented cloud without ground
+//        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubSegmentedCloudPure.publish(laserCloudTemp);
+//        }
+//        // projected full cloud info
+//        if (pubFullInfoCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubFullInfoCloud.publish(laserCloudTemp);
+//        }
+//    }
+
+
+};
+
+
+
+
+//int main(int argc, char** argv){
+
+//    ros::init(argc, argv, "lego_loam");
+    
+//    ImageProjection IP;
+
+//    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
+
+//    ros::spin();
+//    return 0;
+//}

src/detection/detection_localization_LeGO_LOAM/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}

src/detection/detection_localization_LeGO_LOAM/utility.h

@@ -0,0 +1,191 @@
+#ifndef _UTILITY_LIDAR_ODOMETRY_H_
+#define _UTILITY_LIDAR_ODOMETRY_H_
+
+
+//#include <ros/ros.h>
+
+//#include <sensor_msgs/Imu.h>
+//#include <sensor_msgs/PointCloud2.h>
+//#include <nav_msgs/Odometry.h>
+
+//#include "cloud_msgs/cloud_info.h"
+
+#include <opencv2/opencv.hpp>
+//#include <opencv/cv.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/range_image/range_image.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/common/common.h>
+#include <pcl/registration/icp.h>
+
+//#include <tf/transform_broadcaster.h>
+//#include <tf/transform_datatypes.h>
+ 
+#include <vector>
+#include <cmath>
+#include <algorithm>
+#include <queue>
+#include <deque>
+#include <iostream>
+#include <fstream>
+#include <ctime>
+#include <cfloat>
+#include <iterator>
+#include <sstream>
+#include <string>
+#include <limits>
+#include <iomanip>
+#include <array>
+#include <thread>
+#include <mutex>
+
+#define PI 3.14159265
+
+using namespace std;
+
+typedef pcl::PointXYZI  PointType;
+
+extern const string pointCloudTopic = "/velodyne_points";
+extern const string imuTopic = "/imu/data";
+
+// Save pcd
+extern const string fileDirectory = "/tmp/";
+
+// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
+extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used
+
+// VLP-16
+extern const int N_SCAN = 16;
+extern const int Horizon_SCAN = 1800;
+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;
+
+// HDL-32E
+// extern const int N_SCAN = 32;
+// extern const int Horizon_SCAN = 1800;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 41.33/float(N_SCAN-1);
+// extern const float ang_bottom = 30.67;
+// extern const int groundScanInd = 20;
+
+// VLS-128
+// extern const int N_SCAN = 128;
+// extern const int Horizon_SCAN = 1800;
+// extern const float ang_res_x = 0.2;
+// extern const float ang_res_y = 0.3;
+// extern const float ang_bottom = 25.0;
+// extern const int groundScanInd = 10;
+
+// Ouster users may need to uncomment line 159 in imageProjection.cpp
+// Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
+// Ouster OS1-16
+// extern const int N_SCAN = 16;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 7;
+
+// Ouster OS1-64
+// extern const int N_SCAN = 64;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 15;
+
+extern const bool loopClosureEnableFlag = false;
+extern const double mappingProcessInterval = 0.3;
+
+extern const float scanPeriod = 0.1;
+extern const int systemDelay = 0;
+extern const int imuQueLength = 200;
+
+extern const float sensorMinimumRange = 1.0;
+extern const float sensorMountAngle = 0.0;
+extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
+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;
+
+
+extern const int edgeFeatureNum = 2;
+extern const int surfFeatureNum = 4;
+extern const int sectionsTotal = 6;
+extern const float edgeThreshold = 0.1;
+extern const float surfThreshold = 0.1;
+extern const float nearestFeatureSearchSqDist = 25;
+
+
+// Mapping Params
+extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
+extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
+// history key frames (history submap for loop closure)
+extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
+extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
+extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
+
+extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
+
+
+struct smoothness_t{ 
+    float value;
+    size_t ind;
+};
+
+struct by_value{ 
+    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
+        return left.value < right.value;
+    }
+};
+
+/*
+    * A point cloud type that has "ring" channel
+    */
+struct PointXYZIR
+{
+    PCL_ADD_POINT4D
+    PCL_ADD_INTENSITY;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (uint16_t, ring, ring)
+)
+
+/*
+    * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
+    */
+struct PointXYZIRPYT
+{
+    PCL_ADD_POINT4D
+    PCL_ADD_INTENSITY;
+    float roll;
+    float pitch;
+    float yaw;
+    double time;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
+                                   (double, time, time)
+)
+
+typedef PointXYZIRPYT  PointTypePose;
+
+#endif