Fawkes API  Fawkes Development Version
LaserCalibration Class Referenceabstract

Abstract base class for laser calibration. More...

#include "laser_calibration.h"

Inheritance diagram for LaserCalibration:

Public Member Functions

 LaserCalibration (LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
 Constructor. More...
 
virtual ~LaserCalibration ()
 Destructor. More...
 
virtual void calibrate ()=0
 The actual calibration procedure. More...
 

Protected Member Functions

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

LaserInterfacelaser_
 The laser that provides the input data. More...
 
fawkes::tf::Transformertf_transformer_
 The transformer used to compute transforms. More...
 
fawkes::NetworkConfigurationconfig_
 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...
 

Static Protected Attributes

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...
 

Detailed Description

Abstract base class for laser calibration.

The class provides functions that are common for all calibration methods.

Author
Till Hofmann

Definition at line 66 of file laser_calibration.h.

Constructor & Destructor Documentation

◆ LaserCalibration()

LaserCalibration::LaserCalibration ( LaserInterface laser,
fawkes::tf::Transformer tf_transformer,
fawkes::NetworkConfiguration config,
std::string  config_path 
)

Constructor.

Parameters
laserThe laser interface to fetch data from
tf_transformerThe transformer to use to compute transforms
configThe network config to read from and write updates to
config_pathThe config path to read from and write updates to

Definition at line 52 of file laser_calibration.cpp.

◆ ~LaserCalibration()

LaserCalibration::~LaserCalibration ( )
virtual

Destructor.

Definition at line 61 of file laser_calibration.cpp.

Member Function Documentation

◆ calibrate()

LaserCalibration::calibrate ( )
pure virtual

The actual calibration procedure.

Virtual function that is called once to calibrate the laser.

Implemented in YawCalibration, TimeOffsetCalibration, RollCalibration, and PitchCalibration.

◆ filter_center_cloud()

PointCloudPtr LaserCalibration::filter_center_cloud ( PointCloudPtr  input)
protected

Remove the center of a pointcloud This removes all points around the origin of the pointcloud.

Use this to remove the robot from the data.

Parameters
inputThe pointcloud to filter
Returns
The same cloud as the input but without any points around the center

Definition at line 236 of file laser_calibration.cpp.

Referenced by YawCalibration::get_current_cost().

◆ filter_cloud_in_rear()

PointCloudPtr LaserCalibration::filter_cloud_in_rear ( PointCloudPtr  input)
protected

Remove points in the rear of the robot.

Parameters
inputThe pointcloud to remove the points from.
Returns
The same pointcloud but without any points in the rear.

Definition at line 113 of file laser_calibration.cpp.

Referenced by PitchCalibration::calibrate(), and RollCalibration::get_lr_mean_diff().

◆ filter_left_cloud()

PointCloudPtr LaserCalibration::filter_left_cloud ( PointCloudPtr  input)
protected

Remove all points that are left of the robot.

Parameters
inputThe cloud to remove the points from
Returns
The same cloud as the input but without any points left of the robot

Definition at line 153 of file laser_calibration.cpp.

Referenced by RollCalibration::get_lr_mean_diff().

◆ filter_out_ground()

PointCloudPtr LaserCalibration::filter_out_ground ( PointCloudPtr  input)
protected

Remove all points that belong to the ground.

Points that have a height < 0.1 are assumed to be part of the ground.

Parameters
inputThe pointcloud to remove the points form
Returns
The same cloud as the input but without any points on the ground

Definition at line 186 of file laser_calibration.cpp.

◆ filter_right_cloud()

PointCloudPtr LaserCalibration::filter_right_cloud ( PointCloudPtr  input)
protected

Remove all points that are right of the robot.

Parameters
inputThe cloud to remove the points from
Returns
The same cloud as the input but without any points right of the robot

Definition at line 169 of file laser_calibration.cpp.

Referenced by RollCalibration::get_lr_mean_diff().

◆ get_matching_cost()

float LaserCalibration::get_matching_cost ( PointCloudPtr  cloud1,
PointCloudPtr  cloud2,
float *  rot_yaw 
)
protected

Compare two pointclouds with ICP.

Compute the best angle to rotate cloud2 into cloud1 with ICP and get the cost.

Parameters
cloud1The first input cloud, used as target cloud in ICP
cloud2The second input cloud, this is used as ICP input
rot_yawA pointer to a float to write the resulting rotation to
Returns
The ICP fitness score as matching cost

Definition at line 206 of file laser_calibration.cpp.

References min_points.

Referenced by TimeOffsetCalibration::calibrate(), and YawCalibration::get_current_cost().

◆ get_mean_z()

float LaserCalibration::get_mean_z ( PointCloudPtr  cloud)
protected

Compute the mean z value of all points in the given pointcloud.

This can be used to compute the height of a line, e.g., a line that should be on the ground. The value can be used to tweak the roll or pitch of the lasers.

Parameters
cloudThe cloud that is used to compute the mean z
Returns
The mean z of all points in the input cloud

Definition at line 132 of file laser_calibration.cpp.

References min_points.

Referenced by PitchCalibration::calibrate(), and RollCalibration::get_lr_mean_diff().

◆ laser_to_pointcloud()

PointCloudPtr LaserCalibration::laser_to_pointcloud ( const LaserInterface laser)
protected

Convert the laser data into a pointcloud.

The frame of the pointcloud is set to the frame of the laser, no transform is applied.

Parameters
laserThe laser interface to read the data from
Returns
A pointer to a pointcloud that contains the laser data

Definition at line 72 of file laser_calibration.cpp.

References fawkes::deg2rad(), fawkes::Laser360Interface::distances(), fawkes::Laser360Interface::frame(), and fawkes::Laser360Interface::maxlenof_distances().

Referenced by PitchCalibration::calibrate(), YawCalibration::get_current_cost(), TimeOffsetCalibration::get_lasercloud(), and RollCalibration::get_lr_mean_diff().

◆ transform_pointcloud()

void LaserCalibration::transform_pointcloud ( const std::string &  target_frame,
PointCloudPtr  cloud 
)
protected

Transform the points in a pointcloud.

The pointcloud is transformed in-place, i.e., the referenced input pointcloud is updated to be in the target frame.

Parameters
target_frameThe frame all points should be transformed into
cloudA reference to the pointcloud to transform

Definition at line 94 of file laser_calibration.cpp.

References tf_transformer_, and fawkes::tf::Transformer::transform_point().

Referenced by PitchCalibration::calibrate(), YawCalibration::get_current_cost(), TimeOffsetCalibration::get_lasercloud(), and RollCalibration::get_lr_mean_diff().

Member Data Documentation

◆ config_

fawkes::NetworkConfiguration* LaserCalibration::config_
protected

The network config to use for reading and updating config values.

Definition at line 93 of file laser_calibration.h.

Referenced by PitchCalibration::calibrate(), RollCalibration::calibrate(), TimeOffsetCalibration::calibrate(), and YawCalibration::calibrate().

◆ config_path_

const std::string LaserCalibration::config_path_
protected

The config path to use for reading and updating config values.

Definition at line 95 of file laser_calibration.h.

Referenced by PitchCalibration::calibrate(), RollCalibration::calibrate(), TimeOffsetCalibration::calibrate(), and YawCalibration::calibrate().

◆ laser_

LaserInterface* LaserCalibration::laser_
protected

◆ max_iterations_

const uint LaserCalibration::max_iterations_ = 100
staticprotected

The number of iterations to run before aborting the calibration.

Definition at line 99 of file laser_calibration.h.

Referenced by RollCalibration::calibrate(), and YawCalibration::calibrate().

◆ min_points

const size_t LaserCalibration::min_points = 10
staticprotected

The number of points required in a pointcloud to use it as input data.

Definition at line 101 of file laser_calibration.h.

Referenced by RollCalibration::get_lr_mean_diff(), get_matching_cost(), and get_mean_z().

◆ sleep_time_

const long LaserCalibration::sleep_time_ = 50000
staticprotected

Time in micro seconds to sleep between iterations.

Definition at line 97 of file laser_calibration.h.

Referenced by PitchCalibration::calibrate(), RollCalibration::calibrate(), and YawCalibration::calibrate().

◆ tf_transformer_

fawkes::tf::Transformer* LaserCalibration::tf_transformer_
protected

The transformer used to compute transforms.

Definition at line 91 of file laser_calibration.h.

Referenced by transform_pointcloud().


The documentation for this class was generated from the following files: