147 lines
4.4 KiB
C++
147 lines
4.4 KiB
C++
/**
|
|
* @file Pose2.h
|
|
* @brief 2D Pose
|
|
* @author: Frank Dellaert
|
|
* @author: Richard Roberts
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <boost/optional.hpp>
|
|
#include "Matrix.h"
|
|
#include "Testable.h"
|
|
#include "Lie.h"
|
|
#include "Point2.h"
|
|
#include "Rot2.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A 2D pose (Point2,Rot2)
|
|
*/
|
|
class Pose2: Testable<Pose2>, public Lie<Pose2> {
|
|
private:
|
|
Point2 t_;
|
|
Rot2 r_;
|
|
|
|
public:
|
|
|
|
/** default constructor = origin */
|
|
Pose2() : t_(0.0, 0.0), r_(0) {} // default is origin
|
|
|
|
/** copy constructor */
|
|
Pose2(const Pose2& pose) : t_(pose.t_), r_(pose.r_) {}
|
|
|
|
/**
|
|
* construct from (x,y,theta)
|
|
* @param x x coordinate
|
|
* @param y y coordinate
|
|
* @param theta angle with positive X-axis
|
|
*/
|
|
Pose2(double x, double y, double theta) : t_(x,y), r_(theta) {}
|
|
|
|
/** construct from rotation and translation */
|
|
Pose2(double theta, const Point2& t) : t_(t), r_(theta) {}
|
|
Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) {}
|
|
|
|
/** print with optional string */
|
|
void print(const std::string& s = "") const;
|
|
|
|
/** assert equality up to a tolerance */
|
|
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
|
|
|
/** return transformation matrix */
|
|
Matrix matrix() const;
|
|
|
|
/** get functions for x, y, theta */
|
|
double x() const { return t_.x(); }
|
|
double y() const { return t_.y(); }
|
|
double theta() const { return r_.theta(); }
|
|
Point2 t() const { return t_; }
|
|
Rot2 r() const { return r_; }
|
|
|
|
static inline size_t dim() { return 3; };
|
|
|
|
}; // Pose2
|
|
|
|
|
|
/** return DOF, dimensionality of tangent space = 3 */
|
|
inline size_t dim(const Pose2&) { return 3; }
|
|
|
|
/** inverse transformation */
|
|
inline Pose2 inverse(const Pose2& pose) {
|
|
return Pose2(inverse(pose.r()),
|
|
pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); }
|
|
|
|
/** compose this transformation onto another (pre-multiply this*p1) */
|
|
inline Pose2 compose(const Pose2& p0, const Pose2& p1) {
|
|
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
|
|
|
|
/** exponential and log maps around identity */
|
|
|
|
/** Create an incremental pose from x,y,theta */
|
|
template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); }
|
|
|
|
/** Return the x,y,theta of this pose */
|
|
inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); }
|
|
|
|
/** print using member print function, currently used by LieConfig */
|
|
inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); }
|
|
|
|
/** Return point coordinates in pose coordinate frame */
|
|
inline Point2 transform_to(const Pose2& pose, const Point2& point) {
|
|
return unrotate(pose.r(), point-pose.t()); }
|
|
Point2 transform_to(const Pose2& pose, const Point2& point,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
|
|
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
|
|
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
|
|
|
|
/** Return point coordinates in global frame */
|
|
inline Point2 transform_from(const Pose2& pose, const Point2& point) {
|
|
return rotate(pose.r(), point)+pose.t(); }
|
|
|
|
/** Return relative pose between p1 and p2, in p1 coordinate frame */
|
|
Pose2 between(const Pose2& p1, const Pose2& p2,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
|
|
|
|
/** same as compose (pre-multiply this*p1) */
|
|
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); }
|
|
|
|
/** Transform a point in this coordinate frame to global coordinates,
|
|
* same as transform_from */
|
|
inline Point2 operator*(const Pose2& pose, const Point2& point) {
|
|
return transform_from(pose, point); }
|
|
|
|
/**
|
|
* Calculate bearing to a landmark
|
|
* @param pose 2D pose of robot
|
|
* @param point 2D location of landmark
|
|
* @return 2D rotation \in SO(2)
|
|
*/
|
|
Rot2 bearing(const Pose2& pose, const Point2& point);
|
|
|
|
/**
|
|
* Calculate bearing and optional derivative(s)
|
|
*/
|
|
Rot2 bearing(const Pose2& pose, const Point2& point,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
|
|
|
|
/**
|
|
* Calculate range to a landmark
|
|
* @param pose 2D pose of robot
|
|
* @param point 2D location of landmark
|
|
* @return range (double)
|
|
*/
|
|
double range(const Pose2& pose, const Point2& point);
|
|
|
|
/**
|
|
* Calculate range and optional derivative(s)
|
|
*/
|
|
double range(const Pose2& pose, const Point2& point,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
|
|
|
|
} // namespace gtsam
|
|
|