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 
26 using namespace fawkes;
27 using 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,
47  NetworkConfiguration *config,
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  */
62 void
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  */
112 float
113 YawCalibration::get_current_cost(float *new_yaw)
114 {
115  front_laser_->read();
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  */
134 float
135 YawCalibration::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;
147  step_ = init_step_;
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 }
YawCalibration::min_cost_
float min_cost_
The minimal cost.
Definition: yaw_calibration.h:59
YawCalibration::step_
float step_
The current step size.
Definition: yaw_calibration.h:51
LaserCalibration::max_iterations_
const static uint max_iterations_
The number of iterations to run before aborting the calibration.
Definition: laser_calibration.h:98
YawCalibration::get_current_cost
float get_current_cost(float *new_yaw)
Get the cost of the current configuration.
Definition: yaw_calibration.cpp:112
LaserCalibration::laser_to_pointcloud
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
Definition: laser_calibration.cpp:72
LaserCalibration::laser_
LaserInterface * laser_
The laser that provides the input data.
Definition: laser_calibration.h:88
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:475
fawkes::tf::Transformer
Definition: transformer.h:71
LaserCalibration::config_path_
const std::string config_path_
The config path to use for reading and updating config values.
Definition: laser_calibration.h:94
YawCalibration::get_new_yaw
float get_new_yaw(float current_cost, float last_yaw)
Compute the new yaw.
Definition: yaw_calibration.cpp:134
LaserCalibration
Definition: laser_calibration.h:65
YawCalibration::YawCalibration
YawCalibration(LaserInterface *laser, LaserInterface *front_laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
Definition: yaw_calibration.cpp:43
LaserCalibration::transform_pointcloud
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
Definition: laser_calibration.cpp:94
LaserCalibration::config_
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
Definition: laser_calibration.h:92
YawCalibration::front_laser_
LaserInterface * front_laser_
The laser interface used to read the front laser data from.
Definition: yaw_calibration.h:47
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
YawCalibration::min_cost_yaw_
float min_cost_yaw_
A yaw configuration with the minimal cost.
Definition: yaw_calibration.h:61
fawkes::NetworkConfiguration
Definition: netconf.h:53
fawkes
LaserCalibration::sleep_time_
const static long sleep_time_
Time in micro seconds to sleep between iterations.
Definition: laser_calibration.h:96
fawkes::NetworkConfiguration::set_float
virtual void set_float(const char *path, float f)
Definition: netconf.cpp:730
YawCalibration::random_float_dist_
std::uniform_real_distribution< float > random_float_dist_
The distribution used to compute the random reset probability.
Definition: yaw_calibration.h:55
YawCalibration::random_generator_
std::mt19937 random_generator_
Random number generator used to compute the random reset probability.
Definition: yaw_calibration.h:53
fawkes::Laser360Interface
Definition: Laser360Interface.h:37
YawCalibration::costs_
std::map< float, float > costs_
A map of yaw config values to costs.
Definition: yaw_calibration.h:57
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
YawCalibration::init_step_
const float init_step_
The initial step size.
Definition: yaw_calibration.h:49
fawkes::NetworkConfiguration::get_float
virtual float get_float(const char *path)
Definition: netconf.cpp:246
YawCalibration::calibrate
virtual void calibrate()
The actual calibration.
Definition: yaw_calibration.cpp:62