track cloud filter

TrackingRos.cpp

@@ -22,6 +22,27 @@
 #endif
 #include <GeographicLib/Geodesic.hpp>
 
+// headers in PCL
+#include <pcl/io/pcd_io.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/ia_ransac.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/fpfh.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/filter.h>
+#include <pcl/registration/icp.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/filters/conditional_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/impl/conditional_removal.hpp>
+#include <pcl/impl/pcl_base.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+
 using namespace GeographicLib;
 
 float to360(float rot_y,float lidar_x_angle)
@@ -398,6 +419,10 @@ void TrackingRos::Init(ros::NodeHandle& nh)
         m_cross_center_lat = config["CROSS_CENTER_LAT"].as<double>();
     if(config["CLEAR_DISTANCE"])
         m_clear_distance = config["CLEAR_DISTANCE"].as<double>();
+    if(config["cloud_filter"])
+	m_cloud_filter = config["cloud_filter"].as<int>();
+    if(config["cloud_leafsize"])
+	m_cloud_leafsize = config["cloud_leafsize"].as<double>();
 
     m_objsQueue.set_max_num_items(2);
 
@@ -732,6 +757,7 @@ void TrackingRos::ThreadTrackingProcess()
                 }
                 if (is_need_send == 1)
                 {
+		    /* for test 
                     if(m_cross_center_lon != 0 && m_clear_distance != 0)
                     {
                         double s12, azi1, azi2;
@@ -739,6 +765,7 @@ void TrackingRos::ThreadTrackingProcess()
                         if(s12 > m_clear_distance)
                             continue;
                     }
+		    */
                     //暂存rot_y
                     float rot_y_angle_temp = obj.rot_y;
 
@@ -939,7 +966,24 @@ void TrackingRos::ThreadTrackingProcess()
             m_pubBoundingBoxes.publish(sendBoxes);
             m_pubMarkerArray.publish(sendMarkers);
             m_pubMarkerArrow.publish(sendArrows);
-            m_pubCloud.publish(objsPtr->cloud);
+            if(m_cloud_filter==1)
+            {
+              pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
+              pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
+              pcl::PCLPointCloud2 msg_filtered;
+	      pcl_conversions::toPCL(objsPtr->cloud, *cloud);
+              pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+              sor.setInputCloud(cloudPtr);                //设置需要过滤的点云给滤波对象
+              sor.setLeafSize(m_cloud_leafsize,m_cloud_leafsize,m_cloud_leafsize);               //设置滤波时创建的体素大小为20cm立方体，通过设置该值可以直接改变滤波结果，值越大结果越稀疏
+              sor.filter(msg_filtered);                   //执行滤波处理，存储输出msg_filtered
+              sensor_msgs::PointCloud2 point_output;
+	      pcl_conversions::fromPCL(msg_filtered, point_output);
+              m_pubCloud.publish(point_output);
+            }
+            else
+            {
+              m_pubCloud.publish(objsPtr->cloud);
+            }
 #ifdef _SEND_ABUZHABI_
             static int sendNum = 0;
             sendNum++;
@@ -962,6 +1006,5 @@ void TrackingRos::ThreadTrackingProcess()
             nvtxRangePop();
 #endif
         }
-
     }
 }

TrackingRos.h

@@ -72,4 +72,6 @@ public:
     double m_cross_center_lon = 0;
     double m_cross_center_lat = 0;
     double m_clear_distance = 0;
+    int m_cloud_filter = 0;
+    double m_cloud_leafsize = 0.2;
 };