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
32using namespace fawkes;
33using 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,
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 */
71PointCloudPtr
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 */
93void
94LaserCalibration::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 */
112PointCloudPtr
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 */
131float
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 */
152PointCloudPtr
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 */
168PointCloudPtr
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 */
185PointCloudPtr
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 */
205float
206LaserCalibration::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.setInputSource(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 */
235PointCloudPtr
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}
Exception that is thrown if there are not enough laser points to do a matching.
virtual ~LaserCalibration()
Destructor.
fawkes::tf::Transformer * tf_transformer_
The transformer used to compute transforms.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
LaserCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
PointCloudPtr filter_out_ground(PointCloudPtr input)
Remove all points that belong to the ground.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
char * frame() const
Get frame value.
size_t maxlenof_distances() const
Get maximum length of distances value.
Remote configuration via Fawkes net.
Definition: netconf.h:50
A class for handling time.
Definition: time.h:93
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
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.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36