add detection_ray_ground_filter.

src/detection/detection_ray_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_ray_ground_filter/detection_ray_ground_filter.pro

@@ -0,0 +1,39 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+CONFIG += c++14
+
+# 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 \
+    ray_ground_filter.cpp
+
+HEADERS += \
+    ray_ground_filter.h
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+INCLUDEPATH += /usr/include/opencv4
+
+LIBS += -lpcl_filters -lpcl_common

src/detection/detection_ray_ground_filter/gencolors.cpp

@@ -0,0 +1,146 @@
+#ifndef GENCOLORS_CPP_
+#define GENCOLORS_CPP_
+
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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.
+//
+//M*/
+#include "opencv2/core/core.hpp"
+//#include "precomp.hpp"
+#include <opencv2/opencv.hpp>
+
+#include <iostream>
+
+using namespace cv;
+
+static void downsamplePoints( const Mat& src, Mat& dst, size_t count )
+{
+    CV_Assert( count >= 2 );
+    CV_Assert( src.cols == 1 || src.rows == 1 );
+    CV_Assert( src.total() >= count );
+    CV_Assert( src.type() == CV_8UC3);
+
+    dst.create( 1, (int)count, CV_8UC3 );
+    //TODO: optimize by exploiting symmetry in the distance matrix
+    Mat dists( (int)src.total(), (int)src.total(), CV_32FC1, Scalar(0) );
+    if( dists.empty() )
+        std::cerr << "Such big matrix cann't be created." << std::endl;
+
+    for( int i = 0; i < dists.rows; i++ )
+    {
+        for( int j = i; j < dists.cols; j++ )
+        {
+            float dist = (float)norm(src.at<Point3_<uchar> >(i) - src.at<Point3_<uchar> >(j));
+            dists.at<float>(j, i) = dists.at<float>(i, j) = dist;
+        }
+    }
+
+    double maxVal;
+    Point maxLoc;
+    minMaxLoc(dists, 0, &maxVal, 0, &maxLoc);
+
+    dst.at<Point3_<uchar> >(0) = src.at<Point3_<uchar> >(maxLoc.x);
+    dst.at<Point3_<uchar> >(1) = src.at<Point3_<uchar> >(maxLoc.y);
+
+    Mat activedDists( 0, dists.cols, dists.type() );
+    Mat candidatePointsMask( 1, dists.cols, CV_8UC1, Scalar(255) );
+    activedDists.push_back( dists.row(maxLoc.y) );
+    candidatePointsMask.at<uchar>(0, maxLoc.y) = 0;
+
+    for( size_t i = 2; i < count; i++ )
+    {
+        activedDists.push_back(dists.row(maxLoc.x));
+        candidatePointsMask.at<uchar>(0, maxLoc.x) = 0;
+
+        Mat minDists;
+        reduce( activedDists, minDists, 0, 3 ); //CV_REDUCE_MIN 3
+        minMaxLoc( minDists, 0, &maxVal, 0, &maxLoc, candidatePointsMask );
+        dst.at<Point3_<uchar> >((int)i) = src.at<Point3_<uchar> >(maxLoc.x);
+    }
+}
+
+void generateColors( std::vector<Scalar>& colors, size_t count, size_t factor=100 )
+{
+    if( count < 1 )
+        return;
+
+    colors.resize(count);
+
+    if( count == 1 )
+    {
+        colors[0] = Scalar(0,0,255); // red
+        return;
+    }
+    if( count == 2 )
+    {
+        colors[0] = Scalar(0,0,255); // red
+        colors[1] = Scalar(0,255,0); // green
+        return;
+    }
+
+    // Generate a set of colors in RGB space. A size of the set is severel times (=factor) larger then
+    // the needed count of colors.
+    Mat bgr( 1, (int)(count*factor), CV_8UC3 );
+    randu( bgr, 0, 256 );
+
+    // Convert the colors set to Lab space.
+    // Distances between colors in this space correspond a human perception.
+    Mat lab;
+    cvtColor( bgr, lab, cv::COLOR_BGR2Lab);
+
+    // Subsample colors from the generated set so that
+    // to maximize the minimum distances between each other.
+    // Douglas-Peucker algorithm is used for this.
+    Mat lab_subset;
+    downsamplePoints( lab, lab_subset, count );
+
+    // Convert subsampled colors back to RGB
+    Mat bgr_subset;
+    cvtColor( lab_subset, bgr_subset, cv::COLOR_BGR2Lab );
+
+    CV_Assert( bgr_subset.total() == count );
+    for( size_t i = 0; i < count; i++ )
+    {
+        Point3_<uchar> c = bgr_subset.at<Point3_<uchar> >((int)i);
+        colors[i] = Scalar(c.x, c.y, c.z);
+    }
+}
+
+#endif //GENCOLORS_CPP

