Please note that active projects have migrated to https://github.com/fawkesrobotics.

laser calibration: add documentation
authorTill Hofmann <hofmann@kbsg.rwth-aachen.de>
Tue, 29 May 2018 16:37:28 +0000 (18:37 +0200)
committerTill Hofmann <hofmann@kbsg.rwth-aachen.de>
Tue, 29 May 2018 16:39:01 +0000 (18:39 +0200)
src/tools/laser_calibration/laser_calibration.cpp
src/tools/laser_calibration/laser_calibration.h
src/tools/laser_calibration/main.cpp
src/tools/laser_calibration/pitch_calibration.cpp
src/tools/laser_calibration/pitch_calibration.h
src/tools/laser_calibration/roll_calibration.cpp
src/tools/laser_calibration/roll_calibration.h
src/tools/laser_calibration/time_offset_calibration.cpp
src/tools/laser_calibration/time_offset_calibration.h
src/tools/laser_calibration/yaw_calibration.cpp
src/tools/laser_calibration/yaw_calibration.h

index bc5e193..c2bb1af 100644 (file)
@@ -2,7 +2,7 @@
  *  laser_calibration.cpp - Tool to calibrate laser transforms
  *
  *  Created: Mon 10 Jul 2017 17:37:21 CEST 17:37
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
 using namespace fawkes;
 using namespace std;
 
+/** @class LaserCalibration "laser_calibration.h"
+ *  Abstract base class for laser calibration. The class provides functions that
+ *  are common for all calibration methods.
+ *  @author Till Hofmann
+ */
+
+/** @fn LaserCalibration::calibrate
+ *  The actual calibration procedure.
+ *  Virtual function that is called once to calibrate the laser.
+ */
+
+/** Constructor.
+ *  @param laser The laser interface to fetch data from
+ *  @param tf_transformer The transformer to use to compute transforms
+ *  @param config The network config to read from and write updates to
+ *  @param config_path The config path to read from and write updates to
+ */
 LaserCalibration::LaserCalibration(LaserInterface *laser, tf::Transformer
     *tf_transformer, NetworkConfiguration *config, string config_path)
 : laser_(laser), tf_transformer_(tf_transformer), config_(config),
   config_path_(config_path) {}
+
+/** Destructor. */
 LaserCalibration::~LaserCalibration() {}
 
