diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index f59721a1e..6d38cfa69 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -80,6 +80,7 @@ Point3 Similarity3::transform_from(const Point3& p, // *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() return R_ * (s_ * p) + t_; // TODO: Effect of scale change is this, right? + // No, this is incorrect. Zhaoyang Lv // sR t * (1+v)I 0 * p = s(1+v)R t * p = s(1+v)Rp + t = sRp + vRp + t // 0001 000 1 1 000 1 1 } @@ -186,38 +187,15 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { // www.ethaneade.org/latex2html/lie/node29.html Vector3 w = Rot3::Logmap(s.R_); double lambda = log(s.s_); - Matrix33 V = GetV(w, lambda); - Vector3 u = V.inverse() * s.t_.vector(); - Vector7 result; - result << w, u, lambda; + result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; return result; } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { - Matrix31 w(v.head<3>()); + Vector3 w(v.head<3>()); double lambda = v[6]; - Matrix31 u(v.segment<3>(3)); - - Matrix33 V = GetV(w, lambda); -// Matrix33 wx = skewSymmetric(w[0], w[1], w[2]); -// double lambdasquared = lambda * lambda; -// double thetasquared = w.transpose() * w; -// double theta = sqrt(thetasquared); -// double X = sin(theta)/theta; -// double Y = (1-cos(theta))/thetasquared; -// double Z = (1-X)/thetasquared; -// double W = (0.5-Y)/thetasquared; -// double alpha = lambdasquared / (lambdasquared + thetasquared); -// double beta = (exp(-lambda)-1+lambda)/lambdasquared; -// double gama = Y - (lambda * Z); -// double mu = (1-lambda+(0.5*lambdasquared)-exp(-lambda))/(lambdasquared*lambda); -// double upsilon = Z-(lambda*W); -// double A = (1-exp(-lambda))/lambda; -// double B = alpha*(beta-gama)+gama; -// double C = alpha*(mu-upsilon)+upsilon; -// Matrix33 V = A*Matrix33::Identity() + B*wx + C*wx*wx; - return Similarity3(Rot3::Expmap(w), Point3(V*u), 1.0/exp(-lambda)); + return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); } @@ -227,33 +205,6 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) { return os; } -Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { - // Will retracting or localCoordinating R work if R is not a unit rotation? - // Also, how do we actually get s out? Seems like we need to store it somewhere. -// Rot3 r; //Create a zero rotation to do our retraction. -// return Similarity3( // -// r.retract(v.head<3>()), // retract rotation using v[0,1,2] -// Point3(v.segment<3>(3)), // Retract the translation -// 1.0 + v[6]); //finally, update scale using v[6] - - // Use the Expmap - return Similarity3::Expmap(v); -} - -Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, - ChartJacobian H) { -// Rot3 r; //Create a zero rotation to do the retraction -// Vector7 v; -// v.head<3>() = r.localCoordinates(other.R_); -// v.segment<3>(3) = other.t_.vector(); -// //v.segment<3>(3) = translation().localCoordinates(other.translation()); -// v[6] = other.s_ - 1.0; -// return v; - - // Use the Logmap - return Similarity3::Logmap(other); -} - const Matrix4 Similarity3::matrix() const { Matrix4 T; T.topRows<3>() << s_ * R_.matrix(), t_.vector(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index a03201d57..30d8ab694 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -103,21 +103,25 @@ public: /// @name Lie Group /// @{ - /// Not currently implemented, required because this is a lie group + /** Log map at the identity + * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ + */ static Vector7 Logmap(const Similarity3& s, // OptionalJacobian<7, 7> Hm = boost::none); - /// Not currently implemented, required because this is a lie group + /** Exponential map at the identity + */ static Similarity3 Expmap(const Vector7& v, // OptionalJacobian<7, 7> Hm = boost::none); - /// Update Similarity transform via 7-dim vector in tangent space + /// Chart at the origin struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); - - /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - static Vector7 Local(const Similarity3& other, - ChartJacobian H = boost::none); + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { + return Similarity3::Expmap(v); + } + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + return Similarity3::Logmap(other); + } }; /// Project from one tangent space to another diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index da2920336..9ad165180 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -13,6 +13,7 @@ * @file testSimilarity3.cpp * @brief Unit tests for Similarity3 class * @author Paul Drews + * @author Zhaoyang Lv */ #include @@ -73,18 +74,30 @@ TEST(Similarity3, Getters2) { TEST(Similarity3, AdjointMap) { Similarity3 test(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); Matrix7 result; - result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, 0, 0, 0, 0, 0, 0, 1.0000; + result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, + 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, + -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, + 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, + 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, + 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, + 0, 0, 0, 0, 0, 0, 1.0000; EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); } //****************************************************************************** TEST(Similarity3, inverse) { - Similarity3 test(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); - Matrix3 Re; + Similarity3 sim3(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Matrix3 Re; // some values from matlab Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; Vector3 te(-9.8472, 59.7640, 10.2125); Similarity3 expected(Re, te, 1.0 / 7.0); - EXPECT(assert_equal(expected, test.inverse(), 1e-3)); + EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); + + // test lie group inverse + Matrix H1, H2; + EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); } //****************************************************************************** @@ -169,7 +182,10 @@ TEST(Similarity3, manifold_first_order) { // Return as a 4*4 Matrix TEST(Similarity3, Matrix) { Matrix4 expected; - expected << 2, 0, 0, 1, 0, 2, 0, 1, 0, 0, 2, 0, 0, 0, 0, 1; + expected << 2, 0, 0, 1, + 0, 2, 0, 1, + 0, 0, 2, 0, + 0, 0, 0, 1; Matrix4 actual = T4.matrix(); EXPECT(assert_equal(expected, actual)); } @@ -200,6 +216,7 @@ TEST(Similarity3, ExpLogMap) { Similarity3 Scalc = Similarity3::Expmap(expected); EXPECT(assert_equal(Sexpm, Scalc, 1e-3)); } + //****************************************************************************** // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) {