21 #include "laser_calibration.h" 23 #include <config/netconf.h> 24 #include <pcl/common/geometry.h> 25 #include <pcl/filters/filter.h> 26 #include <pcl/filters/passthrough.h> 27 #include <pcl/registration/icp.h> 28 #include <tf/transformer.h> 56 : laser_(laser), tf_transformer_(tf_transformer), config_(config), config_path_(config_path)
74 PointCloudPtr cloud = PointCloudPtr(
new PointCloud());
76 cloud->header.frame_id = laser.
frame();
79 float const *distances = laser.
distances();
96 for (
auto &point : cloud->points) {
99 cloud->header.frame_id);
102 point.x = static_cast<float>(point_in_base_frame[0]);
103 point.y = static_cast<float>(point_in_base_frame[1]);
104 point.z = static_cast<float>(point_in_base_frame[2]);
115 pcl::PassThrough<Point> pass;
116 pass.setInputCloud(input);
117 pass.setFilterFieldName(
"x");
118 pass.setFilterLimits(-2., -0.8);
120 pass.filter(*output);
136 error <<
"Not enough laser points in rear cloud, got " << cloud->size() <<
", need " 141 zs.resize(cloud->points.size());
142 for (
auto &point : *cloud) {
143 zs.push_back(point.z);
145 return accumulate(zs.begin(), zs.end(), 0.) / zs.size();
155 pcl::PassThrough<Point> pass;
156 pass.setInputCloud(input);
157 pass.setFilterFieldName(
"y");
158 pass.setFilterLimits(0., 2.);
160 pass.filter(*output);
171 pcl::PassThrough<Point> pass;
172 pass.setInputCloud(input);
173 pass.setFilterFieldName(
"y");
174 pass.setFilterLimits(-2., 0.);
176 pass.filter(*output);
188 pcl::PassThrough<Point> pass;
189 pass.setInputCloud(input);
190 pass.setFilterFieldName(
"z");
191 pass.setFilterLimits(0.1, 1);
193 pass.filter(*output);
210 error <<
"Not enough points, got " << cloud1->points.size() <<
" and " << cloud2->points.size()
214 pcl::IterativeClosestPoint<Point, Point> icp;
215 icp.setInputCloud(cloud2);
216 icp.setInputTarget(cloud1);
219 if (!icp.hasConverged()) {
223 pcl::Registration<Point, Point, float>::Matrix4 transformation = icp.getFinalTransformation();
224 *rot_yaw = atan2(transformation(1, 0), transformation(0, 0));
226 return icp.getFitnessScore();
238 pcl::PassThrough<Point> pass_x;
239 pass_x.setInputCloud(input);
240 pass_x.setFilterFieldName(
"x");
241 pass_x.setFilterLimits(-2, 2);
243 pass_x.filter(*x_filtered);
244 pcl::PassThrough<Point> pass_y;
246 pass_y.setInputCloud(x_filtered);
247 pass_y.setFilterFieldName(
"y");
248 pass_y.setFilterLimitsNegative(
true);
249 pass_y.setFilterLimits(-0.5, 0.5);
250 PointCloudPtr xy_filtered_inner(
new PointCloud());
251 pass_y.filter(*xy_filtered_inner);
252 pcl::PassThrough<Point> pass_y_outer;
253 pass_y_outer.setInputCloud(xy_filtered_inner);
254 pass_y_outer.setFilterFieldName(
"y");
255 pass_y_outer.setFilterLimits(-3, 3);
257 pass_y_outer.filter(*xy_filtered);
258 pcl::PassThrough<Point> pass_z;
259 pass_z.setInputCloud(xy_filtered);
260 pass_z.setFilterFieldName(
"z");
261 pass_z.setFilterLimits(0.1, 1);
263 pass_z.filter(*xyz_filtered);
Laser360Interface Fawkes BlackBoard Interface.
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
Fawkes library namespace.
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
A class for handling time.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
PointCloudPtr filter_out_ground(PointCloudPtr input)
Remove all points that belong to the ground.
fawkes::tf::Transformer * tf_transformer_
The transformer used to compute transforms.
Wrapper class to add time stamp and frame ID to base types.
LaserCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
virtual ~LaserCalibration()
Destructor.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
Exception that is thrown if there are not enough laser points to do a matching.
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
char * frame() const
Get frame value.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
size_t maxlenof_distances() const
Get maximum length of distances value.
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
Remote configuration via Fawkes net.