Fawkes API
Fawkes Development Version
|
#include "roll_calibration.h"
Public Member Functions | |
RollCalibration (LaserInterface *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_lr_mean_diff () |
Get the difference of the mean of z of the left and right pointclouds. More... | |
float | get_new_roll (float mean_error, float old_roll) |
Compute a new roll angle based on the mean error and the old roll. More... | |
PointCloudPtr | filter_calibration_cloud (PointCloudPtr input) |
Filter the input cloud to be useful for roll calibration. 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... | |
Static Protected Attributes | |
constexpr static float | threshold = 0.00001 |
The threshold of the left-right difference to stop calibration. More... | |
![]() | |
const static long | sleep_time_ = 50000 |
Time in micro seconds to sleep between iterations. More... | |
const static uint | max_iterations_ = 100 |
The number of iterations to run before aborting the calibration. More... | |
const static size_t | min_points = 10 |
The number of points required in a pointcloud to use it as input data. More... | |
Additional Inherited Members | |
![]() | |
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... | |
Calibrate the roll angle of a laser. This is done by splitting the pointcloud in the rear of the robot into a left and a right cloud, and comparing the mean z of both clouds.
Definition at line 25 of file roll_calibration.h.
RollCalibration::RollCalibration | ( | LaserInterface * | laser, |
fawkes::tf::Transformer * | tf_transformer, | ||
fawkes::NetworkConfiguration * | config, | ||
std::string | config_path | ||
) |
Constructor.
laser | The 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 46 of file roll_calibration.cpp.
|
virtual |
The actual calibration.
Iteratively run the calibration until a good roll angle has been found. The new value is written to the config in each iteration.
Implements LaserCalibration.
Definition at line 59 of file roll_calibration.cpp.
References LaserCalibration::config_, LaserCalibration::config_path_, fawkes::NetworkConfiguration::get_float(), get_lr_mean_diff(), get_new_roll(), LaserCalibration::max_iterations_, fawkes::NetworkConfiguration::set_float(), LaserCalibration::sleep_time_, threshold, and fawkes::Exception::what_no_backtrace().
|
protected |
Filter the input cloud to be useful for roll calibration.
input | The pointcloud to filter |
Definition at line 129 of file roll_calibration.cpp.
Referenced by get_lr_mean_diff().
|
protected |
Get the difference of the mean of z of the left and right pointclouds.
Definition at line 86 of file roll_calibration.cpp.
References filter_calibration_cloud(), LaserCalibration::filter_cloud_in_rear(), LaserCalibration::filter_left_cloud(), LaserCalibration::filter_right_cloud(), LaserCalibration::get_mean_z(), LaserCalibration::laser_, LaserCalibration::laser_to_pointcloud(), LaserCalibration::min_points, fawkes::Interface::read(), and LaserCalibration::transform_pointcloud().
Referenced by calibrate().
|
protected |
Compute a new roll angle based on the mean error and the old roll.
mean_error | The mean difference between the left and right cloud |
old_roll | The roll angle used to get the data |
Definition at line 119 of file roll_calibration.cpp.
Referenced by calibrate().
|
staticconstexprprotected |
The threshold of the left-right difference to stop calibration.
Definition at line 47 of file roll_calibration.h.
Referenced by calibrate().