10 #include <potracelib.h>
24 std::pair<potrace_bitmap_t, std::unique_ptr<potrace_word[]> >
26 int map_occupancy_threshold)
28 constexpr
auto N = 8*
sizeof(potrace_word);
32 ret.w = grid[0].size();
33 ret.dy = (ret.w - 1)/N + 1;
34 std::unique_ptr<potrace_word[]> data(
new potrace_word[ret.dy*ret.h]);
37 for (
auto y = 0u; y < ret.h; ++y)
40 for (
auto w = 0u; w < ret.dy; ++w) ret.map[y*ret.dy + w] = 0;
42 for (
auto x = 0u; x < ret.w; ++x)
44 const bool val = (grid[y][x] >= map_occupancy_threshold);
45 const auto bit = (N - 1 - x%N);
47 ret.map[y*ret.dy + x/N] |= (potrace_word(
val) << bit);
51 return std::make_pair(ret, std::move(data));
57 return (1-t)*
bezier<N-1>(points, t) + t*bezier<N-1>(points+1, t);
69 return bezier<N>(&points[0], t);
78 double cross2(
const Eigen::Vector2d& a,
const Eigen::Vector2d& b)
80 return a[0]*b[1] - a[1]*b[0];
86 std::vector<Segment> segments;
88 for (
auto path = path_head; path !=
nullptr; path = path->next)
90 const auto &curve = path->curve;
93 for (
auto i = 0; i < curve.n; ++i)
97 if (curve.tag[i] == POTRACE_CORNER)
100 segments.push_back(
Segment{seg_start, corner});
101 segments.push_back(
Segment{corner, seg_end});
103 else if (curve.tag[i] == POTRACE_CURVETO)
105 constexpr
auto segs_per_curve = 4;
107 const Point points[] = {
114 Point prev = points[0];
115 for (
auto j = 1; j <= segs_per_curve; ++j)
117 const double t = double(j)/segs_per_curve;
119 segments.push_back(
Segment{prev,
p});
133 map_params_(map_params)
136 const auto &bmp =
p.first;
138 auto potrace_params =
139 std::unique_ptr<potrace_param_t, decltype(&potrace_param_free)>(
140 potrace_param_default(), &potrace_param_free);
142 potrace_params->turdsize = 5;
143 potrace_params->alphamax = 0.6;
144 potrace_params->opticurve =
false;
147 std::unique_ptr<potrace_state_t, decltype(&potrace_state_free)>(
148 potrace_trace(potrace_params.get(), &bmp), &potrace_state_free);
150 if (potrace_state->status != POTRACE_STATUS_OK)
151 throw std::runtime_error(
"ScanConstructor: potrace_trace failed to trace");
155 for (
auto &seg : extracted_segments)
157 const auto normal = seg.surface_normal();
160 seg.first -=
Point{0.5, 0.5};
161 seg.second -=
Point{0.5, 0.5};
188 const auto range_min = scan_params.
range_min;
189 const auto range_max = scan_params.
range_max;
190 const auto angle_min = scan_params.
angle_min;
191 const auto angle_max = scan_params.
angle_max;
192 const auto angle_inc = scan_params.
angle_inc;
195 Point scan_origin{laser_x, laser_y};
199 int num_angles = (int)round((angle_max - angle_min + angle_inc) / angle_inc);
200 scan_t constructed_ranges(num_angles, -1.0);
202 for (
const auto &world_seg : neighborhood)
205 world_seg.first - scan_origin,
206 world_seg.second - scan_origin
210 if (local_seg.first.dot(local_seg.surface_normal()) > 0)
continue;
212 const auto seg_delta = local_seg.
diff();
214 const double thetas[] = {
215 std::atan2(local_seg.first[1], local_seg.first[0]) - laser_yaw,
216 std::atan2(local_seg.second[1], local_seg.second[0]) - laser_yaw
219 const double min_theta = std::min(thetas[0], thetas[1]);
220 const double max_theta = std::max(thetas[0], thetas[1]);
222 double delta_theta = max_theta - min_theta;
223 double start_theta = min_theta;
225 if (delta_theta >
M_PI) {
226 start_theta = max_theta;
227 delta_theta = 2 *
M_PI - delta_theta;
230 const int start_index =
231 (int)((start_theta - angle_min)/angle_inc + num_angles) % num_angles;
233 const int n_thetas = (int)(delta_theta / angle_inc) + 1;
235 for (
int i = 0; i <= n_thetas; i++)
237 const auto theta_index = (start_index + i) % num_angles;
238 const auto scan_theta = angle_min + theta_index*angle_inc;
239 const auto theta = laser_yaw + scan_theta;
241 const Eigen::Vector2d laser{std::cos(theta), std::sin(theta)};
245 cross2(local_seg.first, seg_delta)/
cross2(laser, seg_delta);
249 const double t = (rho*laser - local_seg.first).
dot(seg_delta)/
250 seg_delta.squaredNorm();
252 if (t < 0.0 || t > 1.0)
continue;
255 if (rho < range_min || rho >= range_max)
257 if (max_allowed_range > 0 && rho > max_allowed_range)
262 if (constructed_ranges[theta_index] <= 0.0
263 || rho < constructed_ranges[theta_index])
265 constructed_ranges[theta_index] = rho;
270 return constructed_ranges;