add lidar detection ground filter

src/detection/detection_ground_filter/.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_ground_filter/base_segmenter.hpp

@@ -0,0 +1,36 @@
+#ifndef SEGMENTERS_INCLUDE_SEGMENTERS_BASE_SEGMENTER_HPP_
+#define SEGMENTERS_INCLUDE_SEGMENTERS_BASE_SEGMENTER_HPP_
+
+#include <string>
+#include <vector>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <Eigen/Core>
+#include <string>
+#include <tuple>
+#include <vector>
+
+
+namespace lidar {
+namespace segmenter {
+
+typedef pcl::PointXYZ Point;
+typedef pcl::PointXYZI PointI;
+typedef pcl::PointCloud<PointI> PointICloud;
+typedef PointICloud::Ptr PointICloudPtr;
+typedef PointICloud::ConstPtr PointICloudConstPtr;
+
+class BaseSegmenter {
+ public:
+    /// @brief Segment the point cloud.
+    virtual void segment(
+        const PointICloud &cloud_in,
+        std::vector<PointICloudPtr> &cloud_clusters) = 0;  // NOLINT
+
+    virtual std::string name() const = 0;
+};  // BaseSegmenter
+
+}  // namespace segmenter
+}  // namespace lidar
+
+#endif  // SEGMENTERS_INCLUDE_SEGMENTERS_BASE_SEGMENTER_HPP_

src/detection/detection_ground_filter/detection_ground_filter.pro

@@ -0,0 +1,58 @@
+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 \
+    ground_plane_fitting_segmenter.cpp
+
+HEADERS += \
+    base_segmenter.hpp \
+    ground_plane_fitting_segmenter.hpp
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization

src/detection/detection_ground_filter/ground_plane_fitting_segmenter.cpp

