├── README.md
├── CMakeLists.txt
├── include
    └── at128_jpc
    │   ├── make_pointxyzitr.h
    │   └── at128_jpc.h
├── package.xml
└── src
    └── at128_jpc.cpp


/README.md:
--------------------------------------------------------------------------------
 1 | # AT128_ground_remove_JCP
 2 | 基于地面分割论文**<<Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process>>** 重构了一下代码，以适配禾赛128线激光雷达，论文 github 链接为：https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC
 3 | 
 4 | 数据采用禾赛 AT128 激光雷达录制的 rosbag，在ubuntu18.04 ROS-Melodic下可直接编译运行。
 5 | 
 6 | rosbag百度云下载地址：https://pan.baidu.com/s/19pUUNX0xZJi4spdRDnJXPA?pwd=6zus   提取码：6zus，下载后将 zip 包解压缩为 AT128.bag
 7 | 
 8 | 
 9 | **运行：**
10 | 
11 | ```shell
12 | $ roscore
13 | # 将项目 AT128_Ground_Remove_JCP 放入工作目录 src 下，返回工作目录空间
14 | $ source devel/setup.bash
15 | $ rosrun at128_jpc at128_jpc
16 | ```
17 | 
18 | 
19 | 
20 | **播放数据包：**
21 | 
22 | ```shell
23 | $ rosbag play AT128.bag
24 | ```
25 | 
26 | 
27 | 
28 | **可视化：**
29 | 
30 | ```shell
31 | $ rviz
32 | ```
33 | 
34 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(at128_jpc)
 3 | 
 4 | ## Compile as C++11, supported in ROS Kinetic and newer
 5 | add_compile_options(-std=c++11)
 6 | set(CMAKE_BUILD_TYPE Release)
 7 | set(CMAKE_CXX_FLAGS_RELEASE -O2)
 8 | ## Find catkin macros and libraries
 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
10 | ## is used, also find other catkin packages
11 | find_package(Boost REQUIRED COMPONENTS system iostreams)
12 | find_package(catkin REQUIRED COMPONENTS
13 |   cv_bridge
14 |   image_transport
15 |   pcl_ros
16 |   roscpp
17 |   rospy
18 |   sensor_msgs
19 |   std_msgs
20 |   pcl_conversions
21 | )
22 | find_package(OpenCV REQUIRED)
23 | catkin_package(
24 |   INCLUDE_DIRS include
25 |   LIBRARIES at128_jpc
26 |  CATKIN_DEPENDS cv_bridge image_transport pcl_ros roscpp rospy sensor_msgs std_msgs
27 | #  DEPENDS system_lib
28 | )
29 | 
30 | include_directories(
31 |   include
32 |   ${catkin_INCLUDE_DIRS}
33 |   ${PCL_INCLUDE_DIRS}
34 |   ${OpenCV_INCLUDE_DIRS}
35 | )
36 | 
37 | 
38 | link_directories(${PCL_LIBRARY_DIRS})
39 | add_definitions(${PCL_DEFINITIONS})
40 | 
41 | 
42 | add_executable(at128_jpc src/at128_jpc.cpp) 
43 | target_link_libraries(at128_jpc ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_INCLUDE_DIRS}) 
44 | add_dependencies(at128_jpc beginner_tutorials_generate_messages_cpp)
45 | 
46 | 


--------------------------------------------------------------------------------
/include/at128_jpc/make_pointxyzitr.h:
--------------------------------------------------------------------------------
 1 | #define PCL_NO_PRECOMPILE //从PCL-1.7开始，您需要定义PCL_NO_PRECOMPILE，然后才能包含任何PCL头文件来包含模板化算
 2 | #ifndef MAKE_POINTXYZITR_H
 3 | #define MAKE_POINTXYZITR_H
 4 | 
 5 | #include <algorithm>
 6 | #include <cmath>
 7 | #include <ctime>
 8 | #include <iostream>
 9 | #include <ros/console.h>
