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 
31 using namespace fawkes;
32 using 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,
49  NetworkConfiguration *config,
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  */
59 void
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  */
86 float
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  */
119 float
120 RollCalibration::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  */
129 PointCloudPtr
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 }
RollCalibration::calibrate
virtual void calibrate()
The actual calibration.
Definition: roll_calibration.cpp:60
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
RollCalibration::filter_calibration_cloud
PointCloudPtr filter_calibration_cloud(PointCloudPtr input)
Filter the input cloud to be useful for roll calibration.
Definition: roll_calibration.cpp:130
LaserCalibration::laser_
LaserInterface * laser_
The laser that provides the input data.
Definition: laser_calibration.h:89
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::max_iterations_
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
Definition: laser_calibration.h:99
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
LaserCalibration::min_points
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
Definition: laser_calibration.h:101
fawkes::tf::Transformer
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
LaserCalibration::config_path_
const std::string config_path_
The config path to use for reading and updating config values.
Definition: laser_calibration.h:95
LaserCalibration
Abstract base class for laser calibration.
Definition: laser_calibration.h:67
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
RollCalibration::get_lr_mean_diff
float get_lr_mean_diff()
Get the difference of the mean of z of the left and right pointclouds.
Definition: roll_calibration.cpp:87
LaserCalibration::config_
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
Definition: laser_calibration.h:93
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
RollCalibration::threshold
constexpr static float threshold
The threshold of the left-right difference to stop calibration.
Definition: roll_calibration.h:44
fawkes::NetworkConfiguration
Remote configuration via Fawkes net.
Definition: netconf.h:50
fawkes
Fawkes library namespace.
fawkes::NetworkConfiguration::set_float
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
Definition: netconf.cpp:731
pcl::PointCloud
Definition: pointcloud.h:32
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
LaserCalibration::sleep_time_
static const long sleep_time_
Time in micro seconds to sleep between iterations.
Definition: laser_calibration.h:97
RollCalibration::RollCalibration
RollCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
Definition: roll_calibration.cpp:47
RollCalibration::get_new_roll
float get_new_roll(float mean_error, float old_roll)
Compute a new roll angle based on the mean error and the old roll.
Definition: roll_calibration.cpp:120
fawkes::Laser360Interface
Laser360Interface Fawkes BlackBoard Interface.
Definition: Laser360Interface.h:34
InsufficientDataException
Exception that is thrown if there are not enough laser points to do a matching.
Definition: laser_calibration.h:56
fawkes::NetworkConfiguration::get_float
virtual float get_float(const char *path)
Get value from configuration which is of type float.
Definition: netconf.cpp:247