src/detection/detection_ray_ground_filter/main.cpp

@@ -0,0 +1,14 @@
+#include <QCoreApplication>
+
+
+#include "ray_ground_filter.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    RayGroundFilter xFilter;
+    xFilter.Run();
+
+    return a.exec();
+}

src/detection/detection_ray_ground_filter/ray_ground_filter.cpp

@@ -0,0 +1,635 @@
+/*
+ * Copyright 2017-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************
+ *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
+ */
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl_ros/transforms.h>
+//#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/extract_indices.h>
+//#include <velodyne_pointcloud/point_types.h>
+//#include "autoware_config_msgs/ConfigRayGroundFilter.h"
+
+#include <opencv2/core/version.hpp>
+
+#include "gencolors.cpp"
+//#if (CV_MAJOR_VERSION == 3)
+//#include "gencolors.cpp"
+//#else
+//#include <opencv2/contrib/contrib.hpp>
+//#endif
+
+#include "ray_ground_filter.h"
+
+
+
+//void RayGroundFilter::update_config_params(const autoware_config_msgs::ConfigRayGroundFilter::ConstPtr& param)
+//{
+//  general_max_slope_ = param->general_max_slope;
+//  local_max_slope_ = param->local_max_slope;
+//  radial_divider_angle_ = param->radial_divider_angle;
+//  concentric_divider_distance_ = param->concentric_divider_distance;
+//  min_height_threshold_ = param->min_height_threshold;
+//  clipping_height_ = param->clipping_height;
+//  min_point_distance_ = param->min_point_distance;
+//  reclass_distance_threshold_ = param->reclass_distance_threshold;
+//}
+
+//bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
+//                                          const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
+//                                          const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr)
+//{
+//  if (in_target_frame == in_cloud_ptr->header.frame_id)
+//  {
+//    *out_cloud_ptr = *in_cloud_ptr;
+//    return true;
+//  }
+
+//  geometry_msgs::TransformStamped transform_stamped;
+//  try
+//  {
+//    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
+//                                                   in_cloud_ptr->header.stamp, ros::Duration(1.0));
+//  }
+//  catch (tf2::TransformException& ex)
+//  {
+//    ROS_WARN("%s", ex.what());
+//    return false;
+//  }
+//  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
+//  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+//  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
+//  out_cloud_ptr->header.frame_id = in_target_frame;
+//  return true;
+//}
+
+//void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher,
+//                                    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+//                                    const std_msgs::Header& in_header)
+//{
+//  sensor_msgs::PointCloud2::Ptr cloud_msg_ptr(new sensor_msgs::PointCloud2);
+//  sensor_msgs::PointCloud2::Ptr trans_cloud_msg_ptr(new sensor_msgs::PointCloud2);
+//  pcl::toROSMsg(*in_cloud_to_publish_ptr, *cloud_msg_ptr);
+//  cloud_msg_ptr->header.frame_id = base_frame_;
+//  cloud_msg_ptr->header.stamp = in_header.stamp;
+//  const bool succeeded = TransformPointCloud(in_header.frame_id, cloud_msg_ptr, trans_cloud_msg_ptr);
+//  if (!succeeded)
+//  {
+//    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << cloud_msg_ptr->header.frame_id << " to "
+//                                                           << in_header.frame_id);
+//    return;
+//  }
+//  in_publisher.publish(*trans_cloud_msg_ptr);
+//}
+
+/*!
+ *
+ * @param[in] in_cloud Input Point Cloud to be organized in radial segments
+ * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
+ * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
+ * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
+ */
+void RayGroundFilter::ConvertXYZIToRTZColor(
+    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
+    const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
+    const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
+    const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds)
+{
+  out_organized_points->resize(in_cloud->points.size());
+  out_radial_divided_indices->clear();
+  out_radial_divided_indices->resize(radial_dividers_num_);
+  out_radial_ordered_clouds->resize(radial_dividers_num_);
+
+  for (size_t i = 0; i < in_cloud->points.size(); i++)
+  {
+    PointXYZIRTColor new_point;
+    auto radius = static_cast<float>(
+        sqrt(in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y));
+    auto theta = static_cast<float>(atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI);
+    if (theta < 0)
+    {
+      theta += 360;
+    }
+    if (theta >= 360)
+    {
+      theta -= 360;
+    }
+
+    auto radial_div = (size_t)floor(theta / radial_divider_angle_);
+
+    auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));
+
+    new_point.point = in_cloud->points[i];
+    new_point.radius = radius;
+    new_point.theta = theta;
+    new_point.radial_div = radial_div;
+    new_point.concentric_div = concentric_div;
+    new_point.red = (size_t)colors_[new_point.radial_div % color_num_].val[0];
+    new_point.green = (size_t)colors_[new_point.radial_div % color_num_].val[1];
+    new_point.blue = (size_t)colors_[new_point.radial_div % color_num_].val[2];
+    new_point.original_index = i;
+
+    out_organized_points->at(i) = new_point;
+
+    // radial divisions
+    out_radial_divided_indices->at(radial_div).indices.push_back(i);
+
+    out_radial_ordered_clouds->at(radial_div).push_back(new_point);
+  }  // end for
+
+// order radial points on each division
+#pragma omp for
+  for (size_t i = 0; i < radial_dividers_num_; i++)
+  {
+    std::sort(out_radial_ordered_clouds->at(i).begin(), out_radial_ordered_clouds->at(i).end(),
+              [](const PointXYZIRTColor& a, const PointXYZIRTColor& b) { return a.radius < b.radius; });  // NOLINT
+  }
+}
+
+/*!
+ * Classifies Points in the PointCoud as Ground and Not Ground
+ * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud ordered by radial distance from the origin
+ * @param out_ground_indices Returns the indices of the points classified as ground in the original PointCloud
+ * @param out_no_ground_indices Returns the indices of the points classified as not ground in the original PointCloud
+ */
+void RayGroundFilter::ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor>& in_radial_ordered_clouds,
+                                         const pcl::PointIndices::Ptr& out_ground_indices,
+                                         const pcl::PointIndices::Ptr& out_no_ground_indices)
+{
+  out_ground_indices->indices.clear();
+  out_no_ground_indices->indices.clear();
+#pragma omp for
+  for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++)  // sweep through each radial division
+  {
+    float prev_radius = 0.f;
+    float prev_height = 0.f;
+    bool prev_ground = false;
+    bool current_ground = false;
+    for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++)  // loop through each point in the radial div
+    {
+      float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
+      float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;
+      float current_height = in_radial_ordered_clouds[i][j].point.z;
+      float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;
+
+      // for points which are very close causing the height threshold to be tiny, set a minimum value
+      if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
+      {
+        height_threshold = min_height_threshold_;
+      }
+
+      // check current point height against the LOCAL threshold (previous point)
+      if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
+      {
+        // Check again using general geometry (radius from origin) if previous points wasn't ground
+        if (!prev_ground)
+        {
+          if (current_height <= general_height_threshold && current_height >= -general_height_threshold)
+          {
+            current_ground = true;
+          }
+          else
+          {
+            current_ground = false;
+          }
+        }
+        else
+        {
+          current_ground = true;
+        }
+      }
+      else
+      {
+        // check if previous point is too far from previous one, if so classify again
+        if (points_distance > reclass_distance_threshold_ &&
+            (current_height <= height_threshold && current_height >= -height_threshold))
+        {
+          current_ground = true;
+        }
+        else
+        {
+          current_ground = false;
+        }
+      }
+
+      if (current_ground)
+      {
+        out_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
+        prev_ground = true;
+      }
+      else
+      {
+        out_no_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
+        prev_ground = false;
+      }
+
+      prev_radius = in_radial_ordered_clouds[i][j].radius;
+      prev_height = in_radial_ordered_clouds[i][j].point.z;
+    }
+  }
+}
+
+/*!
+ * Removes the points higher than a threshold
+ * @param in_cloud_ptr PointCloud to perform Clipping
+ * @param in_clip_height Maximum allowed height in the cloud
+ * @param out_clipped_cloud_ptr Resultung PointCloud with the points removed
+ */
+void RayGroundFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
+                                pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extractor;
+  extractor.setInputCloud(in_cloud_ptr);
+  pcl::PointIndices indices;
+
+#pragma omp for
+  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
+  {
+    if (in_cloud_ptr->points[i].z > in_clip_height)
+    {
+      indices.indices.push_back(i);
+    }
+  }
+  extractor.setIndices(std::make_shared<pcl::PointIndices>(indices));
+//  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
+  extractor.setNegative(true);  // true removes the indices, false leaves only the indices
+  extractor.filter(*out_clipped_cloud_ptr);
+}
+
+/*!
+ * Returns the resulting complementary PointCloud, one with the points kept and the other removed as indicated
+ * in the indices
+ * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
+ * @param in_indices Indices of the points to be both removed and kept
+ * @param out_only_indices_cloud_ptr Resulting PointCloud with the indices kept
+ * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
+ */
+void RayGroundFilter::ExtractPointsIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                                           const pcl::PointIndices& in_indices,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr out_only_indices_cloud_ptr,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr out_removed_indices_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extract_ground;
+  extract_ground.setInputCloud(in_cloud_ptr);
+//  extract_ground.setIndices(boost::make_shared<pcl::PointIndices>(in_indices));
+  extract_ground.setIndices(std::make_shared<pcl::PointIndices>(in_indices));
+
+  extract_ground.setNegative(false);  // true removes the indices, false leaves only the indices
+  extract_ground.filter(*out_only_indices_cloud_ptr);
+
+  extract_ground.setNegative(true);  // true removes the indices, false leaves only the indices
+  extract_ground.filter(*out_removed_indices_cloud_ptr);
+}
+
+/*!
+ * Removes points up to a certain distance in the XY Plane
+ * @param in_cloud_ptr Input PointCloud
+ * @param in_min_distance Minimum valid distance, points closer than this will be removed.
+ * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
+ */
+void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
+                                       pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extractor;
+  extractor.setInputCloud(in_cloud_ptr);
+  pcl::PointIndices indices;
+
+#pragma omp for
+  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
+  {
+    if (sqrt(in_cloud_ptr->points[i].x * in_cloud_ptr->points[i].x +
+             in_cloud_ptr->points[i].y * in_cloud_ptr->points[i].y) < in_min_distance)
+    {
+      indices.indices.push_back(i);
+    }
+  }
+//  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
+  extractor.setIndices(std::make_shared<pcl::PointIndices>(indices));
+  extractor.setNegative(true);  // true removes the indices, false leaves only the indices
+  extractor.filter(*out_filtered_cloud_ptr);
+}
+
+
+void RayGroundFilter::sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize =  reinterpret_cast<int * >(strOut);
+    *pHeadSize = static_cast<int >(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());
+    uint32_t * pu32 =  reinterpret_cast<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 = reinterpret_cast<pcl::PointXYZI *>(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+    std::cout<<"width: "<<point_cloud->width<<std::endl;
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
+    delete [] strOut;
+}
+
+void RayGroundFilter::DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &current_sensor_cloud_ptr)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+    // remove points above certain point
+    ClipCloud(current_sensor_cloud_ptr, clipping_height_, clipped_cloud_ptr);
+
+    // remove closer points than a threshold
+    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+    RemovePointsUpTo(clipped_cloud_ptr, min_point_distance_, filtered_cloud_ptr);
+
+    // GetCloud Normals
+    // pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals_ptr (new pcl::PointCloud<pcl::PointXYZINormal>);
+    // GetCloudNormals(current_sensor_cloud_ptr, cloud_with_normals_ptr, 5.0);
+
+    std::shared_ptr<PointCloudXYZIRTColor> organized_points(new PointCloudXYZIRTColor);
+    std::shared_ptr<std::vector<pcl::PointIndices> > radial_division_indices(new std::vector<pcl::PointIndices>);
+    std::shared_ptr<std::vector<PointCloudXYZIRTColor> > radial_ordered_clouds(new std::vector<PointCloudXYZIRTColor>);
+
+    radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+
+    ConvertXYZIToRTZColor(filtered_cloud_ptr, organized_points, radial_division_indices, radial_ordered_clouds);
+
+    pcl::PointIndices::Ptr ground_indices(new pcl::PointIndices), no_ground_indices(new pcl::PointIndices);
+
+    ClassifyPointCloud(*radial_ordered_clouds, ground_indices, no_ground_indices);
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+    pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+    ExtractPointsIndices(filtered_cloud_ptr, *ground_indices, ground_cloud_ptr, no_ground_cloud_ptr);
+
+    sharepointcloud(mpalidar_ground,ground_cloud_ptr);
+    sharepointcloud(mpalidar_noground,no_ground_cloud_ptr);
+}
+
+void RayGroundFilter::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;
+    }
+
+    QTime xTime;
+    xTime.start();
+
+    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;
+//        if((p->x<-30)||(p->x>30)||(p->y>50)||(p->y<-50))
+//        {
+
+//        }
+//        else
+//        {
+            memcpy(&xp,p,sizeof(pcl::PointXYZI));
+            xp.z = xp.z;
+            point_cloud->push_back(xp);
+
+//        }
+        p++;
+//        xp.x = p->x;
+//        xp.y = p->y;
+//        xp.z = p->z;
+//        xp.intensity = p->intensity;
+//        point_cloud->push_back(xp);
+//        p++;
+    }
+
+//    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;
+
+    DectectOnePCD(point_cloud);
+
+    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" use: "<<xTime.elapsed()<<std::endl;
+
+}
+
+//void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud)
+//{
+//  health_checker_ptr_->NODE_ACTIVATE();
+//  health_checker_ptr_->CHECK_RATE("topic_rate_points_raw_slow", 8, 5, 1, "topic points_raw subscribe rate slow.");
+
+//  sensor_msgs::PointCloud2::Ptr trans_sensor_cloud(new sensor_msgs::PointCloud2);
+//  const bool succeeded = TransformPointCloud(base_frame_, in_sensor_cloud, trans_sensor_cloud);
+//  if (!succeeded)
+//  {
+//    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << base_frame_ << " to "
+//                                                           << in_sensor_cloud->header.frame_id);
+//    return;
+//  }
+
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+//  pcl::fromROSMsg(*trans_sensor_cloud, *current_sensor_cloud_ptr);
+
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+//  // remove points above certain point
+//  ClipCloud(current_sensor_cloud_ptr, clipping_height_, clipped_cloud_ptr);
+
+//  // remove closer points than a threshold
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+//  RemovePointsUpTo(clipped_cloud_ptr, min_point_distance_, filtered_cloud_ptr);
+
+//  // GetCloud Normals
+//  // pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals_ptr (new pcl::PointCloud<pcl::PointXYZINormal>);
+//  // GetCloudNormals(current_sensor_cloud_ptr, cloud_with_normals_ptr, 5.0);
+
+//  std::shared_ptr<PointCloudXYZIRTColor> organized_points(new PointCloudXYZIRTColor);
+//  std::shared_ptr<std::vector<pcl::PointIndices> > radial_division_indices(new std::vector<pcl::PointIndices>);
+//  std::shared_ptr<std::vector<PointCloudXYZIRTColor> > radial_ordered_clouds(new std::vector<PointCloudXYZIRTColor>);
+
+//  radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+
+//  ConvertXYZIToRTZColor(filtered_cloud_ptr, organized_points, radial_division_indices, radial_ordered_clouds);
+
+//  pcl::PointIndices::Ptr ground_indices(new pcl::PointIndices), no_ground_indices(new pcl::PointIndices);
+
+//  ClassifyPointCloud(*radial_ordered_clouds, ground_indices, no_ground_indices);
+
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+//  ExtractPointsIndices(filtered_cloud_ptr, *ground_indices, ground_cloud_ptr, no_ground_cloud_ptr);
+
+//  publish_cloud(ground_points_pub_, ground_cloud_ptr, in_sensor_cloud->header);
+//  publish_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header);
+//}
+
+RayGroundFilter::RayGroundFilter() //: node_handle_("~"), tf_listener_(tf_buffer_)
+
+{
+    iv::xmlparam::Xmlparam xparam("./detection_ray_ground_filter.xml");
+    // Model   |   Horizontal   |   Vertical   | FOV(Vertical)    degrees / rads
+    // ----------------------------------------------------------
+    // HDL-64  |0.08-0.35(0.32) |     0.4      |  -24.9 <=x<=2.0   (26.9  / 0.47)
+    // HDL-32  |     0.1-0.4    |     1.33     |  -30.67<=x<=10.67 (41.33 / 0.72)
+    // VLP-16  |     0.1-0.4    |     2.0      |  -15.0<=x<=15.0   (30    / 0.52)
+    // VLP-16HD|     0.1-0.4    |     1.33     |  -10.0<=x<=10.0   (20    / 0.35)
+    ivstdcolorout("Initializing Ground Filter, please wait...");
+    xparam.GetParam("input_point_topic", input_point_topic_, "/points_raw");
+    printf("Input point_topic: %s", input_point_topic_.c_str());
+
+    xparam.GetParam("base_frame", base_frame_, "base_link");
+    printf("base_frame: %s", base_frame_.c_str());
+
+    xparam.GetParam("general_max_slope", general_max_slope_, 3.0);
+    printf("general_max_slope[deg]: %f", general_max_slope_);
+
+    xparam.GetParam("local_max_slope", local_max_slope_, 5.0);
+    printf("local_max_slope[deg]: %f", local_max_slope_);
+
+    xparam.GetParam("radial_divider_angle", radial_divider_angle_, 0.1);  // 1 degree default
+    printf("radial_divider_angle[deg]: %f", radial_divider_angle_);
+    xparam.GetParam("concentric_divider_distance", concentric_divider_distance_, 0.0);  // 0.0 meters default
+    printf("concentric_divider_distance[meters]: %f", concentric_divider_distance_);
+    xparam.GetParam("min_height_threshold", min_height_threshold_, 0.05);  // 0.05 meters default
+    printf("min_height_threshold[meters]: %f", min_height_threshold_);
+    xparam.GetParam("clipping_height", clipping_height_, 2.0);  // 2.0 meters default above the car
+    printf("clipping_height[meters]: %f", clipping_height_);
+    xparam.GetParam("min_point_distance", min_point_distance_, 1.85);  // 1.85 meters default
+    printf("min_point_distance[meters]: %f", min_point_distance_);
+    xparam.GetParam("reclass_distance_threshold", reclass_distance_threshold_, 0.2);  // 0.5 meters default
+    printf("reclass_distance_threshold[meters]: %f", reclass_distance_threshold_);
+
+    generateColors(colors_, color_num_);
+//  #if (CV_MAJOR_VERSION == 3)
+//    generateColors(colors_, color_num_);
+//  #else
+//    cv::generateColors(colors_, color_num_);
+//  #endif
+
+    radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+    printf("Radial Divisions: %d", (int)radial_dividers_num_);
+
+    std::string no_ground_topic, ground_topic;
+    xparam.GetParam("no_ground_point_topic", no_ground_topic, "/points_no_ground");
+    printf("No Ground Output Point Cloud no_ground_point_topic: %s", no_ground_topic.c_str());
+    xparam.GetParam("ground_point_topic", ground_topic, "/points_ground");
+    printf("Only Ground Output Point Cloud ground_topic: %s", ground_topic.c_str());
+
+    printf("Subscribing to... %s", input_point_topic_.c_str());
+
+    ModuleFun funlidar = std::bind(&RayGroundFilter::ListenPointCloud,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpalidar = iv::modulecomm::RegisterRecvPlus("lidar_pc",funlidar);
+    mpalidar_ground = iv::modulecomm::RegisterSend("lidarpc_ground",10000000,1);
+    mpalidar_noground = iv::modulecomm::RegisterSend("lidarpc_noground",10000000,1);
+//    points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &RayGroundFilter::CloudCallback, this);
+
+//    config_node_sub_ =
+//        node_handle_.subscribe("/config/ray_ground_filter", 1, &RayGroundFilter::update_config_params, this);
+
+//    groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
+//    ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);
+
+    ivstdcolorout("Ready");
+
+
+//  ros::NodeHandle nh;
+//  ros::NodeHandle pnh("~");
+//  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh, pnh);
+//  health_checker_ptr_->ENABLE();
+}
+
+void RayGroundFilter::Run()
+{
+  // Model   |   Horizontal   |   Vertical   | FOV(Vertical)    degrees / rads
+  // ----------------------------------------------------------
+  // HDL-64  |0.08-0.35(0.32) |     0.4      |  -24.9 <=x<=2.0   (26.9  / 0.47)
+  // HDL-32  |     0.1-0.4    |     1.33     |  -30.67<=x<=10.67 (41.33 / 0.72)
+  // VLP-16  |     0.1-0.4    |     2.0      |  -15.0<=x<=15.0   (30    / 0.52)
+  // VLP-16HD|     0.1-0.4    |     1.33     |  -10.0<=x<=10.0   (20    / 0.35)
+//  ROS_INFO("Initializing Ground Filter, please wait...");
+//  node_handle_.param<std::string>("input_point_topic", input_point_topic_, "/points_raw");
+//  ROS_INFO("Input point_topic: %s", input_point_topic_.c_str());
+
+//  node_handle_.param<std::string>("base_frame", base_frame_, "base_link");
+//  ROS_INFO("base_frame: %s", base_frame_.c_str());
+
+//  node_handle_.param("general_max_slope", general_max_slope_, 3.0);
+//  ROS_INFO("general_max_slope[deg]: %f", general_max_slope_);
+
+//  node_handle_.param("local_max_slope", local_max_slope_, 5.0);
+//  ROS_INFO("local_max_slope[deg]: %f", local_max_slope_);
+
+//  node_handle_.param("radial_divider_angle", radial_divider_angle_, 0.1);  // 1 degree default
+//  ROS_INFO("radial_divider_angle[deg]: %f", radial_divider_angle_);
+//  node_handle_.param("concentric_divider_distance", concentric_divider_distance_, 0.0);  // 0.0 meters default
+//  ROS_INFO("concentric_divider_distance[meters]: %f", concentric_divider_distance_);
+//  node_handle_.param("min_height_threshold", min_height_threshold_, 0.05);  // 0.05 meters default
+//  ROS_INFO("min_height_threshold[meters]: %f", min_height_threshold_);
+//  node_handle_.param("clipping_height", clipping_height_, 2.0);  // 2.0 meters default above the car
+//  ROS_INFO("clipping_height[meters]: %f", clipping_height_);
+//  node_handle_.param("min_point_distance", min_point_distance_, 1.85);  // 1.85 meters default
+//  ROS_INFO("min_point_distance[meters]: %f", min_point_distance_);
+//  node_handle_.param("reclass_distance_threshold", reclass_distance_threshold_, 0.2);  // 0.5 meters default
+//  ROS_INFO("reclass_distance_threshold[meters]: %f", reclass_distance_threshold_);
+
+//  generateColors(colors_, color_num_);
+
+//#if (CV_MAJOR_VERSION == 3)
+//  generateColors(colors_, color_num_);
+//#else
+//  cv::generateColors(colors_, color_num_);
+//#endif
+
+//  radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+//  ROS_INFO("Radial Divisions: %d", (int)radial_dividers_num_);
+
+//  std::string no_ground_topic, ground_topic;
+//  node_handle_.param<std::string>("no_ground_point_topic", no_ground_topic, "/points_no_ground");
+//  ROS_INFO("No Ground Output Point Cloud no_ground_point_topic: %s", no_ground_topic.c_str());
+//  node_handle_.param<std::string>("ground_point_topic", ground_topic, "/points_ground");
+//  ROS_INFO("Only Ground Output Point Cloud ground_topic: %s", ground_topic.c_str());
+
+//  ROS_INFO("Subscribing to... %s", input_point_topic_.c_str());
+//  points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &RayGroundFilter::CloudCallback, this);
+
+//  config_node_sub_ =
+//      node_handle_.subscribe("/config/ray_ground_filter", 1, &RayGroundFilter::update_config_params, this);
+
+//  groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
+//  ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);
+
+//  ROS_INFO("Ready");
+
+//  ros::spin();
+}

src/detection/detection_ray_ground_filter/ray_ground_filter.h

@@ -0,0 +1,195 @@
+/*
+ * Copyright 2017-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************
+ *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
+ */
+#ifndef POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H
+#define POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H
+
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include <memory>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/extract_indices.h>
+//#include <velodyne_pointcloud/point_types.h>
+//#include "autoware_config_msgs/ConfigRayGroundFilter.h"
+
+//#include <tf2/transform_datatypes.h>
+//#include <tf2_ros/transform_listener.h>
+//#include <tf2_eigen/tf2_eigen.h>
+
+// headers in Autoware Health Checker
+//#include <autoware_health_checker/health_checker/health_checker.h>
+
+#include <opencv2/core/version.hpp>
+
+#include <opencv2/core/core.hpp>
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+#include "ivstdcolorout.h"
+
+//#include "gencolors.cpp"
+//#if (CV_MAJOR_VERSION == 3)
+//#include "gencolors.cpp"
+//#else
+//#include <opencv2/contrib/contrib.hpp>
+//#endif
+
+class RayGroundFilter
+{
+private:
+//  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
+//  ros::NodeHandle node_handle_;
+//  ros::Subscriber points_node_sub_;
+//  ros::Subscriber config_node_sub_;
+//  ros::Publisher groundless_points_pub_;
+//  ros::Publisher ground_points_pub_;
+
+//  tf2_ros::Buffer tf_buffer_;
+//  tf2_ros::TransformListener tf_listener_;
+
+  std::string input_point_topic_;
+  std::string base_frame_;
+
+  double general_max_slope_;            // degrees
+  double local_max_slope_;              // degrees
+  double radial_divider_angle_;         // distance in rads between dividers
+  double concentric_divider_distance_;  // distance in meters between concentric divisions
+  double min_height_threshold_;         // minimum height threshold regardless the slope, useful for close points
+  double clipping_height_;              // the points higher than this will be removed from the input cloud.
+  double min_point_distance_;           // minimum distance from the origin to consider a point as valid
+  double reclass_distance_threshold_;   // distance between points at which re classification will occur
+
+  size_t radial_dividers_num_;
+  size_t concentric_dividers_num_;
+
+  std::vector<cv::Scalar> colors_;
+  const size_t color_num_ = 60;  // different number of color to generate
+
+  void * mpalidar;
+  void * mpalidar_ground;
+  void * mpalidar_noground;
+
+  struct PointXYZIRTColor
+  {
+    pcl::PointXYZI point;
+
+    float radius;  // cylindric coords on XY Plane
+    float theta;   // angle deg on XY plane
+
+    size_t radial_div;      // index of the radial divsion to which this point belongs to
+    size_t concentric_div;  // index of the concentric division to which this points belongs to
+
+    size_t red;    // Red component  [0-255]
+    size_t green;  // Green Component[0-255]
+    size_t blue;   // Blue component [0-255]
+
+    size_t original_index;  // index of this point in the source pointcloud
+  };
+  typedef std::vector<PointXYZIRTColor> PointCloudXYZIRTColor;
+
+//  void update_config_params(const autoware_config_msgs::ConfigRayGroundFilter::ConstPtr& param);
+
+  /*!
+   * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
+   * @param[in] in_target_frame Coordinate system to perform transform
+   * @param[in] in_cloud_ptr PointCloud to perform transform
+   * @param[out] out_cloud_ptr Resulting transformed PointCloud
+   * @retval true transform successed
+   * @retval false transform faild
+   */
+//  bool TransformPointCloud(const std::string& in_target_frame, const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
+//                           const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr);
+
+//  void publish_cloud(const ros::Publisher& in_publisher,
+//                     const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+//                     const std_msgs::Header& in_header);
+
+  /*!
+   *
+   * @param[in] in_cloud Input Point Cloud to be organized in radial segments
+   * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
+   * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
+   * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
+   */
+  void ConvertXYZIToRTZColor(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
+                             const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
+                             const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
+                             const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds);
+
+  /*!
+   * Classifies Points in the PointCoud as Ground and Not Ground
+   * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud ordered by radial distance from the origin
+   * @param out_ground_indices Returns the indices of the points classified as ground in the original PointCloud
+   * @param out_no_ground_indices Returns the indices of the points classified as not ground in the original PointCloud
+   */
+  void ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor>& in_radial_ordered_clouds,
+                          const pcl::PointIndices::Ptr& out_ground_indices,
+                          const pcl::PointIndices::Ptr& out_no_ground_indices);
+
+  /*!
+   * Removes the points higher than a threshold
+   * @param in_cloud_ptr PointCloud to perform Clipping
+   * @param in_clip_height Maximum allowed height in the cloud
+   * @param out_clipped_cloud_ptr Resultung PointCloud with the points removed
+   */
+  void ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
+                 pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr);
+
+  /*!
+   * Returns the resulting complementary PointCloud, one with the points kept and the other removed as indicated
+   * in the indices
+   * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
+   * @param in_indices Indices of the points to be both removed and kept
+   * @param out_only_indices_cloud_ptr Resulting PointCloud with the indices kept
+   * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
+   */
+  void ExtractPointsIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                            const pcl::PointIndices& in_indices,
+                            pcl::PointCloud<pcl::PointXYZI>::Ptr out_only_indices_cloud_ptr,
+                            pcl::PointCloud<pcl::PointXYZI>::Ptr out_removed_indices_cloud_ptr);
+
+  /*!
+   * Removes points up to a certain distance in the XY Plane
+   * @param in_cloud_ptr Input PointCloud
+   * @param in_min_distance Minimum valid distance, points closer than this will be removed.
+   * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
+   */
+  void RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
+                        pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr);
+
+//  void CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud);
+
+  friend class RayGroundFilter_clipCloud_Test;
+
+  void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr);
+  void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+  void sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
+public:
+  RayGroundFilter();
+  void Run();
+};
+
+#endif  // POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H

src/tool/pointcloudviewer/pointcloudviewer.pro

@@ -1,6 +1,6 @@
 QT -= gui
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 QMAKE_LFLAGS += -no-pie
@@ -25,6 +25,9 @@ INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/eigen3
 INCLUDEPATH += /usr/include/vtk-6.3
+INCLUDEPATH += /usr/include/vtk-9.1
+
+unix:INCLUDEPATH += /usr/include/pcl-1.12
 
 unix:LIBS +=  -lpcl_common\
         -lpcl_features\
@@ -54,5 +57,9 @@ LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
 }
 
 LIBS += -lboost_system
+
+#LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+#       -lvtkFiltersSources-9.1
+
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
 #        -lvtkFiltersSources-6.3