@@ -0,0 +1,242 @@
+#include "ground_plane_fitting_segmenter.hpp"
+#include <boost/array.hpp>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/io/io.h>
+
+
+namespace lidar {
+namespace segmenter {
+
+GroundPlaneFittingSegmenter::GroundPlaneFittingSegmenter() {}
+
+GroundPlaneFittingSegmenter::GroundPlaneFittingSegmenter(
+    const SegmenterParams& params)
+    : params_(params) {}
+
+GroundPlaneFittingSegmenter::~GroundPlaneFittingSegmenter() {}
+
+/**
+ * @brief Selection of initial seed points
+ * @note
+ *  Introduces the lowest point representative(LPR)
+ *      ==> guarantees that noisy measurements will not affect the plane
+ * estimation step
+ * @input
+ *  params_.gpf_num_lpr: number of  lowest point representative(LPR),
+ * @param cloud_in
+ * @param cloud_seeds
+ */
+void GroundPlaneFittingSegmenter::extractInitialSeeds(
+    const PointICloud& cloud_in, PointICloudPtr cloud_seeds) {
+    std::vector<PointI> points(cloud_in.points.begin(), cloud_in.points.end());
+    std::sort(points.begin(), points.end(), sortByAxisZAsc<PointI>);
+
+    int cnt_lpr = 0;
+    double height_average = 0.;
+    // filter negative obstacles
+    bool negative = true;
+    for (size_t pt = 0u; pt < points.size() && cnt_lpr < params_.gpf_num_lpr;
+         ++pt) {
+        const double& h = points[pt].z;
+        if (negative) {
+            if (fabs(h + params_.gpf_sensor_height) > params_.gpf_th_lprs) {
+                continue;
+            } else {
+                // because points are in "Incremental Order"
+                negative = false;
+            }
+        }
+        // collect from non-negative obstacles
+        height_average += h;
+        cnt_lpr++;
+    }
+
+    if (cnt_lpr > 0) {
+        height_average /= cnt_lpr;
+    } else {
+        height_average = (-1.0) * params_.gpf_sensor_height;
+    }
+
+    // the points inside the height threshold are used as the initial seeds for
+    // the plane model estimation
+    (*cloud_seeds).clear();
+    for (size_t pt = 0u; pt < points.size(); ++pt) {
+        if (points[pt].z < height_average + params_.gpf_th_seeds) {
+            (*cloud_seeds).points.push_back(points[pt]);
+        }
+    }
+}
+
+/**
+ * @brief Estimate the ground plane(N^T X = -d) by SVD
+ * @param cloud_seeds
+ * @retval plane model estimation model_t
+ */
+model_t GroundPlaneFittingSegmenter::estimatePlane(
+    const PointICloud& cloud_ground) {
+    model_t model;
+
+    // Create covariance matrix.
+    // 1. calculate (x,y,z) mean
+    float mean_x = 0., mean_y = 0., mean_z = 0.;
+    for (size_t pt = 0u; pt < cloud_ground.points.size(); ++pt) {
+        mean_x += cloud_ground.points[pt].x;
+        mean_y += cloud_ground.points[pt].y;
+        mean_z += cloud_ground.points[pt].z;
+    }
+    if (cloud_ground.points.size()) {
+        mean_x /= cloud_ground.points.size();
+        mean_y /= cloud_ground.points.size();
+        mean_z /= cloud_ground.points.size();
+    }
+    // 2. calculate covariance
+    // cov(x,x), cov(y,y), cov(z,z)
+    // cov(x,y), cov(x,z), cov(y,z)
+    float cov_xx = 0., cov_yy = 0., cov_zz = 0.;
+    float cov_xy = 0., cov_xz = 0., cov_yz = 0.;
+    for (int i = 0; i < cloud_ground.points.size(); i++) {
+        cov_xx += (cloud_ground.points[i].x - mean_x) *
+                  (cloud_ground.points[i].x - mean_x);
+        cov_xy += (cloud_ground.points[i].x - mean_x) *
+                  (cloud_ground.points[i].y - mean_y);
+        cov_xz += (cloud_ground.points[i].x - mean_x) *
+                  (cloud_ground.points[i].z - mean_z);
+        cov_yy += (cloud_ground.points[i].y - mean_y) *
+                  (cloud_ground.points[i].y - mean_y);
+        cov_yz += (cloud_ground.points[i].y - mean_y) *
+                  (cloud_ground.points[i].z - mean_z);
+        cov_zz += (cloud_ground.points[i].z - mean_z) *
+                  (cloud_ground.points[i].z - mean_z);
+    }
+    // 3. setup covariance matrix Cov
+    Eigen::MatrixXf Cov(3, 3);
+    Cov << cov_xx, cov_xy, cov_xz, cov_xy, cov_yy, cov_yz, cov_xz, cov_yz,
+        cov_zz;
+    Cov /= cloud_ground.points.size();
+
+    // Singular Value Decomposition: SVD
+    Eigen::JacobiSVD<Eigen::MatrixXf> SVD(
+        Cov, Eigen::DecompositionOptions::ComputeFullU);
+    // use the least singular vector as normal
+    model.normal = (SVD.matrixU().col(2));
+    // d is directly computed substituting x with s^ which is a good
+    // representative for the points belonging to the plane
+    Eigen::MatrixXf mean_seeds(3, 1);
+    mean_seeds << mean_x, mean_y, mean_z;
+    // according to normal^T*[x,y,z]^T = -d
+    model.d = -(model.normal.transpose() * mean_seeds)(0, 0);
+
+    // ROS_WARN_STREAM("Model: " << model.normal << " " << model.d);
+
+    return model;
+}
+
+/**
+ * @brief GPF main loop for one segemnt of the point cloud
+ * @param cloud_in
+ * @param cloud_gnds
+ * @param cloud_ngnds
+ */
+void GroundPlaneFittingSegmenter::mainLoop(const PointICloud& cloud_in,
+                                           PointICloudPtr cloud_gnds,
+                                           PointICloudPtr cloud_ngnds) {
+    cloud_gnds->clear();
+    cloud_ngnds->clear();
+
+    PointICloudPtr cloud_seeds(new PointICloud);
+    extractInitialSeeds(cloud_in, cloud_seeds);
+
+    pcl::PointIndices::Ptr gnds_indices(new pcl::PointIndices);
+    *cloud_gnds = *cloud_seeds;
+    for (size_t iter = 0u; iter < params_.gpf_num_iter; ++iter) {
+        model_t model = estimatePlane(*cloud_gnds);
+        // clear
+        cloud_gnds->clear();
+        gnds_indices->indices.clear();
+        // pointcloud to matrix
+        Eigen::MatrixXf cloud_matrix(cloud_in.points.size(), 3);
+        size_t pi = 0u;
+        for (auto p : cloud_in.points) {
+            cloud_matrix.row(pi++) << p.x, p.y, p.z;
+        }
+        // distance to extimated ground plane model (N^T X)^T = (X^T N)
+        Eigen::VectorXf dists = cloud_matrix * model.normal;
+        // threshold filter: N^T xi + d = dist < th_dist ==> N^T xi < th_dist -
+        // d
+        double th_dist = params_.gpf_th_gnds - model.d;
+        for (size_t pt = 0u; pt < dists.rows(); ++pt) {
+            if (dists[pt] < th_dist) {
+                gnds_indices->indices.push_back(pt);
+            }
+        }
+        // extract ground points
+        pcl::copyPointCloud(cloud_in, *gnds_indices, *cloud_gnds);
+    }
+
+    // extract non-ground points
+    pcl::ExtractIndices<PointI> indiceExtractor;
+    indiceExtractor.setInputCloud(cloud_in.makeShared());
+    indiceExtractor.setIndices(gnds_indices);
+    indiceExtractor.setNegative(true);
+    indiceExtractor.filter(*cloud_ngnds);
+}
+
+void GroundPlaneFittingSegmenter::segment(
+    const PointICloud& cloud_in, std::vector<PointICloudPtr>& cloud_clusters) {
+    if (cloud_in.empty()) {
+        std::cout<<"Empty ground for segmentation, do nonthing."<<std::endl;
+        return;
+    }
+    // Clear segments.
+    cloud_clusters.clear();
+
+
+    std::cout<<"Starting Ground Plane Fitting segmentation."<<std::endl;
+
+    // Main Loop
+    PointICloudPtr cloud_ground(new PointICloud),
+        cloud_nonground(new PointICloud);
+    if (params_.gpf_num_segment > 1) {
+        // divide into multi-segments by X-axis
+        boost::array<std::vector<int>, segment_upper_bound_> segment_indices;
+
+        std::vector<PointI> points(cloud_in.points.begin(),
+                                   cloud_in.points.end());
+        std::sort(points.begin(), points.end(), sortByAxisXAsc<PointI>);
+        const double res =
+            ((--points.end())->x - points.begin()->x) / params_.gpf_num_segment;
+        for (size_t pt = 0u; pt < cloud_in.points.size(); ++pt) {
+            double Xval = points.begin()->x;
+            for (size_t idx = 0u; idx < params_.gpf_num_segment; ++idx) {
+                const double x = cloud_in.points[pt].x;
+                if (x > Xval && x < (Xval + res)) {
+                    segment_indices[idx].push_back(pt);
+                }
+                Xval += res;
+            }
+        }
+
+        PointICloudPtr cloud(new PointICloud);
+        PointICloudPtr gnds(new PointICloud), ngnds(new PointICloud);
+        for (size_t segmentIdx = 0u; segmentIdx < params_.gpf_num_segment;
+             ++segmentIdx) {
+            // get pointcloud in different segments
+            pcl::copyPointCloud(cloud_in, segment_indices[segmentIdx], *cloud);
+
+            mainLoop(*cloud, gnds, ngnds);
+
+            *cloud_ground += *gnds;
+            *cloud_nonground += *ngnds;
+        }
+    } else {
+        mainLoop(cloud_in, cloud_ground, cloud_nonground);
+    }
+
+    // construct ground/non-ground clusters
+    cloud_clusters.push_back(cloud_ground);
+    cloud_clusters.push_back(cloud_nonground);
+
+}
+
+}  // namespace segmenter
+}  // namespace lidar

