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 */
Exception that is thrown if there are not enough laser points to do a matching.
InsufficientDataException(const char *error)
Constructor.
Abstract base class for laser calibration.
virtual ~LaserCalibration()
Destructor.
fawkes::tf::Transformer * tf_transformer_
The transformer used to compute transforms.
LaserInterface * laser_
The laser that provides the input data.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
LaserCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
const std::string config_path_
The config path to use for reading and updating config values.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
virtual void calibrate()=0
The actual calibration procedure.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
PointCloudPtr filter_out_ground(PointCloudPtr input)
Remove all points that belong to the ground.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
static const long sleep_time_
Time in micro seconds to sleep between iterations.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Exception()
Constructor for subclasses.
Definition: exception.cpp:253
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Definition: netconf.h:50
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36