lib/odometry_helper.cpp

Go to the documentation of this file.

 
/*****************************************************************************
** Includes
*****************************************************************************/
 
#include <ecl/config/macros.hpp>
#include "../../include/ecl/geometry/odometry_helper.hpp"
#include "../../include/ecl/geometry/linear_segment.hpp"
 
/*****************************************************************************
** Namespaces
*****************************************************************************/
 
namespace ecl {
namespace odometry {
 
/*****************************************************************************
** Implementation
*****************************************************************************/
 
double distance(const Pose2D& pose, const Trajectory2D& trajectory)
{
  return distance(getPosition(pose), trajectory);
}
 
double distance(const Position2D& position, const Trajectory2D& trajectory)
{
  Position2D segment_start = trajectory.topLeftCorner<2, 1>();
 
  if (size(trajectory) == 1)
  {
    return (position - segment_start).norm();
  }
 
  double min_squared_distance = std::numeric_limits<double>::infinity();
  double temp_squared_distance;
 
  for (int i = 1; i < size(trajectory); ++i)
  {
    Position2D segment_end = trajectory.block<2, 1>(0, i);
    ecl::LinearSegment segment(getX(segment_start), getY(segment_start), getX(segment_end), getY(segment_end));
 
    temp_squared_distance = segment.squaredDistanceFromPoint(getX(position), getY(position));
 
    if (min_squared_distance > temp_squared_distance)
    {
      min_squared_distance = temp_squared_distance;
    }
 
    segment_start = segment_end;
  }
 
  return std::sqrt(min_squared_distance);
}
 
bool empty(const Trajectory2D& trajectory)
{
  return size(trajectory) == 0;
}
 
bool empty(const Odom2DTrajectory& trajectory)
{
  return size(trajectory) == 0;
}
 
int size(const Trajectory2D& trajectory)
{
  return trajectory.cols();
}
 
int size(const Odom2DTrajectory& trajectory)
{
  return trajectory.cols();
}
 
double distance(const Odom2D& a, const Odom2D& b)
{
  return (getPosition(a) - getPosition(b)).norm();
}
 
double distance(const Pose2D& a, const Odom2D& b)
{
  return (getPosition(a) - getPosition(b)).norm();
}
 
double distance(const Pose2D& a, const Pose2D& b)
{
  return (getPosition(a) - getPosition(b)).norm();
}
 
double distanceSqared(const Odom2D& a, const Odom2D& b)
{
  return (getPosition(a) - getPosition(b)).squaredNorm();
}
 
double distanceSqared(const Pose2D& a, const Pose2D& b)
{
  return (getPosition(a) - getPosition(b)).squaredNorm();
}
 
void addAtEnd(Trajectory2D& target, const Trajectory2D& addition)
{
  if(size(target) == 0)
  {
    target = addition;
    return;
  }
 
  target << addition;
}
 
void addAtEnd(Odom2DTrajectory& target, const Odom2DTrajectory& addition)
{
  if(size(target) == 0)
  {
    target = addition;
    return;
  }
 
  target << addition;
}
 
Trajectory2D vectorToTrajectory(const std::vector<Pose2D>& vec)
{
  Trajectory2D trajectory;
  trajectory.resize(Eigen::NoChange, vec.size());
 
  for (int i = 0; i < vec.size(); ++i)
  {
    setAt(trajectory, i, vec[i]);
  }
 
  return trajectory;
}
 
Odom2DTrajectory vectorToTrajectory(const std::vector<Odom2D>& vec)
{
  Odom2DTrajectory trajectory;
  trajectory.resize(Eigen::NoChange, vec.size());
 
  for (int i = 0; i < vec.size(); ++i)
  {
    setAt(trajectory, i, vec[i]);
  }
 
  return trajectory;
}
 
void resize(Trajectory2D& trajectory, const int& size)
{
  trajectory.conservativeResize(Eigen::NoChange, size);
}
 
void resize(Odom2DTrajectory& trajectory, const int& size)
{
  trajectory.conservativeResize(Eigen::NoChange, size);
}
 
void setAt(Trajectory2D& trajectory, const int& index, const Pose2D& pose)
{
  trajectory.col(index) = pose;
}
 
void setAt(Odom2DTrajectory& trajectory, const int& index, const Odom2D& odom)
{
  trajectory.col(index) = odom;
}
 
Pose2D getAt(const Trajectory2D& trajectory, const int& index)
{
  return trajectory.col(index);
}
 
Odom2D getAt(const Odom2DTrajectory& trajectory, const int& index)
{
  return trajectory.col(index);
}
 
Pose2D getFront(const Trajectory2D& trajectory)
{
  return trajectory.leftCols<1>();
}
 
Pose2D getBack(const Trajectory2D& trajectory)
{
  return trajectory.rightCols<1>();
}
 
Odom2D getFront(const Odom2DTrajectory& trajectory)
{
  return trajectory.leftCols<1>();
}
 
Odom2D getBack(const Odom2DTrajectory& trajectory)
{
  return trajectory.rightCols<1>();
}
 
Trajectory2D getPoses(const Odom2DTrajectory& trajectory)
{
  return trajectory.topRows<3>();
}
 
Twist2DVector getTwists(const Odom2DTrajectory& trajectory)
{
  return trajectory.bottomRows<3>();
}
 
void setVelocityX(Odom2D& odom, const float& value)
{
  odom(3) = value;
}
 
void setVelocityY(Odom2D& odom, const float& value)
{
  odom(4) = value;
}
 
void setVelocityAngular(Odom2D& odom, const float& value)
{
  odom(5) = value;
}
 
void setVelocityX(Twist2D& twist, const float& value)
{
  twist(0) = value;
}
 
void setVelocityY(Twist2D& twist, const float& value)
{
  twist(1) = value;
}
 
void setVelocityAngular(Twist2D& twist, const float& value)
{
  twist(2) = value;
}
 
void setX(Odom2D& odom, const float& value)
{
  odom(0) = value;
}
 
void setY(Odom2D& odom, const float& value)
{
  odom(1) = value;
}
 
void setYaw(Odom2D& odom, const float& value)
{
  odom(2) = value;
}
 
void setX(Pose2D& pose, const float& value)
{
  pose(0) = value;
}
 
void setY(Pose2D& pose, const float& value)
{
  pose(1) = value;
}
 
void setYaw(Pose2D& pose, const float& value)
{
  pose(2) = value;
}
 
void setX(Position2D& position, const float& value)
{
  position(0) = value;
}
 
void setY(Position2D& position, const float& value)
{
  position(1) = value;
}
 
float getVelocityX(const Odom2D& odom)
{
  return getVelocityX(getTwist(odom));
}
 
float getVelocityY(const Odom2D& odom)
{
  return getVelocityY(getTwist(odom));
}
 
float getVelocityAngular(const Odom2D& odom)
{
  return getVelocityAngular(getTwist(odom));
}
 
float getVelocityX(const Twist2D& twist)
{
  return twist(0);
}
 
float getVelocityY(const Twist2D& twist)
{
  return twist(1);
}
 
float getVelocityAngular(const Twist2D& twist)
{
  return twist(2);
}
 
float getX(const Odom2D& odom)
{
  return getX(getPose(odom));
}
float getY(const Odom2D& odom)
{
  return getY(getPose(odom));
}
float getYaw(const Odom2D& odom)
{
  return getYaw(getPose(odom));
}
 
Position2D getPosition(const Odom2D& odom)
{
  return getPosition(getPose(odom));
}
 
Pose2D getPose(const Odom2D& odom)
{
  return odom.head<3>();
}
 
Twist2D getTwist(const Odom2D& odom)
{
  return odom.tail<3>();
}
 
float getX(const Pose2D& pose)
{
  return getX(getPosition(pose));
}
 
float getY(const Pose2D& pose)
{
  return getY(getPosition(pose));
}
 
float getYaw(const Pose2D& pose)
{
  return pose(2);
}
 
Position2D getPosition(const Pose2D& pose)
{
  return pose.head<2>();
}
 
float getX(const Position2D& position)
{
  return position(0);
}
 
float getY(const Position2D& position)
{
  return position(1);
}
 
/*****************************************************************************
** C++11 Implementations
*****************************************************************************/
 
#if defined(ECL_CXX11_FOUND)
 
