├── CMakeLists.txt
├── README.md
├── config
    └── show.rviz
├── include
    ├── common.h
    ├── filter_extract_lib.hpp
    └── tic_toc.h
├── launch
    └── launch_filter_extract.launch
├── package.xml
├── resources
    └── edge_planar_points.gif
└── src
    ├── extract_feature_node.cpp
    └── filter_node.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(filter_extract_feature)
 3 | 
 4 | set(CMAKE_BUILD_TYPE "Release")
 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -fext-numeric-literals")
 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O2 -Wall -g")
 7 | 
 8 | find_package(catkin REQUIRED COMPONENTS
 9 |   geometry_msgs
10 |   nav_msgs
11 |   sensor_msgs
12 |   roscpp
13 |   rospy
14 |   rosbag
15 |   std_msgs
16 |   image_transport
17 |   cv_bridge
18 |   tf
19 |   livox_ros_driver
20 | )
21 | 
22 | find_package(Eigen3 REQUIRED)
23 | find_package(PCL REQUIRED)
24 | find_package(OpenCV REQUIRED)
25 | find_package(Ceres REQUIRED)
26 | 
27 | include_directories(
28 |   include
29 |   ${catkin_INCLUDE_DIRS}
30 |   ${PCL_INCLUDE_DIRS}
31 |   ${CERES_INCLUDE_DIRS}
32 |   ${OpenCV_INCLUDE_DIRS})
33 | 
34 | 
35 | catkin_package(
36 |   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs livox_ros_driver
37 |   DEPENDS EIGEN3 PCL 
38 |   INCLUDE_DIRS include
39 | )
40 | 
41 | 
42 | add_executable(filter_node src/filter_node.cpp)
43 | target_link_libraries(filter_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
44 | 
45 | add_executable(extract_feature_node src/extract_feature_node.cpp)
46 | target_link_libraries(extract_feature_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
47 | 
48 | 
49 | 
50 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # 激光雷达点云滤波和特征提取
 2 | ## 一、介绍
 3 | 在激光雷达里程计和建图(Lidar Odometry and Mapping， LOAM)任务中，前端在里程计计算位姿前需要对传感器的点云流数据进行滤波(去除噪点)和特征提取处理。
 4 | 
 5 | 其中点云滤波往往需要根据激光雷达具体的扫描方式、型号、特性以及后续任务等进行调整<sup>[4]</sup>；在点云特征提取方面，论文<sup>[3]</sup>中率先提出了将点云曲率较大的边缘点(Edge Points)和曲率较小的平面点(Planar Points)作为点云的特征，进行后续ICP匹配，从而减少内存消耗量，后续LOAM论文基本沿用这一方法。
 6 | 
 7 | 本仓库参考Horizon loam<sup>[5]</sup>中的点云滤波和特征点提取方法,其中Horizon激光雷达CustomMsg的Tag信息<sup>[7]</sup>可以用于基本的滤波操作。
 8 | 
 9 | 调试环境：
10 | 
11 | * Ubuntu 18.04 ROS melodic. [ROS installation](http://wiki.ros.org/ROS/Installation)
12 | * PCL 1.8. [PCL installation](https://pointclouds.org/downloads/#linux)
13 | * 硬件: livox Horizon激光雷达 
14 | 
15 | ## 二、使用方法
16 | 
17 | 1. 安装ROS
18 | 2. 安装livox ROS驱动<sup>[1, 2, 3]</sup>
19 | 3. 安装本仓库
20 | ```
21 | mkdir -p ~/xxx/src
22 | cd ~/xxx/src
23 | catkin_init_workspace
24 | git clone https://github.com/GCaptainNemo/fusion-lidar-camera-ROS.git
25 | cd ..
26 | catkin_make
27 | ```
28 | 4. 运行激光雷达设备驱动
29 | 
30 | ```
31 | roslaunch livox_ros_driver livox_lidar_msg.launch
32 | ```
33 | 
34 | 5. 运行filter_node和extract_feature_node节点，并在rviz中显示
35 | ```
36 | roslaunch filter_extract_feature launch_filter_extract.launch
37 | ```
38 | 
39 | ## 三、效果
40 | <p align="center"><img src="./resources/edge_planar_points.gif" width=50%></p>
41 | <h6 align="center">提取边缘点(红色)和平面点(蓝色)</h6>
42 | 
43 | ## 四、参考资料
44 | [1] [livox 驱动安装](https://github.com/Livox-SDK/livox_ros_driver)
45 | 
46 | [2] [livox SDK安装](https://github.com/Livox-SDK/Livox-SDK)
47 | 
48 | [3] Ji Z , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014.
49 | 
50 | [4] Lin J ,  Zhang F . Loam_livox: A fast, robust, high-precision LiDAR  odometry and mapping package for LiDARs of small FoV[J].  2019.
51 | 
52 | [5] [livox-horizon-loam](https://github.com/Livox-SDK/livox_horizon_loam)
53 | 
54 | [6] [livox-CustomPoint格式](http://docs.ros.org/en/kinetic/api/livox_ros_driver/html/msg/CustomPoint.html)
55 |   
56 | [7] [livox-CustomPoint-tag信息含义](https://livox-wiki-cn.readthedocs.io/zh_CN/latest/introduction/Point_Cloud_Characteristics_and_Coordinate_System%20.html)
57 | 


--------------------------------------------------------------------------------
/config/show.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 138
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /Grid1
 10 |         - /PointCloud21
 11 |       Splitter Ratio: 0.5
 12 |     Tree Height: 1537
 13 |   - Class: rviz/Selection
 14 |     Name: Selection
 15 |   - Class: rviz/Tool Properties
 16 |     Expanded:
 17 |       - /2D Pose Estimate1
 18 |       - /2D Nav Goal1
 19 |       - /Publish Point1
 20 |     Name: Tool Properties
 21 |     Splitter Ratio: 0.5886790156364441
 22 |   - Class: rviz/Views
 23 |     Expanded:
 24 |       - /Current View1
 25 |     Name: Views
 26 |     Splitter Ratio: 0.5
 27 |   - Class: rviz/Time
 28 |     Experimental: false
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: PointCloud2
 32 | Preferences:
 33 |   PromptSaveOnExit: true
 34 | Toolbars:
 35 |   toolButtonStyle: 2
 36 | Visualization Manager:
 37 |   Class: ""
 38 |   Displays:
 39 |     - Alpha: 0.5
 40 |       Cell Size: 1
 41 |       Class: rviz/Grid
 42 |       Color: 160; 160; 164
 43 |       Enabled: true
 44 |       Line Style:
 45 |         Line Width: 0.029999999329447746
 46 |         Value: Lines
 47 |       Name: Grid
 48 |       Normal Cell Count: 0
 49 |       Offset:
 50 |         X: 0
 51 |         Y: 0
 52 |         Z: 0
 53 |       Plane: XY
 54 |       Plane Cell Count: 10
 55 |       Reference Frame: <Fixed Frame>
 56 |       Value: true
 57 |     - Alpha: 1
 58 |       Autocompute Intensity Bounds: true
 59 |       Autocompute Value Bounds:
 60 |         Max Value: 10
 61 |         Min Value: -10
 62 |         Value: true
 63 |       Axis: Z
 64 |       Channel Name: intensity
 65 |       Class: rviz/PointCloud2
 66 |       Color: 255; 255; 255
 67 |       Color Transformer: RGB8
 68 |       Decay Time: 0
 69 |       Enabled: true
 70 |       Invert Rainbow: false
 71 |       Max Color: 255; 255; 255
 72 |       Min Color: 0; 0; 0
 73 |       Name: PointCloud2
 74 |       Position Transformer: XYZ
 75 |       Queue Size: 10
 76 |       Selectable: true
 77 |       Size (Pixels): 3
 78 |       Size (m): 0.009999999776482582
 79 |       Style: Flat Squares
 80 |       Topic: /feature_less_color_sharp_flat
 81 |       Unreliable: false
 82 |       Use Fixed Frame: true
 83 |       Use rainbow: false
 84 |       Value: true
 85 |   Enabled: true
 86 |   Global Options:
 87 |     Background Color: 48; 48; 48
 88 |     Default Light: true
 89 |     Fixed Frame: aft_extract
 90 |     Frame Rate: 30
 91 |   Name: root
 92 |   Tools:
 93 |     - Class: rviz/Interact
 94 |       Hide Inactive Objects: true
 95 |     - Class: rviz/MoveCamera
 96 |     - Class: rviz/Select
 97 |     - Class: rviz/FocusCamera
 98 |     - Class: rviz/Measure
 99 |     - Class: rviz/SetInitialPose
100 |       Theta std deviation: 0.2617993950843811
101 |       Topic: /initialpose
102 |       X std deviation: 0.5
103 |       Y std deviation: 0.5
104 |     - Class: rviz/SetGoal
105 |       Topic: /move_base_simple/goal
106 |     - Class: rviz/PublishPoint
107 |       Single click: true
108 |       Topic: /clicked_point
109 |   Value: true
110 |   Views:
111 |     Current:
112 |       Class: rviz/Orbit
113 |       Distance: 6.619870185852051
114 |       Enable Stereo Rendering:
115 |         Stereo Eye Separation: 0.05999999865889549
116 |         Stereo Focal Distance: 1
117 |         Swap Stereo Eyes: false
118 |         Value: false
119 |       Focal Point:
120 |         X: 0
121 |         Y: 0
122 |         Z: 0
123 |       Focal Shape Fixed Size: true
124 |       Focal Shape Size: 0.05000000074505806
125 |       Invert Z Axis: false
126 |       Name: Current View
127 |       Near Clip Distance: 0.009999999776482582
128 |       Pitch: 0.2303980439901352
129 |       Target Frame: <Fixed Frame>
130 |       Value: Orbit (rviz)
131 |       Yaw: 0.11039817333221436
132 |     Saved: ~
133 | Window Geometry:
134 |   Displays:
135 |     collapsed: false
136 |   Height: 2049
137 |   Hide Left Dock: false
138 |   Hide Right Dock: false
139 |   QMainWindow State: 000000ff00000000fd0000000400000000000001f7000006f9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006f90000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000006f9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006f90000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a00000060fc0100000002fb0000000800540069006d0065010000000000000e7a0000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000b0c000006f900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
140 |   Selection:
141 |     collapsed: false
142 |   Time:
143 |     collapsed: false
144 |   Tool Properties:
145 |     collapsed: false
146 |   Views:
147 |     collapsed: false
148 |   Width: 3706
149 |   X: 134
150 |   Y: 55
151 | 


--------------------------------------------------------------------------------
/include/common.h:
--------------------------------------------------------------------------------
 1 | 
 2 | #pragma once
 3 | 
 4 | #include <pcl/point_cloud.h>
 5 | #include <pcl/point_types.h>
 6 | #include <cmath>
 7 | 
 8 | typedef pcl::PointXYZINormal PointType;
 9 | typedef pcl::PointCloud<PointType> PointCloudXYZI;
10 | inline double rad2deg(double radians) { return radians * 180.0 / M_PI; }
11 | 
12 | inline double deg2rad(double degrees) { return degrees * M_PI / 180.0; }
13 | 


--------------------------------------------------------------------------------
/include/filter_extract_lib.hpp:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #include <pcl/point_types.h>
  3 | 
  4 | 
  5 | constexpr bool dbg_show_id = false;
  6 | 
  7 | // 原位操作,避免push_back耗时操作
  8 | template <typename PointT>
  9 | void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
 10 |                             pcl::PointCloud<PointT> &cloud_out, float thres) {
 11 |   if (&cloud_in != &cloud_out) {
 12 |     cloud_out.header = cloud_in.header;
 13 |     cloud_out.points.resize(cloud_in.points.size());
 14 |   }
 15 |   size_t j = 0;
 16 |   float thresh_hold = thres * thres;
 17 |   for (size_t i = 0; i < cloud_in.points.size(); ++i) {
 18 |     if (cloud_in.points[i].x * cloud_in.points[i].x +
 19 |             cloud_in.points[i].y * cloud_in.points[i].y +
 20 |             cloud_in.points[i].z * cloud_in.points[i].z <
 21 |         thresh_hold)
 22 |       continue;
 23 |     cloud_out.points[j] = cloud_in.points[i];
 24 |     j++;
 25 |   }
 26 |   if (j != cloud_in.points.size()) {
 27 |     cloud_out.points.resize(j);
 28 |   }
 29 | 
 30 |   cloud_out.height = 1;
 31 |   cloud_out.width = static_cast<uint32_t>(j);
 32 |   cloud_out.is_dense = true;
 33 | }
 34 | 
 35 | template <typename PointT>
 36 | void VisualizeCurvature(float *v_curv, int *v_label,
 37 |                         const pcl::PointCloud<PointT> &pcl_in,
 38 |                         const std_msgs::Header &hdr) {
 39 |   ROS_ASSERT(pcl_in.size() < 400000);
 40 | 
 41 |   /// Same marker attributes
 42 |   visualization_msgs::Marker txt_mk;
 43 |   txt_mk.header = hdr;
 44 |   txt_mk.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
 45 |   txt_mk.ns = "default";
 46 |   txt_mk.id = 0;
 47 |   txt_mk.action = visualization_msgs::Marker::ADD;
 48 |   txt_mk.pose.orientation.x = 0;
 49 |   txt_mk.pose.orientation.y = 0;
 50 |   txt_mk.pose.orientation.z = 0;
 51 |   txt_mk.pose.orientation.w = 1;
 52 |   txt_mk.scale.z = 0.05;
 53 |   txt_mk.color.a = 1;
 54 |   txt_mk.color.r = 0;
 55 |   txt_mk.color.g = 1;
 56 |   txt_mk.color.b = 0;
 57 | 
 58 |   static visualization_msgs::MarkerArray curv_txt_msg;
 59 |  
 60 | 
 61 |   /// Marger array message
 62 |   static size_t pre_pt_num = 0;
 63 |   size_t pt_num = pcl_in.size();
 64 | 
 65 |   if (pre_pt_num == 0) {
 66 |     curv_txt_msg.markers.reserve(400000);
 67 |   }
 68 |   if (pre_pt_num > pt_num) {
 69 |     curv_txt_msg.markers.resize(pre_pt_num);
 70 |   } else {
 71 |     curv_txt_msg.markers.resize(pt_num);
 72 |   }
 73 | 
 74 |   int edge_num = 0, edgeless_num = 0, flat_num = 0, flatless_num = 0, nn = 0;
 75 | 
 76 |   /// Add marker and namespace
 77 |   for (size_t i = 0; i < pcl_in.size(); ++i) {
 78 |     auto curv = v_curv[i];
 79 |     auto label = v_label[i];  /// -1: flat, 0: less-flat, 1:less-edge, 2:edge
 80 |     const auto &pt = pcl_in[i];
 81 | 
 82 |     switch (label) {
 83 |       case 2: {
 84 |         /// edge
 85 |         auto &mk_i = curv_txt_msg.markers[i];
 86 |         mk_i = txt_mk;
 87 |         mk_i.ns = "edge";
 88 |         mk_i.id = i;
 89 |         mk_i.pose.position.x = pt.x;
 90 |         mk_i.pose.position.y = pt.y;
 91 |         mk_i.pose.position.z = pt.z;
 92 |         mk_i.color.a = 1;
 93 |         mk_i.color.r = 1;
 94 |         mk_i.color.g = 0;
 95 |         mk_i.color.b = 0;
 96 |         char cstr[10];
 97 |         snprintf(cstr, 9, "%.2f", curv);
 98 |         mk_i.text = std::string(cstr);
 99 |         /// debug
100 |         if (dbg_show_id) {
101 |           mk_i.text = std::to_string(i);
102 |         }
103 | 
104 |         edge_num++;
105 |         break;
106 |       }
107 |       case 1: {
108 |         /// less edge
109 |         auto &mk_i = curv_txt_msg.markers[i];
110 |         mk_i = txt_mk;
111 |         mk_i.ns = "edgeless";
112 |         mk_i.id = i;
113 |         mk_i.pose.position.x = pt.x;
114 |         mk_i.pose.position.y = pt.y;
115 |         mk_i.pose.position.z = pt.z;
116 |         mk_i.color.a = 0.5;
117 |         mk_i.color.r = 0.5;
118 |         mk_i.color.g = 0;
119 |         mk_i.color.b = 0.8;
120 |         char cstr[10];
121 |         snprintf(cstr, 9, "%.2f", curv);
122 |         mk_i.text = std::string(cstr);
123 |         /// debug
124 |         if (dbg_show_id) {
125 |           mk_i.text = std::to_string(i);
126 |         }
127 | 
128 |         edgeless_num++;
129 |         break;
130 |       }
131 |       case 0: {
132 |         /// less flat
133 |         auto &mk_i = curv_txt_msg.markers[i];
134 |         mk_i = txt_mk;
135 |         mk_i.ns = "flatless";
136 |         mk_i.id = i;
137 |         mk_i.pose.position.x = pt.x;
138 |         mk_i.pose.position.y = pt.y;
139 |         mk_i.pose.position.z = pt.z;
140 |         mk_i.color.a = 0.5;
141 |         mk_i.color.r = 0;
142 |         mk_i.color.g = 0.5;
143 |         mk_i.color.b = 0.8;
144 |         char cstr[10];
145 |         snprintf(cstr, 9, "%.2f", curv);
146 |         mk_i.text = std::string(cstr);
147 |         /// debug
148 |         if (dbg_show_id) {
149 |           mk_i.text = std::to_string(i);
150 |         }
151 | 
152 |         flatless_num++;
153 |         break;
154 |       }
155 |       case -1: {
156 |         /// flat
157 |         auto &mk_i = curv_txt_msg.markers[i];
158 |         mk_i = txt_mk;
159 |         mk_i.ns = "flat";
160 |         mk_i.id = i;
161 |         mk_i.pose.position.x = pt.x;
162 |         mk_i.pose.position.y = pt.y;
163 |         mk_i.pose.position.z = pt.z;
164 |         mk_i.color.a = 1;
165 |         mk_i.color.r = 0;
166 |         mk_i.color.g = 1;
167 |         mk_i.color.b = 0;
168 |         char cstr[10];
169 |         snprintf(cstr, 9, "%.2f", curv);
170 |         mk_i.text = std::string(cstr);
171 |         /// debug
172 |         if (dbg_show_id) {
173 |           mk_i.text = std::to_string(i);
174 |         }
175 | 
176 |         flat_num++;
177 |         break;
178 |       }
179 |       default: {
180 |         /// Un-reliable
181 |         /// Do nothing for label=99
182 |         // ROS_ASSERT_MSG(false, "%d", label);
183 |         auto &mk_i = curv_txt_msg.markers[i];
184 |         mk_i = txt_mk;
185 |         mk_i.ns = "unreliable";
186 |         mk_i.id = i;
187 |         mk_i.pose.position.x = pt.x;
188 |         mk_i.pose.position.y = pt.y;
189 |         mk_i.pose.position.z = pt.z;
190 |         mk_i.color.a = 0;
191 |         mk_i.color.r = 0;
192 |         mk_i.color.g = 0;
193 |         mk_i.color.b = 0;
194 |         char cstr[10];
195 |         snprintf(cstr, 9, "%.2f", curv);
196 |         mk_i.text = std::string(cstr);
197 | 
198 |         nn++;
199 |         break;
200 |       }
201 |     }
202 |   }
203 |   ROS_INFO("edge/edgeless/flatless/flat/nn num: [%d / %d / %d / %d / %d] - %lu",
204 |            edge_num, edgeless_num, flatless_num, flat_num, nn, pt_num);
205 | 
206 |   /// Delete old points
207 |   if (pre_pt_num > pt_num) {
208 |     ROS_WARN("%lu > %lu", pre_pt_num, pt_num);
209 |     // curv_txt_msg.markers.resize(pre_pt_num);
210 |     for (size_t i = pt_num; i < pre_pt_num; ++i) {
211 |       auto &mk_i = curv_txt_msg.markers[i];
212 |       mk_i.action = visualization_msgs::Marker::DELETE;
213 |       mk_i.color.a = 0;
214 |       mk_i.color.r = 0;
215 |       mk_i.color.g = 0;
216 |       mk_i.color.b = 0;
217 |       mk_i.ns = "old";
218 |       mk_i.text = "";
219 |     }
220 |   }
221 |   pre_pt_num = pt_num;
222 | 
223 |   // pub_curvature.publish(curv_txt_msg);
224 | }
225 | 


--------------------------------------------------------------------------------
/include/tic_toc.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <chrono>
 4 | #include <cstdlib>
 5 | #include <ctime>
 6 | 
 7 | class TicToc {
 8 |  public:
 9 |   TicToc() { tic(); }
10 | 
11 |   void tic() { start = std::chrono::system_clock::now(); }
12 | 
13 |   double toc() {
14 |     end = std::chrono::system_clock::now();
15 |     std::chrono::duration<double> elapsed_seconds = end - start; // 微秒
16 |     return elapsed_seconds.count() * 1000;
17 |   }
18 | 
19 |  private:
20 |   std::chrono::time_point<std::chrono::system_clock> start, end;
21 | };
22 | 


--------------------------------------------------------------------------------
/launch/launch_filter_extract.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <param name="scan_line" type="int" value="6" />
 3 | 
 4 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 5 |     <!-- remove too closed points -->
 6 |     <param name="minimum_range" type="double" value="0.3"/>
 7 |     <param name="threshold_flat" type="double" value="0.01"/>
 8 |     <param name="threshold_sharp" type="double" value="0.1"/>
 9 |     <param name="mapping_line_resolution" type="double" value="0.3"/>
10 |     <param name="mapping_plane_resolution" type="double" value="0.6"/>
11 | 
12 |     <node pkg="filter_extract_feature" type="extract_feature_node" name="extract_feature_node" output="screen" >
13 |         <remap from="/livox_undistort" to="/livox_pcl0" />
14 |     </node>
15 |     <node pkg="filter_extract_feature" type="filter_node" name="filter_node" output="screen" />
16 |     <node pkg="rviz" type="rviz" name="rviz_filter_and_extract" args="-d $(find filter_extract_feature)/config/show.rviz"  />
17 | </launch>
18 | 
19 | 
20 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>filter_extract_feature</name>
 4 |   <version>0.0.0</version>
 5 | 
 6 |   <description>
 7 |     This is an advanced implementation of the following open source project:
 8 |   </description>
 9 | 
10 |   <maintainer email="why@todo.com">why</maintainer>
11 | 
12 |   <license>BSD</license>
13 | 
14 |   <author email="why@todo.com">livox</author>
15 |   
16 |   <buildtool_depend>catkin</buildtool_depend>
17 |   <build_depend>geometry_msgs</build_depend>
18 |   <build_depend>nav_msgs</build_depend>
19 |   <build_depend>roscpp</build_depend>
20 |   <build_depend>rospy</build_depend>
21 |   <build_depend>std_msgs</build_depend>  
22 |   <build_depend>rosbag</build_depend>
23 |   <build_depend>sensor_msgs</build_depend>
24 |   <build_depend>tf</build_depend>
25 |   <build_depend>image_transport</build_depend>
26 |   <build_depend>livox_ros_driver</build_depend>
27 |   
28 |   <run_depend>geometry_msgs</run_depend>
29 |   <run_depend>nav_msgs</run_depend>
30 |   <run_depend>sensor_msgs</run_depend>
31 |   <run_depend>roscpp</run_depend>
32 |   <run_depend>rospy</run_depend>
33 |   <run_depend>std_msgs</run_depend>
34 |   <run_depend>rosbag</run_depend>
35 |   <run_depend>tf</run_depend>
36 |   <run_depend>image_transport</run_depend>
37 |   <run_depend>livox_ros_driver</run_depend>
38 | 
39 |   <export>
40 |   </export>
41 | </package>
42 | 


--------------------------------------------------------------------------------
/resources/edge_planar_points.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/GCaptainNemo/LOAM-Preprocessing/d782cf35942ece064280efcda2e8378c216992be/resources/edge_planar_points.gif


--------------------------------------------------------------------------------
/src/extract_feature_node.cpp:
--------------------------------------------------------------------------------
  1 | #include <nav_msgs/Odometry.h>
  2 | #include <opencv/cv.h>
  3 | #include <pcl/filters/voxel_grid.h>
  4 | #include <pcl/kdtree/kdtree_flann.h>
  5 | #include <pcl/point_cloud.h>
  6 | #include <pcl/point_types.h>
  7 | #include <pcl_conversions/pcl_conversions.h>
  8 | #include <ros/ros.h>
  9 | #include <sensor_msgs/Imu.h>
 10 | #include <sensor_msgs/PointCloud2.h>
 11 | #include <tf/transform_datatypes.h>
 12 | #include <visualization_msgs/MarkerArray.h>
 13 | #include <cmath>
 14 | #include <string>
 15 | #include <vector>
 16 | #include "filter_extract_lib.hpp"
 17 | #include "common.h"
 18 | #include "tic_toc.h"
 19 | 
 20 | using std::atan2;
 21 | using std::cos;
 22 | using std::sin;
 23 | 
 24 | 
 25 | constexpr bool b_viz_curv = false;
 26 | constexpr bool b_voxel_filter = false;
 27 | constexpr bool b_normalize_curv = true;
 28 | 
 29 | const double scanPeriod = 0.1;
 30 | 
 31 | const int systemDelay = 0;
 32 | int systemInitCount = 0;
 33 | bool systemInited = false;
 34 | int N_SCANS = 0;
 35 | 
 36 | float cloudCurvature[400000];
 37 | int cloudSortInd[400000];
 38 | int cloudNeighborPicked[400000]; // not picked 0, picked 1(不能作为特征点)
 39 | // cloudLabel:
 40 | // normal 0, curvature < kThresholdFlat -1, too far or too near 99, 
 41 | // edgePointsLessSharp 1, edgePointsSharp 2
 42 | int cloudLabel[400000];  
 43 | 
 44 | bool comp(int i, int j) { return (cloudCurvature[i] < cloudCurvature[j]); }
 45 | 
 46 | ros::Publisher pubLaserCloud;
 47 | ros::Publisher pubEdgePointsSharp;
 48 | ros::Publisher pubEdgePointsLessSharp;
 49 | ros::Publisher pubPlanarPointsFlat;
 50 | ros::Publisher pubPlanarPointsLessFlat;
 51 | ros::Publisher pubColorEdgePlanarPoints;
 52 | ros::Publisher pubColorLessEdgePlanarPoints;
 53 | 
 54 | std::vector<ros::Publisher> pubEachScan;
 55 | 
 56 | 
 57 | bool b_PUB_EACH_LINE = false;
 58 | 
 59 | double MINIMUM_RANGE = 0.1;
 60 | double THRESHOLD_FLAT = 0.01;
 61 | double THRESHOLD_SHARP = 0.01;
 62 | 
 63 | int kNumCurvSize = 5;
 64 | constexpr int kNumRegion = 50;       // 6
 65 | constexpr int kNumEdge = 2;          // 2 最大edge points数目
 66 | constexpr int kNumFlat = 4;          // 4 最大 planar points数目
 67 | constexpr int kNumEdgeNeighbor = 5;  // 5;
 68 | constexpr int kNumFlatNeighbor = 5;  // 5;
 69 | float kThresholdSharp = 50;          // 0.1;
 70 | float kThresholdFlat = 30;           // 0.1;
 71 | constexpr float kThresholdLessflat = 0.1;
 72 | 
 73 | constexpr float kDistanceFaraway = 25;
 74 | 
 75 | 
 76 | void initialize_and_filter(const int &cloudSize, const pcl::PointCloud<PointType>::Ptr &laserCloud)
 77 | {
 78 |   // 1. 计算曲率 2. 初始化cloudSortInd, cloudNeighborPicked, cloudLabel 3. unreliable points(太近或太远)
 79 |   for (int i = 5; i < cloudSize - 5; i++) {
 80 |     float dis = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
 81 |                      laserCloud->points[i].y * laserCloud->points[i].y +
 82 |                      laserCloud->points[i].z * laserCloud->points[i].z);
 83 |     if (dis > kDistanceFaraway) {
 84 |       kNumCurvSize = 2;
 85 |     }
 86 |     float diffX = 0, diffY = 0, diffZ = 0;
 87 |     for (int j = 1; j <= kNumCurvSize; ++j) {
 88 |       diffX += laserCloud->points[i - j].x + laserCloud->points[i + j].x;
 89 |       diffY += laserCloud->points[i - j].y + laserCloud->points[i + j].y;
 90 |       diffZ += laserCloud->points[i - j].z + laserCloud->points[i + j].z;
 91 |     }
 92 |     diffX -= 2 * kNumCurvSize * laserCloud->points[i].x;
 93 |     diffY -= 2 * kNumCurvSize * laserCloud->points[i].y;
 94 |     diffZ -= 2 * kNumCurvSize * laserCloud->points[i].z;
 95 |    
 96 | 
 97 |     float tmp2 = diffX * diffX + diffY * diffY + diffZ * diffZ;
 98 |     float tmp = sqrt(tmp2);
 99 |     // 在一个邻域内计算扫描方向点云的Laplacian算子，作为点云在该点的曲率
100 |     cloudCurvature[i] = tmp2;
101 |     if (b_normalize_curv) {
102 |       /// use normalized curvature(论文LOAM中提出)
103 |       cloudCurvature[i] = tmp / (2 * kNumCurvSize * dis + 1e-3);
104 |     }
105 |     cloudSortInd[i] = i;
106 |     cloudNeighborPicked[i] = 0;
107 |     cloudLabel[i] = 0;
108 | 
109 |     /// Mark un-reliable points
110 |     constexpr float kMaxFeatureDis = 1e4;
111 |     if (fabs(dis) > kMaxFeatureDis || fabs(dis) < 1e-4 || !std::isfinite(dis)) {
112 |       cloudLabel[i] = 99;
113 |       cloudNeighborPicked[i] = 1; // 标签为1不能作为特征点
114 |     }
115 |   }
116 | }
117 | 
118 | void filter_sharp_points(const int &cloudSize, const pcl::PointCloud<PointType>::Ptr &laserCloud)
119 | {
120 |   // 一个点距前一个点和距后一个点的距离都太远就舍弃(尖锐的点)
121 |   for (int i = 5; i < cloudSize - 6; i++) {
122 |     float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
123 |     float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
124 |     float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
125 |     float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
126 | 
127 |     float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
128 |     float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
129 |     float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
130 |     float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
131 |     float dis = laserCloud->points[i].x * laserCloud->points[i].x +
132 |                 laserCloud->points[i].y * laserCloud->points[i].y +
133 |                 laserCloud->points[i].z * laserCloud->points[i].z;
134 | 
135 |     if (diff > 0.00015 * dis && diff2 > 0.00015 * dis) {
136 |       cloudNeighborPicked[i] = 1;
137 |     }
138 |   }
139 | }
140 | 
141 | void extract_edge_points(pcl::PointCloud<PointType> &edgePointsSharp, pcl::PointCloud<PointType> &edgePointsLessSharp, const pcl::PointCloud<PointType>::Ptr &laserCloud, 
142 | const int &sp, const int &ep)
143 | {
144 |   // ////////////////////////////////////////////////////////////////////////
145 |   // 提取edge point
146 |   // ////////////////////////////////////////////////////////////////////////
147 |   int largestPickedNum = 0;
148 |   for (int k = ep; k >= sp; k--) {
149 |     int ind = cloudSortInd[k];
150 | 
151 |     if (cloudNeighborPicked[ind] != 0) continue;
152 |     // 提取edge points(曲率很大的点)
153 |     if (cloudCurvature[ind] > kThresholdSharp) {
154 |       largestPickedNum++;
155 |       // kNumEdge，edge points个数有上界，2个以内Sharp，20个以内lessSharp
156 |       if (largestPickedNum <= kNumEdge) {
157 |         cloudLabel[ind] = 2;
158 |         edgePointsSharp.push_back(laserCloud->points[ind]);
159 |         edgePointsLessSharp.push_back(laserCloud->points[ind]);
160 |       } else if (largestPickedNum <= 20) {
161 |         cloudLabel[ind] = 1;
162 |         edgePointsLessSharp.push_back(laserCloud->points[ind]);
163 |       } else {
164 |         break;
165 |       }
166 | 
167 |       cloudNeighborPicked[ind] = 1;
168 | 
169 |       // 如果在周围5个点之内的点的距离小于0.02m，就认为周围点被picked了
170 |       for (int l = 1; l <= kNumEdgeNeighbor; l++) {
171 |         float diffX = laserCloud->points[ind + l].x -
172 |                       laserCloud->points[ind + l - 1].x;
173 |         float diffY = laserCloud->points[ind + l].y -
174 |                       laserCloud->points[ind + l - 1].y;
175 |         float diffZ = laserCloud->points[ind + l].z -
176 |                       laserCloud->points[ind + l - 1].z;
177 |         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.02) {
178 |           break;
179 |         }
180 | 
181 |         cloudNeighborPicked[ind + l] = 1;
182 |       }
183 |       for (int l = -1; l >= -kNumEdgeNeighbor; l--) {
184 |         float diffX = laserCloud->points[ind + l].x -
185 |                       laserCloud->points[ind + l + 1].x;
186 |         float diffY = laserCloud->points[ind + l].y -
187 |                       laserCloud->points[ind + l + 1].y;
188 |         float diffZ = laserCloud->points[ind + l].z -
189 |                       laserCloud->points[ind + l + 1].z;
190 |         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.02) {
191 |           break;
192 |         }
193 | 
194 |         cloudNeighborPicked[ind + l] = 1;
195 |       }
196 |     }
197 |   }
198 | }
199 | 
200 | void extract_planar_points(pcl::PointCloud<PointType> &planarPointsFlat, pcl::PointCloud<PointType>::Ptr planarPointsLessFlatScan, 
201 | const pcl::PointCloud<PointType>::Ptr &laserCloud, const int &sp, const int &ep)
202 | {
203 |   int smallestPickedNum = 0;
204 |   for (int k = sp; k <= ep; k++) 
205 |   {
206 |     int ind = cloudSortInd[k];
207 | 
208 |     if (cloudNeighborPicked[ind] != 0) continue;
209 | 
210 |     if (cloudCurvature[ind] < kThresholdFlat) {
211 |       cloudLabel[ind] = -1;
212 |       planarPointsFlat.push_back(laserCloud->points[ind]);
213 |       cloudNeighborPicked[ind] = 1;
214 | 
215 |       smallestPickedNum++;
216 |       if (smallestPickedNum >= kNumFlat) {
217 |         // 4 points
218 |         break;
219 |       }
220 |       // 如果在周围5个点之内的点的距离小于0.02m，就认为周围点被picked了
221 |       for (int l = 1; l <= kNumFlatNeighbor; l++) {
222 |         float diffX = laserCloud->points[ind + l].x -
223 |                       laserCloud->points[ind + l - 1].x;
224 |         float diffY = laserCloud->points[ind + l].y -
225 |                       laserCloud->points[ind + l - 1].y;
226 |         float diffZ = laserCloud->points[ind + l].z -
227 |                       laserCloud->points[ind + l - 1].z;
228 |         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.02) {
229 |           break;
230 |         }
231 | 
232 |         cloudNeighborPicked[ind + l] = 1;
233 |       }
234 |       for (int l = -1; l >= -kNumFlatNeighbor; l--) {
235 |         float diffX = laserCloud->points[ind + l].x -
236 |                       laserCloud->points[ind + l + 1].x;
237 |         float diffY = laserCloud->points[ind + l].y -
238 |                       laserCloud->points[ind + l + 1].y;
239 |         float diffZ = laserCloud->points[ind + l].z -
240 |                       laserCloud->points[ind + l + 1].z;
241 |         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.02) {
242 |           break;
243 |         }
244 | 
245 |         cloudNeighborPicked[ind + l] = 1;
246 |       }
247 |     }
248 |   }
249 |   // 标签小于等于0，激光点云的点曲率小于阈值0.1 lessflat，planar points多多益善
250 |   for (int k = sp; k <= ep; k++) {
251 |     if (cloudLabel[k] <= 0 && cloudCurvature[k] < kThresholdLessflat) {
252 |       planarPointsLessFlatScan->push_back(laserCloud->points[k]);
253 |     }
254 |   }
255 | }
256 | 
257 | void pub_color( const pcl::PointCloud<PointType> &edgePointsSharp, 
258 |                   const pcl::PointCloud<PointType> &edgePointsLessSharp,
259 |                   const pcl::PointCloud<PointType> &planarPointsFlat,
260 |                   const pcl::PointCloud<PointType> &planarPointsLessFlat, const ros::Time &stamp)
261 | {
262 |   
263 |   pcl::PointXYZRGB point;
264 |   pcl::PointCloud<pcl::PointXYZRGB> sharp_flat_points;
265 |   pcl::PointCloud<pcl::PointXYZRGB> less_sharp_flat_points;
266 |   for(int i=0; i < edgePointsSharp.size(); ++i)
267 |   {
268 |     point.x = edgePointsSharp.points[i].x;
269 |     point.y = edgePointsSharp.points[i].y;
270 |     point.z = edgePointsSharp.points[i].z;
271 |     point.r = 255;
272 |     point.g = 0;
273 |     point.b = 0;
274 |     sharp_flat_points.push_back(point);
275 |   }
276 |   for(int i=0; i < planarPointsFlat.size(); ++i)
277 |   {
278 |     point.x = planarPointsFlat.points[i].x;
279 |     point.y = planarPointsFlat.points[i].y;
280 |     point.z = planarPointsFlat.points[i].z;
281 |     point.r = 0;
282 |     point.g = 0;
283 |     point.b = 255;
284 |     sharp_flat_points.push_back(point);
285 |   }
286 |   // /////////////////////////////////////////////
287 |   for(int i=0; i < edgePointsLessSharp.size(); ++i)
288 |   {
289 |     point.x = edgePointsLessSharp.points[i].x;
290 |     point.y = edgePointsLessSharp.points[i].y;
291 |     point.z = edgePointsLessSharp.points[i].z;
292 |     point.r = 255;
293 |     point.g = 0;
294 |     point.b = 0;
295 |     less_sharp_flat_points.push_back(point);
296 |   }
297 |   for(int i=0; i < planarPointsLessFlat.size(); ++i)
298 |   {
299 |     point.x = planarPointsLessFlat.points[i].x;
300 |     point.y = planarPointsLessFlat.points[i].y;
301 |     point.z = planarPointsLessFlat.points[i].z;
302 |     point.r = 0;
303 |     point.g = 0;
304 |     point.b = 255;
305 |     less_sharp_flat_points.push_back(point);
306 |   }
307 |   sensor_msgs::PointCloud2 sharp_flat_points2;
308 |   pcl::toROSMsg(sharp_flat_points, sharp_flat_points2);
309 |   sharp_flat_points2.header.stamp = stamp;
310 |   sharp_flat_points2.header.frame_id = "/aft_extract";
311 |   pubColorEdgePlanarPoints.publish(sharp_flat_points2);
312 | 
313 |   sensor_msgs::PointCloud2 less_sharp_flat_points2;
314 |   pcl::toROSMsg(less_sharp_flat_points, less_sharp_flat_points2);
315 |   less_sharp_flat_points2.header.stamp = stamp;
316 |   less_sharp_flat_points2.header.frame_id = "/aft_extract";
317 |   pubColorLessEdgePlanarPoints.publish(less_sharp_flat_points2);
318 | 
319 | };
320 | 
321 | 
322 | 
323 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
324 |   if (!systemInited) {
325 |     systemInitCount++;
326 |     if (systemInitCount >= systemDelay) {
327 |       systemInited = true;
328 |     } else
329 |       return;
330 |   }
331 | 
332 |   TicToc t_whole;
333 |   TicToc t_prepare;
334 |   std::vector<int> scanStartInd(N_SCANS, 0);
335 |   std::vector<int> scanEndInd(N_SCANS, 0);
336 | 
337 |   pcl::PointCloud<PointType> laserCloudIn;
338 |   pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
339 |   std::vector<int> indices;
340 |   // 1. filtering
341 |   pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
342 |   removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
343 |   int cloudSize = laserCloudIn.points.size();
344 | 
345 |   // 2. order according to scan num and merge
346 |   PointType point;
347 |   std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); // 6个SCAN
348 |   for (int i = 0; i < cloudSize; i++) {
349 |     point.x = laserCloudIn.points[i].x;
350 |     point.y = laserCloudIn.points[i].y;
351 |     point.z = laserCloudIn.points[i].z;
352 |     point.intensity = laserCloudIn.points[i].intensity; // intensity 整数是scan的数目，小数是时间戳
353 |     point.curvature = laserCloudIn.points[i].curvature; // curvature 是0.1的反射率
354 |     int scanID = 0;
355 |     // main函数中改成了6
356 |     if (N_SCANS == 6) {
357 |       scanID = (int)point.intensity;
358 |     }
359 |     laserCloudScans[scanID].push_back(point);
360 |   }
361 | 
362 |   // printf("points size %d \n", cloudSize);
363 |   // PointType -> PointXYZINormal(merge)
364 |   pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
365 |   for (int i = 0; i < N_SCANS; i++) {
366 |     scanStartInd[i] = laserCloud->size() + 5;
367 |     *laserCloud += laserCloudScans[i];
368 |     scanEndInd[i] = laserCloud->size() - 6;
369 |   } 
370 |   // printf("prepare time %f \n", t_prepare.toc());
371 | 
372 |   
373 |   // 3. 三个内容: 1. 计算曲率 2. 初始化cloudSortInd, cloudNeighborPicked, cloudLabel 3. unreliable points(太近或太远)
374 |   initialize_and_filter(cloudSize, laserCloud);
375 | 
376 |   // 4. 一个点距前一个点和距后一个点的距离都太远就舍弃(尖锐的点，或是某种空间上的噪点)
377 |   filter_sharp_points(cloudSize, laserCloud);
378 | 
379 |   TicToc t_pts;
380 | 
381 |   pcl::PointCloud<PointType> edgePointsSharp;
382 |   pcl::PointCloud<PointType> edgePointsLessSharp;
383 |   pcl::PointCloud<PointType> planarPointsFlat;
384 |   pcl::PointCloud<PointType> planarPointsLessFlat;
385 | 
386 |   if (b_normalize_curv) {
387 |     kThresholdFlat = THRESHOLD_FLAT;
388 |     kThresholdSharp = THRESHOLD_SHARP;
389 |   }
390 | 
391 |   float t_q_sort = 0;
392 |   // kNumCurvSize计算曲率的邻居数6； kNumRegion：把一个扫描线再分成若干区域(50)
393 |   for (int i = 0; i < N_SCANS; i++) {
394 |     if (scanEndInd[i] - scanStartInd[i] < kNumCurvSize) continue;
395 |     pcl::PointCloud<PointType>::Ptr planarPointsLessFlatScan(new pcl::PointCloud<PointType>);
396 |     for (int j = 0; j < kNumRegion; j++) {
397 |       // start point
398 |       int sp =
399 |           scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / kNumRegion;
400 |       int ep = scanStartInd[i] +
401 |                (scanEndInd[i] - scanStartInd[i]) * (j + 1) / kNumRegion - 1;
402 |       
403 |       TicToc t_tmp;
404 |       // sort the curvatures from small to large： 按照曲率由小到大排序，不改变点，改变id
405 |       for (int k = sp + 1; k <= ep; k++) {
406 |         for (int l = k; l >= sp + 1; l--) {
407 |           if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) 
408 |           {
409 |             int temp = cloudSortInd[l - 1];
410 |             cloudSortInd[l - 1] = cloudSortInd[l];
411 |             cloudSortInd[l] = temp;
412 |           }
413 |         }
414 |       }
415 | 
416 |       float SumCurRegion = 0.0;
417 |       float MaxCurRegion = cloudCurvature[cloudSortInd[ep]];  //the largest curvature in sp ~ ep
418 |       for (int k = ep - 1; k >= sp; k--) {
419 |         SumCurRegion += cloudCurvature[cloudSortInd[k]];
420 |       }
421 | 
422 |       if (MaxCurRegion > 3 * SumCurRegion)
423 |         cloudNeighborPicked[cloudSortInd[ep]] = 1;  // 如果edge_points曲率大于三倍的曲率的和，则该点picked,是噪点，不能作为edge point
424 | 
425 |       t_q_sort += t_tmp.toc();
426 |       // ////////////////////////////////////////////////////////////////////////
427 |       // 提取edge point
428 |       // ////////////////////////////////////////////////////////////////////////
429 |       extract_edge_points(edgePointsSharp, edgePointsLessSharp, laserCloud, sp, ep);
430 |       
431 | 
432 |       // ////////////////////////////////////////////////////////////////////////
433 |       // 提取planar point
434 |       // ////////////////////////////////////////////////////////////////////////
435 |       extract_planar_points(planarPointsFlat, planarPointsLessFlatScan, laserCloud, sp, ep);
436 |       
437 |     }
438 |     // /////////////////////////////////////////////////////////
439 |     // 对surfPointsLessFlat体素滤波
440 |     // /////////////////////////////////////////////////////////
441 |     planarPointsLessFlat += planarPointsFlat;
442 |     edgePointsLessSharp += edgePointsSharp;
443 |     /// Whether downsample less-flat points
444 |     if (b_voxel_filter) {
445 |       pcl::PointCloud<PointType> planarPointsLessFlatScanDS;
446 |       // 体素滤波
447 |       pcl::VoxelGrid<PointType> downSizeFilter;
448 |       downSizeFilter.setInputCloud(planarPointsLessFlatScan);
449 |       downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
450 |       downSizeFilter.filter(planarPointsLessFlatScanDS);
451 |       planarPointsLessFlat += planarPointsLessFlatScanDS;
452 |     } else {
453 |       planarPointsLessFlat += *planarPointsLessFlatScan;
454 |     }
455 |   }
456 |   // printf("sort q time %f \n", t_q_sort);
457 |   // printf("seperate points time %f \n", t_pts.toc());
458 | 
459 |   /// Visualize curvature
460 |   if (b_viz_curv) {
461 |     std_msgs::Header ros_hdr = laserCloudMsg->header;
462 |     ros_hdr.frame_id = "/aft_extract";
463 |     VisualizeCurvature(cloudCurvature, cloudLabel, *laserCloud, ros_hdr);
464 |   }
465 |   const auto& stamp = laserCloudMsg->header.stamp;
466 |   sensor_msgs::PointCloud2 laserCloudOutMsg;
467 |   pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
468 |   laserCloudOutMsg.header.stamp = stamp;
469 |   laserCloudOutMsg.header.frame_id = "/aft_extract";
470 |   pubLaserCloud.publish(laserCloudOutMsg);
471 | 
472 |   sensor_msgs::PointCloud2 edgePointsSharpMsg;
473 |   pcl::toROSMsg(edgePointsSharp, edgePointsSharpMsg);
474 |   edgePointsSharpMsg.header.stamp = stamp;
475 |   edgePointsSharpMsg.header.frame_id = "/aft_extract";
476 |   pubEdgePointsSharp.publish(edgePointsSharpMsg);
477 | 
478 |   sensor_msgs::PointCloud2 edgePointsLessSharpMsg;
479 |   pcl::toROSMsg(edgePointsLessSharp, edgePointsLessSharpMsg);
480 |   edgePointsLessSharpMsg.header.stamp = stamp;
481 |   edgePointsLessSharpMsg.header.frame_id = "/aft_extract";
482 |   pubEdgePointsLessSharp.publish(edgePointsLessSharpMsg);
483 | 
484 |   sensor_msgs::PointCloud2 planarPointsFlat2;
485 |   pcl::toROSMsg(planarPointsFlat, planarPointsFlat2);
486 |   planarPointsFlat2.header.stamp = stamp;
487 |   planarPointsFlat2.header.frame_id = "/aft_extract";
488 |   pubPlanarPointsFlat.publish(planarPointsFlat2);
489 | 
490 |   sensor_msgs::PointCloud2 planarPointsLessFlat2;
491 |   pcl::toROSMsg(planarPointsLessFlat, planarPointsLessFlat2);
492 |   planarPointsLessFlat2.header.stamp = stamp;
493 |   planarPointsLessFlat2.header.frame_id = "/aft_extract";
494 |   pubPlanarPointsLessFlat.publish(planarPointsLessFlat2);
495 |   // ////////////////////////////////////////////////////////
496 |   // publish color
497 |   // ////////////////////////////////////////////////////////
498 |   pub_color(edgePointsSharp, edgePointsLessSharp, planarPointsFlat, planarPointsLessFlat, stamp);
499 | 
500 |   // pub each scam
501 |   if (b_PUB_EACH_LINE) {
502 |     for (int i = 0; i < N_SCANS; i++) {
503 |       sensor_msgs::PointCloud2 scanMsg;
504 |       pcl::toROSMsg(laserCloudScans[i], scanMsg);
505 |       scanMsg.header.stamp = stamp;
506 |       scanMsg.header.frame_id = "/aft_extract";
507 |       pubEachScan[i].publish(scanMsg);
508 |     }
509 |   }
510 | 
511 |   printf("scan registration time %f ms *************\n", t_whole.toc());
512 |   if (t_whole.toc() > 100) ROS_WARN("scan registration process over 100ms");
513 | }
514 | 
515 | int main(int argc, char **argv) {
516 |   ros::init(argc, argv, "extract_feature");
517 |   ros::NodeHandle nh;
518 | 
519 |   nh.param<int>("scan_line", N_SCANS, 6); // Horizon has 6 scan lines
520 |   nh.param<double>("threshold_flat", THRESHOLD_FLAT, 0.01);
521 |   nh.param<double>("threshold_sharp", THRESHOLD_SHARP, 0.01);
522 | 
523 |   printf("scan line number %d \n", N_SCANS);
524 | 
525 |   if (N_SCANS != 6) {
526 |     printf("only support livox horizon lidar!");
527 |     return 0;
528 |   }
529 | 
530 |   ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(
531 |       "/livox_repub", 100, laserCloudHandler);
532 |   
533 |   // 滤波后的全部点
534 |   pubLaserCloud =
535 |       nh.advertise<sensor_msgs::PointCloud2>("/cloud_filter", 100);
536 | 
537 |   pubEdgePointsSharp =
538 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_sharp", 100);
539 | 
540 |   pubEdgePointsLessSharp =
541 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_less_sharp", 100);
542 | 
543 |   pubPlanarPointsFlat =
544 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_flat", 100);
545 | 
546 |   pubPlanarPointsLessFlat =
547 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_less_flat", 100);
548 | 
549 |   pubColorEdgePlanarPoints = 
550 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_color_sharp_flat", 100);
551 |   
552 |   pubColorLessEdgePlanarPoints = 
553 |       nh.advertise<sensor_msgs::PointCloud2>("/feature_less_color_sharp_flat", 100);
554 | 
555 |   
556 |   if (b_PUB_EACH_LINE) {
557 |     for (int i = 0; i < N_SCANS; i++) {
558 |       ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>(
559 |           "/laser_scanid_" + std::to_string(i), 100);
560 |       pubEachScan.push_back(tmp);
561 |     }
562 |   }
563 |   ros::spin();
564 | 
565 |   return 0;
566 | }
567 | 
568 | 


