21 #include "time_offset_calibration.h"
23 #include <config/netconf.h>
24 #include <interfaces/MotorInterface.h>
25 #include <pcl/common/geometry.h>
26 #include <pcl/filters/filter.h>
27 #include <pcl/filters/passthrough.h>
28 #include <pcl/registration/icp.h>
29 #include <tf/transformer.h>
64 step_(numeric_limits<float>::max())
79 map<float, float> costs;
80 float min_cost = numeric_limits<float>::max();
81 float min_offset = 0.;
83 uniform_real_distribution<float> random_float_dist(0, 1);
85 printf(
"Rotating bot with omega %f\n",
omega_);
96 printf(
"Taking snapshot (moving)\n");
100 printf(
"Stopping bot.\n");
106 printf(
"Taking snapshot (resting)\n");
107 PointCloudPtr rest_cloud;
119 printf(
"Insufficient data: %s.\nPlease move the robot.\n", e.
what_no_backtrace());
123 float jump_probability =
static_cast<float>((current_cost - min_cost)) / current_cost;
124 float rand_01 = random_float_dist(random_gen);
125 if (current_cost > min_cost && rand_01 > 1 - jump_probability) {
126 printf(
"Setting back to minimum: %f -> %f (probability %f)\n",
130 next_offset = min_offset;
131 step_ = next_offset - current_offset;
133 min_cost = current_cost;
134 min_offset = current_offset;
136 next_offset = current_offset +
step_;
138 printf(
"Updating time offset from %f to %f (step %f), current cost %f\n",
144 current_offset = next_offset;
146 }
while (abs(
step_) > 0.0005);
147 printf(
"Setting to offset with minimal cost %f\n", min_offset);
166 pcl::PassThrough<Point> pass_x;
167 pass_x.setInputCloud(laser_cloud);
168 pass_x.setFilterFieldName(
"x");
169 pass_x.setFilterLimits(-3., 3.);
171 pass_x.filter(*x_filtered);
172 pcl::PassThrough<Point> pass_y;
174 pass_y.setInputCloud(x_filtered);
175 pass_y.setFilterFieldName(
"y");
176 pass_y.setFilterLimitsNegative(
true);
177 pass_y.setFilterLimits(-0.3, 0.3);
178 PointCloudPtr xy_filtered_inner(
new PointCloud());
179 pass_y.filter(*xy_filtered_inner);
180 pcl::PassThrough<Point> pass_y_outer;
181 pass_y_outer.setInputCloud(xy_filtered_inner);
182 pass_y_outer.setFilterFieldName(
"y");
183 pass_y_outer.setFilterLimits(-3, 3);
185 pass_y_outer.filter(*xy_filtered);
186 pcl::PassThrough<Point> pass_z;
187 pass_z.setInputCloud(xy_filtered);
188 pass_z.setFilterFieldName(
"z");
189 pass_z.setFilterLimits(0.1, 1);
191 pass_z.filter(*xyz_filtered);