  bool empty(const Trajectory2DPtr& trajectory_ptr)
  {
    return !trajectory_ptr || size(trajectory_ptr) == 0;
  }
 
  bool empty(const Odom2DTrajectoryPtr& trajectory_ptr)
  {
    return !trajectory_ptr || size(trajectory_ptr) == 0;
  }
 
  int size(const Trajectory2DPtr& trajectory)
  {
    return size(*trajectory);
  }
 
  int size(const Odom2DTrajectoryPtr& trajectory)
  {
    return size(*trajectory);
  }
 
  Trajectory2DPtr vectorToTrajectoryPtr(const std::vector<Pose2D>& vec)
  {
    Trajectory2DPtr trajectory = std::make_shared<Trajectory2D>();
    trajectory->resize(Eigen::NoChange, vec.size());
 
    for (int i = 0; i < vec.size(); ++i)
    {
      setAt(*trajectory, i, vec[i]);
    }
 
    return trajectory;
  }
 
  Odom2DTrajectoryPtr vectorToTrajectoryPtr(const std::vector<Odom2D>& vec)
  {
    Odom2DTrajectoryPtr trajectory = std::make_shared<Odom2DTrajectory>();
    trajectory->resize(Eigen::NoChange, vec.size());
 
    for (int i = 0; i < vec.size(); ++i)
    {
      setAt(*trajectory, i, vec[i]);
    }
 
    return trajectory;
  }
 
#endif /*ECL_CXX11_FOUND*/
 
/*****************************************************************************
** Trailers
*****************************************************************************/
 
} // namespace odometry
} // namespace ecl