10 | #include <ros/ros.h>
11 | #include <stdlib.h>
12 | #include <time.h>
13 | #include <unordered_set>
14 | #include <vector>
15 | 
16 | #include <pcl/PCLPointCloud2.h>
17 | #include <pcl/common/transforms.h>
18 | #include <pcl/io/pcd_io.h>
19 | #include <pcl/point_cloud.h>
20 | #include <pcl/point_types.h>
21 | #include <pcl/visualization/cloud_viewer.h>
22 | #include <pcl_conversions/pcl_conversions.h>
23 | 
24 | #include <Eigen/Core>
25 | #include <Eigen/Dense>
26 | #include <Eigen/Geometry>
27 | #include <queue>
28 | #include <unordered_map>
29 | 
30 | #include <cv_bridge/cv_bridge.h>
31 | #include <opencv2/core/eigen.hpp>
32 | #include <opencv2/highgui/highgui.hpp>
33 | #include <opencv2/imgproc.hpp>
34 | #include <opencv2/opencv.hpp>
35 | 
36 | namespace pcl {
37 | 
38 | struct PointXYZITR {
39 |   PCL_ADD_POINT4D;
40 |   uint8_t intensity;
41 |   double timestamp;
42 |   uint16_t ring;
43 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 | } EIGEN_ALIGN16;
45 | 
46 | } // namespace pcl
47 | 
48 | POINT_CLOUD_REGISTER_POINT_STRUCT(
49 |     PointXYZITR,
50 |     (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(
51 |         double, timestamp, timestamp)(uint16_t, ring, ring))
52 | 
53 | #endif
54 | 


--------------------------------------------------------------------------------
/include/at128_jpc/at128_jpc.h:
--------------------------------------------------------------------------------
 1 | #include "at128_jpc/make_pointxyzitr.h"
 2 | #ifndef AT128_JPC_H
 3 | #define AT128_JPC_H
 4 | 
 5 | #define PI 3.1415926
 6 | #define RAD 57.295780
 7 | 
 8 | typedef pcl::PointCloud<pcl::PointXYZITR> PointType;
 9 | typedef pcl::PointXYZITR Point;
10 | 
11 | extern int radial_index;
12 | const std::string pts_topic = "/hesai/pandar_points";
13 | const int horizon_pts_num = 1281, scan_num = 128, neighbor_num = 25, s = 5;
14 | const float th_g = 0.2, sigma = 10., delta_R = 2., th_d = 1., max_range = 70,
15 |             min_range = 2;
16 | const int radial_num = int((max_range - min_range) / delta_R);
17 | const Eigen::Vector3d lidar_pos{5.4, -0.2, 3.2};
18 | 
19 | struct Index {
20 |   int x = 0;
21 |   int y = 0;
22 | };
23 | 
24 | class ProjectionJpc {
25 | public:
26 |   ProjectionJpc();
27 |   ~ProjectionJpc() {}
28 |   void ptsCopy(const sensor_msgs::PointCloud2::ConstPtr &origin_pts);
29 |   void setParams();
30 |   void memAllocation();
31 |   void mapMake();
32 |   void RECM();
33 |   void JPC();
34 |   void ptsPub();
35 |   void runProject(const sensor_msgs::PointCloud2ConstPtr &pts_msg);
36 |   void pts_trans(PointType::Ptr untrans_pts);
37 | 
38 | private:
39 |   std_msgs::Header pts_header;
40 | 
41 |   Point nan_pt;
42 |   PointType::Ptr pts_in;
43 |   PointType::Ptr pts_full;
44 |   PointType::Ptr pts_ground;
45 |   PointType::Ptr pts_obstacle;
46 | 
47 |   ros::NodeHandle nh;
48 |   ros::Subscriber pts_sub;
49 |   ros::Publisher pub_pts_in;
50 |   ros::Publisher pub_pts_full;
51 |   ros::Publisher pub_pts_ground;
52 |   ros::Publisher pub_pts_obstacle;
53 | 
54 |   std::vector<float> region_minz;
55 |   std::vector<int> cloud_index;
56 |   std::vector<std::pair<int, int>> neighborIterator;
57 | 
58 |   cv::Mat range_image;
59 |   cv::Mat region_image;
60 | 
61 |   Eigen::AngleAxisd rollAngle;
62 |   Eigen::AngleAxisd pitchAngle;
63 |   Eigen::AngleAxisd yawAngle;
64 |   Eigen::Matrix3d rotation_matrix;
65 |   Eigen::Matrix4d trans_matrix;
66 | };
67 | #endif
68 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>at128_jpc</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The at128_jpc package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="cui@todo.todo">cui</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/at128_jpc</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>cv_bridge</build_depend>
53 |   <build_depend>image_transport</build_depend>
54 |   <build_depend>pcl_ros</build_depend>
55 |   <build_depend>roscpp</build_depend>
56 |   <build_depend>rospy</build_depend>
57 |   <build_depend>sensor_msgs</build_depend>
58 |   <build_depend>std_msgs</build_depend>
59 |   <build_depend>libpcl-all-dev</build_depend>
60 |   <build_depend>pcl_conversions</build_depend>
61 |   <build_export_depend>cv_bridge</build_export_depend>
62 |   <build_export_depend>image_transport</build_export_depend>
63 |   <build_export_depend>pcl_ros</build_export_depend>
64 |   <build_export_depend>roscpp</build_export_depend>
65 |   <build_export_depend>rospy</build_export_depend>
66 |   <build_export_depend>sensor_msgs</build_export_depend>
67 |   <build_export_depend>std_msgs</build_export_depend>
68 |   <build_export_depend>pcl_conversions</build_export_depend>
69 |   <exec_depend>cv_bridge</exec_depend>
70 |   <exec_depend>image_transport</exec_depend>
71 |   <exec_depend>pcl_ros</exec_depend>
72 |   <exec_depend>roscpp</exec_depend>
73 |   <exec_depend>rospy</exec_depend>
74 |   <exec_depend>sensor_msgs</exec_depend>
75 |   <exec_depend>std_msgs</exec_depend>
76 |   <exec_depend>libpcl-all</exec_depend>
77 |   <exec_depend>pcl_conversions</exec_depend>
78 | 
79 |   <!-- The export tag contains other, unspecified, tags -->
80 |   <export>
81 |     <!-- Other tools can request additional information be placed here -->
82 | 
83 |   </export>
84 | </package>
85 | 


--------------------------------------------------------------------------------
/src/at128_jpc.cpp:
--------------------------------------------------------------------------------
  1 | #include "at128_jpc/at128_jpc.h"
  2 | using namespace std;
  3 | ProjectionJpc::ProjectionJpc() : nh("~") {
  4 |   pts_sub = nh.subscribe<sensor_msgs::PointCloud2>(
  5 |       pts_topic, 10, &ProjectionJpc::runProject, this);
  6 | 
  7 |   pub_pts_in = nh.advertise<sensor_msgs::PointCloud2>("/pts_in", 10);
  8 |   pub_pts_full = nh.advertise<sensor_msgs::PointCloud2>("/pts_full", 10);
  9 |   pub_pts_ground = nh.advertise<sensor_msgs::PointCloud2>("/pts_ground", 10);
 10 |   pub_pts_obstacle = nh.advertise<sensor_msgs::PointCloud2>("/pts_obstacle", 10);
 11 | 
 12 |   nan_pt.x = std::numeric_limits<float>::quiet_NaN();
 13 |   nan_pt.y = std::numeric_limits<float>::quiet_NaN();
 14 |   nan_pt.z = std::numeric_limits<float>::quiet_NaN();
 15 |   // nan_pt.intensity = -1;
 16 |   nan_pt.ring = 0;
 17 | 
 18 |   memAllocation();
 19 |   setParams();
 20 | }
 21 | 
 22 | void ProjectionJpc::setParams() {
 23 |   pts_full->clear();
 24 |   pts_ground->clear();
 25 |   pts_obstacle->clear();
 26 |   pts_in->clear();
 27 | 
 28 |   range_image = cv::Mat::zeros(scan_num, horizon_pts_num, CV_8UC3);
 29 |   region_image = cv::Mat::zeros(scan_num, horizon_pts_num, CV_8UC1);
 30 |   fill(pts_full->points.begin(), pts_full->points.end(), nan_pt);
 31 | }
 32 | 
 33 | void ProjectionJpc::memAllocation() {
 34 |   pts_in.reset(new PointType());
 35 |   pts_full.reset(new PointType());
 36 |   pts_ground.reset(new PointType());
 37 |   pts_obstacle.reset(new PointType());
 38 | 
 39 |   region_minz.assign(radial_num * horizon_pts_num, 100);
 40 |   cloud_index.assign(scan_num * horizon_pts_num, -1);
 41 | 
 42 |   neighborIterator.reserve(neighbor_num);
 43 |   for (int i = -2; i <= 2; ++i) {
 44 |     for (int j = -2; j <= 2; ++j) {
 45 |       neighborIterator.push_back(make_pair(i, j));
 46 |     }
 47 |   }
 48 | 
 49 |   neighborIterator.erase(neighborIterator.begin() + 12);
 50 |   pts_full->points.resize(scan_num * horizon_pts_num);
 51 | }
 52 | 
 53 | void ProjectionJpc::mapMake() {
 54 |   float range(0);
 55 |   int row_i(0), col_i(0), index(0), radial_index(0), region_index(0);
 56 |   for (const auto pt : pts_in->points) {
 57 |     //把容器中的点按照索引重新赋值，非空点会替换掉之前用来填充的NAN点。
 58 |     if (pt.ring != 0) {
 59 |       range = (float)sqrt(pt.x * pt.x + pt.y * pt.y);
 60 |       if (range < min_range || range > max_range) {
 61 |         continue;
 62 |       }
 63 |       row_i = 128 - pt.ring;
 64 |       col_i = ((128.1 / 2) - atan2(pt.y, pt.x) * RAD) / 0.1;
 65 |       if (row_i < 0 || row_i >= scan_num) {
 66 |         continue;
 67 |       }
 68 |       if (col_i < 0 || col_i >= horizon_pts_num) {
 69 |         continue;
 70 |       }
 71 |       //范围外的点和nan点对应的range_image为(0,0,0)
 72 |       range_image.at<cv::Vec3b>(row_i, col_i) = cv::Vec3b(0, 255, 0);
 73 |       index = col_i * scan_num + row_i;
 74 |       radial_index = (int)((range - min_range) / delta_R); //该点所在的环数
 75 |       region_index =
 76 |           col_i * radial_num + radial_index; // radial_num为每条轴线总环数
 77 |       //区域最小z值列表按照1281条线排序，每条线分为radial_num个线段区间,后续判断每条线上不同线段区间里的最小z值的时候，直接循环即可。
 78 |       region_minz[region_index] = min(region_minz[region_index], pt.z);
 79 |       region_image.at<uchar>(row_i, col_i) =
 80 |           radial_index;           // region_中存储该点所在环数
 81 |       cloud_index[index] = index; //范围外和nan点的cloud_index为-1
 82 |       pts_full->points[index] = pt;
 83 |     }
 84 |   }
 85 |   // cv::namedWindow("map",CV_WINDOW_NORMAL);
 86 |   // cv::imshow("map",range_image);
 87 |   // cv::waitKey(1);
 88 | }
 89 | 
 90 | void ProjectionJpc::RECM() {
 91 |   //遍历region_minz数组,将(m-1,n)和(m,n)联系起来
 92 |   for (int i = 0; i < region_minz.size(); ++i) {
 93 |     if (i % radial_num == 0) {
 94 |       continue;
 95 |     } else {
 96 |       region_minz[i] =
 97 |           min(region_minz[i],
 98 |               region_minz[i - 1] + (float)(delta_R * tan(sigma / RAD)));
 99 |     }
100 |   }
101 |   //遍历点，如果z值大于对应块内的minz,就判定为障碍点
102 |   for (int row = 0; row < scan_num; ++row) {
103 |     for (int col = 0; col < horizon_pts_num; ++col) {
104 |       int index_i = col * scan_num + row;
105 |       //忽略nan点和范围外的点
106 |       if (cloud_index[index_i] == -1) {
107 |         continue;
108 |       } else {
109 |         int region_i_index = region_image.at<uchar>(row, col);
110 |         float region_i_height = region_minz[col * radial_num + region_i_index];
111 |         if (pts_full->points[index_i].z >= (region_i_height + th_g)) {
112 |           range_image.at<cv::Vec3b>(row, col) = cv::Vec3b(0, 0, 255);
113 |         }
114 |       }
115 |     }
116 |   }
117 | }
118 | 
119 | void ProjectionJpc::JPC() {
120 |   vector<cv::Mat> channels;
121 |   cv::split(range_image, channels);
122 |   cv::Mat core = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
123 |   cv::dilate(channels[2], channels[2], core);
124 |   cv::merge(channels, range_image);
125 |   queue<Index> pts_need_rejudge;
126 |   for (int row = 0; row < scan_num; ++row) {
127 |     for (int col = 0; col < horizon_pts_num; ++col) {
128 |       if (range_image.at<cv::Vec3b>(row, col) == cv::Vec3b(0, 255, 255) &&
129 |           cloud_index[col * scan_num + row] != -1) {
130 |         Index pt_judge;
131 |         pt_judge.x = col;
132 |         pt_judge.y = row;
133 |         pts_need_rejudge.push(pt_judge);
134 |         range_image.at<cv::Vec3b>(row, col) = cv::Vec3b(255, 0, 0);
135 |       }
136 |     }
137 |   }
138 |   while (!pts_need_rejudge.empty()) {
139 |     Index pt_judge = pts_need_rejudge.front();
140 |     pts_need_rejudge.pop();
141 | 
142 |     Eigen::VectorXf D(24);
143 |     int pt_id = pt_judge.x * scan_num +
144 |                 pt_judge.y; // pt_judge.x = col、 pt_judge.y = row
145 |     int mask[24];
146 |     float sumD(0), diff(0);
147 |     Point p1, p2;
148 |     for (int i = 0; i < 24; ++i) {
149 |       int neighbor_xi = neighborIterator[i].first + pt_judge.x;  // col
150 |       int neighbor_yi = neighborIterator[i].second + pt_judge.y; // row
151 |       int neighbori_id = neighbor_xi * scan_num + neighbor_yi;
152 | 
153 |       if (neighbor_xi < 0 || neighbor_xi >= horizon_pts_num ||
154 |           neighbor_yi < 0 || neighbor_yi >= scan_num ||
155 |           cloud_index[neighbori_id] == -1) {
156 |         D(i) == 0;
157 |         sumD += D(i);
158 |         mask[i] = -1;
159 |         continue;
160 |       }
161 |       p1 = pts_full->points[pt_id];
162 |       p2 = pts_full->points[neighbori_id];
163 |       diff =
164 |           sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
165 |                (p1.z - p2.z) * (p1.z - p2.z));
166 |       if (diff > th_d) {
167 |         D(i) = 0;
168 |         sumD += D(i);
169 |       } else {
170 |         D(i) = exp(-s * diff);
171 |         sumD += D(i);
172 |       }
173 |       if (range_image.at<cv::Vec3b>(neighbor_yi, neighbor_xi) ==
174 |           cv::Vec3b(255, 0, 0)) // range内低置信度的点
175 |       {
176 |         mask[i] = 2;
177 |       } else if (range_image.at<cv::Vec3b>(neighbor_yi, neighbor_xi) ==
178 |                  cv::Vec3b(0, 255, 0)) //接地点
179 |       {
180 |         mask[i] = 1;
181 |       } else if (range_image.at<cv::Vec3b>(neighbor_yi, neighbor_xi) ==
182 |                  cv::Vec3b(0, 0, 255)) //障碍点和range之外的低置信度的点
183 |       {
184 |         mask[i] = 0;
185 |       }
186 |     }
187 |     Eigen::VectorXf W(24);
188 |     W = D / sumD;
189 |     float score_r(0), score_g(0);
190 |     for (int i = 0; i < D.size(); ++i) {
191 |       if (mask[i] == 0) {
192 |         score_r += W(i);
193 |       } else if (mask[i] == 1) {
194 |         score_g += W(i);
195 |       }
196 |     }
197 |     if (score_r > score_g) {
198 |       range_image.at<cv::Vec3b>(pt_judge.y, pt_judge.x) = cv::Vec3b(0, 0, 255);
199 |     } else {
200 |       range_image.at<cv::Vec3b>(pt_judge.y, pt_judge.x) = cv::Vec3b(0, 255, 0);
201 |     }
202 |   }
203 | }
204 | 
205 | void ProjectionJpc::ptsPub() {
206 |   for (int row = 0; row < scan_num; ++row) {
207 |     for (int col = 0; col < horizon_pts_num; ++col) {
208 |       int index = col * scan_num + row;
209 |       if (cloud_index[index] == -1) {
210 |         continue;
211 |       }
212 |       if (range_image.at<cv::Vec3b>(row, col) == cv::Vec3b(0, 255, 0)) {
213 |         pts_ground->push_back(pts_full->points[index]);
214 |       }
215 |       if (range_image.at<cv::Vec3b>(row, col) == cv::Vec3b(0, 0, 255)) {
216 |         pts_obstacle->push_back(pts_full->points[index]);
217 |       }
218 |     }
219 |   }
220 |   sensor_msgs::PointCloud2 ros_pts_in;
221 |   sensor_msgs::PointCloud2 ros_pts_full;
222 |   sensor_msgs::PointCloud2 ros_pts_ground;
223 |   sensor_msgs::PointCloud2 ros_pts_obstacle;
224 | 
225 |   // if (pub_pts_in.getNumSubscribers())
226 |   //{
227 |   pcl::toROSMsg(*pts_in, ros_pts_in);
228 |   ros_pts_in.header.stamp = pts_header.stamp;
229 |   ros_pts_in.header.frame_id = "map";
230 |   pub_pts_in.publish(ros_pts_in);
231 |   //}
232 | 
233 |   pcl::toROSMsg(*pts_full, ros_pts_full);
234 |   ros_pts_full.header.stamp = pts_header.stamp;
235 |   ros_pts_full.header.frame_id = "map";
236 |   pub_pts_full.publish(ros_pts_full);
237 | 
238 |   // if (pub_pts_ground.getNumSubscribers())
239 |   //{
240 |   pcl::toROSMsg(*pts_ground, ros_pts_ground);
241 |   ros_pts_ground.header.stamp = pts_header.stamp;
242 |   ros_pts_ground.header.frame_id = "map";
243 |   pub_pts_ground.publish(ros_pts_ground);
244 |   //}
245 | 
246 |   // if (pub_pts_obstacle.getNumSubscribers())
247 |   //{
248 |   pcl::toROSMsg(*pts_obstacle, ros_pts_obstacle);
249 |   ros_pts_obstacle.header.stamp = pts_header.stamp;
250 |   ros_pts_obstacle.header.frame_id = "map";
251 |   pub_pts_obstacle.publish(ros_pts_obstacle);
252 |   //}
253 | }
254 | 
255 | //点云格式转换
256 | void ProjectionJpc::ptsCopy(
257 |     const sensor_msgs::PointCloud2::ConstPtr &origin_pts) {
258 |   pts_header = origin_pts->header;
259 |   pcl::fromROSMsg(*origin_pts, *pts_in);
260 | }
261 | 
262 | //点云位姿转换
263 | void ProjectionJpc::pts_trans(PointType::Ptr untrans_pts) {
264 |   rollAngle = (Eigen::AngleAxisd(0.8 / RAD, Eigen::Vector3d::UnitX()));
265 |   pitchAngle = (Eigen::AngleAxisd(5.2 / RAD, Eigen::Vector3d::UnitY()));
266 |   yawAngle = (Eigen::AngleAxisd(0.2 / RAD, Eigen::Vector3d::UnitZ()));
267 |   rotation_matrix = yawAngle * pitchAngle * rollAngle;
268 |   trans_matrix.block<3, 1>(0, 3) = lidar_pos;
269 |   trans_matrix.block<3, 3>(0, 0) = rotation_matrix;
270 |   pcl::transformPointCloud(*untrans_pts, *untrans_pts, trans_matrix);
271 | }
272 | 
273 | void ProjectionJpc::runProject(
274 |     const sensor_msgs::PointCloud2ConstPtr &pts_msg) {
275 |   double timestart = ros::Time::now().toSec();
276 |   ptsCopy(pts_msg);
277 |   mapMake();
278 |   RECM();
279 |   JPC();
280 |   ptsPub();
281 |   setParams();
282 |   memAllocation();
283 |   double timeend = ros::Time::now().toSec();
284 | 
285 |   std::cout << "time = " << timeend - timestart << endl;
286 | }
287 | 
288 | int main(int argc, char **argv) {
289 |   ros::init(argc, argv, "at128_jpc");
290 |   ProjectionJpc AT128;
291 |   ros::spin();
292 |   return 0;
293 | }
294 | 


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