Fawkes API
Fawkes Development Version
|
Calibrate the yaw angle of the back laser using the front laser. More...
#include "yaw_calibration.h"
Public Member Functions | |
YawCalibration (LaserInterface *laser, LaserInterface *front_laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path) | |
Constructor. More... | |
virtual void | calibrate () |
The actual calibration. More... | |
![]() | |
LaserCalibration (LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path) | |
Constructor. More... | |
virtual | ~LaserCalibration () |
Destructor. More... | |
Protected Member Functions | |
float | get_current_cost (float *new_yaw) |
Get the cost of the current configuration. More... | |
float | get_new_yaw (float current_cost, float last_yaw) |
Compute the new yaw. More... | |
![]() | |
PointCloudPtr | laser_to_pointcloud (const LaserInterface &laser) |
Convert the laser data into a pointcloud. More... | |
void | transform_pointcloud (const std::string &target_frame, PointCloudPtr cloud) |
Transform the points in a pointcloud. More... | |
PointCloudPtr | filter_cloud_in_rear (PointCloudPtr input) |
Remove points in the rear of the robot. More... | |
float | get_mean_z (PointCloudPtr cloud) |
Compute the mean z value of all points in the given pointcloud. More... | |
PointCloudPtr | filter_left_cloud (PointCloudPtr input) |
Remove all points that are left of the robot. More... | |
PointCloudPtr | filter_right_cloud (PointCloudPtr input) |
Remove all points that are right of the robot. More... | |
PointCloudPtr | filter_out_ground (PointCloudPtr input) |
Remove all points that belong to the ground. More... | |
float | get_matching_cost (PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw) |
Compare two pointclouds with ICP. More... | |
PointCloudPtr | filter_center_cloud (PointCloudPtr input) |
Remove the center of a pointcloud This removes all points around the origin of the pointcloud. More... | |
Protected Attributes | |
LaserInterface * | front_laser_ |
The laser interface used to read the front laser data from. More... | |
const float | init_step_ = 0.02 |
The initial step size. More... | |
float | step_ |
The current step size. More... | |
std::mt19937 | random_generator_ |
Random number generator used to compute the random reset probability. More... | |
std::uniform_real_distribution< float > | random_float_dist_ |
The distribution used to compute the random reset probability. More... | |
std::map< float, float > | costs_ |
A map of yaw config values to costs. More... | |
float | min_cost_ |
The minimal cost. More... | |
float | min_cost_yaw_ |
A yaw configuration with the minimal cost. More... | |
![]() | |
LaserInterface * | laser_ |
The laser that provides the input data. More... | |
fawkes::tf::Transformer * | tf_transformer_ |
The transformer used to compute transforms. More... | |
fawkes::NetworkConfiguration * | config_ |
The network config to use for reading and updating config values. More... | |
const std::string | config_path_ |
The config path to use for reading and updating config values. More... | |
Additional Inherited Members | |
![]() | |
static const long | sleep_time_ = 50000 |
Time in micro seconds to sleep between iterations. More... | |
static const uint | max_iterations_ = 100 |
The number of iterations to run before aborting the calibration. More... | |
static const size_t | min_points = 10 |
The number of points required in a pointcloud to use it as input data. More... | |
Calibrate the yaw angle of the back laser using the front laser.
This is done by comparing data of both lasers left and right of the robot. The yaw angle of the back laser is adapted so the matching cost between both lasers is minimzed.
Definition at line 28 of file yaw_calibration.h.
YawCalibration::YawCalibration | ( | LaserInterface * | laser, |
LaserInterface * | front_laser, | ||
fawkes::tf::Transformer * | tf_transformer, | ||
fawkes::NetworkConfiguration * | config, | ||
std::string | config_path | ||
) |
Constructor.
laser | The interface of the back laser to get the data from |
front_laser | The interface of the front laser to get the data from |
tf_transformer | The transformer to use to compute transforms |
config | The network config to read from and write the time offset to |
config_path | The config path to read from and write the time offset to |
Definition at line 44 of file yaw_calibration.cpp.
|
virtual |
The actual calibration.
Continuously compare the data from both lasers and update the yaw config until the cost reaches the threshold.
Implements LaserCalibration.
Definition at line 63 of file yaw_calibration.cpp.
References LaserCalibration::config_, LaserCalibration::config_path_, get_current_cost(), fawkes::NetworkConfiguration::get_float(), LaserCalibration::max_iterations_, min_cost_, min_cost_yaw_, fawkes::NetworkConfiguration::set_float(), LaserCalibration::sleep_time_, and step_.
|
protected |
Get the cost of the current configuration.
new_yaw | A pointer to the yaw configuration to write updates to |
Definition at line 113 of file yaw_calibration.cpp.
References LaserCalibration::filter_center_cloud(), front_laser_, LaserCalibration::get_matching_cost(), LaserCalibration::laser_, LaserCalibration::laser_to_pointcloud(), fawkes::Interface::read(), and LaserCalibration::transform_pointcloud().
Referenced by calibrate().
|
protected |
Compute the new yaw.
The yaw is updated by taking steps into one direction until the cost increases. In that case, the step is size is decreased and negated. Also randomly reset the step size to avoid local minima.
current_cost | The current matching cost between both lasers |
last_yaw | The last yaw configuration |
Definition at line 135 of file yaw_calibration.cpp.
References costs_, init_step_, random_float_dist_, random_generator_, and step_.
|
protected |
A map of yaw config values to costs.
Definition at line 54 of file yaw_calibration.h.
Referenced by get_new_yaw().
|
protected |
The laser interface used to read the front laser data from.
Definition at line 44 of file yaw_calibration.h.
Referenced by get_current_cost().
|
protected |
The initial step size.
Definition at line 46 of file yaw_calibration.h.
Referenced by get_new_yaw().
|
protected |
|
protected |
A yaw configuration with the minimal cost.
Definition at line 58 of file yaw_calibration.h.
Referenced by calibrate().
|
protected |
The distribution used to compute the random reset probability.
Definition at line 52 of file yaw_calibration.h.
Referenced by get_new_yaw().
|
protected |
Random number generator used to compute the random reset probability.
Definition at line 50 of file yaw_calibration.h.
Referenced by get_new_yaw().
|
protected |
The current step size.
Definition at line 48 of file yaw_calibration.h.
Referenced by calibrate(), and get_new_yaw().