src/detection/detection_ground_filter/ground_plane_fitting_segmenter.hpp

@@ -0,0 +1,86 @@
+#ifndef SEGMENTERS_INCLUDE_SEGMENTERS_GROUND_PLANE_FITTING_SEGMENTER_HPP_
+#define SEGMENTERS_INCLUDE_SEGMENTERS_GROUND_PLANE_FITTING_SEGMENTER_HPP_
+
+#include <Eigen/Core>
+#include <string>
+#include <vector>
+
+#include "base_segmenter.hpp"
+
+namespace lidar {
+namespace segmenter {
+
+typedef struct {
+    Eigen::MatrixXf normal;
+    double d = 0.;
+} model_t;
+
+template <typename PointType>
+bool sortByAxisXAsc(PointType p1, PointType p2) {
+    return p1.x < p2.x;
+}
+
+template <typename PointType>
+bool sortByAxisZAsc(PointType p1, PointType p2) {
+    return p1.z < p2.z;
+}
+
+struct SegmenterParams {
+    // Ground Plane Fitting
+    int gpf_sensor_model = 64;
+    double gpf_sensor_height = 1.73;
+    // fitting multiple planes
+    int gpf_num_segment;
+    // number of iterations
+    int gpf_num_iter;
+    // number of points used to estimate the lowest point representative(LPR)
+    // double of senser model???
+    int gpf_num_lpr;
+    double gpf_th_lprs;
+    // threshold for points to be considered initial seeds
+    double gpf_th_seeds;
+    // ground points threshold distance from the plane
+    //   <== large to guarantee safe removal
+    double gpf_th_gnds;
+};  // struct SegmenterParams
+
+/**
+ * @brief Ground Removal based on Ground Plane Fitting(GPF)
+ * @refer
+ *   Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for
+ * Autonomous Vehicle Applications (ICRA, 2017)
+ */
+class GroundPlaneFittingSegmenter : public BaseSegmenter {
+ public:
+    GroundPlaneFittingSegmenter();
+
+    explicit GroundPlaneFittingSegmenter(const SegmenterParams &params);
+
+    ~GroundPlaneFittingSegmenter();
+
+    /// @brief Segment the point cloud.
+    virtual void segment(
+        const PointICloud &cloud_in,
+        std::vector<PointICloudPtr> &cloud_clusters);  // NOLINT
+
+    virtual std::string name() const { return "GroundPlaneFittingSegmenter"; }
+
+ private:
+    void extractInitialSeeds(const PointICloud &cloud_in,
+                             PointICloudPtr cloud_seeds);
+
+    model_t estimatePlane(const PointICloud &cloud_ground);
+
+    void mainLoop(const PointICloud &cloud_in,
+                  PointICloudPtr cloud_gnds,
+                  PointICloudPtr cloud_ngnds);
+
+ private:
+    SegmenterParams params_;
+    static const int segment_upper_bound_ = 6;
+};  // class GroundPlaneFittingSegmenter
+
+}  // namespace segmenter
+}  // namespace lidar
+
+#endif  // SEGMENTERS_INCLUDE_SEGMENTERS_GROUND_PLANE_FITTING_SEGMENTER_HPP_

