Fawkes API Fawkes Development Version
yaw_calibration.cpp
1/***************************************************************************
2 * yaw_calibration.cpp - Calibrate yaw transform of the back laser
3 *
4 * Created: Tue 18 Jul 2017 17:05:30 CEST 17:05
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 "yaw_calibration.h"
22
23#include <config/netconf.h>
24#include <tf/transformer.h>
25
26using namespace fawkes;
27using namespace std;
28
29/** @class YawCalibration "yaw_calibration.h"
30 * Calibrate the yaw angle of the back laser using the front laser.
31 * This is done by comparing data of both lasers left and right of the robot.
32 * The yaw angle of the back laser is adapted so the matching cost between both
33 * lasers is minimzed.
34 * @author Till Hofmann
35 */
36
37/** Constructor.
38 * @param laser The interface of the back laser to get the data from
39 * @param front_laser The interface of the front laser to get the data from
40 * @param tf_transformer The transformer to use to compute transforms
41 * @param config The network config to read from and write the time offset to
42 * @param config_path The config path to read from and write the time offset to
43 */
45 LaserInterface * front_laser,
46 tf::Transformer * tf_transformer,
48 string config_path)
49: LaserCalibration(laser, tf_transformer, config, config_path),
50 front_laser_(front_laser),
51 step_(init_step_),
52 random_float_dist_(0, 1),
53 min_cost_(numeric_limits<float>::max()),
54 min_cost_yaw_(0.)
55{
56}
57
58/** The actual calibration.
59 * Continuously compare the data from both lasers and update the yaw config
60 * until the cost reaches the threshold.
61 */
62void
64{
65 printf("Starting to calibrate yaw angle.\n");
66 float current_cost;
67 while (true) {
68 try {
69 current_cost = get_current_cost(NULL);
70 break;
71 } catch (InsufficientDataException &e) {
72 printf("Insufficient data, please move the robot\n");
73 }
74 }
75 uint iterations = 0;
76 float last_yaw = config_->get_float(config_path_.c_str());
77 min_cost_ = current_cost;
78 min_cost_yaw_ = last_yaw;
79 while (abs(step_) > 0.0005 && iterations++ < max_iterations_) {
80 float next_yaw;
81 try {
82 current_cost = get_current_cost(&step_);
83 next_yaw = last_yaw + step_;
84 if (current_cost < min_cost_) {
85 min_cost_ = current_cost;
86 min_cost_yaw_ = last_yaw;
87 }
88 } catch (InsufficientDataException &e) {
89 printf("Insufficient data, skipping loop.\n");
90 continue;
91 }
92 printf("Updating yaw from %f to %f (step %f), last cost %f\n",
93 last_yaw,
94 next_yaw,
95 step_,
96 current_cost);
97 config_->set_float(config_path_.c_str(), next_yaw);
98 last_yaw = next_yaw;
99 usleep(sleep_time_);
100 }
101 if (current_cost > min_cost_) {
102 printf("Setting yaw to %f with minimal cost %f\n", min_cost_yaw_, min_cost_);
104 }
105 printf("Yaw calibration finished.\n");
106}
107
108/** Get the cost of the current configuration.
109 * @param new_yaw A pointer to the yaw configuration to write updates to
110 * @return The current matching cost
111 */
112float
114{
116 laser_->read();
117 PointCloudPtr front_cloud = laser_to_pointcloud(*front_laser_);
118 PointCloudPtr back_cloud = laser_to_pointcloud(*laser_);
119 transform_pointcloud("base_link", front_cloud);
120 transform_pointcloud("base_link", back_cloud);
121 front_cloud = filter_center_cloud(front_cloud);
122 back_cloud = filter_center_cloud(back_cloud);
123 return get_matching_cost(front_cloud, back_cloud, new_yaw);
124}
125
126/** Compute the new yaw.
127 * The yaw is updated by taking steps into one direction until the cost
128 * increases. In that case, the step is size is decreased and negated.
129 * Also randomly reset the step size to avoid local minima.
130 * @param current_cost The current matching cost between both lasers
131 * @param last_yaw The last yaw configuration
132 * @return The new yaw configuration
133 */
134float
135YawCalibration::get_new_yaw(float current_cost, float last_yaw)
136{
137 static float last_cost = current_cost;
138 costs_[last_yaw] = current_cost;
139 float next_yaw = last_yaw + step_;
140 for (auto &cost_pair : costs_) {
141 if (cost_pair.second < current_cost && cost_pair.first != last_yaw) {
142 float jump_probability = static_cast<float>((current_cost - cost_pair.second)) / current_cost;
143 float rand_01 = random_float_dist_(random_generator_);
144 if (rand_01 > 1 - jump_probability) {
145 last_cost = current_cost;
148 } else {
149 step_ = -init_step_;
150 }
151 printf("Jumping to %f, cost %f -> %f (probability was %f)\n",
152 cost_pair.first,
153 current_cost,
154 cost_pair.second,
155 jump_probability);
156 return cost_pair.first;
157 }
158 }
159 }
160 if (current_cost > last_cost) {
161 step_ = -step_ / 2;
162 }
163 last_cost = current_cost;
164 return next_yaw;
165}
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.
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 laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
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.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
float get_new_yaw(float current_cost, float last_yaw)
Compute the new yaw.
std::map< float, float > costs_
A map of yaw config values to costs.
float min_cost_yaw_
A yaw configuration with the minimal cost.
float step_
The current step size.
std::mt19937 random_generator_
Random number generator used to compute the random reset probability.
const float init_step_
The initial step size.
std::uniform_real_distribution< float > random_float_dist_
The distribution used to compute the random reset probability.
float get_current_cost(float *new_yaw)
Get the cost of the current configuration.
LaserInterface * front_laser_
The laser interface used to read the front laser data from.
YawCalibration(LaserInterface *laser, LaserInterface *front_laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
float min_cost_
The minimal cost.
virtual void calibrate()
The actual calibration.
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.