Improvements to the Lie objects, with new member/static functions in most of the geometry objects. Many of the functions that were previously global have been moved to static functions. See Lie.h for more details.
Alex Cunningham
parent
f1132359d4
commit
8e364cb34e
|
|
@ -18,10 +18,10 @@
|
|||
template class Lie<T>;
|
||||
|
||||
namespace gtsam {
|
||||
template<class T>
|
||||
size_t Lie<T>::dim() const {
|
||||
return gtsam::dim(*((T*)this));
|
||||
}
|
||||
// template<class T>
|
||||
// size_t Lie<T>::dim() const {
|
||||
// return gtsam::dim(*((T*)this));
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns Exponential mapy
|
||||
|
|
|
|||
92
base/Lie.h
92
base/Lie.h
|
|
@ -1,37 +1,55 @@
|
|||
/*
|
||||
* Lie.h
|
||||
*
|
||||
* Created on: Jan 5, 2010
|
||||
* Author: Richard Roberts
|
||||
/**
|
||||
* @file Lie.h
|
||||
* @brief Base class and basic functions for Lie types
|
||||
* @author Richard Roberts
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* These core global functions can be specialized by new Lie types
|
||||
* for better performance.
|
||||
*/
|
||||
|
||||
/* Exponential map about identity */
|
||||
template<class T>
|
||||
T expmap(const Vector& v); /* Exponential map about identity */
|
||||
T expmap(const Vector& v) { return T::Expmap(v); }
|
||||
|
||||
// The following functions may be overridden in your own class file
|
||||
// with more efficient versions if possible.
|
||||
/* Logmap (inverse exponential map) about identity */
|
||||
template<class T>
|
||||
Vector logmap(const T& p) { return T::Logmap(p); }
|
||||
|
||||
// Compute l1 s.t. l2=l1*l0
|
||||
/** Compute l1 s.t. l2=l1*l0 */
|
||||
template<class T>
|
||||
inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); }
|
||||
|
||||
// Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
|
||||
/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
|
||||
template<class T>
|
||||
inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
|
||||
|
||||
/* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
|
||||
/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
|
||||
template<class T>
|
||||
inline T expmap(const T& t, const Vector& d) { return compose(t,expmap<T>(d)); }
|
||||
|
||||
/**
|
||||
* Base class for Lie group type
|
||||
* This class uses the Curiously Recurring Template design pattern to allow
|
||||
* for static polymorphism.
|
||||
*
|
||||
* T is the derived Lie type, like Point2, Pose3, etc.
|
||||
*
|
||||
* By convention, we use capital letters to designate a static function
|
||||
*
|
||||
* FIXME: Need to find a way to check for actual implementations in T
|
||||
* so that there are no recursive function calls. This could be handled
|
||||
* by not using the same name
|
||||
*/
|
||||
template <class T>
|
||||
class Lie {
|
||||
|
|
@ -40,23 +58,65 @@ namespace gtsam {
|
|||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
size_t dim() const;
|
||||
inline size_t dim() const {
|
||||
return static_cast<const T*>(this)->dim();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns Exponential mapy
|
||||
* Returns Exponential map update of T
|
||||
* Default implementation calls global binary function
|
||||
*/
|
||||
T expmap(const Vector& v) const;
|
||||
|
||||
/** expmap around identity */
|
||||
static T Expmap(const Vector& v) {
|
||||
return T::Expmap(v);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns Log map
|
||||
* Default Implementation calls global binary function
|
||||
*/
|
||||
Vector logmap(const T& lp) const;
|
||||
|
||||
/** Logmap around identity */
|
||||
static Vector Logmap(const T& p) {
|
||||
return T::Logmap(p);
|
||||
}
|
||||
|
||||
/** compose with another object */
|
||||
inline T compose(const T& p) const {
|
||||
return static_cast<const T*>(this)->compose(p);
|
||||
}
|
||||
|
||||
/** invert the object and yield a new one */
|
||||
inline T inverse() const {
|
||||
return static_cast<const T*>(this)->inverse();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** get the dimension of an object with a global function */
|
||||
template<class T>
|
||||
inline size_t dim(const T& object) {
|
||||
return object.dim();
|
||||
}
|
||||
|
||||
/** compose two Lie types */
|
||||
template<class T>
|
||||
inline T compose(const T& p1, const T& p2) {
|
||||
return p1.compose(p2);
|
||||
}
|
||||
|
||||
/** invert an object */
|
||||
template<class T>
|
||||
inline T inverse(const T& p) {
|
||||
return p.inverse();
|
||||
}
|
||||
|
||||
/** Call print on the object */
|
||||
template<class T>
|
||||
inline void print_(const T& object, const std::string& s = "") {
|
||||
inline void print(const T& object, const std::string& s = "") {
|
||||
object.print(s);
|
||||
}
|
||||
|
||||
|
|
@ -100,11 +160,11 @@ namespace gtsam {
|
|||
inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||||
|
||||
/**
|
||||
* Three term approximation of the BakerÐCampbellÐHausdorff formula
|
||||
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
|
||||
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
|
||||
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
|
||||
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
|
||||
* http://en.wikipedia.org/wiki/BakerÐCampbellÐHausdorff_formula
|
||||
* http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
|
||||
*/
|
||||
template<class T>
|
||||
T BCH(const T& X, const T& Y) {
|
||||
|
|
|
|||
|
|
@ -22,8 +22,7 @@ namespace gtsam {
|
|||
class Point2: Testable<Point2>, public Lie<Point2> {
|
||||
public:
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
static inline size_t dim() {return 2;}
|
||||
|
||||
static const size_t dimension = 2;
|
||||
private:
|
||||
double x_, y_;
|
||||
|
||||
|
|
@ -33,12 +32,32 @@ namespace gtsam {
|
|||
Point2(double x, double y): x_(x), y_(y) {}
|
||||
Point2(const Vector& v) : x_(v(0)), y_(v(1)) {}
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance, prints out message if unequal*/
|
||||
bool equals(const Point2& q, double tol = 1e-9) const;
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** Size of the tangent space of the Lie type */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. */
|
||||
Point2 compose(const Point2& p1) const { return *this+p1; }
|
||||
|
||||
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
|
||||
Point2 inverse() const { return Point2(-x_, -y_); }
|
||||
|
||||
/** 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 */
|
||||
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
|
||||
|
||||
/** get functions for x, y */
|
||||
double x() const {return x_;}
|
||||
double y() const {return y_;}
|
||||
|
|
@ -75,20 +94,7 @@ namespace gtsam {
|
|||
|
||||
/** Lie group functions */
|
||||
|
||||
/** Global print calls member function */
|
||||
inline void print(const Point2& p, const std::string& s = "Point2") { p.print(s); }
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim(const Point2& obj) { return 2; }
|
||||
|
||||
/** Exponential map around identity - just create a Point2 from a vector */
|
||||
template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); }
|
||||
|
||||
/** Log map around identity - just return the Point2 as a vector */
|
||||
inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. */
|
||||
inline Point2 compose(const Point2& p1, const Point2& p2) { return p1+p2; }
|
||||
inline Point2 compose(const Point2& p1, const Point2& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
|
||||
if(H1) *H1 = eye(2);
|
||||
|
|
@ -113,9 +119,6 @@ namespace gtsam {
|
|||
return between(p1, p2);
|
||||
}
|
||||
|
||||
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
|
||||
inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); }
|
||||
|
||||
/** multiply with scalar */
|
||||
inline Point2 operator*(double s, const Point2& p) {return p*s;}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@ namespace gtsam {
|
|||
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
void Point3::print(const std::string& s) const {
|
||||
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,7 +22,8 @@ namespace gtsam {
|
|||
class Point3: Testable<Point3>, public Lie<Point3> {
|
||||
public:
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
static inline size_t dim() {return 3;}
|
||||
static const size_t dimension = 3;
|
||||
|
||||
private:
|
||||
double x_, y_, z_;
|
||||
|
||||
|
|
@ -38,6 +39,26 @@ namespace gtsam {
|
|||
/** 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; }
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** "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 */
|
||||
inline Point3 compose(const Point3& p1) const { return *this+p1; }
|
||||
|
||||
/** 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()); }
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
Vector vector() const {
|
||||
//double r[] = { x_, y_, z_ };
|
||||
|
|
@ -80,22 +101,7 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
|
||||
/** Global print calls member function */
|
||||
inline void print(const Point3& p, const std::string& s) { p.print(s); }
|
||||
inline void print(const Point3& p) { p.print(); }
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim(const Point3&) { return 3; }
|
||||
|
||||
/** Exponential map at identity - just create a Point3 from x,y,z */
|
||||
template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); }
|
||||
|
||||
/** Log map at identity - return the x,y,z of this point */
|
||||
inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
|
||||
|
||||
/** "Compose" - just adds coordinates of two points */
|
||||
inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; }
|
||||
inline Matrix Dcompose1(const Point3& p1, const Point3& p0) {
|
||||
return Matrix_(3,3,
|
||||
1.0, 0.0, 0.0,
|
||||
|
|
@ -109,10 +115,6 @@ namespace gtsam {
|
|||
0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
|
||||
inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); }
|
||||
|
||||
|
||||
/** Syntactic sugar for multiplying coordinates by a scalar s*p */
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
|
|
|
|||
|
|
@ -68,13 +68,15 @@ namespace gtsam {
|
|||
|
||||
#else
|
||||
|
||||
template<> Pose2 expmap(const Vector& v) {
|
||||
return Pose2(v[0], v[1], v[2]);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::Expmap(const Vector& v) {
|
||||
return Pose2(v[0], v[1], v[2]);
|
||||
}
|
||||
|
||||
Vector logmap(const Pose2& p) {
|
||||
return Vector_(3, p.x(), p.y(), p.theta());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::Logmap(const Pose2& p) {
|
||||
return Vector_(3, p.x(), p.y(), p.theta());
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -93,15 +95,15 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 inverse(const Pose2& pose) {
|
||||
const Rot2& R = pose.r();
|
||||
const Point2& t = pose.t();
|
||||
return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y())));
|
||||
}
|
||||
Pose2 Pose2::inverse() const {
|
||||
const Rot2& R = r_;
|
||||
const Point2& t = t_;
|
||||
return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y())));
|
||||
}
|
||||
|
||||
Matrix Dinverse(const Pose2& pose) {
|
||||
return -AdjointMap(pose);
|
||||
}
|
||||
Matrix Dinverse(const Pose2& pose) {
|
||||
return -AdjointMap(pose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
|
|
@ -187,7 +189,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Rot2 bearing(const Pose2& pose, const Point2& point) {
|
||||
Point2 d = transform_to(pose, point);
|
||||
return relativeBearing(d);
|
||||
return Rot2::relativeBearing(d);
|
||||
}
|
||||
|
||||
Rot2 bearing(const Pose2& pose, const Point2& point,
|
||||
|
|
@ -195,7 +197,7 @@ namespace gtsam {
|
|||
if (!H1 && !H2) return bearing(pose, point);
|
||||
Point2 d = transform_to(pose, point, H1, H2);
|
||||
Matrix D_result_d;
|
||||
Rot2 result = relativeBearing(d, D_result_d);
|
||||
Rot2 result = Rot2::relativeBearing(d, D_result_d);
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
return result;
|
||||
|
|
|
|||
|
|
@ -22,6 +22,9 @@ namespace gtsam {
|
|||
* A 2D pose (Point2,Rot2)
|
||||
*/
|
||||
class Pose2: Testable<Pose2>, public Lie<Pose2> {
|
||||
|
||||
public:
|
||||
static const size_t dimension = 3;
|
||||
private:
|
||||
Rot2 r_;
|
||||
Point2 t_;
|
||||
|
|
@ -60,6 +63,34 @@ namespace gtsam {
|
|||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
||||
|
||||
inline Pose2 operator*(const Pose2& p2) const {
|
||||
return Pose2(r_*p2.r(), t_ + r_*p2.t());
|
||||
}
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space = 3 */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** inverse of a pose */
|
||||
Pose2 inverse() const;
|
||||
|
||||
/** compose with another pose */
|
||||
inline Pose2 compose(const Pose2& p) const { return *this * p; }
|
||||
|
||||
/**
|
||||
* Exponential map from se(2) to SE(2)
|
||||
*/
|
||||
static Pose2 Expmap(const Vector& v);
|
||||
|
||||
/**
|
||||
* Inverse of exponential map, from SE(2) to se(2)
|
||||
*/
|
||||
static Vector Logmap(const Pose2& p);
|
||||
|
||||
/** return transformation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
|
|
@ -71,8 +102,6 @@ namespace gtsam {
|
|||
inline const Point2& t() const { return t_; }
|
||||
inline const Rot2& r() const { return r_; }
|
||||
|
||||
static inline size_t dim() { return 3; };
|
||||
|
||||
private:
|
||||
// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -83,22 +112,6 @@ namespace gtsam {
|
|||
}
|
||||
}; // Pose2
|
||||
|
||||
/** print using member print function, currently used by LieConfig */
|
||||
inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); }
|
||||
|
||||
/** return DOF, dimensionality of tangent space = 3 */
|
||||
inline size_t dim(const Pose2&) { return 3; }
|
||||
|
||||
/**
|
||||
* Exponential map from se(2) to SE(2)
|
||||
*/
|
||||
template<> Pose2 expmap(const Vector& v);
|
||||
|
||||
/**
|
||||
* Inverse of exponential map, from SE(2) to se(2)
|
||||
*/
|
||||
Vector logmap(const Pose2& p);
|
||||
|
||||
/**
|
||||
* Calculate Adjoint map
|
||||
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
|
|
@ -128,15 +141,11 @@ namespace gtsam {
|
|||
/**
|
||||
* inverse transformation
|
||||
*/
|
||||
Pose2 inverse(const Pose2& pose);
|
||||
Matrix Dinverse(const Pose2& pose);
|
||||
|
||||
/**
|
||||
* compose this transformation onto another (first p1 and then p2)
|
||||
*/
|
||||
inline Pose2 operator*(const Pose2& p1, const Pose2& p2) {
|
||||
return Pose2(p1.r()*p2.r(), p1.t() + p1.r()*p2.t()); }
|
||||
inline Pose2 compose(const Pose2& p1, const Pose2& p2) { return p1*p2; }
|
||||
Pose2 compose(const Pose2& p1, const Pose2& p2,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2 = boost::none);
|
||||
|
|
|
|||
|
|
@ -89,17 +89,19 @@ namespace gtsam {
|
|||
|
||||
#else
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* incorrect versions for which we know how to compute derivatives */
|
||||
template<> Pose3 expmap(const Vector& d) {
|
||||
Pose3 Pose3::Expmap(const Vector& d) {
|
||||
Vector w = sub(d, 0,3);
|
||||
Vector u = sub(d, 3,6);
|
||||
return Pose3(expmap<Rot3> (w), expmap<Point3> (u));
|
||||
return Pose3(Rot3::Expmap(w), Point3::Expmap(u));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the translation and canonical rotation
|
||||
// coordinates of a pose.
|
||||
Vector logmap(const Pose3& p) {
|
||||
const Vector w = logmap(p.rotation()), u = logmap(p.translation());
|
||||
Vector Pose3::Logmap(const Pose3& p) {
|
||||
const Vector w = Rot3::Logmap(p.rotation()), u = Point3::Logmap(p.translation());
|
||||
return concatVectors(2, &w, &u);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@ namespace gtsam {
|
|||
|
||||
/** A 3D pose (R,t) : (Rot3,Point3) */
|
||||
class Pose3 : Testable<Pose3>, public Lie<Pose3> {
|
||||
public:
|
||||
static const size_t dimension = 6;
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
|
|
@ -59,21 +62,37 @@ namespace gtsam {
|
|||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose3& pose, double tol = 1e-9) const;
|
||||
|
||||
/** Find the inverse pose s.t. inverse(p)*p = Pose3() */
|
||||
inline Pose3 inverse() const {
|
||||
const Rot3 Rt(R_.inverse());
|
||||
return Pose3(Rt, - (Rt*t_));
|
||||
}
|
||||
|
||||
/** Compose two poses */
|
||||
inline Pose3 operator*(const Pose3& T) const {
|
||||
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
||||
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
||||
}
|
||||
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
||||
/** get the dimension by the type */
|
||||
inline static size_t dim() { return 6; }
|
||||
/** 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; }
|
||||
|
||||
/** Find the inverse pose s.t. inverse(p)*p = Pose3() */
|
||||
inline Pose3 inverse() const {
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
|
||||
/** composes two poses */
|
||||
inline Pose3 compose(const Pose3& t) const { return *this * t; }
|
||||
|
||||
/** Exponential map at identity - create a pose with a translation and
|
||||
* rotation (in canonical coordinates). */
|
||||
static Pose3 Expmap(const Vector& v);
|
||||
|
||||
/** Log map at identity - return the translation and canonical rotation
|
||||
* coordinates of a pose. */
|
||||
static Vector Logmap(const Pose3& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -85,29 +104,6 @@ namespace gtsam {
|
|||
}
|
||||
}; // Pose3 class
|
||||
|
||||
/** global print */
|
||||
inline void print(const Pose3& p, const std::string& s = "") { p.print(s);}
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim(const Pose3&) { return 6; }
|
||||
|
||||
/** Compose two poses */
|
||||
inline Pose3 compose(const Pose3& p0, const Pose3& p1) { return p0*p1;}
|
||||
|
||||
/** Find the inverse pose s.t. inverse(p)*p = Pose3() */
|
||||
inline Pose3 inverse(const Pose3& p) {
|
||||
Rot3 Rt = inverse(p.rotation());
|
||||
return Pose3(Rt, Rt*(-p.translation()));
|
||||
}
|
||||
|
||||
/** Exponential map at identity - create a pose with a translation and
|
||||
* rotation (in canonical coordinates). */
|
||||
template<> Pose3 expmap(const Vector& d);
|
||||
|
||||
/** Log map at identity - return the translation and canonical rotation
|
||||
* coordinates of a pose. */
|
||||
Vector logmap(const Pose3& p);
|
||||
|
||||
/** Exponential map around another pose */
|
||||
Pose3 expmap(const Pose3& T, const Vector& d);
|
||||
|
||||
|
|
|
|||
|
|
@ -93,13 +93,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 relativeBearing(const Point2& d) {
|
||||
Rot2 Rot2::relativeBearing(const Point2& d) {
|
||||
double n = d.norm();
|
||||
return Rot2::fromCosSin(d.x() / n, d.y() / n);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
|
||||
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
|
||||
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
|
||||
if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
|
||||
return Rot2::fromCosSin(x / n, y / n);
|
||||
|
|
|
|||
|
|
@ -21,6 +21,10 @@ namespace gtsam {
|
|||
*/
|
||||
class Rot2: Testable<Rot2>, public Lie<Rot2> {
|
||||
|
||||
public:
|
||||
/** get the dimension by the type */
|
||||
static const size_t dimension = 1;
|
||||
|
||||
private:
|
||||
|
||||
/** we store cos(theta) and sin(theta) */
|
||||
|
|
@ -52,6 +56,22 @@ namespace gtsam {
|
|||
static Rot2 fromCosSin(double c, double s);
|
||||
|
||||
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
||||
|
||||
/**
|
||||
* Named constructor
|
||||
* Calculate relative bearing to a landmark in local coordinate frame
|
||||
* @param point 2D location of landmark
|
||||
* @param H optional reference for Jacobian
|
||||
* @return 2D rotation \in SO(2)
|
||||
*/
|
||||
static Rot2 relativeBearing(const Point2& d);
|
||||
|
||||
/**
|
||||
* Named constructor with derivative
|
||||
* Calculate relative bearing and optional derivative
|
||||
*/
|
||||
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H);
|
||||
|
||||
static Rot2 atan2(double y, double x);
|
||||
|
||||
/** return angle (RADIANS) */
|
||||
|
|
@ -75,6 +95,28 @@ namespace gtsam {
|
|||
/** 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; }
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
inline Rot2 compose(const Rot2& R1) const { return *this * R1;}
|
||||
|
||||
/** Expmap around identity - create a rotation from an angle */
|
||||
static Rot2 Expmap(const Vector& v) {
|
||||
if (zero(v)) return (Rot2());
|
||||
else return Rot2::fromAngle(v(0));
|
||||
}
|
||||
|
||||
/** Logmap around identity - return the angle of the rotation */
|
||||
static inline Vector Logmap(const Rot2& r) {
|
||||
return Vector_(1, r.theta());
|
||||
}
|
||||
|
||||
/** return 2*2 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
|
|
@ -95,9 +137,6 @@ namespace gtsam {
|
|||
/** rotate from world to rotated = R'*p */
|
||||
Point2 unrotate(const Point2& p) const;
|
||||
|
||||
/** get the dimension by the type */
|
||||
static inline size_t dim() { return 1; };
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -116,29 +155,6 @@ namespace gtsam {
|
|||
|
||||
// Lie group functions
|
||||
|
||||
/** Global print calls member function */
|
||||
inline void print(const Rot2& r, const std::string& s = "") { r.print(s); }
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim(const Rot2&) { return 1; }
|
||||
|
||||
/** Expmap around identity - create a rotation from an angle */
|
||||
template<> inline Rot2 expmap(const Vector& v) {
|
||||
if (zero(v)) return (Rot2());
|
||||
else return Rot2::fromAngle(v(0));
|
||||
}
|
||||
|
||||
/** Logmap around identity - return the angle of the rotation */
|
||||
inline Vector logmap(const Rot2& r) {
|
||||
return Vector_(1, r.theta());
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
inline Rot2 compose(const Rot2& R1, const Rot2& R2) { return R1*R2;}
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
inline Rot2 inverse(const Rot2& R) { return R.inverse();}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
@ -154,19 +170,6 @@ namespace gtsam {
|
|||
Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* Calculate relative bearing to a landmark in local coordinate frame
|
||||
* @param point 2D location of landmark
|
||||
* @param H optional reference for Jacobian
|
||||
* @return 2D rotation \in SO(2)
|
||||
*/
|
||||
Rot2 relativeBearing(const Point2& d);
|
||||
|
||||
/**
|
||||
* Calculate relative bearing and optional derivative
|
||||
*/
|
||||
Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H);
|
||||
|
||||
} // gtsam
|
||||
|
||||
#endif /* ROT2_H_ */
|
||||
|
|
|
|||
|
|
@ -68,6 +68,35 @@ namespace gtsam {
|
|||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3( c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = norm_2(w);
|
||||
if (t < 1e-5) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
|
|
@ -113,7 +142,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
inline Vector logmap(const Rot3& R) {
|
||||
inline Vector Rot3::Logmap(const Rot3& R) {
|
||||
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc.
|
||||
return zero(3);
|
||||
|
|
@ -136,35 +165,6 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3( c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 rodriguez(const Vector& w) {
|
||||
double t = norm_2(w);
|
||||
if (t < 1e-5) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Drotate1(const Rot3& R, const Point3& p) {
|
||||
return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
|
|
|
|||
101
geometry/Rot3.h
101
geometry/Rot3.h
|
|
@ -19,6 +19,9 @@ namespace gtsam {
|
|||
|
||||
/* 3D Rotation */
|
||||
class Rot3: Testable<Rot3>, public Lie<Rot3> {
|
||||
public:
|
||||
static const size_t dimension = 3;
|
||||
|
||||
private:
|
||||
/** we store columns ! */
|
||||
Point3 r1_, r2_, r3_;
|
||||
|
|
@ -76,6 +79,31 @@ namespace gtsam {
|
|||
static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
|
||||
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param w is the rotation axis, unit length
|
||||
* @param theta rotation angle
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
static Rot3 rodriguez(const Vector& w, double theta);
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param v a vector of incremental roll,pitch,yaw
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
static Rot3 rodriguez(const Vector& v);
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param wx
|
||||
* @param wy
|
||||
* @param wz
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
static inline Rot3 rodriguez(double wx, double wy, double wz)
|
||||
{ return rodriguez(Vector_(3,wx,wy,wz));}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
|
||||
|
|
@ -106,8 +134,27 @@ namespace gtsam {
|
|||
*/
|
||||
Vector ypr() const;
|
||||
|
||||
/** get the dimension by the type */
|
||||
static inline size_t dim() { return 3; };
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** Compose two rotations */
|
||||
inline Rot3 compose(const Rot3& R1) const { return *this * R1;}
|
||||
|
||||
/** Exponential map at identity - create a rotation from canonical coordinates
|
||||
* using Rodriguez' formula
|
||||
*/
|
||||
static Rot3 Expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector Logmap(const Rot3& R);
|
||||
|
||||
/* Find the inverse rotation R^T s.t. inverse(R)*R = I */
|
||||
inline Rot3 inverse() const {
|
||||
|
|
@ -142,55 +189,7 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
/** Global print calls member function */
|
||||
inline void print(const Rot3& r, std::string& s) { r.print(s); }
|
||||
inline void print(const Rot3& r) { r.print(); }
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param w is the rotation axis, unit length
|
||||
* @param theta rotation angle
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
Rot3 rodriguez(const Vector& w, double theta);
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param v a vector of incremental roll,pitch,yaw
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
Rot3 rodriguez(const Vector& v);
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
* @param wx
|
||||
* @param wy
|
||||
* @param wz
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
inline Rot3 rodriguez(double wx, double wy, double wz)
|
||||
{ return rodriguez(Vector_(3,wx,wy,wz));}
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim(const Rot3&) { return 3; }
|
||||
|
||||
// Exponential map at identity - create a rotation from canonical coordinates
|
||||
// using Rodriguez' formula
|
||||
template<> inline Rot3 expmap(const Vector& v) {
|
||||
if(zero(v)) return Rot3();
|
||||
else return rodriguez(v);
|
||||
}
|
||||
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector logmap(const Rot3& R);
|
||||
|
||||
// Compose two rotations
|
||||
inline Rot3 compose(const Rot3& R1, const Rot3& R2) { return R1*R2;}
|
||||
|
||||
// Find the inverse rotation R^T s.t. inverse(R)*R = Rot3()
|
||||
inline Rot3 inverse(const Rot3& R) { return R.inverse();}
|
||||
|
||||
// and its derivative
|
||||
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
|
||||
inline Matrix Dinverse(Rot3 R) { return -R.matrix();}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@ namespace gtsam {
|
|||
* A 2D stereo point, v will be same for rectified images
|
||||
*/
|
||||
class StereoPoint2: Testable<StereoPoint2>, Lie<StereoPoint2> {
|
||||
public:
|
||||
static const size_t dimension = 3;
|
||||
private:
|
||||
double uL_, uR_, v_;
|
||||
|
||||
|
|
@ -47,6 +49,12 @@ namespace gtsam {
|
|||
- q.v_) < tol);
|
||||
}
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/** Lie requirements */
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/** convert to vector */
|
||||
Vector vector() const {
|
||||
return Vector_(3, uL_, uR_, v_);
|
||||
|
|
@ -68,34 +76,28 @@ namespace gtsam {
|
|||
inline Point2 point2(){
|
||||
return Point2(uL_, v_);
|
||||
}
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. */
|
||||
inline StereoPoint2 compose(const StereoPoint2& p1) const {
|
||||
return *this + p1;
|
||||
}
|
||||
|
||||
/** inverse */
|
||||
inline StereoPoint2 inverse() const {
|
||||
return StereoPoint2()- (*this);
|
||||
}
|
||||
|
||||
/** 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));
|
||||
}
|
||||
|
||||
/** Log map around identity - just return the Point2 as a vector */
|
||||
static inline Vector Logmap(const StereoPoint2& p) {
|
||||
return p.vector();
|
||||
}
|
||||
};
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim(const StereoPoint2& obj) {
|
||||
return 3;
|
||||
}
|
||||
|
||||
/** Exponential map around identity - just create a Point2 from a vector */
|
||||
template<> inline StereoPoint2 expmap(const Vector& d) {
|
||||
return StereoPoint2(d(0), d(1), d(2));
|
||||
}
|
||||
|
||||
/** Log map around identity - just return the Point2 as a vector */
|
||||
inline Vector logmap(const StereoPoint2& p) {
|
||||
return p.vector();
|
||||
}
|
||||
|
||||
/** "Compose", just adds the coordinates of two points. */
|
||||
inline StereoPoint2 compose(const StereoPoint2& p1, const StereoPoint2& p0) {
|
||||
return p0 + p1;
|
||||
}
|
||||
|
||||
/** inverse */
|
||||
inline StereoPoint2 inverse(const StereoPoint2& p) {
|
||||
return StereoPoint2()-p;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif /* STEREOPOINT2_H_ */
|
||||
|
|
|
|||
|
|
@ -12,10 +12,10 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
static Point3 P(0.2,0.7,-2);
|
||||
static Rot3 R = rodriguez(0.3,0,0);
|
||||
static Rot3 R = Rot3::rodriguez(0.3,0,0);
|
||||
static Pose3 T(R,Point3(3.5,-8.2,4.2));
|
||||
static Pose3 T2(rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2));
|
||||
static Pose3 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3));
|
||||
static Pose3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2));
|
||||
static Pose3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, equals)
|
||||
|
|
@ -43,12 +43,13 @@ TEST( Pose3, expmap_a)
|
|||
CHECK(assert_equal(Pose3(R, P),expmap(id,v),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_b)
|
||||
{
|
||||
Pose3 p1(Rot3(), Point3(100, 0, 0));
|
||||
Pose3 p2 = expmap(p1, Vector_(6,
|
||||
0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
|
||||
Pose3 expected(rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
|
||||
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
|
||||
CHECK(assert_equal(expected, p2));
|
||||
}
|
||||
|
||||
|
|
@ -178,6 +179,7 @@ TEST( Pose3, compose2 )
|
|||
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
|
||||
CHECK(assert_equal(numericalH2,actualDcompose2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, inverse)
|
||||
{
|
||||
|
|
@ -193,7 +195,7 @@ TEST( Pose3, inverse)
|
|||
/* ************************************************************************* */
|
||||
TEST( Pose3, inverseDerivatives2)
|
||||
{
|
||||
Rot3 R = rodriguez(0.3,0.4,-0.5);
|
||||
Rot3 R = Rot3::rodriguez(0.3,0.4,-0.5);
|
||||
Point3 t(3.5,-8.2,4.2);
|
||||
Pose3 T(R,t);
|
||||
|
||||
|
|
@ -298,25 +300,25 @@ TEST( Pose3, transform_to_translate)
|
|||
{
|
||||
Point3 actual = transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.));
|
||||
Point3 expected(9.,18.,27.);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, transform_to_rotate)
|
||||
{
|
||||
Pose3 transform(rodriguez(0,0,-1.570796), Point3());
|
||||
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3());
|
||||
Point3 actual = transform_to(transform, Point3(2,1,10));
|
||||
Point3 expected(-1,2,10);
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, transform_to)
|
||||
{
|
||||
Pose3 transform(rodriguez(0,0,-1.570796), Point3(2,4, 0));
|
||||
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0));
|
||||
Point3 actual = transform_to(transform, Point3(3,2,10));
|
||||
Point3 expected(2,1,10);
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -355,10 +357,10 @@ TEST( Pose3, transformPose_to_itself)
|
|||
TEST( Pose3, transformPose_to_translation)
|
||||
{
|
||||
// transform translation only
|
||||
Rot3 r = rodriguez(-1.570796,0,0);
|
||||
Pose3 pose2(r, Point3(21.,32.,13.));
|
||||
Rot3 r = Rot3::rodriguez(-1.570796,0,0);
|
||||
Pose3 pose2(r, Point3(21.,32.,13.));
|
||||
Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
|
||||
Pose3 expected(r, Point3(20.,30.,10.));
|
||||
Pose3 expected(r, Point3(20.,30.,10.));
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
}
|
||||
|
||||
|
|
@ -366,25 +368,25 @@ TEST( Pose3, transformPose_to_translation)
|
|||
TEST( Pose3, transformPose_to_simple_rotate)
|
||||
{
|
||||
// transform translation only
|
||||
Rot3 r = rodriguez(0,0,-1.570796);
|
||||
Pose3 pose2(r, Point3(21.,32.,13.));
|
||||
Rot3 r = Rot3::rodriguez(0,0,-1.570796);
|
||||
Pose3 pose2(r, Point3(21.,32.,13.));
|
||||
Pose3 transform(r, Point3(1,2,3));
|
||||
Pose3 actual = pose2.transform_to(transform);
|
||||
Pose3 expected(Rot3(), Point3(-30.,20.,10.));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
Pose3 expected(Rot3(), Point3(-30.,20.,10.));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, transformPose_to)
|
||||
{
|
||||
// transform to
|
||||
Rot3 r = rodriguez(0,0,-1.570796); //-90 degree yaw
|
||||
Rot3 r2 = rodriguez(0,0,0.698131701); //40 degree yaw
|
||||
Pose3 pose2(r2, Point3(21.,32.,13.));
|
||||
Rot3 r = Rot3::rodriguez(0,0,-1.570796); //-90 degree yaw
|
||||
Rot3 r2 = Rot3::rodriguez(0,0,0.698131701); //40 degree yaw
|
||||
Pose3 pose2(r2, Point3(21.,32.,13.));
|
||||
Pose3 transform(r, Point3(1,2,3));
|
||||
Pose3 actual = pose2.transform_to(transform);
|
||||
Pose3 expected(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -95,19 +95,19 @@ TEST( Rot2, relativeBearing )
|
|||
Matrix expectedH, actualH;
|
||||
|
||||
// establish relativeBearing is indeed zero
|
||||
Rot2 actual1 = relativeBearing(l1, actualH);
|
||||
Rot2 actual1 = Rot2::relativeBearing(l1, actualH);
|
||||
CHECK(assert_equal(Rot2(),actual1));
|
||||
|
||||
// Check numerical derivative
|
||||
expectedH = numericalDerivative11(relativeBearing, l1, 1e-5);
|
||||
expectedH = numericalDerivative11(Rot2::relativeBearing, l1, 1e-5);
|
||||
CHECK(assert_equal(expectedH,actualH));
|
||||
|
||||
// establish relativeBearing is indeed 45 degrees
|
||||
Rot2 actual2 = relativeBearing(l2, actualH);
|
||||
Rot2 actual2 = Rot2::relativeBearing(l2, actualH);
|
||||
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2));
|
||||
|
||||
// Check numerical derivative
|
||||
expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
|
||||
expectedH = numericalDerivative11(Rot2::relativeBearing, l2, 1e-5);
|
||||
CHECK(assert_equal(expectedH,actualH));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
Rot3 R = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Point3 P(0.2, 0.7, -2.0);
|
||||
double error = 1e-9, epsilon = 0.001;
|
||||
|
||||
|
|
@ -80,7 +80,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
|
|||
/* ************************************************************************* */
|
||||
TEST( Rot3, rodriguez)
|
||||
{
|
||||
Rot3 R1 = rodriguez(epsilon, 0, 0);
|
||||
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
|
||||
Vector w = Vector_(3, epsilon, 0., 0.);
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
|
|
@ -91,7 +91,7 @@ TEST( Rot3, rodriguez2)
|
|||
{
|
||||
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
|
||||
double angle = 3.14 / 4.0;
|
||||
Rot3 actual = rodriguez(axis, angle);
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
Rot3 expected(0.707388, 0, 0.706825,
|
||||
0, 1, 0,
|
||||
-0.706825, 0, 0.707388);
|
||||
|
|
@ -102,7 +102,7 @@ TEST( Rot3, rodriguez2)
|
|||
TEST( Rot3, rodriguez3)
|
||||
{
|
||||
Vector w = Vector_(3, 0.1, 0.2, 0.3);
|
||||
Rot3 R1 = rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
}
|
||||
|
|
@ -112,7 +112,7 @@ TEST( Rot3, rodriguez4)
|
|||
{
|
||||
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
|
||||
double angle = M_PI_2;
|
||||
Rot3 actual = rodriguez(axis, angle);
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
double c=cos(angle),s=sin(angle);
|
||||
Rot3 expected(c,-s, 0,
|
||||
s, c, 0,
|
||||
|
|
@ -133,43 +133,43 @@ TEST( Rot3, expmap)
|
|||
TEST(Rot3, log)
|
||||
{
|
||||
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
|
||||
Rot3 R1 = rodriguez(w1);
|
||||
Rot3 R1 = Rot3::rodriguez(w1);
|
||||
CHECK(assert_equal(w1, logmap(R1)));
|
||||
|
||||
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
|
||||
Rot3 R2 = rodriguez(w2);
|
||||
Rot3 R2 = Rot3::rodriguez(w2);
|
||||
CHECK(assert_equal(w2, logmap(R2)));
|
||||
|
||||
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
|
||||
Rot3 R3 = rodriguez(w3);
|
||||
Rot3 R3 = Rot3::rodriguez(w3);
|
||||
CHECK(assert_equal(w3, logmap(R3)));
|
||||
|
||||
Vector w = Vector_(3, 0.1, 0.4, 0.2);
|
||||
Rot3 R = rodriguez(w);
|
||||
Rot3 R = Rot3::rodriguez(w);
|
||||
CHECK(assert_equal(w, logmap(R)));
|
||||
|
||||
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
|
||||
Rot3 R5 = rodriguez(w5);
|
||||
Rot3 R5 = Rot3::rodriguez(w5);
|
||||
CHECK(assert_equal(w5, logmap(R5)));
|
||||
|
||||
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
|
||||
Rot3 R6 = rodriguez(w6);
|
||||
Rot3 R6 = Rot3::rodriguez(w6);
|
||||
CHECK(assert_equal(w6, logmap(R6)));
|
||||
|
||||
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
|
||||
Rot3 R7 = rodriguez(w7);
|
||||
Rot3 R7 = Rot3::rodriguez(w7);
|
||||
CHECK(assert_equal(w7, logmap(R7)));
|
||||
|
||||
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
|
||||
Rot3 R8 = rodriguez(w8);
|
||||
Rot3 R8 = Rot3::rodriguez(w8);
|
||||
CHECK(assert_equal(w8, logmap(R8)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold)
|
||||
{
|
||||
Rot3 gR1 = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
|
|
@ -265,8 +265,8 @@ TEST( Rot3, unrotate)
|
|||
/* ************************************************************************* */
|
||||
TEST( Rot3, compose )
|
||||
{
|
||||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3 expected = R1 * R2;
|
||||
Rot3 actual = compose(R1, R2);
|
||||
|
|
@ -287,7 +287,7 @@ TEST( Rot3, compose )
|
|||
|
||||
TEST( Rot3, inverse )
|
||||
{
|
||||
Rot3 R = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
|
||||
Rot3 I;
|
||||
CHECK(assert_equal(I,R*inverse(R)));
|
||||
|
|
@ -301,13 +301,13 @@ TEST( Rot3, inverse )
|
|||
/* ************************************************************************* */
|
||||
TEST( Rot3, between )
|
||||
{
|
||||
Rot3 R = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 origin;
|
||||
CHECK(assert_equal(R, between(origin,R)));
|
||||
CHECK(assert_equal(inverse(R), between(R,origin)));
|
||||
|
||||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3 expected = inverse(R1) * R2;
|
||||
Rot3 actual = between(R1, R2);
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ int main()
|
|||
// Rodriguez formula given axis angle
|
||||
long timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
rodriguez(v,0.001);
|
||||
Rot3::rodriguez(v,0.001);
|
||||
long timeLog2 = clock();
|
||||
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
cout << seconds << " seconds" << endl;
|
||||
|
|
@ -29,7 +29,7 @@ int main()
|
|||
// Rodriguez formula given canonical coordinates
|
||||
timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
rodriguez(v);
|
||||
Rot3::rodriguez(v);
|
||||
timeLog2 = clock();
|
||||
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
cout << seconds << " seconds" << endl;
|
||||
|
|
|
|||
|
|
@ -32,8 +32,9 @@ namespace gtsam {
|
|||
template<class J, class T>
|
||||
void LieConfig<J,T>::print(const string &s) const {
|
||||
cout << "LieConfig " << s << ", size " << values_.size() << "\n";
|
||||
BOOST_FOREACH(const typename Values::value_type& v, values_)
|
||||
BOOST_FOREACH(const typename Values::value_type& v, values_) {
|
||||
gtsam::print(v.second, (string)(v.first));
|
||||
}
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
|
|
|
|||
|
|
@ -399,12 +399,12 @@ public:
|
|||
typedef boost::shared_ptr<NonlinearEquality1<Config, Key, X> > shared_ptr;
|
||||
|
||||
NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0)
|
||||
: Base(key1, X::dim(), mu), value_(value) {}
|
||||
: Base(key1, X::dimension, mu), value_(value) {}
|
||||
virtual ~NonlinearEquality1() {}
|
||||
|
||||
/** g(x) with optional derivative */
|
||||
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
|
||||
const size_t p = X::dim();
|
||||
const size_t p = X::dimension;
|
||||
if (H1) *H1 = eye(p);
|
||||
return logmap(value_, x1);
|
||||
}
|
||||
|
|
@ -425,14 +425,14 @@ public:
|
|||
typedef boost::shared_ptr<NonlinearEquality2<Config, Key, X> > shared_ptr;
|
||||
|
||||
NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0)
|
||||
: Base(key1, key2, X::dim(), mu) {}
|
||||
: Base(key1, key2, X::dimension, mu) {}
|
||||
virtual ~NonlinearEquality2() {}
|
||||
|
||||
/** g(x) with optional derivative2 */
|
||||
Vector evaluateError(const X& x1, const X& x2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
const size_t p = X::dim();
|
||||
const size_t p = X::dimension;
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p);
|
||||
return logmap(x1, x2);
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ namespace gtsam {
|
|||
typedef boost::shared_ptr<BetweenConstraint<Config, Key, X> > shared_ptr;
|
||||
|
||||
BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0)
|
||||
: Base(key1, key2, X::dim(), mu), measured_(measured) {}
|
||||
: Base(key1, key2, X::dimension, mu), measured_(measured) {}
|
||||
|
||||
/** g(x) with optional derivative2 */
|
||||
Vector evaluateError(const X& x1, const X& x2,
|
||||
|
|
|
|||
|
|
@ -19,8 +19,8 @@ TEST( Pose3Factor, error )
|
|||
{
|
||||
// Create example
|
||||
Pose3 t1; // origin
|
||||
Pose3 t2(rodriguez(0.1,0.2,0.3),Point3(0,1,0));
|
||||
Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
|
||||
Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0));
|
||||
Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
|
||||
|
||||
// Create factor
|
||||
SharedGaussian I6(noiseModel::Unit::Create(6));
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ using namespace simulated3D;
|
|||
/* ************************************************************************* */
|
||||
TEST( simulated3D, Dprior )
|
||||
{
|
||||
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v = logmap(x1);
|
||||
Matrix numerical = numericalDerivative11(prior,v);
|
||||
Matrix computed = Dprior(v);
|
||||
|
|
@ -27,9 +27,9 @@ TEST( simulated3D, Dprior )
|
|||
/* ************************************************************************* */
|
||||
TEST( simulated3D, DOdo1 )
|
||||
{
|
||||
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v1 = logmap(x1);
|
||||
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Vector v2 = logmap(x2);
|
||||
Matrix numerical = numericalDerivative21(odo,v1,v2);
|
||||
Matrix computed = Dodo1(v1,v2);
|
||||
|
|
@ -39,9 +39,9 @@ TEST( simulated3D, DOdo1 )
|
|||
/* ************************************************************************* */
|
||||
TEST( simulated3D, DOdo2 )
|
||||
{
|
||||
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v1 = logmap(x1);
|
||||
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Vector v2 = logmap(x2);
|
||||
Matrix numerical = numericalDerivative22(odo,v1,v2);
|
||||
Matrix computed = Dodo2(v1,v2);
|
||||
|
|
|
|||
Loading…
Reference in New Issue