odometry_helper.cpp
Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Includes
00006 *****************************************************************************/
00007 
00008 #include <ecl/config/macros.hpp>
00009 #include "../../include/ecl/geometry/odometry_helper.hpp"
00010 #include "../../include/ecl/geometry/linear_segment.hpp"
00011 
00012 /*****************************************************************************
00013 ** Namespaces
00014 *****************************************************************************/
00015 
00016 namespace ecl {
00017 namespace odometry {
00018 
00019 /*****************************************************************************
00020 ** Implementation
00021 *****************************************************************************/
00022 
00023 double distance(const Pose2D& pose, const Trajectory2D& trajectory)
00024 {
00025   return distance(getPosition(pose), trajectory);
00026 }
00027 
00028 double distance(const Position2D& position, const Trajectory2D& trajectory)
00029 {
00030   Position2D segment_start = trajectory.topLeftCorner<2, 1>();
00031 
00032   if (size(trajectory) == 1)
00033   {
00034     return (position - segment_start).norm();
00035   }
00036 
00037   double min_squared_distance = std::numeric_limits<double>::infinity();
00038   double temp_squared_distance;
00039 
00040   for (int i = 1; i < size(trajectory); ++i)
00041   {
00042     Position2D segment_end = trajectory.block<2, 1>(0, i);
00043     ecl::LinearSegment segment(getX(segment_start), getY(segment_start), getX(segment_end), getY(segment_end));
00044 
00045     temp_squared_distance = segment.squaredDistanceFromPoint(getX(position), getY(position));
00046 
00047     if (min_squared_distance > temp_squared_distance)
00048     {
00049       min_squared_distance = temp_squared_distance;
00050     }
00051 
00052     segment_start = segment_end;
00053   }
00054 
00055   return std::sqrt(min_squared_distance);
00056 }
00057 
00058 bool empty(const Trajectory2D& trajectory)
00059 {
00060   return size(trajectory) == 0;
00061 }
00062 
00063 bool empty(const Odom2DTrajectory& trajectory)
00064 {
00065   return size(trajectory) == 0;
00066 }
00067 
00068 int size(const Trajectory2D& trajectory)
00069 {
00070   return trajectory.cols();
00071 }
00072 
00073 int size(const Odom2DTrajectory& trajectory)
00074 {
00075   return trajectory.cols();
00076 }
00077 
00078 double distance(const Odom2D& a, const Odom2D& b)
00079 {
00080   return (getPosition(a) - getPosition(b)).norm();
00081 }
00082 
00083 double distance(const Pose2D& a, const Odom2D& b)
00084 {
00085   return (getPosition(a) - getPosition(b)).norm();
00086 }
00087 
00088 double distance(const Pose2D& a, const Pose2D& b)
00089 {
00090   return (getPosition(a) - getPosition(b)).norm();
00091 }
00092 
00093 double distanceSqared(const Odom2D& a, const Odom2D& b)
00094 {
00095   return (getPosition(a) - getPosition(b)).squaredNorm();
00096 }
00097 
00098 double distanceSqared(const Pose2D& a, const Pose2D& b)
00099 {
00100   return (getPosition(a) - getPosition(b)).squaredNorm();
00101 }
00102 
00103 void addAtEnd(Trajectory2D& target, const Trajectory2D& addition)
00104 {
00105   if(size(target) == 0)
00106   {
00107     target = addition;
00108     return;
00109   }
00110 
00111   target << addition;
00112 }
00113 
00114 void addAtEnd(Odom2DTrajectory& target, const Odom2DTrajectory& addition)
00115 {
00116   if(size(target) == 0)
00117   {
00118     target = addition;
00119     return;
00120   }
00121 
00122   target << addition;
00123 }
00124 
00125 Trajectory2D vectorToTrajectory(const std::vector<Pose2D>& vec)
00126 {
00127   Trajectory2D trajectory;
00128   trajectory.resize(Eigen::NoChange, vec.size());
00129 
00130   for (int i = 0; i < vec.size(); ++i)
00131   {
00132     setAt(trajectory, i, vec[i]);
00133   }
00134 
00135   return trajectory;
00136 }
00137 
00138 Odom2DTrajectory vectorToTrajectory(const std::vector<Odom2D>& vec)
00139 {
00140   Odom2DTrajectory trajectory;
00141   trajectory.resize(Eigen::NoChange, vec.size());
00142 
00143   for (int i = 0; i < vec.size(); ++i)
00144   {
00145     setAt(trajectory, i, vec[i]);
00146   }
00147 
00148   return trajectory;
00149 }
00150 
00151 void resize(Trajectory2D& trajectory, const int& size)
00152 {
00153   trajectory.conservativeResize(Eigen::NoChange, size);
00154 }
00155 
00156 void resize(Odom2DTrajectory& trajectory, const int& size)
00157 {
00158   trajectory.conservativeResize(Eigen::NoChange, size);
00159 }
00160 
00161 void setAt(Trajectory2D& trajectory, const int& index, const Pose2D& pose)
00162 {
00163   trajectory.col(index) = pose;
00164 }
00165 
00166 void setAt(Odom2DTrajectory& trajectory, const int& index, const Odom2D& odom)
00167 {
00168   trajectory.col(index) = odom;
00169 }
00170 
00171 Pose2D getAt(const Trajectory2D& trajectory, const int& index)
00172 {
00173   return trajectory.col(index);
00174 }
00175 
00176 Odom2D getAt(const Odom2DTrajectory& trajectory, const int& index)
00177 {
00178   return trajectory.col(index);
00179 }
00180 
00181 Pose2D getFront(const Trajectory2D& trajectory)
00182 {
00183   return trajectory.leftCols<1>();
00184 }
00185 
00186 Pose2D getBack(const Trajectory2D& trajectory)
00187 {
00188   return trajectory.rightCols<1>();
00189 }
00190 
00191 Odom2D getFront(const Odom2DTrajectory& trajectory)
00192 {
00193   return trajectory.leftCols<1>();
00194 }
00195 
00196 Odom2D getBack(const Odom2DTrajectory& trajectory)
00197 {
00198   return trajectory.rightCols<1>();
00199 }
00200 
00201 Trajectory2D getPoses(const Odom2DTrajectory& trajectory)
00202 {
00203   return trajectory.topRows<3>();
00204 }
00205 
00206 Twist2DVector getTwists(const Odom2DTrajectory& trajectory)
00207 {
00208   return trajectory.bottomRows<3>();
00209 }
00210 
00211 void setVelocityX(Odom2D& odom, const float& value)
00212 {
00213   odom(3) = value;
00214 }
00215 
00216 void setVelocityY(Odom2D& odom, const float& value)
00217 {
00218   odom(4) = value;
00219 }
00220 
00221 void setVelocityAngular(Odom2D& odom, const float& value)
00222 {
00223   odom(5) = value;
00224 }
00225 
00226 void setVelocityX(Twist2D& twist, const float& value)
00227 {
00228   twist(0) = value;
00229 }
00230 
00231 void setVelocityY(Twist2D& twist, const float& value)
00232 {
00233   twist(1) = value;
00234 }
00235 
00236 void setVelocityAngular(Twist2D& twist, const float& value)
00237 {
00238   twist(2) = value;
00239 }
00240 
00241 void setX(Odom2D& odom, const float& value)
00242 {
00243   odom(0) = value;
00244 }
00245 
00246 void setY(Odom2D& odom, const float& value)
00247 {
00248   odom(1) = value;
00249 }
00250 
00251 void setYaw(Odom2D& odom, const float& value)
00252 {
00253   odom(2) = value;
00254 }
00255 
00256 void setX(Pose2D& pose, const float& value)
00257 {
00258   pose(0) = value;
00259 }
00260 
00261 void setY(Pose2D& pose, const float& value)
00262 {
00263   pose(1) = value;
00264 }
00265 
00266 void setYaw(Pose2D& pose, const float& value)
00267 {
00268   pose(2) = value;
00269 }
00270 
00271 void setX(Position2D& position, const float& value)
00272 {
00273   position(0) = value;
00274 }
00275 
00276 void setY(Position2D& position, const float& value)
00277 {
00278   position(1) = value;
00279 }
00280 
00281 float getVelocityX(const Odom2D& odom)
00282 {
00283   return getVelocityX(getTwist(odom));
00284 }
00285 
00286 float getVelocityY(const Odom2D& odom)
00287 {
00288   return getVelocityY(getTwist(odom));
00289 }
00290 
00291 float getVelocityAngular(const Odom2D& odom)
00292 {
00293   return getVelocityAngular(getTwist(odom));
00294 }
00295 
00296 float getVelocityX(const Twist2D& twist)
00297 {
00298   return twist(0);
00299 }
00300 
00301 float getVelocityY(const Twist2D& twist)
00302 {
00303   return twist(1);
00304 }
00305 
00306 float getVelocityAngular(const Twist2D& twist)
00307 {
00308   return twist(2);
00309 }
00310 
00311 float getX(const Odom2D& odom)
00312 {
00313   return getX(getPose(odom));
00314 }
00315 float getY(const Odom2D& odom)
00316 {
00317   return getY(getPose(odom));
00318 }
00319 float getYaw(const Odom2D& odom)
00320 {
00321   return getYaw(getPose(odom));
00322 }
00323 
00324 Position2D getPosition(const Odom2D& odom)
00325 {
00326   return getPosition(getPose(odom));
00327 }
00328 
00329 Pose2D getPose(const Odom2D& odom)
00330 {
00331   return odom.head<3>();
00332 }
00333 
00334 Twist2D getTwist(const Odom2D& odom)
00335 {
00336   return odom.tail<3>();
00337 }
00338 
00339 float getX(const Pose2D& pose)
00340 {
00341   return getX(getPosition(pose));
00342 }
00343 
00344 float getY(const Pose2D& pose)
00345 {
00346   return getY(getPosition(pose));
00347 }
00348 
00349 float getYaw(const Pose2D& pose)
00350 {
00351   return pose(2);
00352 }
00353 
00354 Position2D getPosition(const Pose2D& pose)
00355 {
00356   return pose.head<2>();
00357 }
00358 
00359 float getX(const Position2D& position)
00360 {
00361   return position(0);
00362 }
00363 
00364 float getY(const Position2D& position)
00365 {
00366   return position(1);
00367 }
00368 
00369 /*****************************************************************************
00370 ** C++11 Implementations
00371 *****************************************************************************/
00372 
00373 #if defined(ECL_CXX11_FOUND)
00374 
00375   bool empty(const Trajectory2DPtr& trajectory_ptr)
00376   {
00377     return !trajectory_ptr || size(trajectory_ptr) == 0;
00378   }
00379 
00380   bool empty(const Odom2DTrajectoryPtr& trajectory_ptr)
00381   {
00382     return !trajectory_ptr || size(trajectory_ptr) == 0;
00383   }
00384 
00385   int size(const Trajectory2DPtr& trajectory)
00386   {
00387     return size(*trajectory);
00388   }
00389 
00390   int size(const Odom2DTrajectoryPtr& trajectory)
00391   {
00392     return size(*trajectory);
00393   }
00394 
00395   Trajectory2DPtr vectorToTrajectoryPtr(const std::vector<Pose2D>& vec)
00396   {
00397     Trajectory2DPtr trajectory = std::make_shared<Trajectory2D>();
00398     trajectory->resize(Eigen::NoChange, vec.size());
00399 
00400     for (int i = 0; i < vec.size(); ++i)
00401     {
00402       setAt(*trajectory, i, vec[i]);
00403     }
00404 
00405     return trajectory;
00406   }
00407 
00408   Odom2DTrajectoryPtr vectorToTrajectoryPtr(const std::vector<Odom2D>& vec)
00409   {
00410     Odom2DTrajectoryPtr trajectory = std::make_shared<Odom2DTrajectory>();
00411     trajectory->resize(Eigen::NoChange, vec.size());
00412 
00413     for (int i = 0; i < vec.size(); ++i)
00414     {
00415       setAt(*trajectory, i, vec[i]);
00416     }
00417 
00418     return trajectory;
00419   }
00420 
00421 #endif /*ECL_CXX11_FOUND*/
00422 
00423 /*****************************************************************************
00424 ** Trailers
00425 *****************************************************************************/
00426 
00427 } // namespace odometry
00428 } // namespace ecl


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Thu Jun 6 2019 21:17:52