21#ifndef LASER_CALIBRATION_H
22#define LASER_CALIBRATION_H
24#include <core/exception.h>
25#include <interfaces/Laser360Interface.h>
26#include <pcl/point_cloud.h>
27#include <pcl/point_types.h>
32typedef pcl::PointXYZ Point;
34typedef PointCloud::Ptr PointCloudPtr;
39class NetworkConfiguration;
49 return (deg * M_PI / 180.f);
72 std::string config_path);
84 float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2,
float *rot_yaw);
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.
Exception() noexcept
Constructor for subclasses.
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.