diff --git a/cpp/Cal3_S2.h b/cpp/Cal3_S2.h index 10722c7b7..ba54f9470 100644 --- a/cpp/Cal3_S2.h +++ b/cpp/Cal3_S2.h @@ -54,25 +54,11 @@ namespace gtsam { */ bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /** - * return DOF, dimensionality of tangent space - */ - size_t dim() const { - return 5; - } - /** * load calibration from location (default name is calibration_info.txt) */ Cal3_S2(const std::string &path); - /** - * Given 5-dim tangent vector, create new calibration - */ - Cal3_S2 exmap(const Vector& d) const { - return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); - } - /** * return the principal point */ @@ -107,6 +93,7 @@ namespace gtsam { /** friends */ friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p); + friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d); private: /** Serialization function */ @@ -124,6 +111,19 @@ namespace gtsam { typedef boost::shared_ptr shared_ptrK; + /** + * return DOF, dimensionality of tangent space + */ + inline size_t dim(const Cal3_S2&) { return 5; } + + /** + * Given 5-dim tangent vector, create new calibration + */ + inline Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d) { + return Cal3_S2(cal.fx_ + d(0), cal.fy_ + d(1), + cal.s_ + d(2), cal.u0_ + d(3), cal.v0_ + d(4)); + } + /** * convert intrinsic coordinates xy to image coordinates uv */ diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 22a8b7a0c..d98087c7f 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -25,9 +25,6 @@ #include "Ordering.h" #include "FactorGraph.h" - - - // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index d9a42c647..215a12977 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -286,7 +286,7 @@ VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x, cout << alpha << endl; // return updated estimate by stepping in direction d - return x.exmap(d.scale(alpha)); + return expmap(x, d.scale(alpha)); } /* ************************************************************************* */ @@ -319,3 +319,4 @@ boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_( } /* ************************************************************************* */ + diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 093eec54e..9f99ee201 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -218,7 +218,6 @@ namespace gtsam { boost::shared_ptr conjugateGradientDescent_( const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0) const; - }; } diff --git a/cpp/Lie.h b/cpp/Lie.h new file mode 100644 index 000000000..9429131a0 --- /dev/null +++ b/cpp/Lie.h @@ -0,0 +1,44 @@ +/* + * Lie.h + * + * Created on: Jan 5, 2010 + * Author: Richard Roberts + */ + +#ifndef LIE_H_ +#define LIE_H_ + +#include "Vector.h" +#include "VectorConfig.h" +#include "Matrix.h" + + +namespace gtsam { + + template + T expmap(const Vector& v); /* Exponential map about identity */ + + // Syntactic sugar + template + 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 + inline T between(const T& l0, const T& l2) { return l2*inverse(l0); } + + // Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp + template + inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); } + + /* Exponential map centered at l0, s.t. exp(l0,v) = exp(v)*l0 */ + template + inline T expmap(const T& l0, const Vector& v) { return expmap(v)*l0; } + +} + + +#endif /* LIE_H_ */ diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 5f84895ec..e203e80ac 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -157,6 +157,7 @@ testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp testSQPOptimizer_LDADD = libgtsam.la # geometry +headers += Lie.h sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testPoint2_SOURCES = testPoint2.cpp diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index a96006be0..b57423db2 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -10,6 +10,8 @@ #pragma once +#include + #include "NonlinearFactor.h" #include "GaussianFactorGraph.h" diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 844386fe9..c53f2e5c6 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -82,7 +82,7 @@ namespace gtsam { delta.print("delta"); // take old config and update it - shared_config newConfig(new C(config_->exmap(delta))); + shared_config newConfig(new C(expmap(*config_,delta))); // maybe show output if (verbosity >= CONFIG) @@ -134,7 +134,7 @@ namespace gtsam { delta.print("delta"); // update config - shared_config newConfig(new C(config_->exmap(delta))); // TODO: updateConfig + shared_config newConfig(new C(expmap(*config_,delta))); // TODO: updateConfig if (verbosity >= TRYCONFIG) newConfig->print("config"); diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp index cc73edbab..88ef85c22 100644 --- a/cpp/Point2.cpp +++ b/cpp/Point2.cpp @@ -10,21 +10,21 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - void Point2::print(const string& s) const { - cout << s << "(" << x_ << ", " << y_ << ")" << endl; - } + /* ************************************************************************* */ + void Point2::print(const string& s) const { + cout << s << "(" << x_ << ", " << y_ << ")" << endl; + } - /* ************************************************************************* */ - bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); - } + /* ************************************************************************* */ + bool Point2::equals(const Point2& q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); + } - /* ************************************************************************* */ - double Point2::dist(const Point2& p2) const { - return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0)); - } + /* ************************************************************************* */ + double Point2::dist(const Point2& p2) const { + return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0)); + } -/* ************************************************************************* */ + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Point2.h b/cpp/Point2.h index f50b55a32..a4266ce1a 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -9,6 +9,7 @@ #include #include "Vector.h" #include "Testable.h" +#include "Lie.h" namespace gtsam { @@ -37,20 +38,8 @@ namespace gtsam { double x() const {return x_;} double y() const {return y_;} - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return 2;} - - /** Given 2-dim tangent vector, create new point */ - Point2 exmap(const Vector& d) const { - return Point2(x_+d(0),y_+d(1)); - } - /** return vectorized form (column-wise) */ - Vector vector() const { - Vector v(2); - v(0)=x_;v(1)=y_; - return v; - } + Vector vector() const { return Vector_(2, x_, y_); } /** operators */ inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} @@ -71,5 +60,31 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(y_); } }; + + // Lie group functions + + // 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& p0) { return p0+p1; } + inline Matrix Dcompose1(const Point2& p1, const Point2& p0) { + return Matrix_(2,2, + 1.0, 0.0, + 0.0, 1.0); } + inline Matrix Dcompose2(const Point2& p1, const Point2& p0) { + return Matrix_(2,2, + 1.0, 0.0, + 0.0, 1.0); } + + // "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() + inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); } + } diff --git a/cpp/Point3.cpp b/cpp/Point3.cpp index 0854b202a..1642c9be7 100644 --- a/cpp/Point3.cpp +++ b/cpp/Point3.cpp @@ -8,78 +8,81 @@ namespace gtsam { -/* ************************************************************************* */ -bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); -} -/* ************************************************************************* */ + /* ************************************************************************* */ + bool Point3::equals(const Point3 & q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); + } -bool Point3::operator== (const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; -} + void Point3::print(const std::string& s) const { + std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; + } -/* ************************************************************************* */ -Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); -} + /* ************************************************************************* */ -/* ************************************************************************* */ -Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); -} -/* ************************************************************************* */ -Point3 Point3::operator*(double s) const { - return Point3(x_ * s, y_ * s, z_ * s); -} -/* ************************************************************************* */ -Point3 Point3::operator/(double s) const { - return Point3(x_ / s, y_ / s, z_ / s); -} -/* ************************************************************************* */ -Point3 operator*(double s, const Point3& p) { return p*s;} + bool Point3::operator== (const Point3& q) const { + return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; + } -/* ************************************************************************* */ -Point3 add(const Point3 &p, const Point3 &q) { - return p+q; -} -/* ************************************************************************* */ -Matrix Dadd1(const Point3 &p, const Point3 &q) { - return eye(3,3); -} -/* ************************************************************************* */ -Matrix Dadd2(const Point3 &p, const Point3 &q) { - return eye(3,3); -} -/* ************************************************************************* */ -Point3 sub(const Point3 &p, const Point3 &q) { - return p+q; -} -/* ************************************************************************* */ -Matrix Dsub1(const Point3 &p, const Point3 &q) { - return eye(3,3); -} -/* ************************************************************************* */ -Matrix Dsub2(const Point3 &p, const Point3 &q) { - return -eye(3,3); -} -/* ************************************************************************* */ -Point3 cross(const Point3 &p, const Point3 &q) -{ - return Point3( p.y_*q.z_ - p.z_*q.y_, - p.z_*q.x_ - p.x_*q.z_, - p.x_*q.y_ - p.y_*q.x_ ); -} -/* ************************************************************************* */ -double dot(const Point3 &p, const Point3 &q) -{ - return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ ); -} -/* ************************************************************************* */ -double norm(const Point3 &p) -{ - return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ ); -} -/* ************************************************************************* */ + /* ************************************************************************* */ + Point3 Point3::operator+(const Point3& q) const { + return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); + } + + /* ************************************************************************* */ + Point3 Point3::operator- (const Point3& q) const { + return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); + } + /* ************************************************************************* */ + Point3 Point3::operator*(double s) const { + return Point3(x_ * s, y_ * s, z_ * s); + } + /* ************************************************************************* */ + Point3 Point3::operator/(double s) const { + return Point3(x_ / s, y_ / s, z_ / s); + } + + /* ************************************************************************* */ + Point3 add(const Point3 &p, const Point3 &q) { + return p+q; + } + /* ************************************************************************* */ + Matrix Dadd1(const Point3 &p, const Point3 &q) { + return eye(3,3); + } + /* ************************************************************************* */ + Matrix Dadd2(const Point3 &p, const Point3 &q) { + return eye(3,3); + } + /* ************************************************************************* */ + Point3 sub(const Point3 &p, const Point3 &q) { + return p+q; + } + /* ************************************************************************* */ + Matrix Dsub1(const Point3 &p, const Point3 &q) { + return eye(3,3); + } + /* ************************************************************************* */ + Matrix Dsub2(const Point3 &p, const Point3 &q) { + return -eye(3,3); + } + /* ************************************************************************* */ + Point3 cross(const Point3 &p, const Point3 &q) + { + return Point3( p.y_*q.z_ - p.z_*q.y_, + p.z_*q.x_ - p.x_*q.z_, + p.x_*q.y_ - p.y_*q.x_ ); + } + /* ************************************************************************* */ + double dot(const Point3 &p, const Point3 &q) + { + return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ ); + } + /* ************************************************************************* */ + double norm(const Point3 &p) + { + return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ ); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Point3.h b/cpp/Point3.h index 9cadc0403..1c2e02d5d 100644 --- a/cpp/Point3.h +++ b/cpp/Point3.h @@ -14,6 +14,7 @@ #include "Matrix.h" #include "Testable.h" +#include "Lie.h" namespace gtsam { @@ -29,19 +30,11 @@ namespace gtsam { Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; - } + void print(const std::string& s = "") const; /** equals with an tolerance */ bool equals(const Point3& p, double tol = 1e-9) const; - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return 3;} - - /** Given 3-dim tangent vector, create new rotation*/ - Point3 exmap(const Vector& d) const { return *this + d; } - /** return vectorized form (column-wise)*/ Vector vector() const { //double r[] = { x_, y_, z_ }; @@ -84,7 +77,39 @@ namespace gtsam { } }; - Point3 operator*(double s, const Point3& p); + + /** return DOF, dimensionality of tangent space */ + + // Dimensionality of the 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, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + } + inline Matrix Dcompose2(const Point3& p1, const Point3& p0) { + return Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 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;} /** add two points, add(p,q) is same as p+q */ Point3 add (const Point3 &p, const Point3 &q); diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index bfb26c3eb..f3afa31d4 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -20,16 +20,6 @@ namespace gtsam { } /* ************************************************************************* */ - // Pose2 Pose2::rotate(double theta) const { - // //return Pose2(t_, Rot2(theta)*r_); - // return Pose2(Point2(0.0,0.0),-theta)*(*this); - // } - - /* ************************************************************************* */ - Point2 transform_to(const Pose2& pose, const Point2& point) { - return pose*point; - } - // TODO, have a combined function that returns both function value and derivative Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0); @@ -42,24 +32,18 @@ namespace gtsam { } /* ************************************************************************* */ - Pose2 between(const Pose2& p1, const Pose2& p2) { - return Pose2( - p1.r().invcompose(p2.r()), - p1.r().unrotate(p2.t() - p1.t())); - } - - Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { - Matrix dt_dr = Dunrotate1(p1.r(), p2.t()-p1.t()); - Matrix dt_dt1 = -p2.r().invcompose(p1.r()).matrix(); - Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p1.t()); + Matrix Dbetween1(const Pose2& p0, const Pose2& p2) { + Matrix dt_dr = Dunrotate1(p0.r(), p2.t()-p0.t()); + Matrix dt_dt1 = -invcompose(p2.r(), p0.r()).matrix(); + Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p0.t()); return Matrix_(3,3, dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0), dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0), 0.0, 0.0, -1.0); } - Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { - Matrix db_dt2 = p1.r().transpose(); + Matrix Dbetween2(const Pose2& p0, const Pose2& p2) { + Matrix db_dt2 = p0.r().transpose(); return Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 7e54b5c3c..a58c307b1 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -13,29 +13,14 @@ #include "Rot2.h" #include "Matrix.h" #include "Testable.h" +#include "Lie.h" namespace gtsam { - /** - * Return point coordinates in pose coordinate frame - */ - class Pose2; - Point2 transform_to(const Pose2& pose, const Point2& point); - Matrix Dtransform_to1(const Pose2& pose, const Point2& point); - Matrix Dtransform_to2(const Pose2& pose, const Point2& point); - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p1, const Pose2& p2); - Matrix Dbetween1(const Pose2& p1, const Pose2& p2); - Matrix Dbetween2(const Pose2& p1, const Pose2& p2); - /** * A 2D pose (Point2,Rot2) */ class Pose2: Testable { - private: Point2 t_; Rot2 r_; @@ -43,12 +28,10 @@ namespace gtsam { public: /** default constructor = origin */ - Pose2() : - t_(0.0, 0.0), r_(0) { } // default is origin + Pose2() : t_(0.0, 0.0), r_(0) {} // default is origin /** copy constructor */ - Pose2(const Pose2& pose) : - t_(pose.t_), r_(pose.r_) { } + Pose2(const Pose2& pose) : t_(pose.t_), r_(pose.r_) {} /** * construct from (x,y,theta) @@ -56,14 +39,11 @@ 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_(theta) {} /** construct from rotation and translation */ - Pose2(double theta, const Point2& t) : - t_(t), r_(theta) { } - Pose2(const Rot2& r, const Point2& t) : - t_(t), r_(r) { } + Pose2(double theta, const Point2& t) : t_(t), r_(theta) {} + Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) {} /** print with optional string */ void print(const std::string& s = "") const; @@ -72,39 +52,57 @@ namespace gtsam { bool equals(const Pose2& pose, double tol = 1e-9) const; /** get functions for x, y, theta */ - double x() const { return t_.x();} - double y() const { return t_.y();} - double theta() const { return r_.theta();} + double x() const { return t_.x(); } + double y() const { return t_.y(); } + double theta() const { return r_.theta(); } Point2 t() const { return t_; } Rot2 r() const { return r_; } - /** return this pose2 as a vector (x,y,r) */ - Vector vector() const { return Vector_(3, t_.x(), t_.y(), r_.theta()); } - - /** return DOF, dimensionality of tangent space = 3 */ - size_t dim() const { return 3; } - - /** exponential map */ - Pose2 exmap(const Vector& v) const { return Pose2(v[0], v[1], v[2]) * (*this); } - - /** log map */ - Vector log(const Pose2 &pose) const { return between(*this, pose).vector(); } - - /** rotate pose by theta */ - // Pose2 rotate(double theta) const; - - /** inverse transformation */ - Pose2 inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } - - /** compose this transformation onto another (pre-multiply this*p1) */ - Pose2 compose(const Pose2& p1) const { return Pose2(p1.r_ * r_, p1.r_ * t_ + p1.t_); } - - /** same as compose (pre-multiply this*p1) */ - Pose2 operator*(const Pose2& p1) const { return compose(p1); } - - /** Return point coordinates in pose coordinate frame, same as transform_to */ - Point2 operator*(const Point2& point) const { return r_.unrotate(point-t_); } - }; // Pose2 + + /** return DOF, dimensionality of tangent space = 3 */ + inline size_t dim(const Pose2&) { return 3; } + + /** inverse transformation */ + inline Pose2 inverse(const Pose2& pose) { + return Pose2(inverse(pose.r()), + pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); } + + /** compose this transformation onto another (pre-multiply this*p1) */ + inline Pose2 compose(const Pose2& p1, const Pose2& p0) { + return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } + + /* exponential and log maps around identity */ + // Create an incremental pose from x,y,theta + template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); } + // Return the x,y,theta of this pose + inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); } + + + /** Return point coordinates in pose coordinate frame */ + inline Point2 transform_to(const Pose2& pose, const Point2& point) { + return unrotate(pose.r(), point-pose.t()); } + Matrix Dtransform_to1(const Pose2& pose, const Point2& point); + Matrix Dtransform_to2(const Pose2& pose, const Point2& point); + + /** Return point coordinates in global frame */ + inline Point2 transform_from(const Pose2& pose, const Point2& point) { + return rotate(pose.r(), point)+pose.t(); } + + /** Return relative pose between p1 and p2, in p1 coordinate frame */ + // todo: make sure compiler finds this version of between. + //inline Pose2 between(const Pose2& p0, const Pose2& p2) { + // return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); } + Matrix Dbetween1(const Pose2& p0, const Pose2& p2); + Matrix Dbetween2(const Pose2& p0, const Pose2& p2); + + /** 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); } + } // namespace gtsam diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp index 2148ba6bf..d51b9eefd 100644 --- a/cpp/Pose2Config.cpp +++ b/cpp/Pose2Config.cpp @@ -49,14 +49,14 @@ namespace gtsam { } /* ************************************************************************* */ - Pose2Config Pose2Config::exmap(const VectorConfig& delta) const { + Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta) { Pose2Config newConfig; std::string j; Pose2 pj; - FOREACH_PAIR(j, pj, values_) { + FOREACH_PAIR(j, pj, c.values_) { if (delta.contains(j)) { const Vector& dj = delta[j]; //check_size(j,vj,dj); - newConfig.insert(j, pj.exmap(dj)); + newConfig.insert(j, expmap(pj,dj)); } else newConfig.insert(j, pj); } diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index d919ae120..9e2d8f352 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -13,49 +13,52 @@ namespace gtsam { - class VectorConfig; + class VectorConfig; - class Pose2Config: public Testable { + class Pose2Config: public Testable { - private: - std::map values_; + private: + std::map values_; - public: + public: - typedef std::map::const_iterator iterator; - typedef iterator const_iterator; + typedef std::map::const_iterator iterator; + typedef iterator const_iterator; - Pose2Config() {} + Pose2Config() {} - Pose2Config(const Pose2Config &config) :values_(config.values_) {} + Pose2Config(const Pose2Config &config) :values_(config.values_) {} - virtual ~Pose2Config() {} + virtual ~Pose2Config() {} - const Pose2& get(std::string key) const; + const Pose2& get(std::string key) const; - void insert(const std::string& name, const Pose2& val); + void insert(const std::string& name, const Pose2& val); - inline Pose2Config& operator=(const Pose2Config& rhs) { - values_ = rhs.values_; - return (*this); - } + inline Pose2Config& operator=(const Pose2Config& rhs) { + values_ = rhs.values_; + return (*this); + } - bool equals(const Pose2Config& expected, double tol) const; + bool equals(const Pose2Config& expected, double tol) const; - void print(const std::string &s) const; + void print(const std::string &s) const; - void clear() { values_.clear(); } + void clear() { values_.clear(); } - int size() { return values_.size(); }; + int size() { return values_.size(); }; - /** - * Add a delta config, needed for use in NonlinearOptimizer - */ - Pose2Config exmap(const VectorConfig& delta) const; + inline const_iterator begin() const {return values_.begin();} + inline const_iterator end() const {return values_.end();} + inline iterator begin() {return values_.begin();} + inline iterator end() {return values_.end();} + + friend Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); + }; + + /** + * Add a delta config, needed for use in NonlinearOptimizer + */ + Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); - inline const_iterator begin() const {return values_.begin();} - inline const_iterator end() const {return values_.end();} - inline iterator begin() {return values_.begin();} - inline iterator end() {return values_.end();} -}; } // namespace diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h index 2241b7943..7752c0527 100644 --- a/cpp/Pose2Factor.h +++ b/cpp/Pose2Factor.h @@ -49,7 +49,7 @@ public: Vector error_vector(const Pose2Config& config) const { //z-h Pose2 p1 = config.get(key1_), p2 = config.get(key2_); - return -between(p1,p2).log(measured_); + return -logmap(between(p1,p2), measured_); } std::list keys() const { return keys_; } @@ -58,7 +58,7 @@ public: /** linearize */ boost::shared_ptr linearize(const Pose2Config& config) const { Pose2 p1 = config.get(key1_), p2 = config.get(key2_); - Vector b = -between(p1,p2).log(measured_); + Vector b = -logmap(between(p1,p2), measured_); Matrix H1 = Dbetween1(p1,p2); Matrix H2 = Dbetween2(p1,p2); boost::shared_ptr linearized(new GaussianFactor( @@ -68,4 +68,5 @@ public: return linearized; } }; + } /// namespace gtsam diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 12ef9178a..3c65790f9 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -3,135 +3,184 @@ * @brief 3D Pose */ +#include #include "Pose3.h" using namespace std; +using namespace boost::numeric::ublas; namespace gtsam { -/* ************************************************************************* */ -void Pose3::print(const string& s) const { - R_.print(s + ".R"); - t_.print(s + ".t"); -} + /* ************************************************************************* */ + void Pose3::print(const string& s) const { + R_.print(s + ".R"); + t_.print(s + ".t"); + } -/* ************************************************************************* */ -bool Pose3::equals(const Pose3& pose, double tol) const -{ - return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol); -} + /* ************************************************************************* */ + bool Pose3::equals(const Pose3& pose, double tol) const { + return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol); + } -/* ************************************************************************* */ -// Agrawal06iros, formula (6), seems to suggest this could be wrong: -Pose3 Pose3::exmap(const Vector& v) const { - return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6))); -} + /** Agrawal06iros versions + Pose3 expmap(const Vector& d) { -/* ************************************************************************* */ -Vector Pose3::vector() const { - Vector r = R_.vector(), t = t_.vector(); - return concatVectors(2, &r, &t); -} + // From + Vector w = vector_range(d, range(0,3)); + Vector u = vector_range(d, range(3,6)); + double t = norm_2(w); + if (t < 1e-5) + return Pose3(expmap (w), expmap (u)); + else { + Matrix W = skewSymmetric(w/t); + Matrix A = eye(3, 3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); + return Pose3(expmap (w), expmap (A * u)); + } + } -/* ************************************************************************* */ -Matrix Pose3::matrix() const { - const double row4[] = { 0, 0, 0, 1 }; - Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4); - return stack(2, &A34, &A14); -} + Vector logmap(const Pose3& p) { + Vector w = logmap(p.rotation()), T = p.translation().vector(); + double t = norm_2(w); + if (t < 1e-5) + return concatVectors(2, &w, &T); + else { + Matrix W = skewSymmetric(w/t); + Matrix Ainv = eye(3, 3) - 0.5*t* W + ((2*sin(t)-t*(1+cos(t)))/2*sin(t)) * (W * W); + Vector u = Ainv*T; + return concatVectors(2, &w, &u); + } + } +*/ -/* ************************************************************************* */ -Point3 transform_from(const Pose3& pose, const Point3& p) { - return pose.R_ * p + pose.t_; -} + /* ************************************************************************* */ + Matrix Pose3::matrix() const { + const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector()); + const Matrix A34 = collect(2, &R, &T); + const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0); + return stack(2, &A34, &A14); + } -/* ************************************************************************* */ -Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { - Matrix DR = Drotate1(pose.rotation(), p); - Matrix Dt = Dadd1(pose.translation(), p); - return collect(2,&DR,&Dt); -} + /* ************************************************************************* */ + Pose3 Pose3::transform_to(const Pose3& pose) const { + Rot3 cRv = R_ * Rot3(inverse(pose.R_)); + Point3 t = gtsam::transform_to(pose, t_); + return Pose3(cRv, t); + } -/* ************************************************************************* */ -Matrix Dtransform_from2(const Pose3& pose) { - return Drotate2(pose.rotation()); -} + /* ************************************************************************* */ + Point3 transform_from(const Pose3& pose, const Point3& p) { + return pose.rotation() * p + pose.translation(); + } -/* ************************************************************************* */ -Point3 transform_to(const Pose3& pose, const Point3& p) { - Point3 sub = p - pose.t_; - Point3 r = unrotate(pose.R_, sub); - return r; -} + /* ************************************************************************* */ + Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { +#ifdef NEW_EXMAP + Point3 q = transform_from(pose,p); + Matrix DR = skewSymmetric(-q.x(), -q.y(), -q.z()); +#else + Matrix DR = Drotate1(pose.rotation(), p); +#endif + Matrix Dt = eye(3); + return collect(2,&DR,&Dt); + } -/* ************************************************************************* */ -Matrix Dtransform_to1(const Pose3& pose, const Point3& p) { - Point3 q = p - pose.translation(); - Matrix D_r_R = Dunrotate1(pose.rotation(),q); - Matrix D_r_t = - Dunrotate2(pose.rotation()); // negative because of sub + /* ************************************************************************* */ + Matrix Dtransform_from2(const Pose3& pose) { + return pose.rotation().matrix(); + } - Matrix D_r_pose = collect(2,&D_r_R,&D_r_t); - return D_r_pose; -} + /* ************************************************************************* */ + Point3 transform_to(const Pose3& pose, const Point3& p) { + Point3 sub = p - pose.translation(); + return unrotate(pose.rotation(), sub); + } -/* ************************************************************************* */ -Matrix Dtransform_to2(const Pose3& pose, const Point3& p) { - return Dunrotate2(pose.rotation()); -} + /* ************************************************************************* */ + Matrix Dtransform_to1(const Pose3& pose, const Point3& p) { + Point3 q = transform_to(pose,p); + Matrix Rt = pose.rotation().transpose(); + Matrix DR = skewSymmetric(q.x(), q.y(), q.z()) * Rt; + Matrix DT = - Rt; // negative because of sub + return collect(2,&DR,&DT); + } -/* ************************************************************************* */ -// direct measurement of the deviation of a pose from the origin -// used as soft prior -/* ************************************************************************* */ -Vector hPose (const Vector& x) { - Pose3 pose(x); // transform from vector to Pose3 - Vector w = pose.rotation().ypr(); // get angle differences - Vector d = pose.translation().vector(); // get translation differences - return concatVectors(2,&w,&d); -} + /* ************************************************************************* */ + Matrix Dtransform_to2(const Pose3& pose, const Point3& p) { + return pose.rotation().transpose(); + } -/* ************************************************************************* */ -// derivative of direct measurement -// 6*6, entry i,j is how measurement error will change -/* ************************************************************************* */ -Matrix DhPose(const Vector& x) { - Matrix H = eye(6,6); - return H; -} + /* ************************************************************************* */ + // direct measurement of the deviation of a pose from the origin + // used as soft prior + /* ************************************************************************* */ + Vector hPose (const Vector& x) { + Pose3 pose(x); // transform from vector to Pose3 + Vector w = pose.rotation().ypr(); // get angle differences + Vector d = pose.translation().vector(); // get translation differences + return concatVectors(2,&w,&d); + } -/* ************************************************************************* */ -Pose3 Pose3::inverse() const { - Rot3 Rt = R_.inverse(); - return Pose3(Rt, -(Rt * t_)); -} + /* ************************************************************************* */ + // derivative of direct measurement + // 6*6, entry i,j is how measurement error will change + /* ************************************************************************* */ + Matrix DhPose(const Vector& x) { + Matrix H = eye(6,6); + return H; + } -/* ************************************************************************* */ -Pose3 Pose3::transform_to(const Pose3& pose) const { - Rot3 cRv = R_ * Rot3(pose.R_.inverse()); - Point3 t = gtsam::transform_to(pose, t_); - return Pose3(cRv, t); -} + /* ************************************************************************* */ + /* + Pose3 compose(const Pose3& p1, const Pose3& p2) { + return Pose3(compose(p1.rotation(),p2.rotation()),transform_from(p1,p2.translation()); + } + */ -/* ************************************************************************* */ -Pose3 between(const Pose3& p1, const Pose3& p2){ - Pose3 p; - return p; - // TODO: implement -} + /* ************************************************************************* */ + Matrix Dcompose1(const Pose3& p1, const Pose3& p2){ + Matrix DR_R1 = eye(3); + Matrix DR_t1 = zeros(3,3); + Matrix DR = collect(2,&DR_R1,&DR_t1); + Matrix Dt = Dtransform_from1(p1,p2.translation()); + return stack(2,&DR,&Dt); + } -/* ************************************************************************* */ -Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ - Matrix m; - return m; - // TODO: implement -} + /* ************************************************************************* */ + Matrix Dcompose2(const Pose3& p1, const Pose3& p2){ + Matrix DR_R2 = p1.rotation().matrix(); + Matrix DR_t2 = zeros(3,3); + Matrix DR = collect(2,&DR_R2,&DR_t2); + Matrix Dt_R2 = zeros(3,3); + Matrix Dt_t2 = Dtransform_from2(p1); + Matrix Dt = collect(2,&Dt_R2,&Dt_t2); + return stack(2,&DR,&Dt); + } -/* ************************************************************************* */ -Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ - Matrix m; - return m; - // TODO: implement -} + /* ************************************************************************* */ + // inverse = Pose3(inverse(p.rotation()),-unrotate(p.rotation(),p.translation())); + Matrix Dinverse(const Pose3& p){ + Matrix Rt = p.rotation().transpose(); + Matrix DR_R1 = -Rt; + Matrix DR_t1 = zeros(3,3); + Matrix DR = collect(2,&DR_R1,&DR_t1); + Matrix Dt_R1 = - (skewSymmetric(Rt*p.translation().vector()) * Rt); + Matrix Dt_t1 = - Rt; + Matrix Dt = collect(2,&Dt_R1,&Dt_t1); + return stack(2,&DR,&Dt); + } -/* ************************************************************************* */ + /* ************************************************************************* */ + // between = compose(p2,inverse(p1)); + + Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ + Pose3 invp1 = inverse(p1); + return Dinverse(p1) * Dcompose2(p2,invp1); + } + + Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ + Pose3 invp1 = inverse(p1); + return Dcompose1(p2,invp1); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 4d24ef03b..633fed8b9 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -7,149 +7,149 @@ #pragma once +#include + #include "Point3.h" #include "Rot3.h" #include "Testable.h" +#include "Lie.h" namespace gtsam { - /** A 3D pose (R,t) : (Rot3,Point3) */ - class Pose3 : Testable { - private: - Rot3 R_; - Point3 t_; + /** A 3D pose (R,t) : (Rot3,Point3) */ + class Pose3 : Testable { + private: + Rot3 R_; + Point3 t_; - public: + public: - /** - * Default constructor is origin - */ - Pose3() {} + /** Default constructor is origin */ + Pose3() {} - /** - * Copy constructor - */ - Pose3(const Pose3& pose) : - R_(pose.R_), t_(pose.t_) { - } + /** Copy constructor */ + Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} - /** - * Construct from R,t - */ - Pose3(const Rot3& R, const Point3& t) : - R_(R), t_(t) { - } + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { - } + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), + T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - /** Constructor from 12D vector */ - Pose3(const Vector &V) : - R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), - t_(V(9), V(10),V(11)) { - } + /** Constructor from 12D vector */ + Pose3(const Vector &V) : + R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), + t_(V(9), V(10),V(11)) {} - /** print with optional string */ - void print(const std::string& s = "") const; + const Rot3& rotation() const { return R_; } - /** assert equality up to a tolerance */ - bool equals(const Pose3& pose, double tol = 1e-9) const; + const Point3& translation() const { return t_; } - const Rot3& rotation() const { - return R_; - } + /** convert to 4*4 matrix */ + Matrix matrix() const; - const Point3& translation() const { - return t_; - } + /** print with optional string */ + void print(const std::string& s = "") const; - /** return DOF, dimensionality of tangent space = 6 */ - size_t dim() const { - return 6; - } + /** assert equality up to a tolerance */ + bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Given 6-dim tangent vector, create new pose */ - Pose3 exmap(const Vector& d) const; + Pose3 transform_to(const Pose3& pose) const; - /** inverse transformation */ - Pose3 inverse() const; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } + }; // Pose3 class - // operators - Pose3 operator+(const Pose3& p3) const{ - // TODO implement the operators "+" - Pose3 p; - return p; - }; + // Dimensionality of the tangent space + inline size_t dim(const Pose3&) { return 6; } - Pose3 operator-(const Pose3& p3) const{ - // TODO implement the operators "-" - Pose3 p; - return p; - }; + // Compose two poses + inline Pose3 compose(const Pose3& p0, const Pose3& p1) { + return Pose3(p0.rotation()*p1.rotation(), + p0.translation() + p0.rotation()*p1.translation()); + } - /** composition */ - inline Pose3 operator*(const Pose3& B) const { - return Pose3(R_*B.R_, t_ + R_*B.t_); - } + // 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())); + } - /** return 12D vectorized form (column-wise) */ - Vector vector() const; + // Exponential map at identity - create a pose with a translation and + // rotation (in canonical coordinates) + template<> inline Pose3 expmap(const Vector& d) { + Vector w = sub(d, 0,3); + Vector u = sub(d, 3,6); + return Pose3(expmap (w), expmap (u)); + } - /** convert to 4*4 matrix */ - Matrix matrix() const; + // Log map at identity - return the translation and canonical rotation + // coordinates of a pose. + inline Vector logmap(const Pose3& p) { + const Vector w = logmap(p.rotation()), u = logmap(p.translation()); + return concatVectors(2, &w, &u); + } - /** transforms */ - Pose3 transform_to(const Pose3& transform) const; + // todo: these are the "old-style" expmap and logmap about the specified + // pose. + // Increments the offset and rotation independently given a translation and + // canonical rotation coordinates + template<> inline Pose3 expmap(const Pose3& p0, const Vector& d) { + return Pose3(expmap(p0.rotation(), sub(d, 0, 3)), + expmap(p0.translation(), sub(d, 3, 6))); + } - /** friends */ - friend Point3 transform_from(const Pose3& pose, const Point3& p); - friend Point3 transform_to(const Pose3& pose, const Point3& p); - friend Pose3 compose(const Pose3& current, const Pose3& target); + // Independently computes the logmap of the translation and rotation. + template<> inline Vector logmap(const Pose3& p0, const Pose3& pp) { + const Vector r(logmap(p0.rotation(), pp.rotation())), + t(logmap(p0.translation(), pp.translation())); + return concatVectors(2, &r, &t); + } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - }; // Pose3 class - /** - * Finds a transform from the current frame dTx to the target frame sTx - */ - inline Pose3 compose(const Pose3& dTx, const Pose3& sTx) { - return dTx * sTx.inverse(); - } + /** receives the point in Pose coordinates and transforms it to world coordinates */ + Point3 transform_from(const Pose3& pose, const Point3& p); + inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); } + Matrix Dtransform_from1(const Pose3& pose, const Point3& p); + Matrix Dtransform_from2(const Pose3& pose); // does not depend on p ! - /** receives the point in Pose coordinates and transforms it to world coordinates */ - Point3 transform_from(const Pose3& pose, const Point3& p); - Matrix Dtransform_from1(const Pose3& pose, const Point3& p); - Matrix Dtransform_from2(const Pose3& pose); // does not depend on p ! + /** receives the point in world coordinates and transforms it to Pose coordinates */ + Point3 transform_to(const Pose3& pose, const Point3& p); + Matrix Dtransform_to1(const Pose3& pose, const Point3& p); + Matrix Dtransform_to2(const Pose3& pose, const Point3& p); - /** receives the point in world coordinates and transforms it to Pose coordinates */ - Point3 transform_to(const Pose3& pose, const Point3& p); - Matrix Dtransform_to1(const Pose3& pose, const Point3& p); - Matrix Dtransform_to2(const Pose3& pose, const Point3& p); + /** + * Derivatives of compose + */ + Matrix Dcompose1(const Pose3& p1, const Pose3& p2); + Matrix Dcompose2(const Pose3& p1, const Pose3& p2); - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose3 between(const Pose3& p1, const Pose3& p2); - Matrix Dbetween1(const Pose3& p1, const Pose3& p2); - Matrix Dbetween2(const Pose3& p1, const Pose3& p2); + /** + * Derivative of inverse + */ + Matrix Dinverse(const Pose3& p); - /** direct measurement of a pose */ - Vector hPose(const Vector& x); + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Matrix Dbetween1(const Pose3& p1, const Pose3& p2); + Matrix Dbetween2(const Pose3& p1, const Pose3& p2); - /** - * derivative of direct measurement - * 12*6, entry i,j is how measurement error will change - */ - Matrix DhPose(const Vector& x); + /** direct measurement of a pose */ + Vector hPose(const Vector& x); + + /** + * derivative of direct measurement + * 12*6, entry i,j is how measurement error will change + */ + Matrix DhPose(const Vector& x); } // namespace gtsam diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index e23366b68..8e6c454d3 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -52,7 +52,11 @@ namespace gtsam { Vector error_vector(const Config& config) const { //z-h Pose3 p1 = config.get(key1_), p2 = config.get(key2_); - return (measured_ - between(p1, p2)).vector(); + // todo: removed vector() from Pose3, so emulating it here! This is incorrect! + Pose3 betw = between(p1,p2); + Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation()); + Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation()); + return concatVectors(2, &r_diff, &t_diff); } std::list keys() const { @@ -66,7 +70,11 @@ namespace gtsam { /** linearize */ boost::shared_ptr linearize(const Config& config) const { Pose3 p1 = config.get(key1_), p2 = config.get(key2_); - Vector b = (measured_ - between(p1, p2)).vector(); + Pose3 betw = between(p1,p2); + // todo: removed vector() from Pose3, so emulating it here! This is incorrect! + Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation()); + Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation()); + Vector b = concatVectors(2, &r_diff, &t_diff); Matrix H1 = Dbetween1(p1, p2); Matrix H2 = Dbetween2(p1, p2); boost::shared_ptr linearized(new GaussianFactor(key1_, diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 3c2c2a54e..9151fc6d7 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -21,12 +21,6 @@ namespace gtsam { return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; } - /* ************************************************************************* */ - Rot2 Rot2::exmap(const Vector& v) const { - if (zero(v)) return (*this); - return Rot2(v(0)) * (*this); // exponential map then compose - } - /* ************************************************************************* */ Matrix Rot2::matrix() const { return Matrix_(2, 2, c_, -s_, s_, c_); @@ -42,32 +36,6 @@ namespace gtsam { return Matrix_(2, 2, -c_, -s_, s_, -c_); } - /* ************************************************************************* */ - Rot2 Rot2::inverse() const { return Rot2(c_, -s_);} - - /* ************************************************************************* */ - Rot2 Rot2::invcompose(const Rot2& R) const { - return Rot2( - c_ * R.c_ + s_ * R.s_, - -s_ * R.c_ + c_ * R.s_); - } - - /* ************************************************************************* */ - Rot2 Rot2::operator*(const Rot2& R) const { - return Rot2( - c_ * R.c_ - s_ * R.s_, - s_ * R.c_ + c_ * R.s_ - ); - } - - /* ************************************************************************* */ - Point2 Rot2::operator*(const Point2& p) const { - return Point2( - c_ * p.x() - s_ * p.y(), - s_ * p.x() + c_ * p.y() - ); - } - /* ************************************************************************* */ Point2 Rot2::unrotate(const Point2& p) const { return Point2( @@ -76,14 +44,11 @@ namespace gtsam { ); } - /* ************************************************************************* */ - Rot2 exmap(const Rot2& R, const Vector& v) { - return R.exmap(v); - } - /* ************************************************************************* */ Point2 rotate(const Rot2& R, const Point2& p) { - return R * p; + return Point2( + R.c() * p.x() - R.s() * p.y(), + R.s() * p.x() + R.c() * p.y()); } /* ************************************************************************* */ diff --git a/cpp/Rot2.h b/cpp/Rot2.h index f9dad3c6d..a0a4d139d 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -11,6 +11,7 @@ #include "Testable.h" #include "Point2.h" #include "Matrix.h" +#include "Lie.h" namespace gtsam { @@ -20,13 +21,13 @@ namespace gtsam { /** we store cos(theta) and sin(theta) */ double c_, s_; - /** private constructor from cos/sin */ + public: + + /** constructor from cos/sin */ Rot2(double c, double s) : c_(c), s_(s) { } - public: - /** default constructor, zero rotation */ Rot2() : c_(1.0), s_(0.0) {} @@ -48,18 +49,6 @@ namespace gtsam { /** equals with an tolerance */ bool equals(const Rot2& R, double tol = 1e-9) const; - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return 1;} - - /** Given 1-dim tangent vector, create new rotation */ - Rot2 exmap(const Vector& d) const; - - /** Return the 1-dim tangent vector of R about this rotation */ - Vector log(const Rot2& R) const { return Vector_(1, R.theta() - theta()); } - - /** return vectorized form (column-wise)*/ - inline Vector vector() const { return Vector_(2,c_,s_);} - /** return 2*2 rotation matrix */ Matrix matrix() const; @@ -69,18 +58,6 @@ namespace gtsam { /** return 2*2 negative transpose */ Matrix negtranspose() const; - /** inverse transformation */ - Rot2 inverse() const; - - /** compose with the inverse of this rotation */ - Rot2 invcompose(const Rot2& R) const; - - /** composition via sum and difference formulas */ - Rot2 operator*(const Rot2& R) const; - - /** rotate from rotated to world, syntactic sugar = R*p */ - Point2 operator*(const Point2& p) const; - /** rotate from world to rotated = R'*p */ Point2 unrotate(const Point2& p) const; @@ -97,19 +74,54 @@ namespace gtsam { } }; - /** - * Update Rotation with incremental rotation - * @param v a vector of incremental angle - * @param R a 2D rotation - * @return incremental rotation matrix - */ - Rot2 exmap(const Rot2& R, const Vector& v); + + // Lie group functions + + // 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(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& 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); + } + + // 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()); + } + /** * rotate point from rotated coordinate frame to * world = R*p */ Point2 rotate(const Rot2& R, const Point2& p); + inline Point2 operator*(const Rot2& R, const Point2& p) { + return rotate(R,p); + } Matrix Drotate1(const Rot2& R, const Point2& p); Matrix Drotate2(const Rot2& R); // does not depend on p ! diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 3bd8d0c1b..92a15484e 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -13,202 +13,178 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } + bool Rot3::equals(const Rot3 & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); + } - /* ************************************************************************* */ - Rot3 Rot3::exmap(const Vector& v) const { - if (zero(v)) return (*this); - return rodriguez(v) * (*this); - } - - /* ************************************************************************* */ - Vector Rot3::log() const { - double tr = r1_.x()+r2_.y()+r3_.z(); - if (tr==3.0) return ones(3); - if (tr==-1.0) throw domain_error("Rot3::log: trace == -1 not yet handled :-(");; - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - r2_.z()-r3_.y(), - r3_.x()-r1_.z(), - r1_.y()-r2_.x()); - } /* ************************************************************************* */ - Vector Rot3::vector() const { - double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z() }; - Vector v(9); - copy(r,r+9,v.begin()); - return v; - } + // Vector Rot3::vector() const { + // double r[] = { r1_.x(), r1_.y(), r1_.z(), + // r2_.x(), r2_.y(), r2_.z(), + // r3_.x(), r3_.y(), r3_.z() }; + // Vector v(9); + // copy(r,r+9,v.begin()); + // return v; + // } /* ************************************************************************* */ Matrix Rot3::matrix() const { double r[] = { r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z() }; + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z() }; return Matrix_(3,3, r); } /* ************************************************************************* */ Matrix Rot3::transpose() const { double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()}; + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()}; return Matrix_(3,3, r); } /* ************************************************************************* */ Point3 Rot3::column(int index) const{ - if(index == 3) - return r3_; - else if (index == 2) - return r2_; - else - return r1_; // default returns r1 + if(index == 3) + return r3_; + else if (index == 2) + return r2_; + else + return r1_; // default returns r1 } /* ************************************************************************* */ - Rot3 Rot3::inverse() const { - return Rot3( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } - - /* ************************************************************************* */ - Point3 Rot3::unrotate(const Point3& p) const { - return Point3( - r1_.x() * p.x() + r1_.y() * p.y() + r1_.z() * p.z(), - r2_.x() * p.x() + r2_.y() * p.y() + r2_.z() * p.z(), - r3_.x() * p.x() + r3_.y() * p.y() + r3_.z() * p.z() - ); - } - - /* ************************************************************************* */ Vector Rot3::ypr() const { - Matrix I;Vector q; - boost::tie(I,q)=RQ(matrix()); - return q; + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; } - /* ************************************************************************* */ - Rot3 rodriguez(const Vector& n, double t) { - double n0 = n(0), n1=n(1), n2=n(2); - double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2; + /* ************************************************************************* */ + Rot3 rodriguez(const Vector& n, double t) { + double n0 = n(0), n1=n(1), n2=n(2); + double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2; #ifndef NDEBUG - double l_n = n00+n11+n22; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + double l_n = n00+n11+n22; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif - double ct = cos(t), st = sin(t), ct_1 = 1 - ct; + double ct = cos(t), st = sin(t), ct_1 = 1 - ct; - double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st; - double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2; - double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22; + double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st; + double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2; + double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22; - Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02); - Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12); - Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22); + Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02); + Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12); + Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22); - return Rot3(r1, r2, r3); - } - - /* ************************************************************************* */ - Rot3 rodriguez(const Vector& w) { - double t = norm_2(w); - if (t < 1e-5) return Rot3(); - return rodriguez(w/t, t); - } - - /* ************************************************************************* */ - Rot3 exmap(const Rot3& R, const Vector& v) { - return R.exmap(v); - } - - /* ************************************************************************* */ - Vector log(const Rot3& R, const Rot3& S) { - return between(R,S).log(); + return Rot3(r1, r2, r3); } - /* ************************************************************************* */ - Point3 rotate(const Rot3& R, const Point3& p) { - return R * p; - } - /* ************************************************************************* */ - Matrix Drotate1(const Rot3& R, const Point3& p) { - Point3 q = R * p; - return skewSymmetric(-q.x(), -q.y(), -q.z()); - } + /* ************************************************************************* */ + Rot3 rodriguez(const Vector& w) { + double t = norm_2(w); + if (t < 1e-5) return Rot3(); + return rodriguez(w/t, t); + } - /* ************************************************************************* */ - Matrix Drotate2(const Rot3& R) { - return R.matrix(); - } + /* ************************************************************************* */ + Point3 rotate(const Rot3& R, const Point3& p) { + return R.r1() * p.x() + R.r2() * p.y() + R.r3() * p.z(); + } - /* ************************************************************************* */ - Point3 unrotate(const Rot3& R, const Point3& p) { - return R.unrotate(p); - } + /* ************************************************************************* */ + Matrix Drotate1(const Rot3& R, const Point3& p) { + Point3 q = R * p; + return skewSymmetric(-q.x(), -q.y(), -q.z()); + } - /* ************************************************************************* */ - /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ - /* ************************************************************************* */ - Matrix Dunrotate1(const Rot3 & R, const Point3 & p) { - Point3 q = R.unrotate(p); - return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose(); - } + /* ************************************************************************* */ + Matrix Drotate2(const Rot3& R) { + return R.matrix(); + } - /* ************************************************************************* */ - Matrix Dunrotate2(const Rot3 & R) { - return R.transpose(); - } + /* ************************************************************************* */ + Point3 unrotate(const Rot3& R, const Point3& p) { + return Point3( + R.r1().x() * p.x() + R.r1().y() * p.y() + R.r1().z() * p.z(), + R.r2().x() * p.x() + R.r2().y() * p.y() + R.r2().z() * p.z(), + R.r3().x() * p.x() + R.r3().y() * p.y() + R.r3().z() * p.z() + ); + } - /* ************************************************************************* */ - Rot3 between(const Rot3& R1, const Rot3& R2) { - return R2 * R1.inverse(); - } + /* ************************************************************************* */ + /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ + /* ************************************************************************* */ + Matrix Dunrotate1(const Rot3 & R, const Point3 & p) { + Point3 q = unrotate(R,p); + return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose(); + } - /* ************************************************************************* */ - pair RQ(const Matrix& A) { - double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22); - double Cx = A22 / a; //cosX - double Sx = -A21 / a; //sinX - Matrix Qx = Matrix_(3, 3, - 1.0, 0.0, 0.0, - 0.0, Cx, -Sx, - 0.0, Sx, Cx); - Matrix B = A * Qx; + /* ************************************************************************* */ + Matrix Dunrotate2(const Rot3 & R) { + return R.transpose(); + } - double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22); - double Cy = B22 / b; //cosY - double Sy = B20 / b; //sinY - Matrix Qy = Matrix_(3,3, - Cy, 0.0, Sy, - 0.0, 1.0, 0.0, - -Sy, 0.0, Cy); - Matrix C = B * Qy; + /* ************************************************************************* */ + Matrix Dcompose1(const Rot3& R1, const Rot3& R2){ + return eye(3); + } - double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11); - double Cz = C11 / c; //cosZ - double Sz = -C10 / c; //sinZ - Matrix Qz = Matrix_(3, 3, - Cz, -Sz, 0.0, - Sz, Cz, 0.0, - 0.0, 0.0, 1.0); - Matrix R = C * Qz; + /* ************************************************************************* */ + Matrix Dcompose2(const Rot3& R1, const Rot3& R2){ + return R1.matrix(); + } - Vector angles(3); - angles(0) = -atan2(Sx, Cx); - angles(1) = -atan2(Sy, Cy); - angles(2) = -atan2(Sz, Cz); + /* ************************************************************************* */ + Matrix Dbetween1(const Rot3& R1, const Rot3& R2){ + return -between(R1,R2).matrix(); + } - return make_pair(R,angles); - } + /* ************************************************************************* */ + Matrix Dbetween2(const Rot3& R1, const Rot3& R2){ + return eye(3); + } -/* ************************************************************************* */ + /* ************************************************************************* */ + pair RQ(const Matrix& A) { + double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22); + double Cx = A22 / a; //cosX + double Sx = -A21 / a; //sinX + Matrix Qx = Matrix_(3, 3, + 1.0, 0.0, 0.0, + 0.0, Cx, -Sx, + 0.0, Sx, Cx); + Matrix B = A * Qx; + + double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22); + double Cy = B22 / b; //cosY + double Sy = B20 / b; //sinY + Matrix Qy = Matrix_(3,3, + Cy, 0.0, Sy, + 0.0, 1.0, 0.0, + -Sy, 0.0, Cy); + Matrix C = B * Qy; + + double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11); + double Cz = C11 / c; //cosZ + double Sz = -C10 / c; //sinZ + Matrix Qz = Matrix_(3, 3, + Cz, -Sz, 0.0, + Sz, Cz, 0.0, + 0.0, 0.0, 1.0); + Matrix R = C * Qz; + + Vector angles(3); + angles(0) = -atan2(Sx, Cx); + angles(1) = -atan2(Sy, Cy); + angles(2) = -atan2(Sz, Cz); + + return make_pair(R,angles); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Rot3.h b/cpp/Rot3.h index b290f3892..b8857116d 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -12,71 +12,53 @@ #include "Point3.h" #include "Testable.h" +#include "Lie.h" namespace gtsam { - /* Rotation matrix */ + /* 3D Rotation */ class Rot3: Testable { private: /** we store columns ! */ Point3 r1_, r2_, r3_; - + public: - - /** default constructor, unit rotation */ - Rot3() : r1_(Point3(1.0,0.0,0.0)), - r2_(Point3(0.0,1.0,0.0)), - r3_(Point3(0.0,0.0,1.0)) { - } - /** constructor from columns */ - Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) { - } - - /** constructor from vector */ - Rot3(const Vector &v) : - r1_(Point3(v(0),v(1),v(2))), - r2_(Point3(v(3),v(4),v(5))), - r3_(Point3(v(6),v(7),v(8))) - { } + /** default constructor, unit rotation */ + Rot3() : + r1_(Point3(1.0,0.0,0.0)), + r2_(Point3(0.0,1.0,0.0)), + r3_(Point3(0.0,0.0,1.0)) {} - /** constructor from doubles in *row* order !!! */ - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) : - r1_(Point3(R11, R21, R31)), - r2_(Point3(R12, R22, R32)), - r3_(Point3(R13, R23, R33)) {} + /** constructor from columns */ + Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : + r1_(r1), r2_(r2), r3_(r3) {} - /** constructor from matrix */ - Rot3(const Matrix& R): - r1_(Point3(R(0,0), R(1,0), R(2,0))), - r2_(Point3(R(0,1), R(1,1), R(2,1))), - r3_(Point3(R(0,2), R(1,2), R(2,2))) {} + /** constructor from vector */ + Rot3(const Vector &v) : + r1_(Point3(v(0),v(1),v(2))), + r2_(Point3(v(3),v(4),v(5))), + r3_(Point3(v(6),v(7),v(8))) {} - /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + /** constructor from doubles in *row* order !!! */ + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) : + r1_(Point3(R11, R21, R31)), + r2_(Point3(R12, R22, R32)), + r3_(Point3(R13, R23, R33)) {} - /** equals with an tolerance */ - bool equals(const Rot3& p, double tol = 1e-9) const; + /** constructor from matrix */ + Rot3(const Matrix& R): + r1_(Point3(R(0,0), R(1,0), R(2,0))), + r2_(Point3(R(0,1), R(1,1), R(2,1))), + r3_(Point3(R(0,2), R(1,2), R(2,2))) {} - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return 3;} - - /** - * @param a 3-dim tangent vector d (canonical coordinates of between(R,S)) - * @return new rotation S=exp(d)*R - */ - Rot3 exmap(const Vector& d) const; - - /** - * @return log(R), i.e. canonical coordinates of R - */ - Vector log() const; + /** print */ + void print(const std::string& s="R") const { gtsam::print(matrix(), s);} - /** return vectorized form (column-wise)*/ - Vector vector() const; + /** equals with an tolerance */ + bool equals(const Rot3& p, double tol = 1e-9) const; /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -86,39 +68,26 @@ namespace gtsam { /** returns column vector specified by index */ Point3 column(int index) const; - - /** inverse transformation */ - Rot3 inverse() const; - - /** composition */ - inline Rot3 operator*(const Rot3& B) const { return Rot3(matrix()*B.matrix());} - - /** rotate from rotated to world, syntactic sugar = R*p */ - inline Point3 operator*(const Point3& p) const { - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); - } - - /** rotate from world to rotated = R'*p */ - Point3 unrotate(const Point3& p) const; + Point3 r1() const { return r1_; } + Point3 r2() const { return r2_; } + Point3 r3() const { return r3_; } /** use RQ to calculate yaw-pitch-roll angle representation */ Vector ypr() const; - /** friends */ - friend Matrix Dunrotate1(const Rot3& R, const Point3& p); - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(r1_); - ar & BOOST_SERIALIZATION_NVP(r2_); - ar & BOOST_SERIALIZATION_NVP(r3_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(r1_); + ar & BOOST_SERIALIZATION_NVP(r2_); + ar & BOOST_SERIALIZATION_NVP(r3_); + } }; + /** * Rodriguez' formula to compute an incremental rotation matrix * @param w is the rotation axis, unit length @@ -143,26 +112,62 @@ namespace gtsam { */ 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 + inline Vector logmap(const Rot3& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + if (tr==3.0) return ones(3); // todo: identity? + if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");; + double theta = acos((tr-1.0)/2.0); + return (theta/2.0/sin(theta))*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + } + + // Compose two rotations + inline Rot3 compose(const Rot3& R1, const Rot3& R2) { + return Rot3(R1.matrix() * R2.matrix()); } + + // 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()); + } + /** + * Update Rotation with incremental rotation * @param v a vector of incremental roll,pitch,yaw * @param R a rotated frame * @return incremental rotation matrix */ - Rot3 exmap(const Rot3& R, const Vector& v); + //Rot3 exp(const Rot3& R, const Vector& v); /** * @param a rotation R * @param a rotation S * @return log(S*R'), i.e. canonical coordinates of between(R,S) */ - Vector log(const Rot3& R, const Rot3& S); + //Vector log(const Rot3& R, const Rot3& S); /** * rotate point from rotated coordinate frame to * world = R*p */ - Point3 rotate (const Rot3& R, const Point3& p); + Point3 rotate(const Rot3& R, const Point3& p); + inline Point3 operator*(const Rot3& R, const Point3& p) { return rotate(R,p); } Matrix Drotate1(const Rot3& R, const Point3& p); Matrix Drotate2(const Rot3& R); // does not depend on p ! @@ -170,25 +175,34 @@ namespace gtsam { * rotate point from world to rotated * frame = R'*p */ - Point3 unrotate (const Rot3& R, const Point3& p); + Point3 unrotate(const Rot3& R, const Point3& p); Matrix Dunrotate1(const Rot3& R, const Point3& p); Matrix Dunrotate2(const Rot3& R); // does not depend on p ! - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R1, const Rot3& R2); + /** + * compose two rotations i.e., R=R1*R2 + */ + //Rot3 compose (const Rot3& R1, const Rot3& R2); + Matrix Dcompose1(const Rot3& R1, const Rot3& R2); + Matrix Dcompose2(const Rot3& R1, const Rot3& R2); - /** - * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R - * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' - * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will - * be the identity and Q is a yaw-pitch-roll decomposition of A. - * The implementation uses Givens rotations and is based on Hartley-Zisserman. - * @param a 3 by 3 matrix A=RQ - * @return an upper triangular matrix R - * @return a vector [thetax, thetay, thetaz] in radians. - */ + /** + * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' + */ + //Rot3 between (const Rot3& R1, const Rot3& R2); + Matrix Dbetween1(const Rot3& R1, const Rot3& R2); + Matrix Dbetween2(const Rot3& R1, const Rot3& R2); + + /** + * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R + * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' + * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will + * be the identity and Q is a yaw-pitch-roll decomposition of A. + * The implementation uses Givens rotations and is based on Hartley-Zisserman. + * @param a 3 by 3 matrix A=RQ + * @return an upper triangular matrix R + * @return a vector [thetax, thetay, thetaz] in radians. + */ std::pair RQ(const Matrix& A); } diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index a7d5fd086..9e813403a 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -126,8 +126,8 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { if (verbose) delta.print("Delta Config"); // update both state variables - shared_config newConfig(new C(config_->exmap(delta))); - shared_vconfig newLambdas(new VectorConfig(lagrange_config_->exmap(delta))); + shared_config newConfig(new C(expmap(*config_, delta))); + shared_vconfig newLambdas(new VectorConfig(expmap(*lagrange_config_, delta))); // construct a new optimizer return SQPOptimizer(*graph_, full_ordering_, newConfig, newLambdas); diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp index c6e498abc..46a09d5fe 100644 --- a/cpp/VSLAMConfig.cpp +++ b/cpp/VSLAMConfig.cpp @@ -23,7 +23,7 @@ namespace gtsam { /* ************************************************************************* */ // Exponential map -VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { +VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta) { VSLAMConfig newConfig; @@ -32,16 +32,16 @@ VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { string key = it->first; if (key[0] == 'x') { int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str()); - if (cameraPoseExists(cameraNumber)) { - Pose3 basePose = cameraPose(cameraNumber); - newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second)); + if (c.cameraPoseExists(cameraNumber)) { + Pose3 basePose = c.cameraPose(cameraNumber); + newConfig.addCameraPose(cameraNumber, expmap(basePose, it->second)); } } if (key[0] == 'l') { int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str()); - if (landmarkPointExists(landmarkNumber)) { - Point3 basePoint = landmarkPoint(landmarkNumber); - newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second)); + if (c.landmarkPointExists(landmarkNumber)) { + Point3 basePoint = c.landmarkPoint(landmarkNumber); + newConfig.addLandmarkPoint(landmarkNumber, expmap(basePoint, it->second)); } } } diff --git a/cpp/VSLAMConfig.h b/cpp/VSLAMConfig.h index 071a7bd39..7b3213f22 100644 --- a/cpp/VSLAMConfig.h +++ b/cpp/VSLAMConfig.h @@ -44,13 +44,6 @@ class VSLAMConfig : Testable { VSLAMConfig(const VSLAMConfig& original): cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){} - /** - * Exponential map: takes 6D vectors in VectorConfig - * and applies them to the poses in the VSLAMConfig. - * Needed for use in nonlinear optimization - */ - VSLAMConfig exmap(const VectorConfig & delta) const; - PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses_.begin();} PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses_.end();} PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();} @@ -115,7 +108,18 @@ class VSLAMConfig : Testable { inline size_t size(){ return landmarkPoints_.size() + cameraPoses_.size(); } + + friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); }; + +/** + * Exponential map: takes 6D vectors in VectorConfig + * and applies them to the poses in the VSLAMConfig. + * Needed for use in nonlinear optimization + */ +VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); + + } // namespace gtsam diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp index 4d02dbc80..02aff85b0 100644 --- a/cpp/VectorConfig.cpp +++ b/cpp/VectorConfig.cpp @@ -107,11 +107,11 @@ VectorConfig VectorConfig::operator-(const VectorConfig& b) const { } /* ************************************************************************* */ -VectorConfig VectorConfig::exmap(const VectorConfig& delta) const +VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta) { VectorConfig newConfig; string j; Vector vj; - FOREACH_PAIR(j, vj, values) { + FOREACH_PAIR(j, vj, original.values) { if (delta.contains(j)) { const Vector& dj = delta[j]; check_size(j,vj,dj); @@ -124,12 +124,12 @@ VectorConfig VectorConfig::exmap(const VectorConfig& delta) const } /* ************************************************************************* */ -VectorConfig VectorConfig::exmap(const Vector& delta) const +VectorConfig expmap(const VectorConfig& original, const Vector& delta) { VectorConfig newConfig; size_t i = 0; string j; Vector vj; - FOREACH_PAIR(j, vj, values) { + FOREACH_PAIR(j, vj, original.values) { size_t mj = vj.size(); Vector dj = sub(delta, i, i+mj); newConfig.insert(j, vj + dj); diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h index 0f0d86446..3084f9eb3 100644 --- a/cpp/VectorConfig.h +++ b/cpp/VectorConfig.h @@ -43,18 +43,6 @@ namespace gtsam { /** Add to vector at position j */ void add(const std::string& j, const Vector& a); - /** - * Add a delta config, needed for use in NonlinearOptimizer - * For VectorConfig, this is just addition. - */ - VectorConfig exmap(const VectorConfig& delta) const; - - /** - * Add a delta vector (not a config) - * Will use the ordering that map uses to loop over vectors - */ - VectorConfig exmap(const Vector& delta) const; - const_iterator begin() const {return values.begin();} const_iterator end() const {return values.end();} @@ -107,6 +95,17 @@ namespace gtsam { /** Dot product */ double dot(const VectorConfig& b) const; + /** + * Add a delta config, needed for use in NonlinearOptimizer + * For VectorConfig, this is just addition. + */ + friend VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta); + /** + * Add a delta vector (not a config) + * Will use the ordering that map uses to loop over vectors + */ + friend VectorConfig expmap(const VectorConfig& original, const Vector& delta); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/cpp/numericalDerivative.h b/cpp/numericalDerivative.h index ddbc9cc59..6455d7793 100644 --- a/cpp/numericalDerivative.h +++ b/cpp/numericalDerivative.h @@ -10,12 +10,14 @@ #include "Matrix.h" +//#define LINEARIZE_AT_IDENTITY + namespace gtsam { /** * Numerically compute gradient of scalar function * Class X is the input argument - * The class X needs to have dim, exmap, vector + * The class X needs to have dim, expmap, vector */ template Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { @@ -24,8 +26,8 @@ namespace gtsam { const size_t n = x.dim(); Vector d(n,0.0), g(n,0.0); for (size_t j=0;j Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - Vector hx = h(x).vector(); + Y hx = h(x); double factor = 1.0/(2.0*delta); - const size_t m = hx.size(), n = x.dim(); + const size_t m = dim(hx), n = dim(x); Vector d(n,0.0); Matrix H = zeros(m,n); for (size_t j=0;j Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - Vector hx = h(x1,x2).vector(); + Y hx = h(x1,x2); double factor = 1.0/(2.0*delta); - const size_t m = hx.size(), n = x1.dim(); + const size_t m = dim(hx), n = dim(x1); Vector d(n,0.0); Matrix H = zeros(m,n); for (size_t j=0;j Matrix numericalDerivative22 (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - Vector hx = h(x1,x2).vector(); + Y hx = h(x1,x2); double factor = 1.0/(2.0*delta); - const size_t m = hx.size(), n = x2.dim(); + const size_t m = dim(hx), n = dim(x2); Vector d(n,0.0); Matrix H = zeros(m,n); for (size_t j=0;j Matrix numericalDerivative31 (Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - Vector hx = h(x1,x2,x3).vector(); + Y hx = h(x1,x2,x3); double factor = 1.0/(2.0*delta); - const size_t m = hx.size(), n = x1.dim(); + const size_t m = dim(hx), n = dim(x1); Vector d(n,0.0); Matrix H = zeros(m,n); for (size_t j=0;j(Vector_(3, 0.01, -0.015, 0.018)) * pose; CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(Pose2, log) { +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)); Vector expected = Vector_(3, 0.01, -0.015, 0.018); - Vector actual = pose0.log(pose); + Vector actual = logmap(pose0,pose); CHECK(assert_equal(expected, actual)); } @@ -97,6 +77,7 @@ TEST(Pose2, log) { /* ************************************************************************* */ TEST( Pose2, transform_to ) { + //cout << "transform_to" << endl; Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Point2 point(-1,4); // landmark at (-1,4) @@ -124,6 +105,7 @@ TEST( Pose2, transform_to ) /* ************************************************************************* */ TEST(Pose2, compose_a) { + //cout << "compose_a" << endl; Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5))); Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0)); @@ -133,24 +115,23 @@ TEST(Pose2, compose_a) Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 expected_point(-1.0, -1.0); - Point2 actual_point1 = pose2 * pose1 * point; - Point2 actual_point2 = (pose2 * pose1) * point; - Point2 actual_point3 = pose2 * (pose1 * point); + Point2 actual_point1 = transform_to(pose2 * pose1, point); + Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point)); CHECK(assert_equal(expected_point, actual_point1)); CHECK(assert_equal(expected_point, actual_point2)); - CHECK(assert_equal(expected_point, actual_point3)); } /* ************************************************************************* */ 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 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0)); Pose2 pose_actual_op = pose2 * pose1; - Pose2 pose_actual_fcn = pose2.compose(pose1); + Pose2 pose_actual_fcn = compose(pose2,pose1); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -159,13 +140,14 @@ 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 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0)); Pose2 pose_actual_op = pose2 * pose1; - Pose2 pose_actual_fcn = pose2.compose(pose1); + Pose2 pose_actual_fcn = compose(pose2,pose1); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -175,37 +157,32 @@ TEST(Pose2, compose_c) /* ************************************************************************* */ TEST( Pose2, between ) { + //cout << "between" << endl; Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x - // expected Pose2 expected(M_PI_2, Point2(2,2)); + Pose2 actual = between(p1,p2); + CHECK(assert_equal(expected,actual)); + Matrix expectedH1 = Matrix_(3,3, 0.0,-1.0,-2.0, 1.0, 0.0,-2.0, 0.0, 0.0,-1.0 ); + Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5); + Matrix actualH1 = Dbetween1(p1,p2); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(numericalH1,actualH1)); + Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ); - - // actual - Pose2 actual = between(p1,p2); - Matrix actualH1 = Dbetween1(p1,p2); + Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); Matrix actualH2 = Dbetween2(p1,p2); - - Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5); - numericalH1 = toManifoldDerivative(numericalH1, between(p1,p2)); - - Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); - numericalH2 = toManifoldDerivative(numericalH2, between(p1,p2)); - - CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(expectedH2,actualH2)); - CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH2,actualH2)); } diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index fbc5aadbd..2f9a93616 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -82,7 +82,7 @@ TEST(Pose2Factor, optimize) { Pose2Config feasible; feasible.insert("p0", Pose2(0,0,0)); fg.push_back(Pose2Graph::sharedFactor( - new NonlinearEquality("p0", feasible, Pose2().dim(), poseCompare))); + new NonlinearEquality("p0", feasible, dim(Pose2()), poseCompare))); fg.push_back(Pose2Graph::sharedFactor( new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3, 0.5, 0.0, 0.0, diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index 024990bd0..0b8d8d4bf 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -16,7 +16,7 @@ Pose3 T(R,t); Point3 P(0.2,0.7,-2); Rot3 r1 = rodriguez(-90, 0, 0); Pose3 pose1(r1, Point3(1, 2, 3)); -double error = 1e-9; +double error = 1e-8; #define PI 3.14159265358979323846 @@ -31,15 +31,24 @@ TEST( Pose3, equals) } /* ************************************************************************* */ -TEST( Pose3, exmap) +TEST( Pose3, expmap_a) { Pose3 id; Vector v(6); fill(v.begin(), v.end(), 0); v(0) = 0.3; - CHECK(assert_equal(id.exmap(v), Pose3(R, Point3()))); + CHECK(assert_equal(expmap(id,v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.7;v(5)=-2; - CHECK(assert_equal(id.exmap(v), Pose3(R, P))); + CHECK(assert_equal(expmap(id,v), Pose3(R, P))); +} + +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)); + CHECK(assert_equal(expected, p2)); } /* ************************************************************************* */ @@ -48,35 +57,73 @@ TEST( Pose3, compose ) Matrix actual = (T*T).matrix(); Matrix expected = T.matrix()*T.matrix(); CHECK(assert_equal(actual,expected,error)); -} + + Matrix numericalH1 = numericalDerivative21(compose, T, T, 1e-5); + Matrix actualH1 = Dcompose1(T, T); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix actualH2 = Dcompose2(T, T); + Matrix numericalH2 = numericalDerivative22(compose, T, T, 1e-5); + CHECK(assert_equal(numericalH2,actualH2));} /* ************************************************************************* */ TEST( Pose3, inverse) { - Matrix actual = T.inverse().matrix(); + Matrix actual = inverse(T).matrix(); Matrix expected = inverse(T.matrix()); CHECK(assert_equal(actual,expected,error)); + + Matrix numericalH = numericalDerivative11(inverse, T, 1e-5); + Matrix actualH = Dinverse(T); + CHECK(assert_equal(numericalH,actualH)); } /* ************************************************************************* */ TEST( Pose3, compose_inverse) { - Matrix actual = (T*T.inverse()).matrix(); + Matrix actual = (T*inverse(T)).matrix(); Matrix expected = eye(4,4); CHECK(assert_equal(actual,expected,error)); } /* ************************************************************************* */ -// transform derivatives - -TEST( Pose3, Dtransform_from1) +TEST( Pose3, Dtransform_from1_a) { Matrix computed = Dtransform_from1(T, P); Matrix numerical = numericalDerivative21(transform_from,T,P); CHECK(assert_equal(numerical,computed,error)); } +TEST( Pose3, Dtransform_from1_b) +{ + Pose3 origin; + Matrix computed = Dtransform_from1(origin, P); + Matrix numerical = numericalDerivative21(transform_from,origin,P); + CHECK(assert_equal(numerical,computed,error)); +} +TEST( Pose3, Dtransform_from1_c) +{ + Point3 origin; + Pose3 T0(R,origin); + Matrix computed = Dtransform_from1(T0, P); + Matrix numerical = numericalDerivative21(transform_from,T0,P); + CHECK(assert_equal(numerical,computed,error)); +} + +TEST( Pose3, Dtransform_from1_d) +{ + Rot3 I; + Point3 t0(100,0,0); + Pose3 T0(I,t0); + Matrix computed = Dtransform_from1(T0, P); + //print(computed, "Dtransform_from1_d computed:"); + Matrix numerical = numericalDerivative21(transform_from,T0,P); + //print(numerical, "Dtransform_from1_d numerical:"); + CHECK(assert_equal(numerical,computed,error)); +} + +/* ************************************************************************* */ TEST( Pose3, Dtransform_from2) { Matrix computed = Dtransform_from2(T); @@ -84,6 +131,7 @@ TEST( Pose3, Dtransform_from2) CHECK(assert_equal(numerical,computed,error)); } +/* ************************************************************************* */ TEST( Pose3, Dtransform_to1) { Matrix computed = Dtransform_to1(T, P); @@ -91,12 +139,14 @@ TEST( Pose3, Dtransform_to1) CHECK(assert_equal(numerical,computed,error)); } +/* ************************************************************************* */ TEST( Pose3, Dtransform_to2) { Matrix computed = Dtransform_to2(T,P); Matrix numerical = numericalDerivative22(transform_to,T,P); CHECK(assert_equal(numerical,computed,error)); } + /* ************************************************************************* */ TEST( Pose3, transform_to_translate) { @@ -104,6 +154,8 @@ TEST( Pose3, transform_to_translate) Point3 expected(9.,18.,27.); CHECK(assert_equal(expected, actual)); } + +/* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { Pose3 transform(rodriguez(0,0,-1.570796), Point3()); @@ -111,6 +163,8 @@ TEST( Pose3, transform_to_rotate) Point3 expected(-1,2,10); CHECK(assert_equal(expected, actual, 0.001)); } + +/* ************************************************************************* */ TEST( Pose3, transform_to) { Pose3 transform(rodriguez(0,0,-1.570796), Point3(2,4, 0)); @@ -142,12 +196,16 @@ TEST( Pose3, transformPose_to_origin) Pose3 actual = pose1.transform_to(Pose3()); CHECK(assert_equal(pose1, actual, error)); } + +/* ************************************************************************* */ TEST( Pose3, transformPose_to_itself) { // transform to itself Pose3 actual = pose1.transform_to(pose1); CHECK(assert_equal(Pose3(), actual, error)); } + +/* ************************************************************************* */ TEST( Pose3, transformPose_to_translation) { // transform translation only @@ -157,6 +215,8 @@ TEST( Pose3, transformPose_to_translation) Pose3 expected(r, Point3(20.,30.,10.)); CHECK(assert_equal(expected, actual, error)); } + +/* ************************************************************************* */ TEST( Pose3, transformPose_to_simple_rotate) { // transform translation only @@ -167,6 +227,8 @@ TEST( Pose3, transformPose_to_simple_rotate) Pose3 expected(Rot3(), Point3(-30.,20.,10.)); CHECK(assert_equal(expected, actual, 0.001)); } + +/* ************************************************************************* */ TEST( Pose3, transformPose_to) { // transform to @@ -182,22 +244,56 @@ TEST( Pose3, transformPose_to) /* ************************************************************************* */ TEST( Pose3, composeTransform ) { - // known transform - Rot3 r = rodriguez(0,0,-1.570796); - Pose3 exp_transform(r, Point3(1,2,3)); - - // current - Rot3 r2 = rodriguez(0,0,0.698131701); - Pose3 current(r2, Point3(21.,32.,13.)); - - // target - Pose3 target(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); - - // calculate transform - Pose3 actual = compose(current, target); - - //verify - CHECK(assert_equal(exp_transform, actual, 0.001)); + // known transform + Rot3 R1 = rodriguez(0, 0, -1.570796); + Pose3 expected(R1, Point3(1, 2, 3)); + + // current + Rot3 R2 = rodriguez(0, 0, 0.698131701); + Pose3 current(R2, Point3(21., 32., 13.)); + + // target + Pose3 target(rodriguez(0, 0, 2.26892803), Point3(-30., 20., 10.)); + + // calculate transform + // todo: which should this be? + //Pose3 actual = compose(current, target); + Pose3 actual = between (target, current); + + //verify + CHECK(assert_equal(expected, actual, 0.001)); +} + +/* ************************************************************************* */ +TEST(Pose3, manifold) { + //cout << "manifold" << endl; + Pose3 t1 = T; + Pose3 t2 = pose1; + Pose3 origin; + Vector d12 = logmap(t1,t2); + CHECK(assert_equal(t2, expmap(t1,d12))); + // todo: richard - commented out because this tests for "compose-style" (new) expmap + //CHECK(assert_equal(t2, expmap(origin,d12)*t1)); + Vector d21 = logmap(t2,t1); + CHECK(assert_equal(t1, expmap(t2,d21))); + // todo: richard - commented out because this tests for "compose-style" (new) expmap + //CHECK(assert_equal(t1, expmap(origin,d21)*t2)); +} + +/* ************************************************************************* */ +TEST( Pose3, between ) +{ + Pose3 expected = pose1 * inverse(T); + Pose3 actual = between(T, pose1); + CHECK(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(between , T, pose1, 1e-5); + Matrix actualH1 = Dbetween1(T, pose1); +// CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ?? + + Matrix actualH2 = Dbetween2(T, pose1); + Matrix numericalH2 = numericalDerivative22(between , T, pose1, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp index 1463a0d64..54c495ff6 100644 --- a/cpp/testRot2.cpp +++ b/cpp/testRot2.cpp @@ -22,13 +22,13 @@ TEST( Rot2, angle) /* ************************************************************************* */ TEST( Rot2, transpose) { - CHECK(assert_equal(R.inverse().matrix(),R.transpose())); + CHECK(assert_equal(inverse(R).matrix(),R.transpose())); } /* ************************************************************************* */ TEST( Rot2, negtranspose) { - CHECK(assert_equal(-R.inverse().matrix(),R.negtranspose())); + CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose())); } /* ************************************************************************* */ @@ -41,7 +41,7 @@ TEST( Rot2, compose) /* ************************************************************************* */ TEST( Rot2, invcompose) { - CHECK(assert_equal(Rot2(0.2), Rot2(0.25).invcompose(Rot2(0.45)))); + CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45)))); } /* ************************************************************************* */ @@ -53,19 +53,19 @@ TEST( Rot2, equals) } /* ************************************************************************* */ -TEST( Rot2, exmap) +TEST( Rot2, expmap) { Vector v = zero(1); - CHECK(assert_equal(R.exmap(v), R)); + CHECK(assert_equal(expmap(R,v), R)); } /* ************************************************************************* */ -TEST(Rot2, log) +TEST(Rot2, logmap) { Rot2 rot0(M_PI_2); Rot2 rot(M_PI); Vector expected = Vector_(1, M_PI_2); - Vector actual = rot0.log(rot); + Vector actual = logmap(rot0,rot); CHECK(assert_equal(expected, actual)); } diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 9f49a9132..66ecff3bd 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -50,7 +50,7 @@ TEST( Rot3, constructor3) { TEST( Rot3, transpose) { Rot3 R(1,2,3,4,5,6,7,8,9); Point3 r1(1,2,3), r2(4,5,6), r3(7,8,9); - CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3))); + CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3))); } /* ************************************************************************* */ @@ -96,54 +96,11 @@ TEST( Rot3, rodriguez3) { } /* ************************************************************************* */ -TEST( Rot3, exmap) +TEST( Rot3, expmap) { - Vector v(3); - fill(v.begin(), v.end(), 0); - CHECK(assert_equal(R.exmap(v), R)); -} - -/* ************************************************************************* */ -TEST(Rot3, log) -{ - Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3 R1 = rodriguez(w1); - CHECK(assert_equal(w1, R1.log())); - - Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3 R2 = rodriguez(w2); - CHECK(assert_equal(w2, R2.log())); - - Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3 R3 = rodriguez(w3); - CHECK(assert_equal(w3, R3.log())); - - Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3 R = rodriguez(w); - CHECK(assert_equal(w, R.log())); -} - -/* ************************************************************************* */ -TEST(Rot3, between) -{ - Rot3 R = rodriguez(0.1, 0.4, 0.2); - Rot3 origin; - CHECK(assert_equal(R, between(origin,R))); - CHECK(assert_equal(R.inverse(), between(R,origin))); -} - -/* ************************************************************************* */ - TEST(Rot3, manifold) -{ - Rot3 t1 = rodriguez(0.1, 0.4, 0.2); - Rot3 t2 = rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - Vector d12 = log(t1, t2); - CHECK(assert_equal(t2, t1.exmap(d12))); - CHECK(assert_equal(t2, origin.exmap(d12)*t1)); - Vector d21 = log(t2, t1); - CHECK(assert_equal(t1, t2.exmap(d21))); - CHECK(assert_equal(t1, origin.exmap(d21)*t2)); + Vector v(3); + fill(v.begin(), v.end(), 0); + CHECK(assert_equal(expmap(R,v), R)); } /* ************************************************************************* */ @@ -156,6 +113,13 @@ TEST( Rot3, Drotate1) CHECK(assert_equal(numerical,computed,error)); } +TEST( Rot3, Drotate1_) + { + Matrix computed = Drotate1(inverse(R), P); + Matrix numerical = numericalDerivative21(rotate,inverse(R),P); + CHECK(assert_equal(numerical,computed,error)); +} + TEST( Rot3, Drotate2_DNrotate2) { Matrix computed = Drotate2(R); @@ -164,6 +128,8 @@ TEST( Rot3, Drotate2_DNrotate2) } /* ************************************************************************* */ +// unrotate + TEST( Rot3, unrotate) { Point3 w = R*P; @@ -188,29 +154,41 @@ TEST( Rot3, Dunrotate2_DNunrotate2) } /* ************************************************************************* */ -TEST( Rot3, RQ) +TEST( Rot3, compose ) { - // Try RQ on a pure rotation - Matrix actualK; Vector actual; - boost::tie(actualK,actual) = RQ(R.matrix()); - Vector expected = Vector_(3,0.14715, 0.385821, 0.231671); - CHECK(assert_equal(eye(3),actualK)); - CHECK(assert_equal(expected,actual,1e-6)); + Rot3 R1 = rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = rodriguez(0.2, 0.3, 0.5); - // Try using ypr call - actual = R.ypr(); - CHECK(assert_equal(expected,actual,1e-6)); + Rot3 expected = R1*R2; + Rot3 actual = compose(R1, R2); + CHECK(assert_equal(expected,actual)); - // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = Matrix_(3,3, - 500.0, 0.0, 320.0, - 0.0, 500.0, 240.0, - 0.0, 0.0, 1.0 - ); - Matrix A = K*R.matrix(); - boost::tie(actualK,actual) = RQ(A); - CHECK(assert_equal(K,actualK)); - CHECK(assert_equal(expected,actual,1e-6)); + Matrix numericalH1 = numericalDerivative21(compose, R1, R2, 1e-5); + Matrix actualH1 = Dcompose1(R1, R2); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix actualH2 = Dcompose2(R1, R2); + Matrix numericalH2 = numericalDerivative22(compose, R1, R2, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); +} + +/* ************************************************************************* */ +TEST( Rot3, between ) +{ + Rot3 R1 = rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = rodriguez(0.2, 0.3, 0.5); + + Rot3 expected = R2*inverse(R1); + Rot3 actual = between(R1, R2); + CHECK(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(between , R1, R2, 1e-5); + Matrix actualH1 = Dbetween1(R1, R2); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix actualH2 = Dbetween2(R1, R2); + Matrix numericalH2 = numericalDerivative22(between , R1, R2, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 8e6fcc671..017ee0c14 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -121,7 +121,7 @@ TEST (SQP, problem1_cholesky ) { if (verbose) delta.print("Delta"); // update initial estimate - VectorConfig newState = state.exmap(delta); + VectorConfig newState = expmap(state, delta); state = newState; if (verbose) state.print("Updated State"); @@ -222,7 +222,7 @@ TEST (SQP, problem1_sqp ) { if (verbose) delta.print("Delta"); // update initial estimate - VectorConfig newState = state.exmap(delta.scale(-1.0)); + VectorConfig newState = expmap(state, delta.scale(-1.0)); // set the state to the updated state state = newState; @@ -445,9 +445,9 @@ TEST (SQP, two_pose ) { if (verbose) delta.print("Delta Config"); // update both state variables - state = state.exmap(delta); + state = expmap(state, delta); if (verbose) state.print("newState"); - state_lambda = state_lambda.exmap(delta); + state_lambda = expmap(state_lambda, delta); if (verbose) state_lambda.print("newStateLam"); } diff --git a/cpp/testSimulated3D.cpp b/cpp/testSimulated3D.cpp index 9d83bba07..cccdf8aae 100644 --- a/cpp/testSimulated3D.cpp +++ b/cpp/testSimulated3D.cpp @@ -17,7 +17,7 @@ using namespace gtsam; TEST( simulated3D, Dprior_3D ) { Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v = x1.vector(); + Vector v = logmap(x1); Matrix numerical = NumericalDerivative11(prior_3D,v); Matrix computed = Dprior_3D(v); CHECK(assert_equal(numerical,computed,1e-9)); @@ -27,9 +27,9 @@ TEST( simulated3D, Dprior_3D ) TEST( simulated3D, DOdo1 ) { Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v1 = x1.vector(); + Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); - Vector v2 = x2.vector(); + Vector v2 = logmap(x2); Matrix numerical = NumericalDerivative21(odo_3D,v1,v2); Matrix computed = Dodo1_3D(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); @@ -39,9 +39,9 @@ TEST( simulated3D, DOdo1 ) TEST( simulated3D, DOdo2 ) { Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v1 = x1.vector(); + Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); - Vector v2 = x2.vector(); + Vector v2 = logmap(x2); Matrix numerical = NumericalDerivative22(odo_3D,v1,v2); Matrix computed = Dodo2_3D(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp index 89abc04de..78bff37bb 100644 --- a/cpp/testVSLAMConfig.cpp +++ b/cpp/testVSLAMConfig.cpp @@ -23,7 +23,7 @@ TEST( VSLAMConfig, update_with_large_delta) { delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1)); delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1)); - VSLAMConfig actual = init.exmap(delta); + VSLAMConfig actual = expmap(init, delta); VSLAMConfig expected; expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1)); diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 8faa80aa1..d193e167f 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -59,11 +59,11 @@ TEST( VSLAMFactor, error ) GaussianFactorGraph actual_lfg = graph.linearize(config); CHECK(assert_equal(expected_lfg,actual_lfg)); - // exmap on a config + // expmap on a config VectorConfig delta; delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.)); delta.insert("l1",Vector_(3, 1.,2.,3.)); - VSLAMConfig actual_config = config.exmap(delta); + VSLAMConfig actual_config = expmap(config, delta); VSLAMConfig expected_config; Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2); Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2); diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp index 0b5472f91..2d4073cb6 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -69,11 +69,11 @@ TEST( VectorConfig, contains) } /* ************************************************************************* */ -TEST( VectorConfig, exmap) +TEST( VectorConfig, expmap) { VectorConfig c = createConfig(); Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 - CHECK(assert_equal(c.exmap(c),c.exmap(v))); + CHECK(assert_equal(expmap(c,c),expmap(c,v))); } /* ************************************************************************* */ @@ -93,7 +93,7 @@ TEST( VectorConfig, plus) expected.insert("x", wx).insert("y",wy); // functional - VectorConfig actual = c.exmap(delta); + VectorConfig actual = expmap(c,delta); CHECK(assert_equal(expected,actual)); } @@ -123,7 +123,7 @@ TEST( VectorConfig, update_with_large_delta) { delta.insert("y", Vector_(2, 0.1, 0.1)); delta.insert("penguin", Vector_(2, 0.1, 0.1)); - VectorConfig actual = init.exmap(delta); + VectorConfig actual = expmap(init,delta); VectorConfig expected; expected.insert("x", Vector_(2, 1.1, 2.1)); expected.insert("y", Vector_(2, 3.1, 4.1));