Rot2 "named constructors" (http://www.parashift.com/c++-faq-lite/ctors.html#faq-10.8):
/** Named constructor from angle == exponential map at identity */
static Rot2 fromAngle(double theta) { return Rot2(cos(theta),sin(theta));}
/** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
static Rot2 fromCosSin(double c, double s);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
static Rot2 atan2(double y, double x);
which are a bit more verbose, but at least won't cause hours of bug-tracking :-(
I also made most compose, inverse, and rotate functions into methods, with the global functions mostly calling the methods. Methods have privileged access to members and hence are typically much more readable.
release/4.3a0
Frank Dellaert
parent
85fe43949d
commit
707627fb3a
|
|
@ -8,7 +8,6 @@
|
|||
#include "Lie.h"
|
||||
|
||||
#define INSTANTIATE_LIE(T) \
|
||||
template T operator*(const T&, const T&); \
|
||||
template T between(const T&, const T&); \
|
||||
template Vector logmap(const T&, const T&); \
|
||||
template T expmap(const T&, const Vector&); \
|
||||
|
|
|
|||
|
|
@ -15,17 +15,12 @@ namespace gtsam {
|
|||
template<class T>
|
||||
T expmap(const Vector& v); /* Exponential map about identity */
|
||||
|
||||
// Syntactic sugar
|
||||
template<class T>
|
||||
inline T operator*(const T& l1, const T& l0) { return compose(l1, l0); }
|
||||
|
||||
|
||||
// The following functions may be overridden in your own class file
|
||||
// with more efficient versions if possible.
|
||||
|
||||
// Compute l1 s.t. l2=l1*l0
|
||||
template<class T>
|
||||
inline T between(const T& l1, const T& l2) { return inverse(l1)*l2; }
|
||||
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
|
||||
template<class T>
|
||||
|
|
@ -33,7 +28,7 @@ namespace gtsam {
|
|||
|
||||
/* 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 t * expmap<T>(d); }
|
||||
inline T expmap(const T& t, const Vector& d) { return compose(t,expmap<T>(d)); }
|
||||
|
||||
/**
|
||||
* Base class for Lie group type
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ namespace gtsam {
|
|||
INSTANTIATE_LIE(Pose2);
|
||||
|
||||
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
|
||||
static const Rot2 R_PI_2(0., 1.);
|
||||
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Pose2::matrix() const {
|
||||
|
|
@ -35,6 +35,50 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
|
||||
template<> Pose2 expmap(const Vector& xi) {
|
||||
Point2 v(xi(0),xi(1));
|
||||
double w = xi(2);
|
||||
if (fabs(w) < 1e-5)
|
||||
return Pose2(xi[0], xi[1], xi[2]);
|
||||
else {
|
||||
Rot2 R(Rot2::fromAngle(w));
|
||||
Point2 v_ortho = R_PI_2 * v; // points towards rot center
|
||||
Point2 t = (v_ortho - rotate(R,v_ortho)) / w;
|
||||
return Pose2(R, t);
|
||||
}
|
||||
}
|
||||
|
||||
Vector logmap(const Pose2& p) {
|
||||
const Rot2& R = p.r();
|
||||
const Point2& t = p.t();
|
||||
double w = R.theta();
|
||||
if (fabs(w) < 1e-5)
|
||||
return Vector_(3, t.x(), t.y(), w);
|
||||
else {
|
||||
double c_1 = R.c()-1.0, s = R.s();
|
||||
double det = c_1*c_1 + s*s;
|
||||
Point2 p = R_PI_2 * (unrotate(R, t) - t);
|
||||
Point2 v = (w/det) * p;
|
||||
return Vector_(3, v.x(), v.y(), w);
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
template<> 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());
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
Matrix AdjointMap(const Pose2& p) {
|
||||
|
|
@ -109,7 +153,7 @@ namespace gtsam {
|
|||
|
||||
// Calculate delta rotation = between(R1,R2)
|
||||
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
|
||||
Rot2 R(c,s);
|
||||
Rot2 R(Rot2::fromCosSin(c,s));
|
||||
|
||||
// Calculate delta translation = unrotate(R1, dt);
|
||||
Point2 dt = p2.t() - p1.t();
|
||||
|
|
|
|||
109
cpp/Pose2.h
109
cpp/Pose2.h
|
|
@ -29,7 +29,7 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** default constructor = origin */
|
||||
Pose2() : t_(0.0, 0.0), r_(0) {} // default is origin
|
||||
Pose2() {} // default is origin
|
||||
|
||||
/** copy constructor */
|
||||
Pose2(const Pose2& pose) : t_(pose.t_), r_(pose.r_) {}
|
||||
|
|
@ -40,12 +40,20 @@ namespace gtsam {
|
|||
* @param y y coordinate
|
||||
* @param theta angle with positive X-axis
|
||||
*/
|
||||
Pose2(double x, double y, double theta) : t_(x,y), r_(theta) {}
|
||||
Pose2(double x, double y, double theta) :
|
||||
t_(x, y), r_(Rot2::fromAngle(theta)) {
|
||||
}
|
||||
|
||||
/** construct from rotation and translation */
|
||||
Pose2(double theta, const Point2& t) : t_(t), r_(theta) {}
|
||||
Pose2(double theta, const Point2& t) :
|
||||
t_(t), r_(Rot2::fromAngle(theta)) {
|
||||
}
|
||||
Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) {}
|
||||
|
||||
/** Constructor from 3*3 matrix */
|
||||
Pose2(const Matrix &T) :
|
||||
r_(Rot2::fromCosSin(T(0, 0), T(1, 0))), t_(T(0, 2), T(1, 2)) {}
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
|
|
@ -67,63 +75,86 @@ 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)
|
||||
*/
|
||||
Matrix AdjointMap(const Pose2& p);
|
||||
|
||||
/** inverse transformation */
|
||||
/**
|
||||
* wedge for SE(2):
|
||||
* @param xi 3-dim twist (v,omega) where
|
||||
* omega is angular velocity
|
||||
* v (vx,vy) = 2D velocity
|
||||
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
|
||||
*/
|
||||
inline Matrix wedge(double vx, double vy, double w) {
|
||||
return Matrix_(3,3,
|
||||
0.,-w, vx,
|
||||
w, 0., vy,
|
||||
0., 0., 0.);
|
||||
}
|
||||
|
||||
template <>
|
||||
inline Matrix wedge<Pose2>(const Vector& xi) {
|
||||
return wedge(xi(0),xi(1),xi(2));
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse transformation
|
||||
*/
|
||||
Pose2 inverse(const Pose2& pose);
|
||||
Matrix Dinverse(const Pose2& pose);
|
||||
|
||||
/** compose this transformation onto another (first p1 and then p2) */
|
||||
inline Pose2 compose(const Pose2& p0, const Pose2& p1) {
|
||||
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
|
||||
/**
|
||||
* compose this transformation onto another (first p1 and then p2)
|
||||
*/
|
||||
inline Pose2 compose(const Pose2& p0, const Pose2& p1)
|
||||
{ return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t());}
|
||||
Matrix Dcompose1(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dcompose2(const Pose2& p1, const Pose2& p2);
|
||||
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0);}
|
||||
|
||||
/** 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());
|
||||
}
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/** Return point coordinates in global frame */
|
||||
inline Point2 transform_from(const Pose2& pose, const Point2& point) {
|
||||
return rotate(pose.r(), point) + pose.t();
|
||||
}
|
||||
/**
|
||||
* Return point coordinates in global frame
|
||||
*/
|
||||
inline Point2 transform_from(const Pose2& pose, const Point2& point)
|
||||
{ return rotate(pose.r(), point) + pose.t();}
|
||||
Point2 transform_from(const Pose2& pose, const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
|
||||
inline Point2 operator*(const Pose2& pose, const Point2& point)
|
||||
{ return transform_from(pose, point);}
|
||||
|
||||
/** Return relative pose between p1 and p2, in p1 coordinate frame */
|
||||
/**
|
||||
* 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
|
||||
|
|
@ -131,10 +162,6 @@ namespace gtsam {
|
|||
* @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);
|
||||
|
||||
|
|
@ -145,10 +172,6 @@ namespace gtsam {
|
|||
* @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);
|
||||
|
||||
|
|
|
|||
|
|
@ -108,7 +108,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transform_to(const Pose3& pose) const {
|
||||
Rot3 cRv = R_ * Rot3(inverse(pose.R_));
|
||||
Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_));
|
||||
Point3 t = gtsam::transform_to(pose, t_);
|
||||
return Pose3(cRv, t);
|
||||
}
|
||||
|
|
|
|||
16
cpp/Pose3.h
16
cpp/Pose3.h
|
|
@ -59,6 +59,17 @@ 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_);
|
||||
}
|
||||
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
||||
/** get the dimension by the type */
|
||||
|
|
@ -81,10 +92,7 @@ namespace gtsam {
|
|||
inline size_t dim(const Pose3&) { return 6; }
|
||||
|
||||
/** Compose two poses */
|
||||
inline Pose3 compose(const Pose3& p0, const Pose3& p1) {
|
||||
return Pose3(p0.rotation()*p1.rotation(),
|
||||
p0.translation() + p0.rotation()*p1.translation());
|
||||
}
|
||||
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) {
|
||||
|
|
|
|||
|
|
@ -35,11 +35,6 @@ namespace gtsam {
|
|||
return Matrix_(2, 2, c_, s_, -s_, c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::negtranspose() const {
|
||||
return Matrix_(2, 2, -c_, -s_, s_, -c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Rot2::rotate(const Point2& p) const {
|
||||
return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
|
||||
|
|
@ -73,14 +68,14 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Rot2 relativeBearing(const Point2& d) {
|
||||
double n = d.norm();
|
||||
return Rot2(d.x() / n, d.y() / n);
|
||||
return Rot2::fromCosSin(d.x() / n, d.y() / n);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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(x / n, y / n);
|
||||
return Rot2::fromCosSin(x / n, y / n);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
79
cpp/Rot2.h
79
cpp/Rot2.h
|
|
@ -18,11 +18,22 @@ namespace gtsam {
|
|||
|
||||
/* Rotation matrix */
|
||||
class Rot2: Testable<Rot2>, public Lie<Rot2> {
|
||||
|
||||
private:
|
||||
|
||||
/** we store cos(theta) and sin(theta) */
|
||||
double c_, s_;
|
||||
|
||||
public:
|
||||
/** normalize to make sure cos and sin form unit vector */
|
||||
inline Rot2& normalize() {
|
||||
double scale = c_*c_ + s_*s_;
|
||||
if(scale != 1.0) {
|
||||
scale = pow(scale, -0.5);
|
||||
c_ *= scale;
|
||||
s_ *= scale;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** constructor from cos/sin */
|
||||
inline Rot2(double c, double s) :
|
||||
|
|
@ -30,23 +41,35 @@ namespace gtsam {
|
|||
#ifdef ROT2_RENORMALIZE
|
||||
// rtodo: Could do this scale correction only when creating from compose
|
||||
// Don't let scale drift
|
||||
double scale = c*c + s*s;
|
||||
if(scale != 1.0) {
|
||||
scale = pow(scale, -0.5);
|
||||
c_ *= scale;
|
||||
s_ *= scale;
|
||||
}
|
||||
normalize();
|
||||
#endif
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor, zero rotation */
|
||||
Rot2() : c_(1.0), s_(0.0) {}
|
||||
|
||||
/** constructor from angle == exponential map at identity */
|
||||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||
/** "named constructors" */
|
||||
|
||||
/** return angle */
|
||||
double theta() const { return atan2(s_,c_); }
|
||||
/** Named constructor from angle == exponential map at identity */
|
||||
static Rot2 fromAngle(double theta) { return Rot2(cos(theta),sin(theta));}
|
||||
|
||||
/** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
|
||||
static Rot2 fromCosSin(double c, double s) {
|
||||
if (fabs(c*c + s*s - 1.0)>1e-9)
|
||||
throw std::invalid_argument("Rot2::fromCosSin: needs cos/sin pair");
|
||||
return Rot2(c, s);
|
||||
}
|
||||
|
||||
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
||||
static Rot2 atan2(double y, double x) {
|
||||
Rot2 R(x,y);
|
||||
return R.normalize();
|
||||
}
|
||||
|
||||
/** return angle */
|
||||
double theta() const { return ::atan2(s_,c_); }
|
||||
|
||||
/** return cos */
|
||||
inline double c() const { return c_; }
|
||||
|
|
@ -66,8 +89,13 @@ namespace gtsam {
|
|||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
|
||||
/** return 2*2 negative transpose */
|
||||
Matrix negtranspose() 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 from world to rotated = R*p */
|
||||
Point2 rotate(const Point2& p) const;
|
||||
|
|
@ -86,8 +114,8 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
};
|
||||
|
||||
}; // Rot2
|
||||
|
||||
// Lie group functions
|
||||
|
||||
|
|
@ -100,7 +128,7 @@ namespace gtsam {
|
|||
/** Expmap around identity - create a rotation from an angle */
|
||||
template<> inline Rot2 expmap(const Vector& v) {
|
||||
if (zero(v)) return (Rot2());
|
||||
else return Rot2(v(0));
|
||||
else return Rot2::fromAngle(v(0));
|
||||
}
|
||||
|
||||
/** Logmap around identity - return the angle of the rotation */
|
||||
|
|
@ -109,27 +137,10 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
inline Rot2 compose(const Rot2& r0, const Rot2& r1) {
|
||||
return Rot2(
|
||||
r0.c() * r1.c() - r0.s() * r1.s(),
|
||||
r0.s() * r1.c() + r0.c() * r1.s());
|
||||
}
|
||||
|
||||
/** Syntactic sugar R1*R2 = compose(R1,R2) */
|
||||
inline Rot2 operator*(const Rot2& r0, const Rot2& r1) {
|
||||
return compose(r0, r1);
|
||||
}
|
||||
inline Rot2 compose(const Rot2& R1, const Rot2& R2) { return R1*R2;}
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
inline Rot2 inverse(const Rot2& r) { return Rot2(r.c(), -r.s());}
|
||||
|
||||
/** Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1 */
|
||||
inline Rot2 invcompose(const Rot2& r0, const Rot2& r1) {
|
||||
return Rot2(
|
||||
r0.c() * r1.c() + r0.s() * r1.s(),
|
||||
-r0.s() * r1.c() + r0.c() * r1.s());
|
||||
}
|
||||
|
||||
inline Rot2 inverse(const Rot2& R) { return R.inverse();}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
|
|
|
|||
|
|
@ -165,11 +165,6 @@ namespace gtsam {
|
|||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 rotate(const Rot3& R, const Point3& p) {
|
||||
return R.r1() * p.x() + R.r2() * p.y() + R.r3() * p.z();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Drotate1(const Rot3& R, const Point3& p) {
|
||||
return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
|
|
|
|||
37
cpp/Rot3.h
37
cpp/Rot3.h
|
|
@ -109,6 +109,27 @@ namespace gtsam {
|
|||
/** get the dimension by the type */
|
||||
static inline size_t dim() { return 3; };
|
||||
|
||||
/* Find the inverse rotation R^T s.t. inverse(R)*R = I */
|
||||
inline Rot3 inverse() const {
|
||||
return Rot3(
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z());
|
||||
}
|
||||
|
||||
/** compose two rotations */
|
||||
Rot3 operator*(const Rot3& R2) const {
|
||||
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
Point3 rotate(const Point3& p) const
|
||||
{return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
|
||||
inline Point3 operator*(const Point3& p) const { return rotate(p);}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -147,7 +168,8 @@ namespace gtsam {
|
|||
* @param wz
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));}
|
||||
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; }
|
||||
|
|
@ -163,16 +185,10 @@ namespace gtsam {
|
|||
Vector logmap(const Rot3& R);
|
||||
|
||||
// Compose two rotations
|
||||
inline Rot3 compose(const Rot3& R1, const Rot3& R2) {
|
||||
return Rot3(R1.matrix() * R2.matrix()); }
|
||||
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 Rot3(
|
||||
R.r1().x(), R.r1().y(), R.r1().z(),
|
||||
R.r2().x(), R.r2().y(), R.r2().z(),
|
||||
R.r3().x(), R.r3().y(), R.r3().z());
|
||||
}
|
||||
inline Rot3 inverse(const Rot3& R) { return R.inverse();}
|
||||
|
||||
// and its derivative
|
||||
inline Matrix Dinverse(Rot3 R) { return -R.matrix();}
|
||||
|
|
@ -181,8 +197,7 @@ namespace gtsam {
|
|||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
Point3 rotate(const Rot3& R, const Point3& p);
|
||||
inline Point3 operator*(const Rot3& R, const Point3& p) { return rotate(R,p); }
|
||||
inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
|
||||
Matrix Drotate1(const Rot3& R, const Point3& p);
|
||||
Matrix Drotate2(const Rot3& R); // does not depend on p !
|
||||
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ SharedGaussian
|
|||
TEST( planarSLAM, BearingFactor )
|
||||
{
|
||||
// Create factor
|
||||
Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
planarSLAM::Bearing factor(2, 3, z, sigma);
|
||||
|
||||
// create config
|
||||
|
|
@ -72,7 +72,7 @@ TEST( planarSLAM, constructor )
|
|||
G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3);
|
||||
|
||||
// Create bearing factor
|
||||
Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
G.addBearing(2, 3, z1, sigma);
|
||||
|
||||
// Create range factor
|
||||
|
|
|
|||
|
|
@ -14,6 +14,8 @@
|
|||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
// #define SLOW_BUT_CORRECT_EXPMAP
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, constructors) {
|
||||
//cout << "constructors" << endl;
|
||||
|
|
@ -21,6 +23,8 @@ TEST(Pose2, constructors) {
|
|||
Pose2 pose(0,p);
|
||||
Pose2 origin;
|
||||
assert_equal(pose,origin);
|
||||
Pose2 t(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
CHECK(assert_equal(t,Pose2(t.matrix())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -41,9 +45,13 @@ TEST(Pose2, manifold) {
|
|||
TEST(Pose2, expmap) {
|
||||
//cout << "expmap" << endl;
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Pose2 expected(1.00811, 2.01528, 2.5608);
|
||||
#else
|
||||
Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01));
|
||||
#endif
|
||||
Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.99));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -66,19 +74,46 @@ TEST(Pose2, expmap2) {
|
|||
TEST(Pose2, expmap0) {
|
||||
//cout << "expmap0" << endl;
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Pose2 expected(1.01491, 2.01013, 1.5888);
|
||||
#else
|
||||
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
#endif
|
||||
Pose2 actual = pose * expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.018));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
/* ************************************************************************* */
|
||||
// test case for screw motion in the plane
|
||||
namespace screw {
|
||||
double w=0.3;
|
||||
Vector xi = Vector_(3, 0.0, w, w);
|
||||
Rot2 expectedR = Rot2::fromAngle(w);
|
||||
Point2 expectedT(-0.0446635, 0.29552);
|
||||
Pose2 expected(expectedR, expectedT);
|
||||
}
|
||||
|
||||
TEST(Pose3, expmap_c)
|
||||
{
|
||||
CHECK(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
|
||||
CHECK(assert_equal(screw::expected, expmap<Pose2>(screw::xi),1e-6));
|
||||
CHECK(assert_equal(screw::xi, logmap(screw::expected),1e-6));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, logmap) {
|
||||
//cout << "logmap" << endl;
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
|
||||
#else
|
||||
Vector expected = Vector_(3, 0.01, -0.015, 0.018);
|
||||
#endif
|
||||
Vector actual = logmap(pose0,pose);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -172,10 +207,10 @@ TEST(Pose2, compose_a)
|
|||
TEST(Pose2, compose_b)
|
||||
{
|
||||
//cout << "compose_b" << endl;
|
||||
Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5));
|
||||
Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
|
||||
Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
|
||||
Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
|
||||
|
||||
Pose2 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0));
|
||||
Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0));
|
||||
|
||||
Pose2 pose_actual_op = pose1 * pose2;
|
||||
Pose2 pose_actual_fcn = compose(pose1, pose2);
|
||||
|
|
@ -184,7 +219,7 @@ TEST(Pose2, compose_b)
|
|||
|
||||
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
|
||||
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1));
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1,1e-5));
|
||||
CHECK(assert_equal(numericalH2,actualDcompose2));
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
|
|
@ -195,10 +230,10 @@ TEST(Pose2, compose_b)
|
|||
TEST(Pose2, compose_c)
|
||||
{
|
||||
//cout << "compose_c" << endl;
|
||||
Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0));
|
||||
Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
|
||||
Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
|
||||
Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
|
||||
|
||||
Pose2 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0));
|
||||
Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0));
|
||||
|
||||
Pose2 pose_actual_op = pose1 * pose2;
|
||||
Pose2 pose_actual_fcn = compose(pose1,pose2);
|
||||
|
|
@ -207,7 +242,7 @@ TEST(Pose2, compose_c)
|
|||
|
||||
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
|
||||
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1));
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1,1e-5));
|
||||
CHECK(assert_equal(numericalH2,actualDcompose2));
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
|
|
@ -373,11 +408,11 @@ TEST( Pose2, bearing )
|
|||
CHECK(assert_equal(Rot2(),bearing(x1,l1)));
|
||||
|
||||
// establish bearing is indeed 45 degrees
|
||||
CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2)));
|
||||
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),bearing(x1,l2)));
|
||||
|
||||
// establish bearing is indeed 45 degrees even if shifted
|
||||
Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual23));
|
||||
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
|
||||
|
|
@ -387,7 +422,7 @@ TEST( Pose2, bearing )
|
|||
|
||||
// establish bearing is indeed 45 degrees even if rotated
|
||||
Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual34));
|
||||
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
|
||||
|
|
|
|||
|
|
@ -10,13 +10,16 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
Rot2 R(0.1);
|
||||
Rot2 R(Rot2::fromAngle(0.1));
|
||||
Point2 P(0.2, 0.7);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot2, angle)
|
||||
TEST( Rot2, constructors_and_angle)
|
||||
{
|
||||
double c=cos(0.1), s=sin(0.1);
|
||||
DOUBLES_EQUAL(0.1,R.theta(),1e-9);
|
||||
CHECK(assert_equal(R,Rot2::fromCosSin(c,s)));
|
||||
CHECK(assert_equal(R,Rot2::atan2(s*5,c*5)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -25,23 +28,11 @@ TEST( Rot2, transpose)
|
|||
CHECK(assert_equal(inverse(R).matrix(),R.transpose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot2, negtranspose)
|
||||
{
|
||||
CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot2, compose)
|
||||
{
|
||||
CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25)));
|
||||
CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot2, invcompose)
|
||||
{
|
||||
CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45))));
|
||||
CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.2)*Rot2::fromAngle(0.25)));
|
||||
CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.25)*Rot2::fromAngle(0.2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -62,8 +53,8 @@ TEST( Rot2, expmap)
|
|||
/* ************************************************************************* */
|
||||
TEST(Rot2, logmap)
|
||||
{
|
||||
Rot2 rot0(M_PI_2);
|
||||
Rot2 rot(M_PI);
|
||||
Rot2 rot0(Rot2::fromAngle(M_PI_2));
|
||||
Rot2 rot(Rot2::fromAngle(M_PI));
|
||||
Vector expected = Vector_(1, M_PI_2);
|
||||
Vector actual = logmap(rot0, rot);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
|
@ -113,7 +104,7 @@ TEST( Rot2, relativeBearing )
|
|||
|
||||
// establish relativeBearing is indeed 45 degrees
|
||||
Rot2 actual2 = relativeBearing(l2, actualH);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual2));
|
||||
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2));
|
||||
|
||||
// Check numerical derivative
|
||||
expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
|
||||
|
|
|
|||
Loading…
Reference in New Issue