Fawkes API Fawkes Development Version
roll_calibration.cpp
1/***************************************************************************
2 * roll_calibration.cpp - Calibrate roll transform of the back laser
3 *
4 * Created: Tue 18 Jul 2017 16:28:09 CEST 16:28
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 "roll_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/transform_listener.h>
29#include <tf/transformer.h>
30
31using namespace fawkes;
32using namespace std;
33
34/** @class RollCalibration "roll_calibration.h"
35 * Calibrate the roll angle of a laser.
36 * This is done by splitting the pointcloud in the rear of the robot into a
37 * left and a right cloud, and comparing the mean z of both clouds.
38 * @author Till Hofmann
39 */
40
41/** Constructor.
42 * @param laser The laser to get the data from
43 * @param tf_transformer The transformer to use to compute transforms
44 * @param config The network config to read from and write the time offset to
45 * @param config_path The config path to read from and write the time offset to
46 */
48 tf::Transformer * tf_transformer,
50 string config_path)
51: LaserCalibration(laser, tf_transformer, config, config_path)
52{
53}
54
55/** The actual calibration.
56 * Iteratively run the calibration until a good roll angle has been found.
57 * The new value is written to the config in each iteration.
58 */
59void
61{
62 printf("Starting to calibrate roll angle.\n");
63 float lrd = 2 * threshold;
64 uint iterations = 0;
65 do {
66 try {
67 lrd = get_lr_mean_diff();
68 } catch (InsufficientDataException &e) {
69 printf("Insufficient data: %s\n", e.what_no_backtrace());
70 usleep(sleep_time_);
71 continue;
72 }
73 printf("Left-right difference is %f.\n", lrd);
74 float old_roll = config_->get_float(config_path_.c_str());
75 float new_roll = get_new_roll(lrd, old_roll);
76 printf("Updating roll from %f to %f.\n", old_roll, new_roll);
77 config_->set_float(config_path_.c_str(), new_roll);
78 usleep(sleep_time_);
79 } while (abs(lrd) > threshold && ++iterations < max_iterations_);
80 printf("Roll calibration finished.\n");
81}
82
83/** Get the difference of the mean of z of the left and right pointclouds.
84 * @return The mean differenze, >0 if the left cloud is higher than the right
85 */
86float
88{
89 laser_->read();
90 PointCloudPtr cloud = laser_to_pointcloud(*laser_);
91 PointCloudPtr calib_cloud = filter_calibration_cloud(cloud);
92 transform_pointcloud("base_link", cloud);
93 PointCloudPtr rear_cloud = filter_cloud_in_rear(cloud);
94 PointCloudPtr left_cloud = filter_left_cloud(rear_cloud);
95 PointCloudPtr right_cloud = filter_right_cloud(rear_cloud);
96 if (left_cloud->size() < min_points) {
97 stringstream error;
98 error << "Not enough laser points on the left, got " << left_cloud->size() << ", need "
99 << min_points;
100 throw InsufficientDataException(error.str().c_str());
101 }
102 if (right_cloud->size() < min_points) {
103 stringstream error;
104 error << "Not enough laser points on the right, got " << right_cloud->size() << ", need "
105 << min_points;
106 throw InsufficientDataException(error.str().c_str());
107 }
108 printf("Using %zu points on the left, %zu points on the right\n",
109 left_cloud->size(),
110 right_cloud->size());
111 return get_mean_z(left_cloud) - get_mean_z(right_cloud);
112}
113
114/** Compute a new roll angle based on the mean error and the old roll.
115 * @param mean_error The mean difference between the left and right cloud
116 * @param old_roll The roll angle used to get the data
117 * @return A new roll angle
118 */
119float
120RollCalibration::get_new_roll(float mean_error, float old_roll)
121{
122 return old_roll + 0.5 * mean_error;
123}
124
125/** Filter the input cloud to be useful for roll calibration.
126 * @param input The pointcloud to filter
127 * @return The same as the input but without NaN points
128 */
129PointCloudPtr
131{
132 PointCloudPtr filtered(new PointCloud());
133 std::vector<int> indices;
134 input->is_dense = false;
135 pcl::removeNaNFromPointCloud(*input, *filtered, indices);
136 return filtered;
137}
Exception that is thrown if there are not enough laser points to do a matching.
Abstract base class for laser calibration.
LaserInterface * laser_
The laser that provides the input data.
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.
const std::string config_path_
The config path to use for reading and updating config values.
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.
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
static const long sleep_time_
Time in micro seconds to sleep between iterations.
static constexpr float threshold
The threshold of the left-right difference to stop calibration.
RollCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
float get_new_roll(float mean_error, float old_roll)
Compute a new roll angle based on the mean error and the old roll.
virtual void calibrate()
The actual calibration.
PointCloudPtr filter_calibration_cloud(PointCloudPtr input)
Filter the input cloud to be useful for roll calibration.
float get_lr_mean_diff()
Get the difference of the mean of z of the left and right pointclouds.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Definition: netconf.h:50
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
Definition: netconf.cpp:731
virtual float get_float(const char *path)
Get value from configuration which is of type float.
Definition: netconf.cpp:247
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.