src/detection/detection_ground_filter/main.cpp

@@ -0,0 +1,106 @@
+#include <QCoreApplication>
+#include "modulecomm.h"
+#include "base_segmenter.hpp"
+#include "ground_plane_fitting_segmenter.hpp"
+void * gpoint_out= iv::modulecomm::RegisterSend("point_no_ground",20000000,1);
+std::unique_ptr<lidar::segmenter::BaseSegmenter> ground_remover_;
+std::unique_ptr<lidar::segmenter::BaseSegmenter> segmenter_;
+
+void PointShare(lidar::segmenter::PointICloudPtr point_cloud)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size()* sizeof(pcl::PointXYZI)];
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->size() * sizeof(pcl::PointXYZI));
+    iv::modulecomm::ModuleSendMsg(gpoint_out,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size() * sizeof(pcl::PointXYZI));
+    delete strOut;
+
+}
+
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->y;
+        xp.y = p->x;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+    std::vector<lidar::segmenter::PointICloudPtr> cloud_clusters;
+    lidar::segmenter::PointICloudPtr cloud_ground(new lidar::segmenter::PointICloud);
+    lidar::segmenter::PointICloudPtr cloud_nonground(new lidar::segmenter::PointICloud);
+    ground_remover_->segment(*point_cloud, cloud_clusters);
+    *cloud_ground = *cloud_clusters[0];
+    *cloud_nonground = *cloud_clusters[1];
+    PointShare(cloud_nonground);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    void * gpoint;
+    lidar::segmenter::SegmenterParams params;
+    // Ground Plane Fitting
+    params.gpf_sensor_model = 64;
+    params.gpf_sensor_height = 0.90;
+    // fitting multiple planes
+    params.gpf_num_segment = 1;
+    // number of iterations
+    params.gpf_num_iter = 10;
+    // number of points used to estimate the lowest point representative(LPR)
+    // double of senser model???
+    params.gpf_num_lpr =128 ;
+    params.gpf_th_lprs = 0.08;
+    // threshold for points to be considered initial seeds
+    params.gpf_th_seeds = 0.1;
+    // ground points threshold distance from the plane
+    //   <== large to guarantee safe removal
+    params.gpf_th_gnds = 0.3;
+    ground_remover_ = std::unique_ptr<lidar::segmenter::BaseSegmenter>(
+        new lidar::segmenter::GroundPlaneFittingSegmenter(params));
+    gpoint = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+
+    return a.exec();
+}