Fawkes API  Fawkes Development Version
laser_calibration.h
1 /***************************************************************************
2  * laser_calibration.h - Base class for laser transform calibration
3  *
4  * Created: Mon 10 Jul 2017 17:37:01 CEST 17:37
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 #ifndef LASER_CALIBRATION_H
22 #define LASER_CALIBRATION_H
23 
24 #include <core/exception.h>
25 #include <interfaces/Laser360Interface.h>
26 #include <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 
29 #include <cmath>
30 #include <string>
31 
32 typedef pcl::PointXYZ Point;
34 typedef PointCloud::Ptr PointCloudPtr;
35 
37 
38 namespace fawkes {
39 class NetworkConfiguration;
40 class MotorInterface;
41 namespace tf {
42 class Transformer;
43 }
44 } // namespace fawkes
45 
46 inline float
47 deg2rad(float deg)
48 {
49  return (deg * M_PI / 180.f);
50 }
51 
52 /** Exception that is thrown if there are not enough laser points to
53  * do a matching.
54  */
56 {
57 public:
58  /** Constructor.
59  * @param error: the error message
60  */
61  InsufficientDataException(const char *error) : Exception(error)
62  {
63  }
64 };
65 
67 {
68 public:
70  fawkes::tf::Transformer * tf_transformer,
72  std::string config_path);
73  virtual ~LaserCalibration();
74  virtual void calibrate() = 0;
75 
76 protected:
77  PointCloudPtr laser_to_pointcloud(const LaserInterface &laser);
78  void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud);
79  PointCloudPtr filter_cloud_in_rear(PointCloudPtr input);
80  float get_mean_z(PointCloudPtr cloud);
81  PointCloudPtr filter_left_cloud(PointCloudPtr input);
82  PointCloudPtr filter_right_cloud(PointCloudPtr input);
83  PointCloudPtr filter_out_ground(PointCloudPtr input);
84  float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw);
85  PointCloudPtr filter_center_cloud(PointCloudPtr input);
86 
87 protected:
88  /** The laser that provides the input data */
90  /** The transformer used to compute transforms */
92  /** The network config to use for reading and updating config values */
94  /** The config path to use for reading and updating config values */
95  const std::string config_path_;
96  /** Time in micro seconds to sleep between iterations */
97  const static long sleep_time_ = 50000;
98  /** The number of iterations to run before aborting the calibration */
99  const static uint max_iterations_ = 100;
100  /** The number of points required in a pointcloud to use it as input data */
101  const static size_t min_points = 10;
102 };
103 
104 #endif /* !LASER_CALIBRATION_H */
LaserCalibration::max_iterations_
const static uint max_iterations_
The number of iterations to run before aborting the calibration.
Definition: laser_calibration.h:99
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
LaserCalibration::LaserCalibration
LaserCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
Definition: laser_calibration.cpp:52
LaserCalibration::laser_
LaserInterface * laser_
The laser that provides the input data.
Definition: laser_calibration.h:89
fawkes::Exception::Exception
Exception()
Constructor for subclasses.
Definition: exception.cpp:253
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::tf_transformer_
fawkes::tf::Transformer * tf_transformer_
The transformer used to compute transforms.
Definition: laser_calibration.h:91
fawkes::tf::Transformer
Definition: transformer.h:74
LaserCalibration::calibrate
virtual void calibrate()=0
LaserCalibration::filter_out_ground
PointCloudPtr filter_out_ground(PointCloudPtr input)
Remove all points that belong to the ground.
Definition: laser_calibration.cpp:186
LaserCalibration::config_path_
const std::string config_path_
The config path to use for reading and updating config values.
Definition: laser_calibration.h:95
InsufficientDataException::InsufficientDataException
InsufficientDataException(const char *error)
Constructor.
Definition: laser_calibration.h:61
LaserCalibration
Definition: laser_calibration.h:66
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
LaserCalibration::config_
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
Definition: laser_calibration.h:93
LaserCalibration::filter_center_cloud
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
Definition: laser_calibration.cpp:236
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
fawkes::NetworkConfiguration
Definition: netconf.h:55
fawkes
LaserCalibration::sleep_time_
const static long sleep_time_
Time in micro seconds to sleep between iterations.
Definition: laser_calibration.h:97
pcl::PointCloud
Definition: pointcloud.h:38
LaserCalibration::~LaserCalibration
virtual ~LaserCalibration()
Destructor.
Definition: laser_calibration.cpp:61
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
LaserCalibration::min_points
const static size_t min_points
The number of points required in a pointcloud to use it as input data.
Definition: laser_calibration.h:101
fawkes::Laser360Interface
Definition: Laser360Interface.h:39
LaserCalibration::get_matching_cost
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
Definition: laser_calibration.cpp:206
InsufficientDataException
Exception that is thrown if there are not enough laser points to do a matching.
Definition: laser_calibration.h:55
fawkes::Exception
Definition: exception.h:41