diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 8d75d767c..e93a7953f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -17,79 +17,38 @@ #include -#include -#include #include - #include namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(s) { -} - Similarity3::Similarity3() : - R_(), t_(), s_(1){ + R_(), t_(), s_(1) { } Similarity3::Similarity3(double s) : - s_ (s) { + s_(s) { } -Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : - R_ (R), t_ (t), s_ (s) { +Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : + R_(R), t_(t), s_(s) { } -Similarity3::operator Pose3() const { - return Pose3(R_, s_*t_); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } -Similarity3 Similarity3::identity() { - return Similarity3(); } +Similarity3::Similarity3(const Matrix4& T) : + R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) { +} -//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { -// return Vector7(); -//} -// -//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { -// return Similarity3(); -//} +bool Similarity3::equals(const Similarity3& other, double tol) const { + return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); +} bool Similarity3::operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); -} - -bool Similarity3::equals(const Similarity3& sim, double tol) const { - return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) - && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); -} - -Similarity3::Translation Similarity3::transform_from(const Translation& p) const { - return R_ * (s_ * p) + t_; -} - -Matrix7 Similarity3::AdjointMap() const{ - const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = s_ * skewSymmetric(t) * R; - Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; - return adj; -} - -inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { - return transform_from(p); -} - -Similarity3 Similarity3::inverse() const { - Rotation Rt = R_.inverse(); - Translation sRt = R_.inverse() * (-s_ * t_); - return Similarity3(Rt, sRt, 1.0/s_); -} - -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); + return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_; } void Similarity3::print(const std::string& s) const { @@ -100,31 +59,171 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } +Similarity3 Similarity3::identity() { + return Similarity3(); +} +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +} + +Similarity3 Similarity3::inverse() const { + Rot3 Rt = R_.inverse(); + Point3 sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0 / s_); +} + +Point3 Similarity3::transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { + Point3 q = R_ * p + t_; + if (H1) { + const Matrix3 R = R_.matrix(); + Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); + // TODO(frank): explain the derivative in lambda + *H1 << DR, s_ * R, s_ * p.vector(); + } + if (H2) + *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() + return s_ * q; +} + +Point3 Similarity3::operator*(const Point3& p) const { + return transform_from(p); +} + +Matrix4 Similarity3::wedge(const Vector7& xi) { + // http://www.ethaneade.org/latex2html/lie/node29.html + const auto w = xi.head<3>(); + const auto u = xi.segment<3>(3); + double lambda = xi[6]; + Matrix4 W; + W << skewSymmetric(w), u, 0, 0, 0, -lambda; + return W; +} + +Matrix7 Similarity3::AdjointMap() const { + // http://www.ethaneade.org/latex2html/lie/node30.html + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + const Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << + R, Z_3x3, Matrix31::Zero(), // 3*7 + A, s_ * R, -s_ * t, // 3*7 + Matrix16::Zero(), 1; // 1*7 + return adj; +} + +Matrix3 Similarity3::GetV(Vector3 w, double lambda) { + // http://www.ethaneade.org/latex2html/lie/node29.html + double lambda2 = lambda * lambda; + double theta2 = w.transpose() * w; + double theta = sqrt(theta2); + double A, B, C; + // TODO(frank): eliminate copy/paste + if (theta2 > 1e-9 && lambda2 > 1e-9) { + const double X = sin(theta) / theta; + const double Y = (1 - cos(theta)) / theta2; + const double Z = (1 - X) / theta2; + const double W = (0.5 - Y) / theta2; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = (exp(-lambda) - 1 + lambda) / lambda2; + const double gamma = Y - (lambda * Z); + const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) + / (lambda2 * lambda); + const double upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else if (theta2 <= 1e-9 && lambda2 > 1e-9) { + //Taylor series expansions + const double Y = 0.5 - theta2 / 24.0; + const double Z = 1.0 / 6.0 - theta2 / 120.0; + const double W = 1.0 / 24.0 - theta2 / 720.0; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = (exp(-lambda) - 1 + lambda) / lambda2; + const double gamma = Y - (lambda * Z); + const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) + / (lambda2 * lambda); + const double upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else if (theta2 > 1e-9 && lambda2 <= 1e-9) { + const double X = sin(theta) / theta; + const double Y = (1 - cos(theta)) / theta2; + const double Z = (1 - X) / theta2; + const double W = (0.5 - Y) / theta2; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 + - (lambda * lambda2) / 120; + const double gamma = Y - (lambda * Z); + const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120 + - (lambda * lambda2) / 720; + const double upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambda2 / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else { + const double Y = 0.5 - theta2 / 24.0; + const double Z = 1.0 / 6.0 - theta2 / 120.0; + const double W = 1.0 / 24.0 - theta2 / 720.0; + const double gamma = Y - (lambda * Z); + const double upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambda2 / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = gamma; + C = upsilon; + } + const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]); + return A * I_3x3 + B * Wx + C * Wx * Wx; +} + +Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) { + // To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at + // www.ethaneade.org/latex2html/lie/node29.html + const Vector3 w = Rot3::Logmap(T.R_); + const double lambda = log(T.s_); + Vector7 result; + result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; + if (Hm) { + throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); + } + return result; +} + +Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { + const auto w = v.head<3>(); + const auto u = v.segment<3>(3); + const double lambda = v[6]; + if (Hm) { + throw std::runtime_error("Similarity3::Expmap: derivative not implemented"); + } + const Matrix3 V = GetV(w, lambda); + return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda)); +} + std::ostream &operator<<(std::ostream &os, const Similarity3& p) { - os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << - p.scale() << "]\';"; + os << "[" << p.rotation().xyz().transpose() << " " + << p.translation().vector().transpose() << " " << p.scale() << "]\';"; 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. - Rotation r; //Create a zero rotation to do our retraction. - return Similarity3( // - r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Translation(v.segment<3>(3)), // Retract the translation - 1.0 + v[6]); //finally, update scale using v[6] +const Matrix4 Similarity3::matrix() const { + Matrix4 T; + T.topRows<3>() << R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1.0/s_; + return T; } -Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rotation 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; -} +Similarity3::operator Pose3() const { + return Pose3(R_, s_ * t_); } - +} diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index eec2124ee..853261fc2 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -32,13 +32,15 @@ class Pose3; */ class Similarity3: public LieGroup { - /** Pose Concept requirements */ + /// @name Pose Concept + /// @{ typedef Rot3 Rotation; typedef Point3 Translation; + /// @} private: - Rotation R_; - Translation t_; + Rot3 R_; + Point3 t_; double s_; public: @@ -46,17 +48,21 @@ public: /// @name Constructors /// @{ + /// Default constructor Similarity3(); /// Construct pure scaling Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rotation& R, const Translation& t, double s); + Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); + /// Construct from matrix [R t; 0 s^-1] + Similarity3(const Matrix4& T); + /// @} /// @name Testable /// @{ @@ -64,7 +70,7 @@ public: /// Compare with tolerance bool equals(const Similarity3& sim, double tol) const; - /// Compare with standard tolerance + /// Exact equality bool operator==(const Similarity3& other) const; /// Print with optional string @@ -79,74 +85,118 @@ public: /// Return an identity transform static Similarity3 identity(); + /// Composition + Similarity3 operator*(const Similarity3& T) const; + /// Return the inverse Similarity3 inverse() const; - Translation transform_from(const Translation& p) const; + /// @} + /// @name Group action on Point3 + /// @{ + + /// Action on a point p is s*(R*p+t) + Point3 transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; /** syntactic sugar for transform_from */ - inline Translation operator*(const Translation& p) const; - - Similarity3 operator*(const Similarity3& T) const; + Point3 operator*(const Point3& p) const; /// @} - /// @name Standard interface + /// @name Lie Group /// @{ - /// Return a GTSAM rotation - const Rotation& rotation() const { - return R_; - }; + /** 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); - /// Return a GTSAM translation - const Translation& translation() const { - return t_; - }; + /** Exponential map at the identity + */ + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); - /// Return the scale - double scale() const { - return s_; - }; - - /// Convert to a rigid body pose - operator Pose3() const; - - /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes - inline static size_t Dim() { - return 7; - }; - - /// Dimensionality of tangent space = 7 DOF - inline size_t dim() const { - return 7; - }; - - /// @} - /// @name Chart - /// @{ - - /// 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, H); + } + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + return Similarity3::Logmap(other, H); + } }; + using LieGroup::inverse; + + /** + * wedge for Similarity3: + * @param xi 7-dim twist (w,u,lambda) where + * @return 4*4 element of Lie algebra that can be exponentiated + * TODO(frank): rename to Hat, make part of traits + */ + static Matrix4 wedge(const Vector7& xi); + /// Project from one tangent space to another Matrix7 AdjointMap() const; /// @} - /// @name Stubs + /// @name Standard interface /// @{ - /// Not currently implemented, required because this is a lie group - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + /// Calculate 4*4 matrix group equivalent + const Matrix4 matrix() const; - using LieGroup::inverse; // version with derivative + /// Return a GTSAM rotation + const Rot3& rotation() const { + return R_; + } + + /// Return a GTSAM translation + const Point3& translation() const { + return t_; + } + + /// Return the scale + double scale() const { + return s_; + } + + /// Convert to a rigid body pose (R, s*t) + /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. + operator Pose3() const; + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + } + + /// Dimensionality of tangent space = 7 DOF + inline size_t dim() const { + return 7; + } + + /// @} + /// @name Helper functions + /// @{ + + /// Calculate expmap and logmap coefficients. +private: + static Matrix3 GetV(Vector3 w, double lambda); + + /// @} }; template<> -struct traits : public internal::LieGroup {}; +inline Matrix wedge(const Vector& xi) { + return Similarity3::wedge(xi); } + +template<> +struct traits : public internal::LieGroup {}; + +template<> +struct traits : public internal::LieGroup {}; + +} // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 849206a7d..111d53d59 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -13,89 +13,108 @@ * @file testSimilarity3.cpp * @brief Unit tests for Similarity3 class * @author Paul Drews + * @author Zhaoyang Lv */ #include -#include -#include -#include -#include #include #include #include +#include #include #include - +#include +#include +#include +#include #include + #include +#include +#include + using namespace gtsam; using namespace std; using symbol_shorthand::X; GTSAM_CONCEPT_TESTABLE_INST(Similarity3) -static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::Rodrigues(0.3,0,0); -static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); -static Similarity3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); -static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const double s = 4; +static const Similarity3 id; +static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Similarity3 T4(R, P, s); +static const Similarity3 T5(R, P, 10); +static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform + +//****************************************************************************** +TEST(Similarity3, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} //****************************************************************************** TEST(Similarity3, Constructors) { - Similarity3 test; + Similarity3 sim3_Construct1; + Similarity3 sim3_Construct2(s); + Similarity3 sim3_Construct3(R, P, s); + Similarity3 sim4_Construct4(R.matrix(), P.vector(), s); } //****************************************************************************** TEST(Similarity3, Getters) { - Similarity3 test; - EXPECT(assert_equal(Rot3(), test.rotation())); - EXPECT(assert_equal(Point3(), test.translation())); - EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); + Similarity3 sim3_default; + EXPECT(assert_equal(Rot3(), sim3_default.rotation())); + EXPECT(assert_equal(Point3(), sim3_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); + + Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); + EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); } //****************************************************************************** -TEST(Similarity3, Getters2) { - Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation())); - EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); - EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); -} - 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; - EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); + const Matrix4 T = T2.matrix(); + // Check Ad with actual definition + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Matrix4 W = Similarity3::wedge(delta); + Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); + EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); } +//****************************************************************************** TEST(Similarity3, inverse) { - Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Matrix3 Re; - Re << -0.2248, 0.9024, -0.3676, - -0.3502, -0.4269, -0.8337, - -0.9093, -0.0587, 0.4120; + 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)); + Similarity3 expected(Re, te, 1.0 / 7.0); + 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)); } -TEST(Similarity3, multiplication) { - Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11); +//****************************************************************************** +TEST(Similarity3, Multiplication) { + Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); Matrix3 re; - re << 0.0688, 0.9863, -0.1496, - -0.5665, -0.0848, -0.8197, - -0.8211, 0.1412, 0.5530; + re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; Vector3 te(-13.6797, 3.2441, -5.7794); Similarity3 expected(re, te, 77); - EXPECT(assert_equal(expected, test1*test2, 1e-2)); + EXPECT(assert_equal(expected, test1 * test2, 1e-2)); } //****************************************************************************** @@ -117,15 +136,14 @@ TEST(Similarity3, Manifold) { v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); -// Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); - Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); Vector vlocal = sim.localCoordinates(other); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1); - Rot3 R = Rot3::Rodrigues(0.3,0,0); + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); + Rot3 R = Rot3::Rodrigues(0.3, 0, 0); Vector vlocal2 = sim.localCoordinates(other2); @@ -134,30 +152,29 @@ TEST(Similarity3, Manifold) { // TODO add unit tests for retract and localCoordinates } -/* ************************************************************************* */ -TEST( Similarity3, retract_first_order) -{ +//****************************************************************************** +TEST( Similarity3, retract_first_order) { Similarity3 id; Vector v = zero(7); v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); - v(3)=0.2;v(4)=0.7;v(5)=-2; - EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); + EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2)); +// v(3) = 0.2; +// v(4) = 0.7; +// v(5) = -2; +// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); } -/* ************************************************************************* */ -TEST(Similarity3, localCoordinates_first_order) -{ - Vector d12 = repeat(7,0.1); +//****************************************************************************** +TEST(Similarity3, localCoordinates_first_order) { + Vector d12 = repeat(7, 0.1); d12(6) = 1.0; - Similarity3 t1 = T, t2 = t1.retract(d12); + Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } -/* ************************************************************************* */ -TEST(Similarity3, manifold_first_order) -{ - Similarity3 t1 = T; +//****************************************************************************** +TEST(Similarity3, manifold_first_order) { + Similarity3 t1 = T1; Similarity3 t2 = T3; Similarity3 origin; Vector d12 = t1.localCoordinates(t2); @@ -166,48 +183,137 @@ TEST(Similarity3, manifold_first_order) EXPECT(assert_equal(t1, t2.retract(d21))); } +//****************************************************************************** +// Return as a 4*4 Matrix +TEST(Similarity3, Matrix) { + Matrix4 expected; + expected << + 1, 0, 0, 1, + 0, 1, 0, 1, + 0, 0, 1, 0, + 0, 0, 0, 0.5; + Matrix4 actual = T6.matrix(); + EXPECT(assert_equal(expected, actual)); +} + +//***************************************************************************** +// Exponential and log maps +TEST(Similarity3, ExpLogMap) { + Vector7 delta; + delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7; + Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); + EXPECT(assert_equal(delta, actual)); + + Vector7 zeros; + zeros << 0,0,0,0,0,0,0; + Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); + EXPECT(assert_equal(zeros, logIdentity)); + + Similarity3 expZero = Similarity3::Expmap(zeros); + Similarity3 ident = Similarity3::identity(); + EXPECT(assert_equal(expZero, ident)); + + // Compare to matrix exponential, using expm in Lie.h + EXPECT(assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); +} + +//****************************************************************************** +// Group action on Point3 (with simpler transform) +TEST(Similarity3, GroupAction) { + EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); + + // Test group action on R^4 via matrix representation + Vector4 qh; + qh << 1, 0, 0, 1; + Vector4 ph; + ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) + EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); + + // Test some more... + Point3 pa = Point3(1, 0, 0); + Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); + Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); + EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); + + Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0); + Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); + EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); + + // Test derivative + boost::function f = boost::bind( + &Similarity3::transform_from, _1, _2, boost::none, boost::none); + + Point3 q(1, 2, 3); + for (const auto T : { T1, T2, T3, T4, T5, T6 }) { + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T1, q); + Matrix H2 = numericalDerivative22(f, T1, q); + Matrix actualH1, actualH2; + T1.transform_from(q, actualH1, actualH2); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } +} + +//****************************************************************************** +// Test very simple prior optimization example TEST(Similarity3, Optimization) { + // Create a PriorFactor with a Sim3 prior Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); - Symbol key('x',1); + Symbol key('x', 1); PriorFactor factor(key, prior, model); + // Create graph NonlinearFactorGraph graph; graph.push_back(factor); + // Create initial estimate with identity transform Values initial; initial.insert(key, Similarity3()); + // Optimize Values result; LevenbergMarquardtParams params; params.setVerbosityLM("TRYCONFIG"); result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // After optimization, result should be prior EXPECT(assert_equal(prior, result.at(key), 1e-4)); } +//****************************************************************************** +// Test optimization with both Prior and BetweenFactors TEST(Similarity3, Optimization2) { Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), + 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(sqrt(8) * 0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), + Point3(sqrt(32) * 0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(6 * 0.7, 0, 0), 1.0); Similarity3 loop = Similarity3(1.42); //prior.print("Goal Transform"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, + 0.01); SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); // Prior ! BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); BetweenFactor b4(X(4), X(5), m4, betweenNoise); BetweenFactor lc(X(5), X(1), loop, betweenNoise2); - - + // Create graph NonlinearFactorGraph graph; graph.push_back(factor); graph.push_back(b1); @@ -217,13 +323,16 @@ TEST(Similarity3, Optimization2) { graph.push_back(lc); //graph.print("Full Graph\n"); - Values initial; initial.insert(X(1), Similarity3()); - initial.insert(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + initial.insert(X(2), + Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), + Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), + Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), + Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); //initial.print("Initial Estimate\n"); @@ -247,6 +356,58 @@ TEST(Similarity3, Optimization2) { EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); } +//****************************************************************************** +// Align points (p,q) assuming that p = T*q + noise +TEST(Similarity3, AlignScaledPointClouds) { +// Create ground truth + Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); + + // Create transformed cloud (noiseless) +// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; + + // Create an unknown expression + Expression unknownT(0); // use key 0 + + // Create constant expressions for the ground truth points + Expression q1_(q1), q2_(q2), q3_(q3); + + // Create prediction expressions + Expression predict1(unknownT, &Similarity3::transform_from, q1_); + Expression predict2(unknownT, &Similarity3::transform_from, q2_); + Expression predict3(unknownT, &Similarity3::transform_from, q3_); + +//// Create Expression factor graph +// ExpressionFactorGraph graph; +// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| +// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| +// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| +} + +//****************************************************************************** +TEST(Similarity3 , Invariants) { + Similarity3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); +} + +//****************************************************************************** +TEST(Similarity3 , LieGroupDerivatives) { + Similarity3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + //****************************************************************************** int main() { TestResult tr;