Fawkes API  Fawkes Development Version
laser_calibration.cpp
1 /***************************************************************************
2  * laser_calibration.cpp - Tool to calibrate laser transforms
3  *
4  * Created: Mon 10 Jul 2017 17:37:21 CEST 17:37
5  * Copyright 2017-2018 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "laser_calibration.h"
22 
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>
29 
30 #include <cmath>
31 
32 using namespace fawkes;
33 using namespace std;
34 
35 /** @class LaserCalibration "laser_calibration.h"
36  * Abstract base class for laser calibration. The class provides functions that
37  * are common for all calibration methods.
38  * @author Till Hofmann
39  */
40 
41 /** @fn LaserCalibration::calibrate
42  * The actual calibration procedure.
43  * Virtual function that is called once to calibrate the laser.
44  */
45 
46 /** Constructor.
47  * @param laser The laser interface to fetch data from
48  * @param tf_transformer The transformer to use to compute transforms
49  * @param config The network config to read from and write updates to
50  * @param config_path The config path to read from and write updates to
51  */
53  tf::Transformer * tf_transformer,
54  NetworkConfiguration *config,
55  string config_path)
56 : laser_(laser), tf_transformer_(tf_transformer), config_(config), config_path_(config_path)
57 {
58 }
59 
60 /** Destructor. */
62 {
63 }
64 
65 /** Convert the laser data into a pointcloud.
66  * The frame of the pointcloud is set to the frame of the laser, no transform
67  * is applied.
68  * @param laser The laser interface to read the data from
69  * @return A pointer to a pointcloud that contains the laser data
70  */
71 PointCloudPtr
73 {
74  PointCloudPtr cloud = PointCloudPtr(new PointCloud());
75  cloud->points.resize(laser.maxlenof_distances());
76  cloud->header.frame_id = laser.frame();
77  cloud->height = 1;
78  cloud->width = laser.maxlenof_distances();
79  float const *distances = laser.distances();
80  for (uint i = 0; i < laser.maxlenof_distances(); i++) {
81  cloud->points[i].x = distances[i] * cosf(deg2rad(i) * (360. / laser.maxlenof_distances()));
82  cloud->points[i].y = distances[i] * sinf(deg2rad(i) * (360. / laser.maxlenof_distances()));
83  }
84  return cloud;
85 }
86 
87 /** Transform the points in a pointcloud.
88  * The pointcloud is transformed in-place, i.e., the referenced input
89  * pointcloud is updated to be in the target frame.
90  * @param target_frame The frame all points should be transformed into
91  * @param cloud A reference to the pointcloud to transform
92  */
93 void
94 LaserCalibration::transform_pointcloud(const string &target_frame, PointCloudPtr cloud)
95 {
96  for (auto &point : cloud->points) {
97  tf::Stamped<tf::Point> point_in_laser_frame(tf::Point(point.x, point.y, point.z),
98  fawkes::Time(0., cloud->header.stamp),
99  cloud->header.frame_id);
100  tf::Stamped<tf::Point> point_in_base_frame;
101  tf_transformer_->transform_point(target_frame, point_in_laser_frame, point_in_base_frame);
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]);
105  }
106 }
107 
108 /** Remove points in the rear of the robot.
109  * @param input The pointcloud to remove the points from.
110  * @return The same pointcloud but without any points in the rear.
111  */
112 PointCloudPtr
114 {
115  pcl::PassThrough<Point> pass;
116  pass.setInputCloud(input);
117  pass.setFilterFieldName("x");
118  pass.setFilterLimits(-2., -0.8);
119  PointCloudPtr output(new PointCloud());
120  pass.filter(*output);
121  return output;
122 }
123 
124 /** Compute the mean z value of all points in the given pointcloud.
125  * This can be used to compute the height of a line, e.g., a line that should
126  * be on the ground. The value can be used to tweak the roll or pitch of the
127  * lasers.
128  * @param cloud The cloud that is used to compute the mean z
129  * @return The mean z of all points in the input cloud
130  */
131 float
132 LaserCalibration::get_mean_z(PointCloudPtr cloud)
133 {
134  if (cloud->points.size() < min_points) {
135  stringstream error;
136  error << "Not enough laser points in rear cloud, got " << cloud->size() << ", need "
137  << min_points;
138  throw InsufficientDataException(error.str().c_str());
139  }
140  vector<float> zs;
141  zs.resize(cloud->points.size());
142  for (auto &point : *cloud) {
143  zs.push_back(point.z);
144  }
145  return accumulate(zs.begin(), zs.end(), 0.) / zs.size();
146 }
147 
148 /** Remove all points that are left of the robot.
149  * @param input The cloud to remove the points from
150  * @return The same cloud as the input but without any points left of the robot
151  */
152 PointCloudPtr
154 {
155  pcl::PassThrough<Point> pass;
156  pass.setInputCloud(input);
157  pass.setFilterFieldName("y");
158  pass.setFilterLimits(0., 2.);
159  PointCloudPtr output(new PointCloud());
160  pass.filter(*output);
161  return output;
162 }
163 
164 /** Remove all points that are right of the robot.
165  * @param input The cloud to remove the points from
166  * @return The same cloud as the input but without any points right of the robot
167  */
168 PointCloudPtr
170 {
171  pcl::PassThrough<Point> pass;
172  pass.setInputCloud(input);
173  pass.setFilterFieldName("y");
174  pass.setFilterLimits(-2., 0.);
175  PointCloudPtr output(new PointCloud());
176  pass.filter(*output);
177  return output;
178 }
179 
180 /** Remove all points that belong to the ground.
181  * Points that have a height < 0.1 are assumed to be part of the ground.
182  * @param input The pointcloud to remove the points form
183  * @return The same cloud as the input but without any points on the ground
184  */
185 PointCloudPtr
187 {
188  pcl::PassThrough<Point> pass;
189  pass.setInputCloud(input);
190  pass.setFilterFieldName("z");
191  pass.setFilterLimits(0.1, 1);
192  PointCloudPtr output(new PointCloud());
193  pass.filter(*output);
194  return output;
195 }
196 
197 /** Compare two pointclouds with ICP.
198  * Compute the best angle to rotate cloud2 into cloud1 with ICP and get the
199  * cost.
200  * @param cloud1 The first input cloud, used as target cloud in ICP
201  * @param cloud2 The second input cloud, this is used as ICP input
202  * @param rot_yaw A pointer to a float to write the resulting rotation to
203  * @return The ICP fitness score as matching cost
204  */
205 float
206 LaserCalibration::get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
207 {
208  if (cloud1->points.size() < min_points || cloud2->points.size() < min_points) {
209  stringstream error;
210  error << "Not enough points, got " << cloud1->points.size() << " and " << cloud2->points.size()
211  << " points, need " << min_points;
212  throw InsufficientDataException(error.str().c_str());
213  }
214  pcl::IterativeClosestPoint<Point, Point> icp;
215  icp.setInputCloud(cloud2);
216  icp.setInputTarget(cloud1);
217  PointCloud final;
218  icp.align(final);
219  if (!icp.hasConverged()) {
220  throw InsufficientDataException("ICP did not converge.");
221  }
222  if (rot_yaw) {
223  pcl::Registration<Point, Point, float>::Matrix4 transformation = icp.getFinalTransformation();
224  *rot_yaw = atan2(transformation(1, 0), transformation(0, 0));
225  }
226  return icp.getFitnessScore();
227 }
228 
229 /** Remove the center of a pointcloud
230  * This removes all points around the origin of the pointcloud. Use this to
231  * remove the robot from the data.
232  * @param input The pointcloud to filter
233  * @return The same cloud as the input but without any points around the center
234  */
235 PointCloudPtr
237 {
238  pcl::PassThrough<Point> pass_x;
239  pass_x.setInputCloud(input);
240  pass_x.setFilterFieldName("x");
241  pass_x.setFilterLimits(-2, 2);
242  PointCloudPtr x_filtered(new PointCloud());
243  pass_x.filter(*x_filtered);
244  pcl::PassThrough<Point> pass_y;
245  ;
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);
256  PointCloudPtr xy_filtered(new PointCloud());
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);
262  PointCloudPtr xyz_filtered(new PointCloud());
263  pass_z.filter(*xyz_filtered);
264  return xyz_filtered;
265 }
LaserCalibration::laser_to_pointcloud
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
Definition: laser_calibration.cpp:72
LaserCalibration::filter_left_cloud
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
Definition: laser_calibration.cpp:153
fawkes::Laser360Interface::frame
char * frame() const
Get frame value.
Definition: Laser360Interface.cpp:81
LaserCalibration::LaserCalibration
LaserCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
Definition: laser_calibration.cpp:52
LaserCalibration::filter_right_cloud
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
Definition: laser_calibration.cpp:169
LaserCalibration::tf_transformer_
fawkes::tf::Transformer * tf_transformer_
The transformer used to compute transforms.
Definition: laser_calibration.h:90
fawkes::tf::Transformer
Definition: transformer.h:71
LaserCalibration::filter_out_ground
PointCloudPtr filter_out_ground(PointCloudPtr input)
Remove all points that belong to the ground.
Definition: laser_calibration.cpp:186
fawkes::Laser360Interface::distances
float * distances() const
Get distances value.
Definition: Laser360Interface.cpp:117
fawkes::Laser360Interface::maxlenof_distances
size_t maxlenof_distances() const
Get maximum length of distances value.
Definition: Laser360Interface.cpp:144
LaserCalibration::filter_cloud_in_rear
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
Definition: laser_calibration.cpp:113
LaserCalibration::transform_pointcloud
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
Definition: laser_calibration.cpp:94
fawkes::tf::Stamped
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:136
LaserCalibration::filter_center_cloud
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
Definition: laser_calibration.cpp:236
LaserCalibration::get_mean_z
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
Definition: laser_calibration.cpp:132
fawkes::NetworkConfiguration
Definition: netconf.h:53
fawkes
pcl::PointCloud
Definition: pointcloud.h:36
LaserCalibration::~LaserCalibration
virtual ~LaserCalibration()
Destructor.
Definition: laser_calibration.cpp:61
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:40
fawkes::Time
Definition: time.h:96
LaserCalibration::min_points
const static size_t min_points
The number of points required in a pointcloud to use it as input data.
Definition: laser_calibration.h:100
fawkes::tf::Transformer::transform_point
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
Definition: transformer.cpp:417
fawkes::Laser360Interface
Definition: Laser360Interface.h:37
LaserCalibration::get_matching_cost
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
Definition: laser_calibration.cpp:206
InsufficientDataException
Exception that is thrown if there are not enough laser points to do a matching.
Definition: laser_calibration.h:54