21 #include "laser_calibration.h"
23 #include <config/netconf.h>
24 #include <pcl/common/geometry.h>
25 #include <pcl/filters/filter.h>
26 #include <pcl/filters/passthrough.h>
27 #include <pcl/registration/icp.h>
28 #include <tf/transformer.h>
56 : laser_(laser), tf_transformer_(tf_transformer), config_(config), config_path_(config_path)
74 PointCloudPtr cloud = PointCloudPtr(
new PointCloud());
76 cloud->header.frame_id = laser.
frame();
79 float const *distances = laser.
distances();
96 for (
auto &point : cloud->points) {
99 cloud->header.frame_id);
102 point.x = static_cast<float>(point_in_base_frame[0]);
103 point.y = static_cast<float>(point_in_base_frame[1]);
104 point.z = static_cast<float>(point_in_base_frame[2]);
115 pcl::PassThrough<Point> pass;
116 pass.setInputCloud(input);
117 pass.setFilterFieldName(
"x");
118 pass.setFilterLimits(-2., -0.8);
120 pass.filter(*output);
136 error <<
"Not enough laser points in rear cloud, got " << cloud->size() <<
", need "
141 zs.resize(cloud->points.size());
142 for (
auto &point : *cloud) {
143 zs.push_back(point.z);
145 return accumulate(zs.begin(), zs.end(), 0.) / zs.size();
155 pcl::PassThrough<Point> pass;
156 pass.setInputCloud(input);
157 pass.setFilterFieldName(
"y");
158 pass.setFilterLimits(0., 2.);
160 pass.filter(*output);
171 pcl::PassThrough<Point> pass;
172 pass.setInputCloud(input);
173 pass.setFilterFieldName(
"y");
174 pass.setFilterLimits(-2., 0.);
176 pass.filter(*output);
188 pcl::PassThrough<Point> pass;
189 pass.setInputCloud(input);
190 pass.setFilterFieldName(
"z");
191 pass.setFilterLimits(0.1, 1);
193 pass.filter(*output);
210 error <<
"Not enough points, got " << cloud1->points.size() <<
" and " << cloud2->points.size()
214 pcl::IterativeClosestPoint<Point, Point> icp;
215 icp.setInputCloud(cloud2);
216 icp.setInputTarget(cloud1);
219 if (!icp.hasConverged()) {
223 pcl::Registration<Point, Point, float>::Matrix4 transformation = icp.getFinalTransformation();
224 *rot_yaw = atan2(transformation(1, 0), transformation(0, 0));
226 return icp.getFitnessScore();
238 pcl::PassThrough<Point> pass_x;
239 pass_x.setInputCloud(input);
240 pass_x.setFilterFieldName(
"x");
241 pass_x.setFilterLimits(-2, 2);
243 pass_x.filter(*x_filtered);
244 pcl::PassThrough<Point> pass_y;
246 pass_y.setInputCloud(x_filtered);
247 pass_y.setFilterFieldName(
"y");
248 pass_y.setFilterLimitsNegative(
true);
249 pass_y.setFilterLimits(-0.5, 0.5);
250 PointCloudPtr xy_filtered_inner(
new PointCloud());
251 pass_y.filter(*xy_filtered_inner);
252 pcl::PassThrough<Point> pass_y_outer;
253 pass_y_outer.setInputCloud(xy_filtered_inner);
254 pass_y_outer.setFilterFieldName(
"y");
255 pass_y_outer.setFilterLimits(-3, 3);
257 pass_y_outer.filter(*xy_filtered);
258 pcl::PassThrough<Point> pass_z;
259 pass_z.setInputCloud(xy_filtered);
260 pass_z.setFilterFieldName(
"z");
261 pass_z.setFilterLimits(0.1, 1);
263 pass_z.filter(*xyz_filtered);