scan_constructor.cpp
Go to the documentation of this file.
1 /*
2  * (c) 2021-2022 Nokia
3  *
4  * Licensed under the BSD 3-Clause "New" or "Revised" License
5  * SPDX-License-Identifier: BSD-3-Clause
6  */
7 
9 
10 #include <potracelib.h>
11 
12 #include <algorithm>
13 #include <utility>
14 #include <memory>
15 #include <stdexcept>
16 
17 #include <cmath>
18 
19 namespace scan_tools
20 {
21 
22 /* See section 2.1.2 of potracelib API manual
23  */
24 std::pair<potrace_bitmap_t, std::unique_ptr<potrace_word[]> >
26  int map_occupancy_threshold)
27 {
28  constexpr auto N = 8*sizeof(potrace_word);
29 
30  potrace_bitmap_t ret;
31  ret.h = grid.size();
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]);
35  ret.map = data.get();
36 
37  for (auto y = 0u; y < ret.h; ++y)
38  {
39  // initialize
40  for (auto w = 0u; w < ret.dy; ++w) ret.map[y*ret.dy + w] = 0;
41 
42  for (auto x = 0u; x < ret.w; ++x)
43  {
44  const bool val = (grid[y][x] >= map_occupancy_threshold);
45  const auto bit = (N - 1 - x%N);
46 
47  ret.map[y*ret.dy + x/N] |= (potrace_word(val) << bit);
48  }
49  }
50 
51  return std::make_pair(ret, std::move(data));
52 }
53 
54 template<int N>
55 Point bezier(const Point* points, double t)
56 {
57  return (1-t)*bezier<N-1>(points, t) + t*bezier<N-1>(points+1, t);
58 }
59 
60 template<>
61 Point bezier<1>(const Point* point, double t)
62 {
63  return *point;
64 }
65 
66 template<int N>
67 Point bezier(const Point (&points)[N], double t)
68 {
69  return bezier<N>(&points[0], t);
70 }
71 
72 Point potrace_to_point(const potrace_dpoint_t &p)
73 {
74  return {p.x, p.y};
75 }
76 
77 // determinant
78 double cross2(const Eigen::Vector2d& a, const Eigen::Vector2d& b)
79 {
80  return a[0]*b[1] - a[1]*b[0];
81 }
82 
83 std::vector<Segment>
84 extractSegments(const potrace_path_t* path_head)
85 {
86  std::vector<Segment> segments;
87 
88  for (auto path = path_head; path != nullptr; path = path->next)
89  {
90  const auto &curve = path->curve;
91 
92  auto seg_start = potrace_to_point(curve.c[curve.n - 1][2]);
93  for (auto i = 0; i < curve.n; ++i)
94  {
95  const auto seg_end = potrace_to_point(curve.c[i][2]);
96 
97  if (curve.tag[i] == POTRACE_CORNER)
98  {
99  const auto corner = potrace_to_point(curve.c[i][1]);
100  segments.push_back(Segment{seg_start, corner});
101  segments.push_back(Segment{corner, seg_end});
102  }
103  else if (curve.tag[i] == POTRACE_CURVETO)
104  {
105  constexpr auto segs_per_curve = 4; // Totally arbitrary
106 
107  const Point points[] = {
108  seg_start,
109  potrace_to_point(curve.c[i][0]),
110  potrace_to_point(curve.c[i][1]),
111  seg_end
112  };
113 
114  Point prev = points[0];
115  for (auto j = 1; j <= segs_per_curve; ++j)
116  {
117  const double t = double(j)/segs_per_curve;
118  const Point p = bezier(points, t);
119  segments.push_back(Segment{prev, p});
120  prev = p;
121  }
122  }
123 
124  seg_start = seg_end;
125  }
126  }
127 
128  return segments;
129 }
130 
132  const map_params_t &map_params):
133  map_params_(map_params)
134 {
135  const auto &p = prepareBitmap(grid, map_params.map_occupancy_threshold);
136  const auto &bmp = p.first;
137 
138  auto potrace_params =
139  std::unique_ptr<potrace_param_t, decltype(&potrace_param_free)>(
140  potrace_param_default(), &potrace_param_free);
141 
142  potrace_params->turdsize = 5;
143  potrace_params->alphamax = 0.6;
144  potrace_params->opticurve = false;
145 
146  auto potrace_state =
147  std::unique_ptr<potrace_state_t, decltype(&potrace_state_free)>(
148  potrace_trace(potrace_params.get(), &bmp), &potrace_state_free);
149 
150  if (potrace_state->status != POTRACE_STATUS_OK)
151  throw std::runtime_error("ScanConstructor: potrace_trace failed to trace");
152 
153  auto extracted_segments = extractSegments(potrace_state->plist);
154 
155  for (auto &seg : extracted_segments)
156  {
157  const auto normal = seg.surface_normal();
158 
159  // Adjust coordinates so they go through the centers of the edge pixels
160  seg.first -= Point{0.5, 0.5};
161  seg.second -= Point{0.5, 0.5};
162 
163  // Hypothetically this would be the unbiased reconstruction
164  // but in practice it seems that edge occupied grid cells don't have the
165  // edge at the center on average.
166  //seg.first -= 0.5*normal;
167  //seg.second -= 0.5*normal;
168 
169  // Use world coordinates
170  seg.first *= map_params.map_res;
171  seg.second *= map_params.map_res;
172  }
173 
174  segments_ = SegmentTree(std::move(extracted_segments));
175 }
176 
178  const map_params_t &map_params):
179  ScanConstructor(grid, map_params)
180 {
181 }
182 
184 ScanConstructor::constructScan(double laser_x, double laser_y,
185  double laser_yaw,
186  const scan_params_t &scan_params) const
187 {
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;
193  const auto max_allowed_range = scan_params.max_allowed_range;
194 
195  Point scan_origin{laser_x, laser_y};
196 
197  auto neighborhood = segments_.segments_within(scan_origin, range_max);
198 
199  int num_angles = (int)round((angle_max - angle_min + angle_inc) / angle_inc);
200  scan_t constructed_ranges(num_angles, -1.0);
201 
202  for (const auto &world_seg : neighborhood)
203  {
204  const Segment local_seg{
205  world_seg.first - scan_origin,
206  world_seg.second - scan_origin
207  };
208 
209  // skip surfaces facing away from the scanner
210  if (local_seg.first.dot(local_seg.surface_normal()) > 0) continue;
211 
212  const auto seg_delta = local_seg.diff();
213 
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
217  };
218 
219  const double min_theta = std::min(thetas[0], thetas[1]);
220  const double max_theta = std::max(thetas[0], thetas[1]);
221 
222  double delta_theta = max_theta - min_theta;
223  double start_theta = min_theta;
224  // handle wrap-around the circle
225  if (delta_theta > M_PI) {
226  start_theta = max_theta;
227  delta_theta = 2 * M_PI - delta_theta;
228  }
229 
230  const int start_index =
231  (int)((start_theta - angle_min)/angle_inc + num_angles) % num_angles;
232 
233  const int n_thetas = (int)(delta_theta / angle_inc) + 1;
234 
235  for (int i = 0; i <= n_thetas; i++)
236  {
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;
240 
241  const Eigen::Vector2d laser{std::cos(theta), std::sin(theta)};
242 
243  // distance from origin to intersection with line 'seg' along angle theta
244  const auto rho =
245  cross2(local_seg.first, seg_delta)/cross2(laser, seg_delta);
246 
247  // weary of numeric degeneracy
248  // manually ensure the intersection point is still within the segment
249  const double t = (rho*laser - local_seg.first).dot(seg_delta)/
250  seg_delta.squaredNorm();
251 
252  if (t < 0.0 || t > 1.0) continue;
253 
254  // Check if beam is valid
255  if (rho < range_min || rho >= range_max)
256  continue;
257  if (max_allowed_range > 0 && rho > max_allowed_range)
258  continue;
259 
260  // either no point ever recorded for this angle, so take it
261  // or the current point is closer than previously recorded point
262  if (constructed_ranges[theta_index] <= 0.0
263  || rho < constructed_ranges[theta_index])
264  {
265  constructed_ranges[theta_index] = rho;
266  }
267  }
268  }
269 
270  return constructed_ranges;
271 }
272 
273 } // scan_tools
scan_tools::ScanConstructor::map_params
const map_params_t & map_params() const
Definition: scan_constructor.h:56
M_PI
#define M_PI
scan_tools::ScanConstructor::scan_params_t::angle_max
double angle_max
Definition: scan_constructor.h:37
scan_tools::Point
Eigen::Vector2d Point
Definition: segment_tree.h:23
scan_tools::cross2
double cross2(const Eigen::Vector2d &a, const Eigen::Vector2d &b)
Definition: scan_constructor.cpp:78
scan_tools::ScanConstructor::scan_params_t
Definition: scan_constructor.h:32
scan_tools::ScanConstructor::map_params_t::map_occupancy_threshold
int map_occupancy_threshold
Definition: scan_constructor.h:29
scan_tools::ScanConstructor::ScanConstructor
ScanConstructor()=default
scan_tools::ScanConstructor
Definition: scan_constructor.h:19
scan_tools::potrace_to_point
Point potrace_to_point(const potrace_dpoint_t &p)
Definition: scan_constructor.cpp:72
scan_tools::prepareBitmap
std::pair< potrace_bitmap_t, std::unique_ptr< potrace_word[]> > prepareBitmap(const ScanConstructor::grid_t &grid, int map_occupancy_threshold)
Definition: scan_constructor.cpp:25
scan_tools::ScanConstructor::grid_t
std::vector< std::vector< int > > grid_t
Definition: scan_constructor.h:22
scan_tools::ScanConstructor::map_params_t::map_res
double map_res
Definition: scan_constructor.h:26
scan_tools::Segment
Definition: segment_tree.h:24
dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
scan_tools::ScanConstructor::scan_t
std::vector< double > scan_t
Definition: scan_constructor.h:23
scan_tools::bezier< 1 >
Point bezier< 1 >(const Point *point, double t)
Definition: scan_constructor.cpp:61
scan_tools
Definition: laser_scan_matcher.h:72
scan_tools::SegmentTree
Definition: segment_tree.h:39
p
struct @4 p
scan_tools::ScanConstructor::scan_params_t::angle_inc
double angle_inc
Definition: scan_constructor.h:38
scan_tools::SegmentTree::segments_within
std::vector< Segment > segments_within(Point p, double radius) const
Definition: segment_tree.cpp:101
scan_tools::ScanConstructor::segments_
SegmentTree segments_
Definition: scan_constructor.h:63
scan_constructor.h
scan_tools::bezier
Point bezier(const Point *points, double t)
Definition: scan_constructor.cpp:55
scan_tools::extractSegments
std::vector< Segment > extractSegments(const potrace_path_t *path_head)
Definition: scan_constructor.cpp:84
scan_tools::ScanConstructor::constructScan
scan_t constructScan(double x, double y, double yaw, const scan_params_t &scan_params) const
Definition: scan_constructor.cpp:184
scan_tools::ScanConstructor::scan_params_t::range_min
double range_min
Definition: scan_constructor.h:34
scan_tools::Segment::diff
Eigen::Vector2d diff() const
Definition: segment_tree.h:30
scan_tools::ScanConstructor::scan_params_t::max_allowed_range
double max_allowed_range
Definition: scan_constructor.h:39
scan_tools::ScanConstructor::scan_params_t::angle_min
double angle_min
Definition: scan_constructor.h:36
scan_tools::ScanConstructor::map_params_t
Definition: scan_constructor.h:24
scan_tools::ScanConstructor::scan_params_t::range_max
double range_max
Definition: scan_constructor.h:35
egsl_val


lsm_localization
Author(s): Ivan Dryanovski , Ilija Hadzic
autogenerated on Fri Dec 23 2022 03:09:11