9 #include "../../include/ecl/geometry/odometry_helper.hpp" 10 #include "../../include/ecl/geometry/linear_segment.hpp" 30 Position2D segment_start = trajectory.topLeftCorner<2, 1>();
32 if (
size(trajectory) == 1)
34 return (position - segment_start).norm();
37 double min_squared_distance = std::numeric_limits<double>::infinity();
38 double temp_squared_distance;
40 for (
int i = 1; i <
size(trajectory); ++i)
42 Position2D segment_end = trajectory.block<2, 1>(0, i);
47 if (min_squared_distance > temp_squared_distance)
49 min_squared_distance = temp_squared_distance;
52 segment_start = segment_end;
55 return std::sqrt(min_squared_distance);
60 return size(trajectory) == 0;
65 return size(trajectory) == 0;
70 return trajectory.cols();
75 return trajectory.cols();
105 if(
size(target) == 0)
116 if(
size(target) == 0)
128 trajectory.resize(Eigen::NoChange, vec.size());
130 for (
int i = 0; i < vec.size(); ++i)
132 setAt(trajectory, i, vec[i]);
141 trajectory.resize(Eigen::NoChange, vec.size());
143 for (
int i = 0; i < vec.size(); ++i)
145 setAt(trajectory, i, vec[i]);
153 trajectory.conservativeResize(Eigen::NoChange, size);
158 trajectory.conservativeResize(Eigen::NoChange, size);
163 trajectory.col(index) = pose;
168 trajectory.col(index) = odom;
173 return trajectory.col(index);
178 return trajectory.col(index);
183 return trajectory.leftCols<1>();
188 return trajectory.rightCols<1>();
193 return trajectory.leftCols<1>();
198 return trajectory.rightCols<1>();
203 return trajectory.topRows<3>();
208 return trajectory.bottomRows<3>();
331 return odom.head<3>();
336 return odom.tail<3>();
356 return pose.head<2>();
373 #if defined(ECL_CXX11_FOUND) 375 bool empty(
const Trajectory2DPtr& trajectory_ptr)
377 return !trajectory_ptr ||
size(trajectory_ptr) == 0;
380 bool empty(
const Odom2DTrajectoryPtr& trajectory_ptr)
382 return !trajectory_ptr ||
size(trajectory_ptr) == 0;
385 int size(
const Trajectory2DPtr& trajectory)
387 return size(*trajectory);
390 int size(
const Odom2DTrajectoryPtr& trajectory)
392 return size(*trajectory);
395 Trajectory2DPtr vectorToTrajectoryPtr(
const std::vector<Pose2D>& vec)
397 Trajectory2DPtr trajectory = std::make_shared<Trajectory2D>();
398 trajectory->resize(Eigen::NoChange, vec.size());
400 for (
int i = 0; i < vec.size(); ++i)
402 setAt(*trajectory, i, vec[i]);
408 Odom2DTrajectoryPtr vectorToTrajectoryPtr(
const std::vector<Odom2D>& vec)
410 Odom2DTrajectoryPtr trajectory = std::make_shared<Odom2DTrajectory>();
411 trajectory->resize(Eigen::NoChange, vec.size());
413 for (
int i = 0; i < vec.size(); ++i)
415 setAt(*trajectory, i, vec[i]);
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).
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.
double squaredDistanceFromPoint(const double &x, const double &y) const
Distance of a point from the segment.
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.