--------------------------------------------------------------------------------
/src/filter_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <pcl_conversions/pcl_conversions.h>
 2 | #include <sensor_msgs/PointCloud2.h>
 3 | #include "livox_ros_driver/CustomMsg.h"
 4 | #include "common.h"
 5 | 
 6 | ros::Publisher pub_pcl_out1;
 7 | std::vector<livox_ros_driver::CustomMsgConstPtr> livox_data;
 8 | void LivoxMsgCallback(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
 9 |   livox_data.push_back(livox_msg_in);
10 |   if (livox_data.size() == 0) return;   
11 |   pcl::PointCloud<PointType> pcl_in;
12 | 
13 |   for (size_t j = 0; j < livox_data.size(); j++) {
14 |     auto& livox_msg = livox_data[j];
15 |     auto time_end = livox_msg->points.back().offset_time;
16 |     for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
17 |       // PointXYZINormal
18 |       PointType pt;
19 |       pt.x = livox_msg->points[i].x;
20 |       pt.y = livox_msg->points[i].y;
21 |       pt.z = livox_msg->points[i].z;
22 |       if (livox_msg->points[i].tag != 16)
23 |       {
24 |         // 去除噪点（1. 基于能量判断的噪点 2. 基于空间位置判断的噪点）
25 |         continue;
26 |       }
27 |       // 实现时间归一化
28 |       float s = livox_msg->points[i].offset_time / (float)time_end;
29 |       // The integer part is line number and the decimal part is timestamp
30 |       pt.intensity = livox_msg->points[i].line + s*0.1; 
31 |       // ROS_INFO("intensity-------- %.6f ",pt.intensity);
32 |       pt.curvature = livox_msg->points[i].reflectivity * 0.1;
33 |       // ROS_INFO("pt.curvature-------- %.3f ",pt.curvature);
34 |       pcl_in.push_back(pt);
35 |       // ROS_INFO("pt.tag -----------%d", livox_msg->points[i].tag);
36 |     }
37 |   }
38 | 
39 |   /// timebase 5ms ~ 50000000, so 10 ~ 1ns
40 | 
41 |   unsigned long timebase_ns = livox_data[0]->timebase;
42 |   ros::Time timestamp;
43 |   timestamp.fromNSec(timebase_ns);
44 | 
45 |   //   ROS_INFO("livox1 republish %u points at time %f buf size %ld",
46 |   //   pcl_in.size(),
47 |   //           timestamp.toSec(), livox_data.size());
48 | 
49 |   sensor_msgs::PointCloud2 pcl_ros_msg;
50 |   pcl::toROSMsg(pcl_in, pcl_ros_msg);
51 |   pcl_ros_msg.header.stamp.fromNSec(timebase_ns);
52 |   pcl_ros_msg.header.frame_id = "/livox";
53 |   pub_pcl_out1.publish(pcl_ros_msg);
54 |   livox_data.clear();
55 | }
56 | 
57 | int main(int argc, char** argv) {
58 |   ros::init(argc, argv, "livox_repub");
59 |   ros::NodeHandle nh;
60 | 
61 |   ROS_INFO("start livox_repub");
62 | 
63 |   ros::Subscriber sub_livox_msg1 = nh.subscribe<livox_ros_driver::CustomMsg>(
64 |       "/livox/lidar", 100, LivoxMsgCallback);
65 |   pub_pcl_out1 = nh.advertise<sensor_msgs::PointCloud2>("/livox_repub", 100);
66 |   ros::spin();
67 | }
68 | 
69 | 


--------------------------------------------------------------------------------