lib/odometry_helper.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
8 #include <ecl/config/macros.hpp>
9 #include "../../include/ecl/geometry/odometry_helper.hpp"
10 #include "../../include/ecl/geometry/linear_segment.hpp"
11 
12 /*****************************************************************************
13 ** Namespaces
14 *****************************************************************************/
15 
16 namespace ecl {
17 namespace odometry {
18 
19 /*****************************************************************************
20 ** Implementation
21 *****************************************************************************/
22 
23 double distance(const Pose2D& pose, const Trajectory2D& trajectory)
24 {
25  return distance(getPosition(pose), trajectory);
26 }
27 
28 double distance(const Position2D& position, const Trajectory2D& trajectory)
29 {
30  Position2D segment_start = trajectory.topLeftCorner<2, 1>();
31 
32  if (size(trajectory) == 1)
33  {
34  return (position - segment_start).norm();
35  }
36 
37  double min_squared_distance = std::numeric_limits<double>::infinity();
38  double temp_squared_distance;
39 
40  for (int i = 1; i < size(trajectory); ++i)
41  {
42  Position2D segment_end = trajectory.block<2, 1>(0, i);
43  ecl::LinearSegment segment(getX(segment_start), getY(segment_start), getX(segment_end), getY(segment_end));
44 
45  temp_squared_distance = segment.squaredDistanceFromPoint(getX(position), getY(position));
46 
47  if (min_squared_distance > temp_squared_distance)
48  {
49  min_squared_distance = temp_squared_distance;
50  }
51 
52  segment_start = segment_end;
53  }
54 
55  return std::sqrt(min_squared_distance);
56 }
57 
58 bool empty(const Trajectory2D& trajectory)
59 {
60  return size(trajectory) == 0;
61 }
62 
63 bool empty(const Odom2DTrajectory& trajectory)
64 {
65  return size(trajectory) == 0;
66 }
67 
68 int size(const Trajectory2D& trajectory)
69 {
70  return trajectory.cols();
71 }
72 
73 int size(const Odom2DTrajectory& trajectory)
74 {
75  return trajectory.cols();
76 }
77 
78 double distance(const Odom2D& a, const Odom2D& b)
79 {
80  return (getPosition(a) - getPosition(b)).norm();
81 }
82 
83 double distance(const Pose2D& a, const Odom2D& b)
84 {
85  return (getPosition(a) - getPosition(b)).norm();
86 }
87 
88 double distance(const Pose2D& a, const Pose2D& b)
89 {
90  return (getPosition(a) - getPosition(b)).norm();
91 }
92 
93 double distanceSqared(const Odom2D& a, const Odom2D& b)
94 {
95  return (getPosition(a) - getPosition(b)).squaredNorm();
96 }
97 
98 double distanceSqared(const Pose2D& a, const Pose2D& b)
99 {
100  return (getPosition(a) - getPosition(b)).squaredNorm();
101 }
102 
103 void addAtEnd(Trajectory2D& target, const Trajectory2D& addition)
104 {
105  if(size(target) == 0)
106  {
107  target = addition;
108  return;
109  }
110 
111  target << addition;
112 }
113 
114 void addAtEnd(Odom2DTrajectory& target, const Odom2DTrajectory& addition)
115 {
116  if(size(target) == 0)
117  {
118  target = addition;
119  return;
120  }
121 
122  target << addition;
123 }
124 
125 Trajectory2D vectorToTrajectory(const std::vector<Pose2D>& vec)
126 {
127  Trajectory2D trajectory;
128  trajectory.resize(Eigen::NoChange, vec.size());
129 
130  for (int i = 0; i < vec.size(); ++i)
131  {
132  setAt(trajectory, i, vec[i]);
133  }
134 
135  return trajectory;
136 }
137 
138 Odom2DTrajectory vectorToTrajectory(const std::vector<Odom2D>& vec)
139 {
140  Odom2DTrajectory trajectory;
141  trajectory.resize(Eigen::NoChange, vec.size());
142 
143  for (int i = 0; i < vec.size(); ++i)
144  {
145  setAt(trajectory, i, vec[i]);
146  }
147 
148  return trajectory;
149 }
150 
151 void resize(Trajectory2D& trajectory, const int& size)
152 {
153  trajectory.conservativeResize(Eigen::NoChange, size);
154 }
155 
156 void resize(Odom2DTrajectory& trajectory, const int& size)
157 {
158  trajectory.conservativeResize(Eigen::NoChange, size);
159 }
160 
161 void setAt(Trajectory2D& trajectory, const int& index, const Pose2D& pose)
162 {
163  trajectory.col(index) = pose;
164 }
165 
166 void setAt(Odom2DTrajectory& trajectory, const int& index, const Odom2D& odom)
167 {
168  trajectory.col(index) = odom;
169 }
170 
171 Pose2D getAt(const Trajectory2D& trajectory, const int& index)
172 {
173  return trajectory.col(index);
174 }
175 
176 Odom2D getAt(const Odom2DTrajectory& trajectory, const int& index)
177 {
178  return trajectory.col(index);
179 }
180 
181 Pose2D getFront(const Trajectory2D& trajectory)
182 {
183  return trajectory.leftCols<1>();
184 }
185 
186 Pose2D getBack(const Trajectory2D& trajectory)
187 {
188  return trajectory.rightCols<1>();
189 }
190 
191 Odom2D getFront(const Odom2DTrajectory& trajectory)
192 {
193  return trajectory.leftCols<1>();
194 }
195 
196 Odom2D getBack(const Odom2DTrajectory& trajectory)
197 {
198  return trajectory.rightCols<1>();
199 }
200 
202 {
203  return trajectory.topRows<3>();
204 }
205 
207 {
208  return trajectory.bottomRows<3>();
209 }
210 
211 void setVelocityX(Odom2D& odom, const float& value)
212 {
213  odom(3) = value;
214 }
215 
216 void setVelocityY(Odom2D& odom, const float& value)
217 {
218  odom(4) = value;
219 }
220 
221 void setVelocityAngular(Odom2D& odom, const float& value)
222 {
223  odom(5) = value;
224 }
225 
226 void setVelocityX(Twist2D& twist, const float& value)
227 {
228  twist(0) = value;
229 }
230 
231 void setVelocityY(Twist2D& twist, const float& value)
232 {
233  twist(1) = value;
234 }
235 
236 void setVelocityAngular(Twist2D& twist, const float& value)
237 {
238  twist(2) = value;
239 }
240 
241 void setX(Odom2D& odom, const float& value)
242 {
243  odom(0) = value;
244 }
245 
246 void setY(Odom2D& odom, const float& value)
247 {
248  odom(1) = value;
249 }
250 
251 void setYaw(Odom2D& odom, const float& value)
252 {
253  odom(2) = value;
254 }
255 
256 void setX(Pose2D& pose, const float& value)
257 {
258  pose(0) = value;
259 }
260 
261 void setY(Pose2D& pose, const float& value)
262 {
263  pose(1) = value;
264 }
265 
266 void setYaw(Pose2D& pose, const float& value)
267 {
268  pose(2) = value;
269 }
270 
271 void setX(Position2D& position, const float& value)
272 {
273  position(0) = value;
274 }
275 
276 void setY(Position2D& position, const float& value)
277 {
278  position(1) = value;
279 }
280 
281 float getVelocityX(const Odom2D& odom)
282 {
283  return getVelocityX(getTwist(odom));
284 }
285 
286 float getVelocityY(const Odom2D& odom)
287 {
288  return getVelocityY(getTwist(odom));
289 }
290 
291 float getVelocityAngular(const Odom2D& odom)
292 {
293  return getVelocityAngular(getTwist(odom));
294 }
295 
296 float getVelocityX(const Twist2D& twist)
297 {
298  return twist(0);
299 }
300 
301 float getVelocityY(const Twist2D& twist)
302 {
303  return twist(1);
304 }
305 
306 float getVelocityAngular(const Twist2D& twist)
307 {
308  return twist(2);
309 }
310 
311 float getX(const Odom2D& odom)
312 {
313  return getX(getPose(odom));
314 }
315 float getY(const Odom2D& odom)
316 {
317  return getY(getPose(odom));
318 }
319 float getYaw(const Odom2D& odom)
320 {
321  return getYaw(getPose(odom));
322 }
323 
325 {
326  return getPosition(getPose(odom));
327 }
328 
329 Pose2D getPose(const Odom2D& odom)
330 {
331  return odom.head<3>();
332 }
333 
334 Twist2D getTwist(const Odom2D& odom)
335 {
336  return odom.tail<3>();
337 }
338 
339 float getX(const Pose2D& pose)
340 {
341  return getX(getPosition(pose));
342 }
343 
344 float getY(const Pose2D& pose)
345 {
346  return getY(getPosition(pose));
347 }
348 
349 float getYaw(const Pose2D& pose)
350 {
351  return pose(2);
352 }
353 
355 {
356  return pose.head<2>();
357 }
358 
359 float getX(const Position2D& position)
360 {
361  return position(0);
362 }
363 
364 float getY(const Position2D& position)
365 {
366  return position(1);
367 }
368 
369 /*****************************************************************************
370 ** C++11 Implementations
371 *****************************************************************************/
372 
373 #if defined(ECL_CXX11_FOUND)
374 
375  bool empty(const Trajectory2DPtr& trajectory_ptr)
376  {
377  return !trajectory_ptr || size(trajectory_ptr) == 0;
378  }
379 
380  bool empty(const Odom2DTrajectoryPtr& trajectory_ptr)
381  {
382  return !trajectory_ptr || size(trajectory_ptr) == 0;
383  }
384 
385  int size(const Trajectory2DPtr& trajectory)
386  {
387  return size(*trajectory);
388  }
389 
390  int size(const Odom2DTrajectoryPtr& trajectory)
391  {
392  return size(*trajectory);
393  }
394 
395  Trajectory2DPtr vectorToTrajectoryPtr(const std::vector<Pose2D>& vec)
396  {
397  Trajectory2DPtr trajectory = std::make_shared<Trajectory2D>();
398  trajectory->resize(Eigen::NoChange, vec.size());
399 
400  for (int i = 0; i < vec.size(); ++i)
401  {
402  setAt(*trajectory, i, vec[i]);
403  }
404 
405  return trajectory;
406  }
407 
408  Odom2DTrajectoryPtr vectorToTrajectoryPtr(const std::vector<Odom2D>& vec)
409  {
410  Odom2DTrajectoryPtr trajectory = std::make_shared<Odom2DTrajectory>();
411  trajectory->resize(Eigen::NoChange, vec.size());
412 
413  for (int i = 0; i < vec.size(); ++i)
414  {
415  setAt(*trajectory, i, vec[i]);
416  }
417 
418  return trajectory;
419  }
420 
421 #endif /*ECL_CXX11_FOUND*/
422 
423 /*****************************************************************************
424 ** Trailers
425 *****************************************************************************/
426 
427 } // namespace odometry
428 } // namespace ecl
ecl_geometry_PUBLIC Twist2DVector getTwists(const Odom2DTrajectory &trajectory)
Extract twists of odom trajectory.
Embedded control libraries.
Eigen::Vector3f Twist2D
Float representation of velocities in 2D (v_x, v_y, w).
ecl_geometry_PUBLIC void setX(Odom2D &odom, const float &value)
Set x position.
ecl_geometry_PUBLIC Trajectory2D getPoses(const Odom2DTrajectory &trajectory)
Extract poses of odom trajectory.
ecl_geometry_PUBLIC Position2D getPosition(const Odom2D &odom)
Extract position from odometry.
ecl_geometry_PUBLIC void setVelocityAngular(Odom2D &odom, const float &value)
Set angular velocity.
ecl_geometry_PUBLIC void resize(Trajectory2D &trajectory, const int &size)
Resizes trajectory appending uninitialised values if needed.
Eigen::Matrix3Xf Trajectory2D
Float representation of a trajectory in 2D (poses in 2D).
ecl_geometry_PUBLIC float getY(const Odom2D &odom)
Get y position.
Eigen::Matrix< float, 6, Eigen::Dynamic > Odom2DTrajectory
Float collection of 2D odometries (x, y, heading, v_x, v_y, w).
ecl_geometry_PUBLIC Pose2D getPose(const Odom2D &odom)
Extract pose from odometry.
ecl_geometry_PUBLIC float getX(const Odom2D &odom)
Get x position.
ecl_geometry_PUBLIC void setY(Odom2D &odom, const float &value)
Set y position.
ecl_geometry_PUBLIC bool empty(const Trajectory2D &trajectory)
Check if trajectory ptr is empty (ptr not set or has no poses)
ecl_geometry_PUBLIC Pose2D getBack(const Trajectory2D &trajectory)
Get back (last) element of trajectory.
Eigen::Vector2f Position2D
Float representation for a 2D position (x-, y-position).
Eigen::Matrix3Xf Twist2DVector
Float collection of 2D twists (twist: v_x, v_y, w).
Eigen::Matrix< float, 6, 1 > Odom2D
Float representation of 2D odometry (x, y, heading, v_x, v_y, w).
ecl_geometry_PUBLIC Trajectory2D vectorToTrajectory(const std::vector< Pose2D > &vec)
Convert vector of Pose2D to Trajectory2D.
ecl_geometry_PUBLIC float getYaw(const Odom2D &odom)
Get yaw (heading)
ecl_geometry_PUBLIC void setVelocityY(Odom2D &odom, const float &value)
Set linear velocity y direction.
ecl_geometry_PUBLIC double distanceSqared(const Odom2D &a, const Odom2D &b)
Squared distance between the positions of odometries.
Eigen::Vector3f Pose2D
Float representation of a pose in 2D (x, y, heading).
double squaredDistanceFromPoint(const double &x, const double &y) const
Distance of a point from the segment.
ecl_geometry_PUBLIC int size(const Trajectory2D &trajectory)
Get the size of the trajectory.
ecl_geometry_PUBLIC void setYaw(Odom2D &odom, const float &value)
Set yaw (heading)
ecl_geometry_PUBLIC void addAtEnd(Trajectory2D &target, const Trajectory2D &addition)
Concat two odometry trajectories.
ecl_geometry_PUBLIC float getVelocityX(const Odom2D &odom)
Get linear velocity x direction.
ecl_geometry_PUBLIC Pose2D getFront(const Trajectory2D &trajectory)
Get front (first) element of trajectory.
ecl_geometry_PUBLIC Pose2D getAt(const Trajectory2D &trajectory, const int &index)
Get element of trajectory.
ecl_geometry_PUBLIC void setAt(Trajectory2D &trajectory, const int &index, const Pose2D &pose)
Set element at index of trajectory.
ecl_geometry_PUBLIC float getVelocityAngular(const Odom2D &odom)
Get angular velocity.
ecl_geometry_PUBLIC float getVelocityY(const Odom2D &odom)
Get linear velocity y direction.
ecl_geometry_PUBLIC void setVelocityX(Odom2D &odom, const float &value)
Set linear velocity x direction.
ecl_geometry_PUBLIC double distance(const Pose2D &pose, const Trajectory2D &trajectory)
Shortest distance between a pose and a trajectory.
ecl_geometry_PUBLIC Twist2D getTwist(const Odom2D &odom)
Extract twist from odometry.


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:18:49