102 lines
2.3 KiB
C++
102 lines
2.3 KiB
C++
/**
|
|
* @file Pose2.h
|
|
* @brief 3D Pose
|
|
* @author: Frank Dellaert
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include "Point2.h"
|
|
#include "Matrix.h"
|
|
#include "Testable.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A 2D pose (x,y,theta)
|
|
*/
|
|
class Pose2: Testable<Pose2> {
|
|
|
|
private:
|
|
double x_, y_, theta_;
|
|
|
|
public:
|
|
|
|
/** default constructor = origin */
|
|
Pose2() :
|
|
x_(0), y_(0), theta_(0) {
|
|
} // default is origin
|
|
|
|
/** copy constructor */
|
|
Pose2(const Pose2& pose) :
|
|
x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
|
|
}
|
|
|
|
/**
|
|
* construct from (x,y,theta)
|
|
* @param x x oordinate
|
|
* @param y y coordinate
|
|
* @param theta angle with positive X-axis
|
|
*/
|
|
Pose2(double x, double y, double theta) :
|
|
x_(x), y_(y), theta_(theta) {
|
|
}
|
|
|
|
/** construct from rotation and translation */
|
|
Pose2(const Point2& t, double theta) :
|
|
x_(t.x()), y_(t.y()), theta_(theta) {
|
|
}
|
|
|
|
/** 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;
|
|
|
|
/** get functions for x, y, theta */
|
|
inline double x() const { return x_;}
|
|
inline double y() const { return y_;}
|
|
inline double theta() const { return theta_;}
|
|
|
|
/** return DOF, dimensionality of tangent space = 3 */
|
|
size_t dim() const {
|
|
return 3;
|
|
}
|
|
|
|
/* exponential map */
|
|
Pose2 exmap(const Vector& v) const;
|
|
|
|
/** return vectorized form (column-wise)*/
|
|
Vector vector() const;
|
|
|
|
/** rotate pose by theta */
|
|
Pose2 rotate(double theta) const;
|
|
|
|
// operators
|
|
Pose2 operator+(const Pose2& p2) const {
|
|
return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_);
|
|
}
|
|
|
|
Pose2 operator-(const Pose2& p2) const {
|
|
return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_);
|
|
}
|
|
}; // Pose2
|
|
|
|
/**
|
|
* Return point coordinates in pose coordinate frame
|
|
*/
|
|
Point2 transform_to(const Pose2& pose, const Point2& point);
|
|
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
|
|
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
|
|
|
|
/**
|
|
* Return relative pose between p1 and p2, in p1 coordinate frame
|
|
*/
|
|
Pose2 between(const Pose2& p1, const Pose2& p2);
|
|
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
|
|
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
|
|
|
|
} // namespace gtsam
|