+/** Convert the laser data into a pointcloud.
+ *  The frame of the pointcloud is set to the frame of the laser, no transform
+ *  is applied.
+ *  @param laser The laser interface to read the data from
+ *  @return A pointer to a pointcloud that contains the laser data
+ */
 PointCloudPtr
 LaserCalibration::laser_to_pointcloud(const LaserInterface &laser) {
     PointCloudPtr cloud = PointCloudPtr(new PointCloud());
@@ -56,6 +81,12 @@ LaserCalibration::laser_to_pointcloud(const LaserInterface &laser) {
     return cloud;
   }
 
+/** 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.
+ *  @param target_frame The frame all points should be transformed into
+ *  @param cloud A reference to the pointcloud to transform
+ */
 void
 LaserCalibration::transform_pointcloud(
     const string &target_frame, PointCloudPtr cloud) {
@@ -72,6 +103,10 @@ LaserCalibration::transform_pointcloud(
   }
 }
 
+/** Remove points in the rear of the robot.
+ *  @param input The pointcloud to remove the points from.
+ *  @return The same pointcloud but without any points in the rear.
+ */
 PointCloudPtr
 LaserCalibration::filter_cloud_in_rear(PointCloudPtr input) {
   pcl::PassThrough<Point> pass;
@@ -83,6 +118,13 @@ LaserCalibration::filter_cloud_in_rear(PointCloudPtr input) {
   return output;
 }
 
+/** 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.
+ *  @param cloud The cloud that is used to compute the mean z
+ *  @return The mean z of all points in the input cloud
+ */
 float
 LaserCalibration::get_mean_z(PointCloudPtr cloud) {
   if (cloud->points.size() < min_points) {
@@ -99,6 +141,10 @@ LaserCalibration::get_mean_z(PointCloudPtr cloud) {
   return accumulate(zs.begin(), zs.end(), 0.) / zs.size();
 }
 
+/** Remove all points that are left of the robot.
+ *  @param input The cloud to remove the points from
+ *  @return The same cloud as the input but without any points left of the robot
+ */
 PointCloudPtr
 LaserCalibration::filter_left_cloud(PointCloudPtr input) {
   pcl::PassThrough<Point> pass;
@@ -110,6 +156,10 @@ LaserCalibration::filter_left_cloud(PointCloudPtr input) {
   return output;
 }
 
+/** Remove all points that are right of the robot.
+ *  @param input The cloud to remove the points from
+ *  @return The same cloud as the input but without any points right of the robot
+ */
 PointCloudPtr
 LaserCalibration::filter_right_cloud(PointCloudPtr input) {
   pcl::PassThrough<Point> pass;
@@ -121,6 +171,11 @@ LaserCalibration::filter_right_cloud(PointCloudPtr input) {
   return output;
 }
 
+/** Remove all points that belong to the ground.
+ *  Points that have a height < 0.1 are assumed to be part of the ground.
+ *  @param input The pointcloud to remove the points form
+ *  @return The same cloud as the input but without any points on the ground
+ */
 PointCloudPtr
 LaserCalibration::filter_out_ground(PointCloudPtr input) {
   pcl::PassThrough<Point> pass;
@@ -132,6 +187,14 @@ LaserCalibration::filter_out_ground(PointCloudPtr input) {
   return output;
 }
 
+/** Compare two pointclouds with ICP.
+ *  Compute the best angle to rotate cloud2 into cloud1 with ICP and get the
+ *  cost.
+ *  @param cloud1 The first input cloud, used as target cloud in ICP
+ *  @param cloud2 The second input cloud, this is used as ICP input
+ *  @param rot_yaw A pointer to a float to write the resulting rotation to
+ *  @return The ICP fitness score as matching cost
+ */
 float
 LaserCalibration::get_matching_cost(
     PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw) {
@@ -157,6 +220,12 @@ LaserCalibration::get_matching_cost(
   return icp.getFitnessScore();
 }
 
+/** 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.
+ *  @param input The pointcloud to filter
+ *  @return The same cloud as the input but without any points around the center
+ */
 PointCloudPtr
 LaserCalibration::filter_center_cloud(PointCloudPtr input) {
   pcl::PassThrough<Point> pass_x;
index 897587c..5251b0b 100644 (file)
@@ -2,7 +2,7 @@
  *  laser_calibration.h - Base class for laser transform calibration
  *
  *  Created: Mon 10 Jul 2017 17:37:01 CEST 17:37
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -50,9 +50,15 @@ deg2rad(float deg)
   return (deg * M_PI / 180.f);
 }
 
+/** Exception that is thrown if there are not enough laser points to
+ *  do a matching.
+ */
 class InsufficientDataException : public fawkes::Exception
 {
 public:
+  /** Constructor.
+   *  @param error: the error message
+   */
   InsufficientDataException(const char *error) : Exception(error) {}
 };
 
@@ -80,12 +86,19 @@ protected:
   PointCloudPtr filter_center_cloud(PointCloudPtr input);
 
 protected:
+  /** The laser that provides the input data */
   LaserInterface *laser_;
+  /** The transformer used to compute transforms */
   fawkes::tf::Transformer *tf_transformer_;
+  /** The network config to use for reading and updating config values */
   fawkes::NetworkConfiguration *config_;
+  /** The config path to use for reading and updating config values */
   const std::string config_path_;
+  /** Time in micro seconds to sleep between iterations */
   const static long sleep_time_ = 50000;
+  /** The number of iterations to run before aborting the calibration */
   const static uint max_iterations_ = 100;
+  /** The number of points required in a pointcloud to use it as input data */
   const static size_t min_points = 10;
 };
 
index a1e5956..13278e6 100644 (file)
@@ -2,7 +2,7 @@
  *  main.cpp - Laser calibration tool
  *
  *  Created: Tue 18 Jul 2017 15:47:58 CEST 15:47
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -39,6 +39,9 @@
 using namespace fawkes;
 using namespace std;
 
+/** Print the usage message.
+ * @param program_name The path of the program.
+ */
 void
 print_usage(const char *program_name)
 {
@@ -54,6 +57,13 @@ print_usage(const char *program_name)
       program_name);
 }
 
+/** Run all calibrations.
+ *  The command line options allow to enable/disable certain calibrations. By
+ *  default, calibrate everything.
+ *  @param argc Number of commandline arguments
+ *  @param argc The commandline arguments
+ *  @return 0 on success, -1 if an error occured.
+ */
 int
 main(int argc, char **argv)
 {
index c713967..a244472 100644 (file)
@@ -2,7 +2,7 @@
  *  pitch_calibration.cpp - Calibrate pitch transform of the back laser
  *
  *  Created: Tue 18 Jul 2017 16:51:36 CEST 16:51
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
 using namespace fawkes;
 using namespace std;
 
+/** @class PitchCalibration "pitch_calibration.h"
+ *  Calibrate the pitch angle of the laser.
+ *  Compute the angle by computing the mean z value of all points in the rear of
+ *  the robot and update the angle accordingly.
+ *  @author Till Hofmann
+ */
+
+/** Constructor.
+ *  @param laser The laser interface to fetch data from
+ *  @param tf_transformer The transformer to use to compute transforms
+ *  @param config The network config to read from and write updates to
+ *  @param config_path The config path to read from and write updates to
+ */
 PitchCalibration::PitchCalibration(LaserInterface *laser,
     tf::Transformer *tf_transformer,
     NetworkConfiguration *config, string config_path)
 : LaserCalibration(laser, tf_transformer, config, config_path) {}
 
+/** The actual calibration.
+ * Apply the method continuously until the mean z reaches the threshold.
+ * Write the upated pitch angle to the config in each iteration.
+ */
 void
 PitchCalibration::calibrate() {
   printf("Starting pitch angle calibration.\n");
@@ -57,6 +74,11 @@ PitchCalibration::calibrate() {
   printf("Pitch calibration finished.\n");
 }
 
+/** Compute the new pitch based on the old pitch and the mean z.
+ *  @param z The mean z value of all points in the rear of the robot.
+ *  @param old_pitch The pitch that was configured when recording the mean z.
+ *  @return The new pitch angle.
+ */
 float
 PitchCalibration::get_new_pitch(float z, float old_pitch) {
   // Note: We could also compute a more accurate new value using the measured
index 7b835ff..e35a136 100644 (file)
@@ -33,6 +33,7 @@ public:
 protected:
   float get_new_pitch(float z, float old_pitch);
 protected:
+  /** The threshold of the mean of z to stop calibration */
   constexpr static float threshold = 0.001;
 };
 
index 357f9d2..96645c6 100644 (file)
@@ -2,7 +2,7 @@
  *  roll_calibration.cpp - Calibrate roll transform of the back laser
  *
  *  Created: Tue 18 Jul 2017 16:28:09 CEST 16:28
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -33,10 +33,27 @@ using namespace fawkes;
 using namespace std;
 
 
+/** @class RollCalibration "roll_calibration.h"
+ *  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.
+ *  @author Till Hofmann
+ */
+
+/** Constructor.
+ *  @param laser The laser to get the data from
+ *  @param tf_transformer The transformer to use to compute transforms
+ *  @param config The network config to read from and write the time offset to
+ *  @param config_path The config path to read from and write the time offset to
+ */
 RollCalibration::RollCalibration(LaserInterface *laser, tf::Transformer
     *tf_transformer, NetworkConfiguration *config, string config_path)
   : LaserCalibration(laser, tf_transformer, config, config_path) {}
 
+/** 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.
+ */
 void
 RollCalibration::calibrate() {
   printf("Starting to calibrate roll angle.\n");
@@ -60,6 +77,9 @@ RollCalibration::calibrate() {
   printf("Roll calibration finished.\n");
 }
 
+/** Get the difference of the mean of z of the left and right pointclouds.
+ *  @return The mean differenze, >0 if the left cloud is higher than the right
+ */
 float
 RollCalibration::get_lr_mean_diff() {
   laser_->read();
@@ -86,11 +106,20 @@ RollCalibration::get_lr_mean_diff() {
   return get_mean_z(left_cloud) - get_mean_z(right_cloud);
 }
 
+/** Compute a new roll angle based on the mean error and the old roll.
+ *  @param mean_error The mean difference between the left and right cloud
+ *  @param old_roll The roll angle used to get the data
+ *  @return A new roll angle
+ */
 float
 RollCalibration::get_new_roll(float mean_error, float old_roll) {
   return old_roll + 0.5 * mean_error;
 }
 
+/** Filter the input cloud to be useful for roll calibration.
+ *  @param input The pointcloud to filter
+ *  @return The same as the input but without NaN points
+ */
 PointCloudPtr
 RollCalibration::filter_calibration_cloud(PointCloudPtr input) {
   PointCloudPtr filtered(new PointCloud());
index 9ea971a..e92bdcf 100644 (file)
@@ -39,6 +39,7 @@ protected:
 
 protected:
   // TODO: make threshold and min_points configurable
+  /** The threshold of the left-right difference to stop calibration */
   constexpr static float threshold = 0.00001;
 };
 
index 4fef1ac..8f66ba1 100644 (file)
@@ -2,7 +2,7 @@
  *  time_offset_calibration.cpp - Laser time offset calibration
  *
  *  Created: Tue 18 Jul 2017 17:40:16 CEST 17:40
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
 using namespace fawkes;
 using namespace std;
 
+/** @class TimeOffsetCalibration "time_offset_calibration.h"
+ *  Calibrate the time offset of a laser. This is done as follows:
+ *    1. Move the robot to a place with some recognizable object in the laser,
+ *       e.g., a corner
+ *    2. Start rotating the robot
+ *    3. Record a reference pointcloud
+ *    4. Stop rotating
+ *    5. Record a second pointcloud
+ *    6. Compare the two pointclouds and update the time offset based on the
+ *       angle between the two pointclouds.
+ *
+ *  @author Till Hofmann
+ */
+
+/** Constructor.
+ *  @param laser The laser to get the data from
+ *  @param motor The MotorInterface used to control the rotation of the robot
+ *  @param tf_transformer The transformer to use to compute transforms
+ *  @param config The network config to read from and write the time offset to
+ *  @param config_path The config path to read from and write the time offset to
+ */
 TimeOffsetCalibration::TimeOffsetCalibration(LaserInterface *laser,
     MotorInterface *motor, tf::Transformer *tf_transformer, NetworkConfiguration
     *config, string config_path)
@@ -43,6 +64,13 @@ TimeOffsetCalibration::TimeOffsetCalibration(LaserInterface *laser,
     step_(numeric_limits<float>::max())
   {}
 
+/** Calibrate the time offset.
+ *  Continuously execute the calibration procedure until the offset is small
+ *  enough. To improve convergence rate, in each iteration, jump to the minimum
+ *  with a certain probability based on the current cost and the minimal cost.
+ *  The time offset is written to the config in each iteration. At the end, the
+ *  time offset is always set to the offset with minimal cost.
+ */
 void TimeOffsetCalibration::calibrate() {
   float current_offset = config_->get_float(config_path_.c_str());
   map<float, float> costs;
@@ -114,6 +142,15 @@ void TimeOffsetCalibration::calibrate() {
   config_->set_float(config_path_.c_str(), min_offset);
 }
 
+/** Prepare the laser data for calibration.
+ *  Convert the laser data into a pointcloud and filter it so it only contains
+ *  data that is useful for calibration. In particular, restrict the data in x
+ *  and y directions to the interval [-3,3], remove any points close to the
+ *  robot in y direction, and limit the data in z direction to points above the
+ *  ground and < 1m.
+ *  @param laser The laser interface to read the unfiltered data from
+ *  @return A filtered pointcloud
+ */
 PointCloudPtr
 TimeOffsetCalibration::get_lasercloud(LaserInterface *laser) {
   laser->read();
index 30acada..69e2828 100644 (file)
@@ -36,11 +36,17 @@ protected:
   PointCloudPtr get_lasercloud(LaserInterface *laser);
 
 protected:
+  /** Time in micro seconds to sleep after each iteration */
   const static long sleep_time_ = 2000000;
+  /** The motor interface used to control the rotation of the robot */
   fawkes::MotorInterface *motor_;
+  /** The angular velocity to use to rotate */
   constexpr static float omega_ = 2.0;
+  /** The frequency for motor commands */
   const static unsigned int frequency_ = 100;
+  /** The time to rotate */
   constexpr static float rotation_time_ = 1.;
+  /** The current step size for the time offset */
   float step_;
 };
 
index 777b3b0..fbb0b43 100644 (file)
@@ -2,7 +2,7 @@
  *  yaw_calibration.cpp - Calibrate yaw transform of the back laser
  *
  *  Created: Tue 18 Jul 2017 17:05:30 CEST 17:05
- *  Copyright  2017  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
+ *  Copyright  2017-2018  Till Hofmann <hofmann@kbsg.rwth-aachen.de>
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
 using namespace fawkes;
 using namespace std;
 
+/** @class YawCalibration "yaw_calibration.h"
+ *  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.
+ *  @author Till Hofmann
+ */
+
+/** Constructor.
+ *  @param laser The interface of the back laser to get the data from
+ *  @param front_laser The interface of the front laser to get the data from
+ *  @param tf_transformer The transformer to use to compute transforms
+ *  @param config The network config to read from and write the time offset to
+ *  @param config_path The config path to read from and write the time offset to
+ */
 YawCalibration::YawCalibration(LaserInterface *laser, LaserInterface *front_laser,
       tf::Transformer *tf_transformer, NetworkConfiguration *config,
       string config_path)
@@ -34,6 +49,10 @@ YawCalibration::YawCalibration(LaserInterface *laser, LaserInterface *front_lase
     random_float_dist_(0,1),
     min_cost_(numeric_limits<float>::max()), min_cost_yaw_(0.) {}
 
+/** The actual calibration.
+ *  Continuously compare the data from both lasers and update the yaw config
+ *  until the cost reaches the threshold.
+ */
 void
 YawCalibration::calibrate() {
   printf("Starting to calibrate yaw angle.\n");
@@ -77,6 +96,10 @@ YawCalibration::calibrate() {
   printf("Yaw calibration finished.\n");
 }
 
+/** Get the cost of the current configuration.
+ *  @param new_yaw A pointer to the yaw configuration to write updates to
+ *  @return The current matching cost
+ */
 float
 YawCalibration::get_current_cost(float *new_yaw) {
   front_laser_->read();
@@ -90,6 +113,14 @@ YawCalibration::get_current_cost(float *new_yaw) {
   return get_matching_cost(front_cloud, back_cloud, new_yaw);
 }
 
+/** 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.
+ *  @param current_cost The current matching cost between both lasers
+ *  @param last_yaw The last yaw configuration
+ *  @return The new yaw configuration
+ */
 float
 YawCalibration::get_new_yaw(float current_cost, float last_yaw) {
   static float last_cost = current_cost;
index b89dacc..3e680ea 100644 (file)
@@ -39,13 +39,21 @@ protected:
   float get_new_yaw(float current_cost, float last_yaw);
 
 protected:
+  /** The laser interface used to read the front laser data from */
   LaserInterface *front_laser_;
+  /** The initial step size */
   const float init_step_ = 0.02;
+  /** The current step size */
   float step_;
+  /** Random number generator used to compute the random reset probability */
   std::mt19937 random_generator_;
+  /** The distribution used to compute the random reset probability */
   std::uniform_real_distribution<float> random_float_dist_;
+  /** A map of yaw config values to costs */
   std::map<float, float> costs_;
+  /** The minimal cost */
   float min_cost_;
+  /** A yaw configuration with the minimal cost */
   float min_cost_yaw_;
 };