diff --git a/gtsam.h b/gtsam.h index 834e1368b..f37bafc67 100644 --- a/gtsam.h +++ b/gtsam.h @@ -11,6 +11,7 @@ class Point3 { Point3(double x, double y, double z); Point3(Vector v); void print(string s) const; + bool equals(const Point3& p, double tol); Vector vector() const; double x(); double y(); @@ -21,11 +22,18 @@ class Rot2 { Rot2(); Rot2(double theta); void print(string s) const; - bool equals(const Rot2& pose, double tol) const; + bool equals(const Rot2& rot, double tol) const; double c() const; double s() const; }; +class Rot3 { + Rot3(); + Rot3(Matrix R); + void print(string s) const; + bool equals(const Rot3& rot, double tol) const; +}; + class Pose2 { Pose2(); Pose2(double x, double y, double theta); @@ -44,6 +52,21 @@ class Pose2 { Pose2* retract_(Vector v); }; +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t); + Pose3(Vector v); + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + int dim() const; + Pose3* compose_(const Pose3& p2); + Pose3* between_(const Pose3& p2); + Vector localCoordinates(const Pose3& p); +}; + class SharedGaussian { SharedGaussian(Matrix covariance); void print(string s) const; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index fa6f02828..c600669b6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -214,22 +214,23 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) + if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + { *H1 = - AdjointMap(p); } #else - { - Matrix Rt = R_.transpose(); - Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; - Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - *H1 = gtsam::stack(2, &DR, &Dt); - } + { + Matrix Rt = R_.transpose(); + Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; + Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; + Matrix DR = collect(2, &DR_R1, &DR_t1); + Matrix Dt = collect(2, &Dt_R1, &Dt_t1); + *H1 = gtsam::stack(2, &DR, &Dt); + } #endif - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt*(-t_)); + } /* ************************************************************************* */ // between = compose(p2,inverse(p1)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f03dcde06..ee08c8956 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -89,6 +89,11 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Pose3& p2) { + return boost::shared_ptr(new Pose3(compose(p2))); + } + /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); @@ -144,12 +149,17 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Pose3& p2) { + return boost::shared_ptr(new Pose3(between(p2))); + } + /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect + Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** * wedge for Pose3: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 928b9b450..a4816914e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -25,224 +25,234 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Rot3M); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Rot3M); - static const Matrix I3 = eye(3); +static const Matrix I3 = eye(3); - /* ************************************************************************* */ - // static member functions to construct rotations +/* ************************************************************************* */ +Rot3M Rot3M::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} - Rot3M Rot3M::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} - Rot3M Rot3M::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} - Rot3M Rot3M::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); - } +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3M Rot3M::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3M( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} - // Considerably faster than composing matrices above ! - Rot3M Rot3M::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3M( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); - } - - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; #ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + double l_n = wwTxx + wwTyy + wwTzz; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif - double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double c = cos(theta), s = sin(theta), c_1 = 1 - c; - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; - return Rot3M( c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); - } + return Rot3M( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3M(); - return rodriguez(w/t, t); - } +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3M(); + return rodriguez(w/t, t); +} - /* ************************************************************************* */ - bool Rot3M::equals(const Rot3M & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } +/* ************************************************************************* */ +bool Rot3M::equals(const Rot3M & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} - /* ************************************************************************* */ - Matrix Rot3M::matrix() const { - double r[] = { r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z() }; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::matrix() const { + Matrix R(3,3); + R << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z(); + return R; +} - /* ************************************************************************* */ - Matrix Rot3M::transpose() const { - double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()}; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::transpose() const { + Matrix Rt(3,3); + Rt << + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z(); + return Rt; +} - /* ************************************************************************* */ - Point3 Rot3M::column(int index) const{ - if(index == 3) - return r3_; - else if (index == 2) - return r2_; - else - return r1_; // default returns r1 - } +/* ************************************************************************* */ +Point3 Rot3M::column(int index) const{ + if(index == 3) + return r3_; + else if (index == 2) + return r2_; + else + return r1_; // default returns r1 +} - /* ************************************************************************* */ - Vector Rot3M::xyz() const { - Matrix I;Vector q; - boost::tie(I,q)=RQ(matrix()); - return q; - } +/* ************************************************************************* */ +Vector Rot3M::xyz() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; +} - Vector Rot3M::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); - } +/* ************************************************************************* */ +Vector Rot3M::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); +} - Vector Rot3M::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); - } +/* ************************************************************************* */ +Vector Rot3M::rpy() const { + Vector q = xyz(); + return Vector_(3,q(0),q(1),q(2)); +} - /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3M::Logmap(const Rot3M& R) { - double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); - } else { - 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()); - } - } - - /* ************************************************************************* */ - Point3 Rot3M::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = matrix(); - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); - } - - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3M::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; - } - - /* ************************************************************************* */ - Rot3M Rot3M::compose (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; - } - - /* ************************************************************************* */ - Rot3M Rot3M::between (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - - /* ************************************************************************* */ - pair RQ(const Matrix& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3M Qx = Rot3M::Rx(-x); - Matrix B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3M Qy = Rot3M::Ry(-y); - Matrix C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3M Qz = Rot3M::Rz(-z); - Matrix R = C * Qz.matrix(); - - Vector xyz = Vector_(3, x, y, z); - return make_pair(R, xyz); +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector Rot3M::Logmap(const Rot3M& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + // FIXME should tr in statement below be absolute value? + if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) + return zero(3); + } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) + double theta = acos((tr-1.0)/2.0); + // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) + return (0.5 + theta*theta/12)*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + // FIXME: in statement below, is this the right comparision? + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * + Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); + else if(fabs(R.r2().y() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * + Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * + Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + } else { + 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()); } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Point3 Rot3M::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = matrix(); + return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3M::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +Rot3M Rot3M::compose (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3M Rot3M::between (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); +} + +/* ************************************************************************* */ +pair RQ(const Matrix& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3M Qx = Rot3M::Rx(-x); + Matrix B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3M Qy = Rot3M::Ry(-y); + Matrix C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3M Qz = Rot3M::Rz(-z); + Matrix R = C * Qz.matrix(); + + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ } // namespace gtsam