Updated comment groupings for a few of the geometry classes.
Nick Barrash
parent
9a21cbc76e
commit
57cfb0f717
|
|
@ -42,18 +42,28 @@ public:
|
|||
Point2(double x, double y): x_(x), y_(y) {}
|
||||
Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance, prints out message if unequal*/
|
||||
/// equals with an tolerance, prints out message if unequal
|
||||
bool equals(const Point2& q, double tol = 1e-9) const;
|
||||
|
||||
// Group requirements
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. With optional derivatives */
|
||||
/// identity
|
||||
inline static Point2 identity() {
|
||||
return Point2();
|
||||
}
|
||||
|
||||
/// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2()
|
||||
inline Point2 inverse() const { return Point2(-x_, -y_); }
|
||||
|
||||
/// "Compose", just adds the coordinates of two points. With optional derivatives
|
||||
inline Point2 compose(const Point2& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
|
|
@ -62,33 +72,60 @@ public:
|
|||
return *this + p2;
|
||||
}
|
||||
|
||||
/** identity */
|
||||
inline static Point2 identity() {
|
||||
return Point2();
|
||||
}
|
||||
/** operators */
|
||||
inline Point2 operator- () const {return Point2(-x_,-y_);}
|
||||
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
|
||||
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
|
||||
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
|
||||
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
|
||||
|
||||
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
|
||||
inline Point2 inverse() const { return Point2(-x_, -y_); }
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
// Manifold requirements
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/** Size of the tangent space */
|
||||
/// Dimensionality of tangent space = 2 DOF
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** Updates a with tangent space delta */
|
||||
/// Updates a with tangent space delta
|
||||
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
|
||||
|
||||
/// Local coordinates of manifold neighborhood around current value
|
||||
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
/** Lie requirements */
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/** Exponential map around identity - just create a Point2 from a vector */
|
||||
/// Exponential map around identity - just create a Point2 from a vector
|
||||
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
|
||||
|
||||
/** Log map around identity - just return the Point2 as a vector */
|
||||
/// Log map around identity - just return the Point2 as a vector
|
||||
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
|
||||
|
||||
/// @}
|
||||
/// @name Vector Operators
|
||||
/// @{
|
||||
|
||||
/** norm of point */
|
||||
double norm() const;
|
||||
|
||||
/** creates a unit vector */
|
||||
Point2 unit() const { return *this/norm(); }
|
||||
|
||||
/** distance between two points */
|
||||
inline double dist(const Point2& p2) const {
|
||||
return (p2 - *this).norm();
|
||||
}
|
||||
|
||||
/** operators */
|
||||
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
|
||||
inline void operator *= (double s) {x_*=s;y_*=s;}
|
||||
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
|
||||
|
||||
/// @}
|
||||
|
||||
/** "Between", subtracts point coordinates */
|
||||
inline Point2 between(const Point2& p2,
|
||||
|
|
@ -106,27 +143,6 @@ public:
|
|||
/** return vectorized form (column-wise) */
|
||||
Vector vector() const { return Vector_(2, x_, y_); }
|
||||
|
||||
/** operators */
|
||||
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
|
||||
inline void operator *= (double s) {x_*=s;y_*=s;}
|
||||
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
|
||||
inline Point2 operator- () const {return Point2(-x_,-y_);}
|
||||
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
|
||||
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
|
||||
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
|
||||
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
|
||||
|
||||
/** norm of point */
|
||||
double norm() const;
|
||||
|
||||
/** creates a unit vector */
|
||||
Point2 unit() const { return *this/norm(); }
|
||||
|
||||
/** distance between two points */
|
||||
inline double dist(const Point2& p2) const {
|
||||
return (p2 - *this).norm();
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -46,29 +46,28 @@ namespace gtsam {
|
|||
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
|
||||
Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Point3& p, double tol = 1e-9) const;
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** identity */
|
||||
/// identity for group operation
|
||||
inline static Point3 identity() {
|
||||
return Point3();
|
||||
}
|
||||
|
||||
/** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
|
||||
/// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
|
||||
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
|
||||
|
||||
/** "Compose" - just adds coordinates of two points */
|
||||
/// "Compose" - just adds coordinates of two points
|
||||
inline Point3 compose(const Point3& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
|
|
@ -77,20 +76,58 @@ namespace gtsam {
|
|||
return *this + p2;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/** Exponential map at identity - just create a Point3 from x,y,z */
|
||||
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
|
||||
|
||||
/** Log map at identity - return the x,y,z of this point */
|
||||
static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
|
||||
|
||||
// Manifold requirements
|
||||
/// @}
|
||||
/// @name Vector Operators
|
||||
/// @{
|
||||
|
||||
inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
|
||||
bool operator ==(const Point3& q) const;
|
||||
Point3 operator + (const Point3& q) const;
|
||||
Point3 operator - (const Point3& q) const;
|
||||
Point3 operator * (double s) const;
|
||||
Point3 operator / (double s) const;
|
||||
|
||||
/**
|
||||
* Returns inverse retraction
|
||||
*/
|
||||
inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
|
||||
/** distance between two points */
|
||||
double dist(const Point3& p2) const {
|
||||
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
|
||||
}
|
||||
|
||||
/** dot product */
|
||||
double norm() const;
|
||||
|
||||
/** cross product @return this x q */
|
||||
Point3 cross(const Point3 &q) const;
|
||||
|
||||
/** dot product @return this * q*/
|
||||
double dot(const Point3 &q) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/** Between using the default implementation */
|
||||
inline Point3 between(const Point3& p2,
|
||||
|
|
@ -112,19 +149,6 @@ namespace gtsam {
|
|||
inline double y() const {return y_;}
|
||||
inline double z() const {return z_;}
|
||||
|
||||
/** operators */
|
||||
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
|
||||
bool operator ==(const Point3& q) const;
|
||||
Point3 operator + (const Point3& q) const;
|
||||
Point3 operator - (const Point3& q) const;
|
||||
Point3 operator * (double s) const;
|
||||
Point3 operator / (double s) const;
|
||||
|
||||
/** distance between two points */
|
||||
double dist(const Point3& p2) const {
|
||||
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
|
||||
}
|
||||
|
||||
/** add two points, add(this,q) is same as this + q */
|
||||
Point3 add (const Point3 &q,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
|
@ -133,15 +157,6 @@ namespace gtsam {
|
|||
Point3 sub (const Point3 &q,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/** cross product @return this x q */
|
||||
Point3 cross(const Point3 &q) const;
|
||||
|
||||
/** dot product @return this * q*/
|
||||
double dot(const Point3 &q) const;
|
||||
|
||||
/** dot product */
|
||||
double norm() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -154,7 +169,7 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
/** Syntactic sugar for multiplying coordinates by a scalar s*p */
|
||||
/// Syntactic sugar for multiplying coordinates by a scalar s*p
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -79,7 +79,6 @@ public:
|
|||
*this = Expmap(v);
|
||||
}
|
||||
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -94,9 +93,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Pose2 identity() {
|
||||
return Pose2();
|
||||
}
|
||||
inline static Pose2 identity() { return Pose2(); }
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
||||
|
|
@ -123,7 +120,6 @@ public:
|
|||
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
|
||||
/// Dimensionality of tangent space = 3 DOF
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
|
|
|
|||
|
|
@ -128,8 +128,6 @@ namespace gtsam {
|
|||
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** receives the point in Pose coordinates and transforms it to world coordinates */
|
||||
Point3 transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
|
|
|||
|
|
@ -109,27 +109,25 @@ namespace gtsam {
|
|||
return s_;
|
||||
}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "theta") const;
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot2& R, double tol = 1e-9) const;
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/** identity */
|
||||
inline static Rot2 identity() {
|
||||
return Rot2();
|
||||
inline static Rot2 identity() { return Rot2(); }
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
Rot2 inverse() const {
|
||||
return Rot2(c_, -s_);
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
|
|
@ -140,7 +138,41 @@ namespace gtsam {
|
|||
return *this * R1;
|
||||
}
|
||||
|
||||
/** Expmap around identity - create a rotation from an angle */
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
Rot2 operator*(const Rot2& R) const {
|
||||
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/** syntactic sugar for rotate */
|
||||
inline Point2 operator*(const Point2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
inline static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Dimensionality of the tangent space, DOF = 1
|
||||
inline size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Expmap around identity - create a rotation from an angle
|
||||
static Rot2 Expmap(const Vector& v) {
|
||||
if (zero(v))
|
||||
return (Rot2());
|
||||
|
|
@ -148,19 +180,23 @@ namespace gtsam {
|
|||
return Rot2::fromAngle(v(0));
|
||||
}
|
||||
|
||||
/** Logmap around identity - return the angle of the rotation */
|
||||
/// Logmap around identity - return the angle of the rotation
|
||||
static inline Vector Logmap(const Rot2& r) {
|
||||
return Vector_(1, r.theta());
|
||||
}
|
||||
|
||||
// Manifold requirements
|
||||
/// @}
|
||||
/// @name Vector Operators
|
||||
/// @{
|
||||
|
||||
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
|
||||
/**
|
||||
* Creates a unit vector as a Point2
|
||||
*/
|
||||
inline Point2 unit() const {
|
||||
return Point2(c_, s_);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns inverse retraction
|
||||
*/
|
||||
inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
|
||||
/// @}
|
||||
|
||||
/** Between using the default implementation */
|
||||
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
|
||||
|
|
@ -176,16 +212,6 @@ namespace gtsam {
|
|||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
Rot2 inverse() const {
|
||||
return Rot2(c_, -s_);
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
Rot2 operator*(const Rot2& R) const {
|
||||
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
@ -193,11 +219,6 @@ namespace gtsam {
|
|||
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/** syntactic sugar for rotate */
|
||||
inline Point2 operator*(const Point2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from world to rotated
|
||||
* frame = R'*p
|
||||
|
|
@ -205,12 +226,6 @@ namespace gtsam {
|
|||
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
* Creates a unit vector as a Point2
|
||||
*/
|
||||
inline Point2 unit() const {
|
||||
return Point2(c_, s_);
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -130,12 +130,76 @@ namespace gtsam {
|
|||
static Rot3M rodriguez(double wx, double wy, double wz)
|
||||
{ return rodriguez(Vector_(3,wx,wy,wz));}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot3M& p, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Rot3M identity() {
|
||||
return Rot3M();
|
||||
}
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3M compose(const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// rotate point from rotated coordinate frame to world = R*p
|
||||
inline Point3 operator*(const Point3& p) const { return rotate(p);}
|
||||
|
||||
/// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
|
||||
Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3M(
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z());
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
* using Rodriguez' formula
|
||||
*/
|
||||
static Rot3M Expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3M();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
/**
|
||||
* Log map at identity - return the canonical coordinates of this rotation
|
||||
*/
|
||||
static Vector Logmap(const Rot3M& R);
|
||||
|
||||
/// @}
|
||||
|
||||
/** return 3*3 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
|
|
@ -176,54 +240,6 @@ namespace gtsam {
|
|||
r1_.z(), r2_.z(), r3_.z()).finished());
|
||||
}
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
/** Compose two rotations i.e., R= (*this) * R2
|
||||
*/
|
||||
Rot3M compose(const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/** Exponential map at identity - create a rotation from canonical coordinates
|
||||
* using Rodriguez' formula
|
||||
*/
|
||||
static Rot3M Expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3M();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
/** identity */
|
||||
inline static Rot3M identity() {
|
||||
return Rot3M();
|
||||
}
|
||||
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector Logmap(const Rot3M& R);
|
||||
|
||||
// Manifold requirements
|
||||
|
||||
Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/**
|
||||
* Returns inverse retraction
|
||||
*/
|
||||
Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
|
||||
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
|
||||
Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3M(
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z());
|
||||
}
|
||||
|
||||
/**
|
||||
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
|
||||
*/
|
||||
|
|
@ -236,12 +252,6 @@ namespace gtsam {
|
|||
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
inline Point3 operator*(const Point3& p) const { return rotate(p);}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
|
|||
|
|
@ -127,12 +127,76 @@ namespace gtsam {
|
|||
static Rot3Q rodriguez(double wx, double wy, double wz)
|
||||
{ return rodriguez(Vector_(3,wx,wy,wz));}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot3Q& p, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Rot3Q identity() {
|
||||
return Rot3Q();
|
||||
}
|
||||
|
||||
/// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q()
|
||||
Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3Q(quaternion_.inverse());
|
||||
}
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3Q compose(const Rot3Q& R2,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// rotate point from rotated coordinate frame to world = R*p
|
||||
inline Point3 operator*(const Point3& p) const {
|
||||
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
|
||||
return Point3(r(0), r(1), r(2));
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose
|
||||
Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
* using Rodriguez' formula
|
||||
*/
|
||||
static Rot3Q Expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3Q();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
/**
|
||||
* Log map at identity - return the canonical coordinates of this rotation
|
||||
*/
|
||||
static Vector Logmap(const Rot3Q& R);
|
||||
|
||||
/// @}
|
||||
|
||||
/** return 3*3 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
|
|
@ -167,51 +231,6 @@ namespace gtsam {
|
|||
*/
|
||||
Quaternion toQuaternion() const { return quaternion_; }
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
/** Compose two rotations i.e., R= (*this) * R2
|
||||
*/
|
||||
Rot3Q compose(const Rot3Q& R2,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/** Exponential map at identity - create a rotation from canonical coordinates
|
||||
* using Rodriguez' formula
|
||||
*/
|
||||
static Rot3Q Expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3Q();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
/** identity */
|
||||
inline static Rot3Q identity() {
|
||||
return Rot3Q();
|
||||
}
|
||||
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector Logmap(const Rot3Q& R);
|
||||
|
||||
// Manifold requirements
|
||||
|
||||
Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/**
|
||||
* Returns inverse retraction
|
||||
*/
|
||||
Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
|
||||
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q()
|
||||
Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3Q(quaternion_.inverse());
|
||||
}
|
||||
|
||||
/**
|
||||
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
|
||||
*/
|
||||
|
|
@ -222,15 +241,6 @@ namespace gtsam {
|
|||
/** compose two rotations */
|
||||
Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); }
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
inline Point3 operator*(const Point3& p) const {
|
||||
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
|
||||
return Point3(r(0), r(1), r(2));
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
|
|||
|
|
@ -44,6 +44,9 @@ namespace gtsam {
|
|||
uL_(uL), uR_(uR), v_(v) {
|
||||
}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="") const;
|
||||
|
||||
|
|
@ -53,49 +56,53 @@ namespace gtsam {
|
|||
- q.v_) < tol);
|
||||
}
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/** Lie requirements */
|
||||
inline size_t dim() const { return dimension; }
|
||||
/// identity
|
||||
inline static StereoPoint2 identity() { return StereoPoint2(); }
|
||||
|
||||
/** convert to vector */
|
||||
Vector vector() const {
|
||||
return Vector_(3, uL_, uR_, v_);
|
||||
/// inverse
|
||||
inline StereoPoint2 inverse() const {
|
||||
return StereoPoint2()- (*this);
|
||||
}
|
||||
|
||||
/** add two stereo points */
|
||||
StereoPoint2 operator+(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
|
||||
}
|
||||
|
||||
/** subtract two stereo points */
|
||||
StereoPoint2 operator-(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
|
||||
}
|
||||
|
||||
/*
|
||||
* convenient function to get a Point2 from the left image
|
||||
*/
|
||||
inline Point2 point2(){
|
||||
return Point2(uL_, v_);
|
||||
}
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. */
|
||||
/// "Compose", just adds the coordinates of two points.
|
||||
inline StereoPoint2 compose(const StereoPoint2& p1) const {
|
||||
return *this + p1;
|
||||
}
|
||||
|
||||
/** identity */
|
||||
inline static StereoPoint2 identity() {
|
||||
return StereoPoint2();
|
||||
/// add two stereo points
|
||||
StereoPoint2 operator+(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
|
||||
}
|
||||
|
||||
/** inverse */
|
||||
inline StereoPoint2 inverse() const {
|
||||
return StereoPoint2()- (*this);
|
||||
/// subtract two stereo points
|
||||
StereoPoint2 operator-(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/** Exponential map around identity - just create a Point2 from a vector */
|
||||
static inline StereoPoint2 Expmap(const Vector& d) {
|
||||
return StereoPoint2(d(0), d(1), d(2));
|
||||
|
|
@ -106,14 +113,17 @@ namespace gtsam {
|
|||
return p.vector();
|
||||
}
|
||||
|
||||
// Manifold requirements
|
||||
/// @}}
|
||||
|
||||
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
/** convert to vector */
|
||||
Vector vector() const {
|
||||
return Vector_(3, uL_, uR_, v_);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns inverse retraction
|
||||
*/
|
||||
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
|
||||
/** convenient function to get a Point2 from the left image */
|
||||
inline Point2 point2(){
|
||||
return Point2(uL_, v_);
|
||||
}
|
||||
|
||||
inline StereoPoint2 between(const StereoPoint2& p2) const {
|
||||
return gtsam::between_default(*this, p2);
|
||||
|
|
|
|||
Loading…
Reference in New Issue