From 1e58c0b0a2c013cd49d499b9f2d4441ac8b2a574 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 19:34:33 -0800 Subject: [PATCH 01/22] Comments and standard BORG formatting --- .../geometry/tests/testSimilarity3.cpp | 119 +++++++++--------- 1 file changed, 62 insertions(+), 57 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b2b5d5702..ba71a5717 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -16,17 +16,15 @@ */ #include -#include -#include -#include -#include #include #include #include #include #include - +#include +#include #include + #include using namespace gtsam; @@ -35,10 +33,11 @@ using symbol_shorthand::X; GTSAM_CONCEPT_TESTABLE_INST(Similarity3) -static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::rodriguez(0.3,0,0); -static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); -static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); +static Point3 P(0.2, 0.7, -2); +static Rot3 R = Rot3::rodriguez(0.3, 0, 0); +static Similarity3 T(R, Point3(3.5, -8.2, 4.2), 1); +static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), + 1); static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); //****************************************************************************** @@ -63,39 +62,29 @@ TEST(Similarity3, Getters2) { } TEST(Similarity3, AdjointMap) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + 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); + 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; + 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); + Similarity3 expected(Re, te, 1.0 / 7.0); EXPECT(assert_equal(expected, test.inverse(), 1e-3)); } 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); + 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)); } //****************************************************************************** @@ -118,14 +107,14 @@ TEST(Similarity3, Manifold) { 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::rodriguez(0.3,0,0); + Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0), Point3(4, 5, 6), 1); + Rot3 R = Rot3::rodriguez(0.3, 0, 0); Vector vlocal2 = sim.localCoordinates(other2); @@ -135,28 +124,27 @@ TEST(Similarity3, Manifold) { } /* ************************************************************************* */ -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); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } /* ************************************************************************* */ -TEST(Similarity3, manifold_first_order) -{ +TEST(Similarity3, manifold_first_order) { Similarity3 t1 = T; Similarity3 t2 = T3; Similarity3 origin; @@ -166,48 +154,62 @@ TEST(Similarity3, manifold_first_order) EXPECT(assert_equal(t1, t2.retract(d21))); } +//****************************************************************************** +// 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 +219,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"); From fcd00450d3eec745f734120d3660d95cbfcbb342 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 20:09:44 -0800 Subject: [PATCH 02/22] Formatting, use Point3/Rot3, resolved link error of operator*(Point3) --- gtsam_unstable/geometry/Similarity3.cpp | 52 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 18 +++++---- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index cfc508ac7..77c54bd46 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -17,36 +17,34 @@ #include -#include -#include #include - #include namespace gtsam { Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(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_); + return Pose3(R_, s_ * t_); } Similarity3 Similarity3::identity() { - return Similarity3(); } + return Similarity3(); +} //Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { // return Vector7(); @@ -61,35 +59,36 @@ bool Similarity3::operator==(const Similarity3& other) const { } 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); + 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 { +Point3 Similarity3::transform_from(const Point3& p) const { return R_ * (s_ * p) + t_; } -Matrix7 Similarity3::AdjointMap() const{ +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; + adj << s_ * R, A, -s_ * t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix< + double, 1, 6>::Zero(), 1; return adj; } -inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { +Point3 Similarity3::operator*(const Point3& 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_); + Rot3 Rt = R_.inverse(); + Point3 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 Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); } void Similarity3::print(const std::string& s) const { @@ -100,18 +99,20 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } -Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { +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. + Rot3 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 + Point3(v.segment<3>(3)), // Retract the translation 1.0 + v[6]); //finally, update scale using v[6] } -Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rotation r; //Create a zero rotation to do the retraction +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(); @@ -121,4 +122,3 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobi } } - diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 89f1010c4..2a03288dd 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: @@ -52,7 +54,7 @@ public: 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); @@ -80,10 +82,10 @@ public: /// Return the inverse Similarity3 inverse() const; - Translation transform_from(const Translation& p) const; + Point3 transform_from(const Point3& p) const; /** syntactic sugar for transform_from */ - inline Translation operator*(const Translation& p) const; + Point3 operator*(const Point3& p) const; Similarity3 operator*(const Similarity3& T) const; @@ -92,12 +94,12 @@ public: /// @{ /// Return a GTSAM rotation - const Rotation& rotation() const { + const Rot3& rotation() const { return R_; }; /// Return a GTSAM translation - const Translation& translation() const { + const Point3& translation() const { return t_; }; From 6bfda9fcba5bb12aa261a29025754b49d7ab89f7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 20:34:56 -0800 Subject: [PATCH 03/22] Added prototype derivatives for transform_from --- gtsam_unstable/geometry/Similarity3.cpp | 8 +++-- gtsam_unstable/geometry/Similarity3.h | 33 +++++++++++-------- .../geometry/tests/testSimilarity3.cpp | 25 ++++++++++++++ 3 files changed, 50 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 77c54bd46..683f73172 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -63,7 +63,8 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { && s_ > (sim.s_ - tol); } -Point3 Similarity3::transform_from(const Point3& p) const { +Point3 Similarity3::transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { return R_ * (s_ * p) + t_; } @@ -72,8 +73,9 @@ Matrix7 Similarity3::AdjointMap() const { 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< - double, 1, 6>::Zero(), 1; + adj << s_ * R, A, -s_ * t, Z_3x3, R, // + Matrix31::Zero(), // + Matrix16::Zero(), 1; return adj; } diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 2a03288dd..0075f05ec 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -82,7 +82,9 @@ public: /// Return the inverse Similarity3 inverse() const; - Point3 transform_from(const Point3& p) const; + Point3 transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; /** syntactic sugar for transform_from */ Point3 operator*(const Point3& p) const; @@ -96,17 +98,17 @@ public: /// 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 operator Pose3() const; @@ -114,12 +116,12 @@ public: /// 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 @@ -127,10 +129,11 @@ public: /// Update Similarity transform via 7-dim vector in tangent space struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); + 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); + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + static Vector7 Local(const Similarity3& other, + ChartJacobian H = boost::none); }; /// Project from one tangent space to another @@ -141,12 +144,16 @@ public: /// @{ /// 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); + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = + boost::none); + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = + boost::none); - using LieGroup::inverse; // version with derivative + using LieGroup::inverse; + // version with derivative }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits { +}; } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index ba71a5717..c68b03d8d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -23,10 +23,14 @@ #include #include #include +#include #include #include +#include +#include + using namespace gtsam; using namespace std; using symbol_shorthand::X; @@ -40,6 +44,9 @@ static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); +// Simpler transform +Similarity3 T4(Rot3(), Point3(1, 1, 0), 2); + //****************************************************************************** TEST(Similarity3, Constructors) { Similarity3 test; @@ -154,6 +161,24 @@ TEST(Similarity3, manifold_first_order) { EXPECT(assert_equal(t1, t2.retract(d21))); } +//****************************************************************************** +// Group action on Point3 (with simpler transform) +TEST(Similarity3, GroupAction) { + EXPECT(assert_equal(Point3(1, 1, 0), T4 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(3, 1, 0), T4 * Point3(1, 0, 0))); + + // Test derivative + boost::function f = boost::bind( + &Similarity3::transform_from, _1, _2, boost::none, boost::none); + Point3 q(1, 0, 0); + Matrix expectedH1 = numericalDerivative21(f,T4,q); + Matrix expectedH2 = numericalDerivative22(f,T4,q); + Matrix actualH1,actualH2; + Point3 p = T4.transform_from(q,actualH1,actualH2); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + //****************************************************************************** // Test very simple prior optimization example TEST(Similarity3, Optimization) { From e0e559085673a7f02029cc74b6db7e60e043c371 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 21:08:23 -0800 Subject: [PATCH 04/22] matrix() returns 4*4 matrix \in GL(4) --- gtsam_unstable/geometry/Similarity3.cpp | 7 +++++++ gtsam_unstable/geometry/Similarity3.h | 3 +++ .../geometry/tests/testSimilarity3.cpp | 17 +++++++++++++++++ 3 files changed, 27 insertions(+) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 683f73172..aaa1acf4a 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -68,6 +68,13 @@ Point3 Similarity3::transform_from(const Point3& p, // return R_ * (s_ * p) + t_; } +const Matrix4 Similarity3::matrix() const { + Matrix4 T; + T.topRows<3>() << s_ * R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1; + return T; +} + Matrix7 Similarity3::AdjointMap() const { const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 0075f05ec..9e9ece8f5 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -95,6 +95,9 @@ public: /// @name Standard interface /// @{ + /// Calculate 4*4 matrix group equivalent + const Matrix4 matrix() const; + /// Return a GTSAM rotation const Rot3& rotation() const { return R_; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index c68b03d8d..a569e3e8f 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -161,12 +161,29 @@ TEST(Similarity3, manifold_first_order) { EXPECT(assert_equal(t1, t2.retract(d21))); } +//****************************************************************************** +// 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; + Matrix4 actual = T4.matrix(); + EXPECT(assert_equal(expected, actual)); +} //****************************************************************************** // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) { EXPECT(assert_equal(Point3(1, 1, 0), T4 * Point3(0, 0, 0))); EXPECT(assert_equal(Point3(3, 1, 0), T4 * Point3(1, 0, 0))); + // Test actual group action on R^4 + Vector4 qh; qh << 1,0,0,1; + Vector4 ph; ph << 3,1,0,1; + EXPECT(assert_equal((Vector)ph, T4.matrix()*qh)); + // Test derivative boost::function f = boost::bind( &Similarity3::transform_from, _1, _2, boost::none, boost::none); From 05198c091e2cde72b117dcb478c01509354db3d2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 21:08:41 -0800 Subject: [PATCH 05/22] comment --- gtsam_unstable/geometry/Similarity3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 9e9ece8f5..85a773498 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -113,7 +113,7 @@ public: return s_; } - /// Convert to a rigid body pose + /// Convert to a rigid body pose (R, s*t) operator Pose3() const; /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes From 728991e31f7f1832273cf9a40bf44347f72304d7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 21:08:57 -0800 Subject: [PATCH 06/22] Fixed H2 --- gtsam_unstable/geometry/Similarity3.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index aaa1acf4a..c69733476 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -65,6 +65,8 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { Point3 Similarity3::transform_from(const Point3& p, // OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { + if (H2) + *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() return R_ * (s_ * p) + t_; } From d8822e5b57f42352156730119061a9dafb404247 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 21:58:54 -0800 Subject: [PATCH 07/22] H1 works for rot/translation, but not scale :-( --- gtsam_unstable/geometry/Similarity3.cpp | 14 +++- .../geometry/tests/testSimilarity3.cpp | 84 ++++++++++++++----- 2 files changed, 74 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index c69733476..662a39916 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -65,9 +65,17 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { Point3 Similarity3::transform_from(const Point3& p, // OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { + if (H1) { + const Matrix3 R = R_.matrix(); + Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); + *H1 << DR, R, R * p.vector(); + } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() return R_ * (s_ * p) + t_; + // TODO: Effect of scale change is this, right? + // 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 } const Matrix4 Similarity3::matrix() const { @@ -82,9 +90,9 @@ Matrix7 Similarity3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; - adj << s_ * R, A, -s_ * t, Z_3x3, R, // - Matrix31::Zero(), // - Matrix16::Zero(), 1; + adj << s_ * R, A, -s_ * t, // 3*7 + Z_3x3, R, Matrix31::Zero(), // 3*7 + Matrix16::Zero(), 1; // 1*7 return adj; } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index a569e3e8f..16ee9c55f 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ TEST(Similarity3, Getters2) { 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; @@ -75,6 +77,7 @@ TEST(Similarity3, AdjointMap) { 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; @@ -84,7 +87,8 @@ TEST(Similarity3, inverse) { EXPECT(assert_equal(expected, test.inverse(), 1e-3)); } -TEST(Similarity3, multiplication) { +//****************************************************************************** +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; @@ -130,7 +134,7 @@ TEST(Similarity3, Manifold) { // TODO add unit tests for retract and localCoordinates } -/* ************************************************************************* */ +//****************************************************************************** TEST( Similarity3, retract_first_order) { Similarity3 id; Vector v = zero(7); @@ -142,7 +146,7 @@ TEST( Similarity3, retract_first_order) { EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(Similarity3, localCoordinates_first_order) { Vector d12 = repeat(7, 0.1); d12(6) = 1.0; @@ -150,7 +154,7 @@ TEST(Similarity3, localCoordinates_first_order) { EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(Similarity3, manifold_first_order) { Similarity3 t1 = T; Similarity3 t2 = T3; @@ -165,11 +169,7 @@ 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)); } @@ -177,23 +177,38 @@ TEST(Similarity3, Matrix) { // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) { EXPECT(assert_equal(Point3(1, 1, 0), T4 * Point3(0, 0, 0))); - EXPECT(assert_equal(Point3(3, 1, 0), T4 * Point3(1, 0, 0))); // Test actual group action on R^4 - Vector4 qh; qh << 1,0,0,1; - Vector4 ph; ph << 3,1,0,1; - EXPECT(assert_equal((Vector)ph, T4.matrix()*qh)); + Vector4 qh; + qh << 1, 0, 0, 1; + Vector4 ph; + ph << 3, 1, 0, 1; + EXPECT(assert_equal((Vector )ph, T4.matrix() * qh)); // Test derivative - boost::function f = boost::bind( + boost::function f = boost::bind( &Similarity3::transform_from, _1, _2, boost::none, boost::none); - Point3 q(1, 0, 0); - Matrix expectedH1 = numericalDerivative21(f,T4,q); - Matrix expectedH2 = numericalDerivative22(f,T4,q); - Matrix actualH1,actualH2; - Point3 p = T4.transform_from(q,actualH1,actualH2); - EXPECT(assert_equal(expectedH1, actualH1)); - EXPECT(assert_equal(expectedH2, actualH2)); + + { // T + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T, q); + Matrix H2 = numericalDerivative22(f, T, q); + Matrix actualH1, actualH2; + T.transform_from(q, actualH1, actualH2); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } + { // T4 + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T4, q); + Matrix H2 = numericalDerivative22(f, T4, q); + Matrix actualH1, actualH2; + Point3 p = T4.transform_from(q, actualH1, actualH2); + EXPECT(assert_equal(Point3(3, 1, 0), p)); + EXPECT(assert_equal(Point3(3, 1, 0), T4 * q)); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } } //****************************************************************************** @@ -294,6 +309,33 @@ 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| +} + //****************************************************************************** int main() { TestResult tr; From 74605df6417f8febe1562482c3c4a539a70fff84 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 3 Mar 2015 07:42:31 -0800 Subject: [PATCH 08/22] Re-ordered methods in .h and .cpp to match them --- gtsam_unstable/geometry/Similarity3.cpp | 89 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 61 +++++++++-------- 2 files changed, 77 insertions(+), 73 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 662a39916..aafbf4c5f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -22,10 +22,6 @@ namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(s) { -} - Similarity3::Similarity3() : R_(), t_(), s_(1) { } @@ -38,24 +34,8 @@ 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::identity() { - return Similarity3(); -} - -//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::operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } bool Similarity3::equals(const Similarity3& sim, double tol) const { @@ -63,6 +43,31 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { && s_ > (sim.s_ - tol); } +bool Similarity3::operator==(const Similarity3& other) const { + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +} + +void Similarity3::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + 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 { if (H1) { @@ -78,11 +83,8 @@ Point3 Similarity3::transform_from(const Point3& p, // // 0001 000 1 1 000 1 1 } -const Matrix4 Similarity3::matrix() const { - Matrix4 T; - T.topRows<3>() << s_ * R_.matrix(), t_.vector(); - T.bottomRows<1>() << 0, 0, 0, 1; - return T; +Point3 Similarity3::operator*(const Point3& p) const { + return transform_from(p); } Matrix7 Similarity3::AdjointMap() const { @@ -96,26 +98,12 @@ Matrix7 Similarity3::AdjointMap() const { return adj; } -Point3 Similarity3::operator*(const Point3& p) const { - return transform_from(p); +Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { + return Vector7(); } -Similarity3 Similarity3::inverse() const { - Rot3 Rt = R_.inverse(); - Point3 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_); -} - -void Similarity3::print(const std::string& s) const { - std::cout << std::endl; - std::cout << s; - rotation().print("R:\n"); - translation().print("t: "); - std::cout << "s: " << scale() << std::endl; +Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { + return Similarity3(); } Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, @@ -139,5 +127,16 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, v[6] = other.s_ - 1.0; return v; } + +const Matrix4 Similarity3::matrix() const { + Matrix4 T; + T.topRows<3>() << s_ * R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1; + return T; } +Similarity3::operator Pose3() const { + return Pose3(R_, s_ * t_); +} + +} diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 85a773498..82a3d8362 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -48,6 +48,7 @@ public: /// @name Constructors /// @{ + /// Default constructor Similarity3(); /// Construct pure scaling @@ -79,9 +80,16 @@ public: /// Return an identity transform static Similarity3 identity(); + /// Composition + Similarity3 operator*(const Similarity3& T) const; + /// Return the inverse Similarity3 inverse() const; + /// @} + /// @name Group action on Point3 + /// @{ + Point3 transform_from(const Point3& p, // OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; @@ -89,7 +97,31 @@ public: /** syntactic sugar for transform_from */ Point3 operator*(const Point3& p) const; - Similarity3 operator*(const Similarity3& T) const; + /// @} + /// @name Lie Group + /// @{ + + /// Not currently implemented, required because this is a lie group + static Vector7 Logmap(const Similarity3& s, // + OptionalJacobian<7, 7> Hm = boost::none); + + /// Not currently implemented, required because this is a lie group + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); + + /// Update Similarity transform via 7-dim vector in tangent space + 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); + }; + + /// Project from one tangent space to another + Matrix7 AdjointMap() const; + + using LieGroup::inverse; /// @} /// @name Standard interface @@ -127,33 +159,6 @@ public: } /// @} - /// @name Chart - /// @{ - - /// Update Similarity transform via 7-dim vector in tangent space - 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); - }; - - /// Project from one tangent space to another - Matrix7 AdjointMap() const; - - /// @} - /// @name Stubs - /// @{ - - /// 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); - - using LieGroup::inverse; - // version with derivative }; template<> From 0d5f0510abbdc742125454f35e309bf69a388001 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 10 Jun 2015 11:01:34 -0400 Subject: [PATCH 09/22] Expmap and Logmap, still incorrect around identity --- gtsam_unstable/geometry/Similarity3.cpp | 80 +++++++++++++++---- .../geometry/tests/testSimilarity3.cpp | 18 +++++ 2 files changed, 84 insertions(+), 14 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index aafbf4c5f..72d8bbbd5 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -99,33 +99,85 @@ Matrix7 Similarity3::AdjointMap() const { } Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { - return Vector7(); + // To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org + // www.ethaneade.org/latex2html/lie/node29.html + Vector3 w = Rot3::Logmap(s.R_); + double lambda = log(s.s_); + + 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; + Vector3 u = V.inverse()*s.t_.vector(); + + Vector7 result; + result << w, u, lambda; + return result; } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { - return Similarity3(); + Matrix31 w(v.head<3>()); + Matrix33 wx = skewSymmetric(w[0], w[1], w[2]); + double lambda = v[6]; + double lambdasquared = lambda * lambda; + Matrix31 u(v.segment<3>(3)); + 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)); } 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] +// 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; +// 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 { diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 16ee9c55f..394c41aa7 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -173,6 +173,24 @@ TEST(Similarity3, Matrix) { Matrix4 actual = T4.matrix(); EXPECT(assert_equal(expected, actual)); } + +//***************************************************************************** +// Exponential and log maps +TEST(Similarity3, ExpLogMap) { + Vector7 expected; + expected << 0.1,0.2,0.3,0.4,0.5,0.6,0.7; + Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(expected)); + EXPECT(assert_equal(expected, 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)); +} //****************************************************************************** // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) { From 5ceb7d9ddc1447d1f2d09476478f9000f6b3caff Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 10 Jun 2015 16:36:56 -0400 Subject: [PATCH 10/22] Fully implemented logmap/expmap, which are used for retract/localCoordinates --- gtsam_unstable/geometry/Similarity3.cpp | 137 +++++++++++++----- gtsam_unstable/geometry/Similarity3.h | 9 ++ .../geometry/tests/testSimilarity3.cpp | 8 +- 3 files changed, 114 insertions(+), 40 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 72d8bbbd5..8ea6c2bcd 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -98,30 +98,93 @@ Matrix7 Similarity3::AdjointMap() const { return adj; } +Matrix33 Similarity3::GetV(Vector3 w, double 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, Y, Z, W, alpha, beta, gama, mu, upsilon, A, B, C; + if (thetasquared > 1e-9 && lambdasquared > 1e-9) { + X = sin(theta) / theta; + Y = (1 - cos(theta)) / thetasquared; + Z = (1 - X) / thetasquared; + W = (0.5 - Y) / thetasquared; + alpha = lambdasquared / (lambdasquared + thetasquared); + beta = (exp(-lambda) - 1 + lambda) / lambdasquared; + gama = Y - (lambda * Z); + mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) + / (lambdasquared * lambda); + upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gama) + gama; + C = alpha * (mu - upsilon) + upsilon; + } + else if(thetasquared <= 1e-9 && lambdasquared > 1e-9) { + //Taylor series expansions + X = 1; + Y = 0.5-thetasquared/24.0; + Z = 1.0/6.0 - thetasquared/120.0; + W = 1.0/24.0 - thetasquared/720.0; + alpha = lambdasquared / (lambdasquared + thetasquared); + beta = (exp(-lambda) - 1 + lambda) / lambdasquared; + gama = Y - (lambda * Z); + mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) + / (lambdasquared * lambda); + upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gama) + gama; + C = alpha * (mu - upsilon) + upsilon; + } + else if(thetasquared > 1e-9 && lambdasquared <= 1e-9) { + X = sin(theta) / theta; + Y = (1 - cos(theta)) / thetasquared; + Z = (1 - X) / thetasquared; + W = (0.5 - Y) / thetasquared; + alpha = lambdasquared / (lambdasquared + thetasquared); + beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 + - (lambda * lambdasquared) / 120; + gama = Y - (lambda * Z); + mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 + - (lambda * lambdasquared) / 720; + upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambdasquared / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = alpha * (beta - gama) + gama; + C = alpha * (mu - upsilon) + upsilon; + } + else { + X = 1; + Y = 0.5-thetasquared/24.0; + Z = 1.0 / 6.0 - thetasquared / 120.0; + W = 1.0 / 24.0 - thetasquared / 720.0; + alpha = lambdasquared / (lambdasquared + thetasquared); + beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 + - (lambda * lambdasquared) / 120; + gama = Y - (lambda * Z); + mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 + - (lambda * lambdasquared) / 720; + upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambdasquared / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = gama; + C = upsilon; + } + return A * Matrix33::Identity() + B * wx + C * wx * wx; +} + Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { // To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org // www.ethaneade.org/latex2html/lie/node29.html Vector3 w = Rot3::Logmap(s.R_); double lambda = log(s.s_); - - 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; - Vector3 u = V.inverse()*s.t_.vector(); + Matrix33 V = GetV(w, lambda); + Vector3 u = V.inverse() * s.t_.vector(); Vector7 result; result << w, u, lambda; @@ -130,25 +193,27 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { Matrix31 w(v.head<3>()); - Matrix33 wx = skewSymmetric(w[0], w[1], w[2]); double lambda = v[6]; - double lambdasquared = lambda * lambda; Matrix31 u(v.segment<3>(3)); - 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; + + 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)); } diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 82a3d8362..81f7e4d1c 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -159,6 +159,15 @@ public: } /// @} + /// @name Helper functions + /// @{ + + /// Calculate expmap and logmap coefficients. +private: + static Matrix33 GetV(Vector3 w, double lambda); + + /// @} + }; template<> diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 394c41aa7..d5ece1a3f 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -140,10 +140,10 @@ TEST( Similarity3, retract_first_order) { 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)); +// v(3) = 0.2; +// v(4) = 0.7; +// v(5) = -2; +// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); } //****************************************************************************** From f6ac546cc3a2c1ee2d6c78765dcd18cfd998cebe Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Thu, 11 Jun 2015 13:56:35 -0400 Subject: [PATCH 11/22] Added comparison to brute force matrix exponential --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d5ece1a3f..27e5bf8fb 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -190,6 +190,15 @@ TEST(Similarity3, ExpLogMap) { Similarity3 expZero = Similarity3::Expmap(zeros); Similarity3 ident = Similarity3::identity(); EXPECT(assert_equal(expZero, ident)); + + // Compare to matrix exponential calculated using matlab expm + Rot3 Rexpm(0.9358, -0.2832, 0.2102, + 0.3029, 0.9506, -0.0680, + -0.1805, 0.1273, 0.9753); + Point3 texpm(0.2724, 0.3825, 0.4213); + Similarity3 Sexpm(Rexpm, texpm, 2.0138); + Similarity3 Scalc = Similarity3::Expmap(expected); + EXPECT(assert_equal(Sexpm, Scalc, 1e-3)); } //****************************************************************************** // Group action on Point3 (with simpler transform) From 99fce3f5afb8fdd16fdf2ed098716a92bb61c6f8 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Thu, 11 Jun 2015 19:55:10 -0400 Subject: [PATCH 12/22] Added a few tests --- .../geometry/tests/testSimilarity3.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 27e5bf8fb..da2920336 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -212,6 +212,25 @@ TEST(Similarity3, GroupAction) { ph << 3, 1, 0, 1; EXPECT(assert_equal((Vector )ph, T4.matrix() * qh)); + Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); + Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); + + Point3 pa = Point3(1, 0, 0); + Point3 pTa = Point3(2, 2, 3); + Point3 pTb = Point3(3, 2, 3); + + EXPECT(assert_equal(pTa, Ta.transform_from(pa))); + EXPECT(assert_equal(pTb, 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); + + Point3 pTc = Point3(1, 3, 3); + Point3 pTd = Point3(1, 4, 3); + + EXPECT(assert_equal(pTc, Tc.transform_from(pa))); + EXPECT(assert_equal(pTd, Td.transform_from(pa))); + // Test derivative boost::function f = boost::bind( &Similarity3::transform_from, _1, _2, boost::none, boost::none); From 400a17d9ab00068c6e4c76a43d3ffbbeb2c7e4c5 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 26 Jun 2015 15:44:08 -0400 Subject: [PATCH 13/22] change: a little clean up --- gtsam_unstable/geometry/Similarity3.cpp | 57 ++----------------- gtsam_unstable/geometry/Similarity3.h | 20 ++++--- .../geometry/tests/testSimilarity3.cpp | 27 +++++++-- 3 files changed, 38 insertions(+), 66 deletions(-) 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) { From 12f9b413ff5cc48738e8eed9c9e85c25b9a7f072 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 26 Jun 2015 16:01:26 -0400 Subject: [PATCH 14/22] feature: add concept test for similarity3 --- gtsam_unstable/geometry/Similarity3.cpp | 7 +++++++ gtsam_unstable/geometry/tests/testSimilarity3.cpp | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 6d38cfa69..a60ec2a24 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -189,12 +189,19 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { double lambda = log(s.s_); Vector7 result; result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; + if (Hm) { + // incomplete + } return result; } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { Vector3 w(v.head<3>()); double lambda = v[6]; + if (Hm) { + Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); + // incomplete + } return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9ad165180..0f31a6755 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -49,6 +49,13 @@ static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); // Simpler transform Similarity3 T4(Rot3(), Point3(1, 1, 0), 2); +//****************************************************************************** +TEST(Similarity3, concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + //****************************************************************************** TEST(Similarity3, Constructors) { Similarity3 test; From 35c23da427847811920b309ee5c4ddcb8f70d75c Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 26 Jun 2015 18:24:30 -0400 Subject: [PATCH 15/22] change: add const traits and renaming some test parameters --- gtsam_unstable/geometry/Similarity3.cpp | 2 +- gtsam_unstable/geometry/Similarity3.h | 5 ++ .../geometry/tests/testSimilarity3.cpp | 65 +++++++++---------- 3 files changed, 38 insertions(+), 34 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index a60ec2a24..f0f673bca 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -44,7 +44,7 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { } bool Similarity3::operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); + return equals(other, 1e-9); } void Similarity3::print(const std::string& s) const { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 30d8ab694..c380c4aa7 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -179,4 +179,9 @@ private: template<> struct traits : public internal::LieGroup { }; + +template<> +struct traits : public internal::LieGroup { +}; + } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 0f31a6755..955d09e89 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -41,16 +41,16 @@ GTSAM_CONCEPT_TESTABLE_INST(Similarity3) static Point3 P(0.2, 0.7, -2); static Rot3 R = Rot3::rodriguez(0.3, 0, 0); -static Similarity3 T(R, Point3(3.5, -8.2, 4.2), 1); -static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), - 1); +static double s = 4; +static Similarity3 T_default(R, Point3(3.5, -8.2, 4.2), 1); +static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); - -// Simpler transform -Similarity3 T4(Rot3(), Point3(1, 1, 0), 2); +static Similarity3 T4(R, P, s); +static Similarity3 T5(R, P, 10); +static Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform //****************************************************************************** -TEST(Similarity3, concepts) { +TEST(Similarity3, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsLieGroup)); @@ -58,23 +58,23 @@ TEST(Similarity3, concepts) { //****************************************************************************** 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); -//****************************************************************************** -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); + 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); } //****************************************************************************** @@ -137,7 +137,6 @@ 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); Vector vlocal = sim.localCoordinates(other); @@ -170,13 +169,13 @@ TEST( Similarity3, retract_first_order) { TEST(Similarity3, localCoordinates_first_order) { Vector d12 = repeat(7, 0.1); d12(6) = 1.0; - Similarity3 t1 = T, t2 = t1.retract(d12); + Similarity3 t1 = T_default, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } //****************************************************************************** TEST(Similarity3, manifold_first_order) { - Similarity3 t1 = T; + Similarity3 t1 = T_default; Similarity3 t2 = T3; Similarity3 origin; Vector d12 = t1.localCoordinates(t2); @@ -193,7 +192,7 @@ TEST(Similarity3, Matrix) { 0, 2, 0, 1, 0, 0, 2, 0, 0, 0, 0, 1; - Matrix4 actual = T4.matrix(); + Matrix4 actual = T6.matrix(); EXPECT(assert_equal(expected, actual)); } @@ -227,14 +226,14 @@ TEST(Similarity3, ExpLogMap) { //****************************************************************************** // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) { - EXPECT(assert_equal(Point3(1, 1, 0), T4 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(1, 1, 0), T6 * Point3(0, 0, 0))); // Test actual group action on R^4 Vector4 qh; qh << 1, 0, 0, 1; Vector4 ph; ph << 3, 1, 0, 1; - EXPECT(assert_equal((Vector )ph, T4.matrix() * qh)); + EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); @@ -259,23 +258,23 @@ TEST(Similarity3, GroupAction) { boost::function f = boost::bind( &Similarity3::transform_from, _1, _2, boost::none, boost::none); - { // T + { // T default Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T, q); - Matrix H2 = numericalDerivative22(f, T, q); + Matrix H1 = numericalDerivative21(f, T_default, q); + Matrix H2 = numericalDerivative22(f, T_default, q); Matrix actualH1, actualH2; - T.transform_from(q, actualH1, actualH2); + T_default.transform_from(q, actualH1, actualH2); EXPECT(assert_equal(H1, actualH1)); EXPECT(assert_equal(H2, actualH2)); } { // T4 Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T4, q); - Matrix H2 = numericalDerivative22(f, T4, q); + Matrix H1 = numericalDerivative21(f, T6, q); + Matrix H2 = numericalDerivative22(f, T6, q); Matrix actualH1, actualH2; - Point3 p = T4.transform_from(q, actualH1, actualH2); + Point3 p = T6.transform_from(q, actualH1, actualH2); EXPECT(assert_equal(Point3(3, 1, 0), p)); - EXPECT(assert_equal(Point3(3, 1, 0), T4 * q)); + EXPECT(assert_equal(Point3(3, 1, 0), T6 * q)); EXPECT(assert_equal(H1, actualH1)); EXPECT(assert_equal(H2, actualH2)); } From c1898acdaa302597c889cd408793d722ffcfaf16 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sat, 27 Jun 2015 00:04:34 -0400 Subject: [PATCH 16/22] feature: a mathematica file for similarity group, currently with exponential map --- doc/Mathematica/SimiliarityGroup.nb | 413 ++++++++++++++++++++++++++++ 1 file changed, 413 insertions(+) create mode 100644 doc/Mathematica/SimiliarityGroup.nb diff --git a/doc/Mathematica/SimiliarityGroup.nb b/doc/Mathematica/SimiliarityGroup.nb new file mode 100644 index 000000000..2dd8a5879 --- /dev/null +++ b/doc/Mathematica/SimiliarityGroup.nb @@ -0,0 +1,413 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 10.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 15602, 404] +NotebookOptionsPosition[ 14491, 364] +NotebookOutlinePosition[ 14850, 380] +CellTagsIndexPosition[ 14807, 377] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ + +Cell[CellGroupData[{ +Cell[TextData[{ + "Similarity Group\n", + StyleBox["Representation\n", "Chapter"], + StyleBox["A similarity transformation is a combination ", "Text"], + "\n", + StyleBox["Exponential and Logmap\n\nRetract and localCoordinate", "Chapter"] +}], "Title", + CellChangeTimes->{{3.6442457705813923`*^9, 3.644245851964954*^9}, { + 3.644245883900199*^9, 3.644245897451631*^9}, {3.644246409411936*^9, + 3.6442464411218433`*^9}, {3.644246632965823*^9, 3.644246721355283*^9}, + 3.644339337635804*^9, {3.6443397539003696`*^9, 3.644339805690949*^9}, { + 3.6443398507824793`*^9, 3.644339940436355*^9}, {3.644339982058689*^9, + 3.64434014492003*^9}, {3.644340183085711*^9, 3.64434030003795*^9}, { + 3.644340442268632*^9, 3.644340457502075*^9}, {3.644340502643257*^9, + 3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}}], + +Cell[BoxData[ + RowBox[{"\[IndentingNewLine]", + RowBox[{ + Cell["Lie group generators for similarity transform:"], + "\[IndentingNewLine]", + RowBox[{"G1", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G2", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G3", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G4", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G5", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G6", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", + RowBox[{"G7", " ", "=", " ", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", + RowBox[{"-", "1"}]}], "}"}]}], "}"}]}]}]}]], "Input", + CellChangeTimes->{{3.644340621916498*^9, 3.644340623238144*^9}, { + 3.64434081043055*^9, 3.644340996367342*^9}, {3.6443410615657*^9, + 3.644341237564106*^9}, {3.644341297291617*^9, 3.6443413676185513`*^9}, { + 3.644341671605256*^9, 3.64434167330084*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[{Cell["Lie vectors similarity3 can be described as:"], "\ +\[IndentingNewLine]", + RowBox[{"u", " ", "=", " ", + RowBox[{"{", + RowBox[{"x", ",", "y", ",", "z"}], "}"}]}], "\[IndentingNewLine]", + RowBox[{"v", " ", "=", " ", + RowBox[{"{", + RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simLieVector", " ", "=", " ", + RowBox[{"{", + RowBox[{"u", ",", "v", ",", "lambda"}], "}"}]}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["The matrix form of \ +the similarity vector is:"], "\[IndentingNewLine]", + RowBox[{"simMatrix", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{"0", ",", + RowBox[{"{", + RowBox[{"i", ",", "4"}], "}"}], ",", + RowBox[{"{", + RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"simMatrix", "[", + RowBox[{"[", + RowBox[{ + RowBox[{"1", ";;", "3"}], ",", + RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", + RowBox[{"SkewSym", "[", "v", "]"}]}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["\<\ +The exponential map of similarity3 is: +\ +\>"]}], "Input", + CellChangeTimes->{{3.64434137002735*^9, 3.644341539824277*^9}, { + 3.644341595117811*^9, 3.644341750502033*^9}, {3.6443418211779013`*^9, + 3.644341871033806*^9}, {3.644341902145*^9, 3.6443419325955687`*^9}, { + 3.6443420032518187`*^9, 3.644342080926795*^9}, {3.644344115577599*^9, + 3.644344138921712*^9}, {3.644344183657415*^9, 3.644344243563266*^9}, { + 3.644344277871182*^9, 3.6443442927775*^9}, {3.6443443719167137`*^9, + 3.6443445055251913`*^9}, {3.6443445402223988`*^9, 3.644344621076252*^9}, { + 3.644344658699892*^9, 3.644344696943632*^9}, {3.644344750998406*^9, + 3.644344755470311*^9}, {3.6443451156161633`*^9, 3.644345149594688*^9}}], + +Cell[BoxData[ + InterpretationBox[Cell["Lie vectors similarity3 can be described as:"], + TextCell["Lie vectors similarity3 can be described as:"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"x", ",", "y", ",", "z"}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.64434513031706*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130321727*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"x", ",", "y", ",", "z"}], "}"}], ",", + RowBox[{"{", + RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}], ",", "lambda"}], + "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130326034*^9}}], + +Cell[BoxData[ + InterpretationBox[Cell["The matrix form of the similarity vector is:"], + TextCell["The matrix form of the similarity vector is:"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.64434513033029*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130334682*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", "w3"}], ",", "w2"}], "}"}], ",", + RowBox[{"{", + RowBox[{"w3", ",", "0", ",", + RowBox[{"-", "w1"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", "w2"}], ",", "w1", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130339306*^9}}], + +Cell[BoxData[ + InterpretationBox[Cell["\<\ +The exponential map of similarity3 is: +\ +\>"], + TextCell["The exponential map of similarity3 is:\n"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130343567*^9}}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{"simExponetialMap", " ", "=", " ", + RowBox[{"Expand", "[", + RowBox[{"Series", "[", "]"}], "]"}]}], "\[IndentingNewLine]"}]], "Input", + CellChangeTimes->{{3.644341752983704*^9, 3.644341812107615*^9}}], + +Cell[BoxData[ + InterpretationBox[Cell["Lie vectors:"], + TextCell["Lie vectors:"]]], "Output", + CellChangeTimes->{3.644341528720538*^9, 3.64434162193827*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"x", ",", "y", ",", "z"}], "}"}]], "Output", + CellChangeTimes->{3.644341528720538*^9, 3.644341621943945*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]], "Output", + CellChangeTimes->{3.644341528720538*^9, 3.644341621947913*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"x", ",", "y", ",", "z"}], "}"}], ",", + RowBox[{"{", + RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}], ",", "lambda"}], + "}"}]], "Output", + CellChangeTimes->{3.644341528720538*^9, 3.644341621951668*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", "w3"}], ",", "w2"}], "}"}], ",", + RowBox[{"{", + RowBox[{"w3", ",", "0", ",", + RowBox[{"-", "w1"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", "w2"}], ",", "w1", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.644341528720538*^9, 3.644341621955081*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"I3", " ", "=", " ", + RowBox[{"IdentyMatrix", "[", "3", "]"}]}]], "Input", + CellChangeTimes->{{3.6443405921748047`*^9, 3.6443406096153393`*^9}}], + +Cell[BoxData[ + RowBox[{"IdentyMatrix", "[", "3", "]"}]], "Output", + CellChangeTimes->{3.64434061119941*^9}] +}, Open ]] +}, Open ]] +}, +WindowSize->{808, 579}, +WindowMargins->{{Automatic, -111}, {Automatic, 20}}, +FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (December 4, \ +2014)", +StyleDefinitions->"Default.nb" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[CellGroupData[{ +Cell[580, 22, 828, 14, 287, "Title"], +Cell[1411, 38, 3548, 95, 166, "Input"], +Cell[CellGroupData[{ +Cell[4984, 137, 1814, 41, 206, "Input"], +Cell[6801, 180, 703, 10, 31, "Output"], +Cell[7507, 192, 627, 10, 28, "Output"], +Cell[8137, 204, 631, 10, 28, "Output"], +Cell[8771, 216, 757, 15, 28, "Output"], +Cell[9531, 233, 702, 10, 31, "Output"], +Cell[10236, 245, 914, 18, 28, "Output"], +Cell[11153, 265, 874, 19, 28, "Output"], +Cell[12030, 286, 702, 13, 49, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[12769, 304, 240, 5, 46, "Input"], +Cell[13012, 311, 157, 3, 31, "Output"], +Cell[13172, 316, 147, 3, 28, "Output"], +Cell[13322, 321, 150, 3, 28, "Output"], +Cell[13475, 326, 276, 8, 28, "Output"], +Cell[13754, 336, 393, 12, 28, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[14184, 353, 169, 3, 28, "Input"], +Cell[14356, 358, 107, 2, 28, "Output"] +}, Open ]] +}, Open ]] +} +] +*) + +(* End of internal cache information *) From b6cf1ad4c357b9d3795e72595729d8bdcb7b29fe Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sat, 27 Jun 2015 00:43:05 -0400 Subject: [PATCH 17/22] fix: correct some mistakes --- doc/Mathematica/SimiliarityGroup.nb | 1541 ++++++++++++++++++++++++--- 1 file changed, 1412 insertions(+), 129 deletions(-) diff --git a/doc/Mathematica/SimiliarityGroup.nb b/doc/Mathematica/SimiliarityGroup.nb index 2dd8a5879..32bf38ffc 100644 --- a/doc/Mathematica/SimiliarityGroup.nb +++ b/doc/Mathematica/SimiliarityGroup.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 15602, 404] -NotebookOptionsPosition[ 14491, 364] -NotebookOutlinePosition[ 14850, 380] -CellTagsIndexPosition[ 14807, 377] +NotebookDataLength[ 76961, 1687] +NotebookOptionsPosition[ 74977, 1628] +NotebookOutlinePosition[ 75336, 1644] +CellTagsIndexPosition[ 75293, 1641] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -22,10 +22,9 @@ Notebook[{ Cell[CellGroupData[{ Cell[TextData[{ "Similarity Group\n", - StyleBox["Representation\n", "Chapter"], - StyleBox["A similarity transformation is a combination ", "Text"], + StyleBox["Representation", "Chapter"], "\n", - StyleBox["Exponential and Logmap\n\nRetract and localCoordinate", "Chapter"] + StyleBox["Exponential and Logmap\nRetract and localCoordinate", "Chapter"] }], "Title", CellChangeTimes->{{3.6442457705813923`*^9, 3.644245851964954*^9}, { 3.644245883900199*^9, 3.644245897451631*^9}, {3.644246409411936*^9, @@ -34,7 +33,10 @@ Cell[TextData[{ 3.6443398507824793`*^9, 3.644339940436355*^9}, {3.644339982058689*^9, 3.64434014492003*^9}, {3.644340183085711*^9, 3.64434030003795*^9}, { 3.644340442268632*^9, 3.644340457502075*^9}, {3.644340502643257*^9, - 3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}}], + 3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}, { + 3.6443655095926237`*^9, 3.644365512168627*^9}}], + +Cell[CellGroupData[{ Cell[BoxData[ RowBox[{"\[IndentingNewLine]", @@ -131,7 +133,120 @@ Cell[BoxData[ CellChangeTimes->{{3.644340621916498*^9, 3.644340623238144*^9}, { 3.64434081043055*^9, 3.644340996367342*^9}, {3.6443410615657*^9, 3.644341237564106*^9}, {3.644341297291617*^9, 3.6443413676185513`*^9}, { - 3.644341671605256*^9, 3.64434167330084*^9}}], + 3.644341671605256*^9, 3.64434167330084*^9}, {3.644345191055595*^9, + 3.644345216636923*^9}}], + +Cell[BoxData[ + TagBox[ + InterpretationBox[Cell["\<\ +The exponential map of similarity3 is: +\ +\>"], + TextCell["The exponential map of similarity3 is:\n"]], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{3.644345200855969*^9}], + +Cell[BoxData[ + InterpretationBox[Cell["Lie group generators for similarity transform:"], + TextCell["Lie group generators for similarity transform:"]]], "Output", + CellChangeTimes->{3.644345200860836*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.644345200865378*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.6443452008696203`*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.64434520087387*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.644345200878134*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.644345200887875*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.644345200892788*^9}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0", ",", + RowBox[{"-", "1"}]}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{3.6443452008972893`*^9}] +}, Open ]], Cell[CellGroupData[{ @@ -147,8 +262,7 @@ Cell[BoxData[{Cell["Lie vectors similarity3 can be described as:"], "\ RowBox[{"simLieVector", " ", "=", " ", RowBox[{"{", RowBox[{"u", ",", "v", ",", "lambda"}], "}"}]}], - "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["The matrix form of \ -the similarity vector is:"], "\[IndentingNewLine]", + "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{"simMatrix", " ", "=", " ", RowBox[{"Table", "[", RowBox[{"0", ",", @@ -157,26 +271,191 @@ the similarity vector is:"], "\[IndentingNewLine]", RowBox[{"{", RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", RowBox[{ - RowBox[{ - RowBox[{"simMatrix", "[", - RowBox[{"[", + RowBox[{"simMatrix", "[", + RowBox[{"[", + RowBox[{ + RowBox[{"1", ";;", "3"}], ",", + RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", + RowBox[{"SkewSym", "[", "v", "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simMatrix", "[", + RowBox[{"[", + RowBox[{ + RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", + "u"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simMatrix", "[", + RowBox[{"[", + RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", + "lambda"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MatrixForm", "[", "simMatrix", "]"}], "\[IndentingNewLine]", + RowBox[{"(*", + RowBox[{"simExponetialMap", " ", "=", " ", + RowBox[{"Series", "[", + RowBox[{"simMatrix", ",", + RowBox[{"{", + RowBox[{"u", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", "2"}], "}"}]}], + "]"}]}], "*)"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["Exponential map, \ +refer to Ethan Eade"], "\[IndentingNewLine]", + RowBox[{"v_skew", " ", "=", " ", + RowBox[{"SkewSym", + RowBox[{"(", "v", ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"theta", " ", "=", + RowBox[{"sqrt", + RowBox[{"(", + RowBox[{"Transpose", RowBox[{ - RowBox[{"1", ";;", "3"}], ",", - RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", - RowBox[{"SkewSym", "[", "v", "]"}]}], - "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["\<\ -The exponential map of similarity3 is: -\ -\>"]}], "Input", - CellChangeTimes->{{3.64434137002735*^9, 3.644341539824277*^9}, { - 3.644341595117811*^9, 3.644341750502033*^9}, {3.6443418211779013`*^9, - 3.644341871033806*^9}, {3.644341902145*^9, 3.6443419325955687`*^9}, { - 3.6443420032518187`*^9, 3.644342080926795*^9}, {3.644344115577599*^9, - 3.644344138921712*^9}, {3.644344183657415*^9, 3.644344243563266*^9}, { - 3.644344277871182*^9, 3.6443442927775*^9}, {3.6443443719167137`*^9, - 3.6443445055251913`*^9}, {3.6443445402223988`*^9, 3.644344621076252*^9}, { - 3.644344658699892*^9, 3.644344696943632*^9}, {3.644344750998406*^9, - 3.644344755470311*^9}, {3.6443451156161633`*^9, 3.644345149594688*^9}}], + RowBox[{"(", "v", ")"}], ".", "v"}]}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"sin", + RowBox[{ + RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", + RowBox[{"Y", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"cos", + RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"Z", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", "X"}], ")"}], "/", + RowBox[{"(", + RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"W", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"0.5", " ", "-", " ", "Y"}], ")"}], "/", + RowBox[{"(", + RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"alpha", " ", "=", " ", + RowBox[{ + RowBox[{"lambda", "^", "2"}], " ", "/", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"lambda", "^", "2"}], " ", "+", " ", + RowBox[{"theta", "^", "2"}]}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"beta", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"exp", + RowBox[{"(", + RowBox[{"-", "lambda"}], ")"}]}], "-", "1", "+", "lambda"}], ")"}], + "/", + RowBox[{"(", + RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"gamma", " ", "=", " ", + RowBox[{"Y", " ", "-", " ", + RowBox[{"lambda", ".", "Z"}]}]}], "\[IndentingNewLine]", + RowBox[{"mu", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", "lambda", " ", "+", " ", + RowBox[{"0.5", "*", + RowBox[{"lambda", "^", "2"}]}], " ", "-", " ", + RowBox[{"exp", + RowBox[{"(", + RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"nu", " ", "=", " ", + RowBox[{"Z", "-", + RowBox[{"lambda", " ", "W"}]}]}], "\[IndentingNewLine]", + RowBox[{"Av", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"exp", + RowBox[{"(", + RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", + "lambda"}]}], "\[IndentingNewLine]", + RowBox[{"Bv", " ", "=", " ", + RowBox[{ + RowBox[{"alpha", " ", ".", + RowBox[{"(", + RowBox[{"beta", " ", "-", " ", "lambda"}], ")"}]}], " ", "+", " ", + "lambda"}]}], "\[IndentingNewLine]", + RowBox[{"Cv", " ", "=", " ", + RowBox[{ + RowBox[{"alpha", " ", ".", + RowBox[{"(", + RowBox[{"mu", " ", "-", " ", "nv"}], ")"}]}], " ", "+", " ", + "nv"}]}], "\[IndentingNewLine]", + RowBox[{"a", " ", "=", " ", + RowBox[{"sin", + RowBox[{ + RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", + RowBox[{"b", " ", "=", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"cos", + RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"R", " ", "=", " ", + RowBox[{ + RowBox[{"IdentityMatrix", "[", "3", "]"}], "+", " ", + RowBox[{"a", ".", "v_skew"}], " ", "+", " ", + RowBox[{"b", ".", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", + RowBox[{"V", " ", "=", " ", + RowBox[{ + RowBox[{"Av", ".", + RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "+", + RowBox[{"Bv", ".", "v_skew"}], " ", "+", " ", + RowBox[{ + "Cv", ".", " ", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", + RowBox[{"simExponetialMap", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{"0", ",", + RowBox[{"{", + RowBox[{"i", ",", "4"}], "}"}], ",", + RowBox[{"[", + RowBox[{"j", ",", "4"}], "]"}]}], "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simExponetialMap", "[", + RowBox[{"[", + RowBox[{ + RowBox[{"1", ";;", "3"}], ",", + RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", + "R"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simExponetialMap", "[", + RowBox[{"[", + RowBox[{ + RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", + RowBox[{"V", ".", "u"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"simExponetialMap", "[", + RowBox[{"[", + RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", + RowBox[{"exp", + RowBox[{"(", + RowBox[{"-", "lambda"}], ")"}]}]}], "\[IndentingNewLine]", + RowBox[{"MatrixForm", "[", "simExponetialMap", + "]"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->CompressedData[" +1:eJwlz2tIk2EYgOGVsbU2x7BZRi20jNFmZOHyRwuMQKwNMzv8SBwrUSuomaMo +S5GwSQwSyqxAR63Tx9ZJp2hEBzIxhMwlkVGsbQwthLKZS8Kw3Z8/Xi4eeHhu +3oyD9uLy+RKJxJB4mBzKa5LpxrfW2Gpu44q+Mh8K6qHnaG2/048TmvBb/Hjr +dQBHVZc/zHkoiIWC9hu2n6keXZjQW1f6HV3yqR+YL62O45PQ079YEL42gz17 +a6VyOoPZSzGqbVyOQpVnDTaU79OjaePK9fgr9tWIA0r7JmztdpThI5m/Eveb +dMfxSl22aMf2+AmcPR9twh7952b0BzKv40lHkRvVlZYb+P701Xti3zvgE7vu +tV2o/F3Wh1lTjiiqapyinsgBxSJ69QYlqt0vHqQkfPmqpBsj+i39aFo3FsKK +im1jKHSVjuPAv4IJ9BqFGGbcPBvH5JnCeYsTDrYGpbijzScX57tJaWhtiIku +mU7V4qXeonTs2LPbMLcXyEdLc44TdbJII2bO1rtRs9niQY/1sE+8Y2sRXeZQ +d2LY9XgYbU7HCPp3Kb5g+v3cSUxLqhMVOo9OY1VLcIGG/7W5FLjqnVmFqz29 +Kfjp1DMDqkpyNmCWvdaI58zFJtRf+JOHQ8dSzajxH9mJD98M/8SLI7mT+B8u +LiVI + "], + EmphasizeSyntaxErrors->True], Cell[BoxData[ InterpretationBox[Cell["Lie vectors similarity3 can be described as:"], @@ -188,7 +467,19 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.6443674615039167`*^9}], Cell[BoxData[ RowBox[{"{", @@ -200,7 +491,19 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.64434513031706*^9}}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461512148*^9}], Cell[BoxData[ RowBox[{"{", @@ -212,7 +515,19 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130321727*^9}}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461520136*^9}], Cell[BoxData[ RowBox[{"{", @@ -229,19 +544,19 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130326034*^9}}], - -Cell[BoxData[ - InterpretationBox[Cell["The matrix form of the similarity vector is:"], - TextCell["The matrix form of the similarity vector is:"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.64434513033029*^9}}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461527494*^9}], Cell[BoxData[ RowBox[{"{", @@ -261,7 +576,19 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130334682*^9}}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.6443674615362377`*^9}], Cell[BoxData[ RowBox[{"{", @@ -282,88 +609,1025 @@ Cell[BoxData[ 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130339306*^9}}], - -Cell[BoxData[ - InterpretationBox[Cell["\<\ -The exponential map of similarity3 is: -\ -\>"], - TextCell["The exponential map of similarity3 is:\n"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130343567*^9}}] -}, Open ]], - -Cell[CellGroupData[{ - -Cell[BoxData[ - RowBox[{ - RowBox[{"simExponetialMap", " ", "=", " ", - RowBox[{"Expand", "[", - RowBox[{"Series", "[", "]"}], "]"}]}], "\[IndentingNewLine]"}]], "Input", - CellChangeTimes->{{3.644341752983704*^9, 3.644341812107615*^9}}], - -Cell[BoxData[ - InterpretationBox[Cell["Lie vectors:"], - TextCell["Lie vectors:"]]], "Output", - CellChangeTimes->{3.644341528720538*^9, 3.64434162193827*^9}], + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461544832*^9}], Cell[BoxData[ RowBox[{"{", RowBox[{"x", ",", "y", ",", "z"}], "}"}]], "Output", - CellChangeTimes->{3.644341528720538*^9, 3.644341621943945*^9}], + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461553432*^9}], + +Cell[BoxData["lambda"], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461560822*^9}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]], "Output", - CellChangeTimes->{3.644341528720538*^9, 3.644341621947913*^9}], + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + {"0", + RowBox[{"-", "w3"}], "w2", "x"}, + {"w3", "0", + RowBox[{"-", "w1"}], "y"}, + { + RowBox[{"-", "w2"}], "w1", "0", "z"}, + {"0", "0", "0", "lambda"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461569426*^9}], + +Cell[BoxData[ + InterpretationBox[Cell["Exponential map, refer to Ethan Eade"], + TextCell["Exponential map, refer to Ethan Eade"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461580261*^9}], Cell[BoxData[ RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"x", ",", "y", ",", "z"}], "}"}], ",", - RowBox[{"{", - RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}], ",", "lambda"}], - "}"}]], "Output", - CellChangeTimes->{3.644341528720538*^9, 3.644341621951668*^9}], + RowBox[{"SkewSym", " ", "w1"}], ",", + RowBox[{"SkewSym", " ", "w2"}], ",", + RowBox[{"SkewSym", " ", "w3"}]}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461588023*^9}], + +Cell[BoxData[ + RowBox[{"sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.6443674615950117`*^9}], + +Cell[BoxData["sin"], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461601419*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461607435*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"1", "-", "sin"}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461613455*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"0.5`", "\[VeryThinSpace]", "-", + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.6443674616197567`*^9}], + +Cell[BoxData[ + FractionBox[ + SuperscriptBox["lambda", "2"], + RowBox[{ + SuperscriptBox["lambda", "2"], "+", + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.64436746162819*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{ + RowBox[{"-", "1"}], "+", "lambda", "-", + RowBox[{"exp", " ", "lambda"}]}], + SuperscriptBox["lambda", "2"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461636879*^9}], + +Cell[BoxData[ + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], "-", + RowBox[{"lambda", ".", + FractionBox[ + RowBox[{"1", "-", "sin"}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461644608*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"1", "-", "lambda", "+", + RowBox[{"exp", " ", "lambda"}], "+", + RowBox[{"0.5`", " ", + SuperscriptBox["lambda", "2"]}]}], + SuperscriptBox["lambda", "2"]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.6443674616517467`*^9}], + +Cell[BoxData[ + RowBox[{ + FractionBox[ + RowBox[{"1", "-", "sin"}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], "-", + FractionBox[ + RowBox[{"lambda", " ", + RowBox[{"(", + RowBox[{"0.5`", "\[VeryThinSpace]", "-", + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}], ")"}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461658148*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"1", "+", + RowBox[{"exp", " ", "lambda"}]}], "lambda"]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461664322*^9}], + +Cell[BoxData[ + RowBox[{"lambda", "+", + RowBox[{ + FractionBox[ + SuperscriptBox["lambda", "2"], + RowBox[{ + SuperscriptBox["lambda", "2"], "+", + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "lambda"}], "+", + FractionBox[ + RowBox[{ + RowBox[{"-", "1"}], "+", "lambda", "-", + RowBox[{"exp", " ", "lambda"}]}], + SuperscriptBox["lambda", "2"]]}], ")"}]}]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461671301*^9}], + +Cell[BoxData[ + RowBox[{"nv", "+", + RowBox[{ + FractionBox[ + SuperscriptBox["lambda", "2"], + RowBox[{ + SuperscriptBox["lambda", "2"], "+", + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", + RowBox[{"(", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", "lambda", "+", + RowBox[{"exp", " ", "lambda"}], "+", + RowBox[{"0.5`", " ", + SuperscriptBox["lambda", "2"]}]}], + SuperscriptBox["lambda", "2"]], "-", "nv"}], ")"}]}]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461677743*^9}], + +Cell[BoxData["sin"], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461684054*^9}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461690474*^9}], Cell[BoxData[ RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", - RowBox[{"-", "w3"}], ",", "w2"}], "}"}], ",", - RowBox[{"{", - RowBox[{"w3", ",", "0", ",", - RowBox[{"-", "w1"}]}], "}"}], ",", RowBox[{"{", RowBox[{ - RowBox[{"-", "w2"}], ",", "w1", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.644341528720538*^9, 3.644341621955081*^9}] -}, Open ]], - -Cell[CellGroupData[{ + RowBox[{"1", "+", + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{"1", "+", + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{ + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}], ",", + RowBox[{"1", "+", + RowBox[{"sin", ".", "v_skew"}], "+", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", + RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}]}]}], + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", + "v_skew"}]}]}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.64436746170076*^9}], Cell[BoxData[ - RowBox[{"I3", " ", "=", " ", - RowBox[{"IdentyMatrix", "[", "3", "]"}]}]], "Input", - CellChangeTimes->{{3.6443405921748047`*^9, 3.6443406096153393`*^9}}], - -Cell[BoxData[ - RowBox[{"IdentyMatrix", "[", "3", "]"}]], "Output", - CellChangeTimes->{3.64434061119941*^9}] + RowBox[{ + RowBox[{ + FractionBox[ + RowBox[{"1", "+", + RowBox[{"exp", " ", "lambda"}]}], "lambda"], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{"lambda", "+", + RowBox[{ + FractionBox[ + SuperscriptBox["lambda", "2"], + RowBox[{ + SuperscriptBox["lambda", "2"], "+", + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "lambda"}], "+", + FractionBox[ + RowBox[{ + RowBox[{"-", "1"}], "+", "lambda", "-", + RowBox[{"exp", " ", "lambda"}]}], + SuperscriptBox["lambda", "2"]]}], ")"}]}]}], ")"}], ".", "v_skew"}], + "+", + RowBox[{ + RowBox[{"(", + RowBox[{"nv", "+", + RowBox[{ + FractionBox[ + SuperscriptBox["lambda", "2"], + RowBox[{ + SuperscriptBox["lambda", "2"], "+", + RowBox[{ + SuperscriptBox["sqrt", "2"], " ", + SuperscriptBox["Transpose", "2"], " ", + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + SuperscriptBox["w1", "2"], "+", + SuperscriptBox["w2", "2"], "+", + SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", + RowBox[{"(", + RowBox[{ + FractionBox[ + RowBox[{"1", "-", "lambda", "+", + RowBox[{"exp", " ", "lambda"}], "+", + RowBox[{"0.5`", " ", + SuperscriptBox["lambda", "2"]}]}], + SuperscriptBox["lambda", "2"]], "-", "nv"}], ")"}]}]}], ")"}], ".", + "v_skew", ".", "v_skew"}]}]], "Output", + CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, + 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { + 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, + 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, + 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { + 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { + 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, + 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, + 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { + 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { + 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, + 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, + 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, + 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, + 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, + 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, + 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, + 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { + 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, + 3.644367373557184*^9, 3.644367461708905*^9}] }, Open ]] }, Open ]] }, -WindowSize->{808, 579}, -WindowMargins->{{Automatic, -111}, {Automatic, 20}}, +WindowSize->{812, 579}, +WindowMargins->{{Automatic, -114}, {Automatic, 30}}, FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (December 4, \ 2014)", StyleDefinitions->"Default.nb" @@ -380,30 +1644,49 @@ CellTagsIndex->{} (*NotebookFileOutline Notebook[{ Cell[CellGroupData[{ -Cell[580, 22, 828, 14, 287, "Title"], -Cell[1411, 38, 3548, 95, 166, "Input"], +Cell[580, 22, 808, 14, 224, "Title"], Cell[CellGroupData[{ -Cell[4984, 137, 1814, 41, 206, "Input"], -Cell[6801, 180, 703, 10, 31, "Output"], -Cell[7507, 192, 627, 10, 28, "Output"], -Cell[8137, 204, 631, 10, 28, "Output"], -Cell[8771, 216, 757, 15, 28, "Output"], -Cell[9531, 233, 702, 10, 31, "Output"], -Cell[10236, 245, 914, 18, 28, "Output"], -Cell[11153, 265, 874, 19, 28, "Output"], -Cell[12030, 286, 702, 13, 49, "Output"] +Cell[1413, 40, 3597, 96, 166, "Input"], +Cell[5013, 138, 263, 9, 60, "Output"], +Cell[5279, 149, 204, 3, 31, "Output"], +Cell[5486, 154, 411, 11, 28, "Output"], +Cell[5900, 167, 413, 11, 28, "Output"], +Cell[6316, 180, 410, 11, 28, "Output"], +Cell[6729, 193, 432, 12, 28, "Output"], +Cell[7164, 207, 432, 12, 28, "Output"], +Cell[7599, 221, 432, 12, 28, "Output"], +Cell[8034, 235, 434, 12, 28, "Output"] }, Open ]], Cell[CellGroupData[{ -Cell[12769, 304, 240, 5, 46, "Input"], -Cell[13012, 311, 157, 3, 31, "Output"], -Cell[13172, 316, 147, 3, 28, "Output"], -Cell[13322, 321, 150, 3, 28, "Output"], -Cell[13475, 326, 276, 8, 28, "Output"], -Cell[13754, 336, 393, 12, 28, "Output"] -}, Open ]], -Cell[CellGroupData[{ -Cell[14184, 353, 169, 3, 28, "Input"], -Cell[14356, 358, 107, 2, 28, "Output"] +Cell[8505, 252, 7181, 205, 645, "Input"], +Cell[15689, 459, 1608, 22, 31, "Output"], +Cell[17300, 483, 1531, 22, 28, "Output"], +Cell[18834, 507, 1534, 22, 28, "Output"], +Cell[20371, 531, 1660, 27, 28, "Output"], +Cell[22034, 560, 1819, 30, 28, "Output"], +Cell[23856, 592, 1777, 31, 28, "Output"], +Cell[25636, 625, 1531, 22, 28, "Output"], +Cell[27170, 649, 1481, 20, 28, "Output"], +Cell[28654, 671, 2184, 42, 92, "Output"], +Cell[30841, 715, 1590, 22, 31, "Output"], +Cell[32434, 739, 1624, 25, 28, "Output"], +Cell[34061, 766, 1659, 26, 35, "Output"], +Cell[35723, 794, 1478, 20, 28, "Output"], +Cell[37204, 816, 1991, 37, 59, "Output"], +Cell[39198, 855, 1788, 31, 55, "Output"], +Cell[40989, 888, 2360, 48, 79, "Output"], +Cell[43352, 938, 1853, 33, 57, "Output"], +Cell[45208, 973, 1613, 25, 49, "Output"], +Cell[46824, 1000, 2392, 50, 59, "Output"], +Cell[49219, 1052, 1664, 26, 51, "Output"], +Cell[50886, 1080, 2821, 62, 79, "Output"], +Cell[53710, 1144, 1555, 23, 48, "Output"], +Cell[55268, 1169, 2162, 43, 57, "Output"], +Cell[57433, 1214, 2186, 43, 57, "Output"], +Cell[59622, 1259, 1478, 20, 28, "Output"], +Cell[61103, 1281, 1991, 37, 59, "Output"], +Cell[63097, 1320, 8325, 217, 475, "Output"], +Cell[71425, 1539, 3524, 85, 186, "Output"] }, Open ]] }, Open ]] } From 06454048d3c6e3db66540d6bbec33366d63dcff3 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sat, 27 Jun 2015 00:49:20 -0400 Subject: [PATCH 18/22] fix: remove the output from mathematica, it seems that generates a super big temporary text file. --- doc/Mathematica/SimiliarityGroup.nb | 1384 +-------------------------- 1 file changed, 36 insertions(+), 1348 deletions(-) diff --git a/doc/Mathematica/SimiliarityGroup.nb b/doc/Mathematica/SimiliarityGroup.nb index 32bf38ffc..c4a35eb1c 100644 --- a/doc/Mathematica/SimiliarityGroup.nb +++ b/doc/Mathematica/SimiliarityGroup.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 158, 7] -NotebookDataLength[ 76961, 1687] -NotebookOptionsPosition[ 74977, 1628] -NotebookOutlinePosition[ 75336, 1644] -CellTagsIndexPosition[ 75293, 1641] +NotebookDataLength[ 13070, 375] +NotebookOptionsPosition[ 12606, 355] +NotebookOutlinePosition[ 12963, 371] +CellTagsIndexPosition[ 12920, 368] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -36,8 +36,6 @@ Cell[TextData[{ 3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}, { 3.6443655095926237`*^9, 3.644365512168627*^9}}], -Cell[CellGroupData[{ - Cell[BoxData[ RowBox[{"\[IndentingNewLine]", RowBox[{ @@ -136,119 +134,8 @@ Cell[BoxData[ 3.644341671605256*^9, 3.64434167330084*^9}, {3.644345191055595*^9, 3.644345216636923*^9}}], -Cell[BoxData[ - TagBox[ - InterpretationBox[Cell["\<\ -The exponential map of similarity3 is: -\ -\>"], - TextCell["The exponential map of similarity3 is:\n"]], - Function[BoxForm`e$, - MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{3.644345200855969*^9}], - -Cell[BoxData[ - InterpretationBox[Cell["Lie group generators for similarity transform:"], - TextCell["Lie group generators for similarity transform:"]]], "Output", - CellChangeTimes->{3.644345200860836*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.644345200865378*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.6443452008696203`*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.64434520087387*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", - RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.644345200878134*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.644345200887875*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", - RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.644345200892788*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", - RowBox[{"-", "1"}]}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{3.6443452008972893`*^9}] -}, Open ]], - -Cell[CellGroupData[{ +Cell[BoxData[""], "Input", + CellChangeTimes->{{3.644369249242889*^9, 3.644369249253479*^9}}], Cell[BoxData[{Cell["Lie vectors similarity3 can be described as:"], "\ \[IndentingNewLine]", @@ -417,8 +304,8 @@ refer to Ethan Eade"], "\[IndentingNewLine]", RowBox[{"0", ",", RowBox[{"{", RowBox[{"i", ",", "4"}], "}"}], ",", - RowBox[{"[", - RowBox[{"j", ",", "4"}], "]"}]}], "]"}]}], "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", RowBox[{ RowBox[{"simExponetialMap", "[", RowBox[{"[", @@ -439,1195 +326,35 @@ refer to Ethan Eade"], "\[IndentingNewLine]", RowBox[{"exp", RowBox[{"(", RowBox[{"-", "lambda"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"MatrixForm", "[", "simExponetialMap", - "]"}], "\[IndentingNewLine]"}], "Input", + RowBox[{ + RowBox[{"MatrixForm", "[", "simExponetialMap", "]"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"f", "[", "lambda_", "]"}], " ", "=", " ", + "simExponetialMap"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"f", "'"}], "[", "lambda", "]"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->CompressedData[" -1:eJwlz2tIk2EYgOGVsbU2x7BZRi20jNFmZOHyRwuMQKwNMzv8SBwrUSuomaMo -S5GwSQwSyqxAR63Tx9ZJp2hEBzIxhMwlkVGsbQwthLKZS8Kw3Z8/Xi4eeHhu -3oyD9uLy+RKJxJB4mBzKa5LpxrfW2Gpu44q+Mh8K6qHnaG2/048TmvBb/Hjr -dQBHVZc/zHkoiIWC9hu2n6keXZjQW1f6HV3yqR+YL62O45PQ079YEL42gz17 -a6VyOoPZSzGqbVyOQpVnDTaU79OjaePK9fgr9tWIA0r7JmztdpThI5m/Eveb -dMfxSl22aMf2+AmcPR9twh7952b0BzKv40lHkRvVlZYb+P701Xti3zvgE7vu -tV2o/F3Wh1lTjiiqapyinsgBxSJ69QYlqt0vHqQkfPmqpBsj+i39aFo3FsKK -im1jKHSVjuPAv4IJ9BqFGGbcPBvH5JnCeYsTDrYGpbijzScX57tJaWhtiIku -mU7V4qXeonTs2LPbMLcXyEdLc44TdbJII2bO1rtRs9niQY/1sE+8Y2sRXeZQ -d2LY9XgYbU7HCPp3Kb5g+v3cSUxLqhMVOo9OY1VLcIGG/7W5FLjqnVmFqz29 -Kfjp1DMDqkpyNmCWvdaI58zFJtRf+JOHQ8dSzajxH9mJD98M/8SLI7mT+B8u -LiVI - "], - EmphasizeSyntaxErrors->True], +1:eJwl0l1Ik3EUBvCZsrU2x7CtlFpoGaN3iiauLlpgBGJtqFntInGshlqBTX2x +bOUQsUnsQiizBB312sfLZvkxRSM0yYkxyJwSFtXaxvADIW3pkqHYnr8Xhx8H +DufhwEm5Yiwq3cHhcNKjBeN9Oc08+dIpk970HO4fNzggK54agbreFxNwReL/ +BGc7XR44J3r4ZdurXpjPyhZg753quZ1R7eaSRWjlr/2GudzqMHzrexeBef4n +G3DoYh2Xj5zJzL0wKGvaB9lK5jBsLNVSUJV1IAP+Cf1SQrfQeAy2D9IG2M1z +lsNLKnkVfGTOJPadCdfArXvBZjhEfW+BTk9qG7xJF9qguFzzFE7ffvyK5Nvd +DpJrOzIAhauGcZi2RgehyGQhMoHLgl3Iq1cIodj2/nVC1NEPxYMwQJ2cgKr0 +eR8sKzs9D9mBkiXo3sxbgXYlG4Ipz+6GYfxGfszuqJPtXi482+Hgk/5lbCLU +NYaIe9alMvhgrDAZ9l04r9ie8+RCTUu2Bcp5gSaYulVvg5ITGgYyumsOskff +Skyixf3Qb+2ZgXoL/RU6zwl+wOSu439hYqyZyPZXrMPKVm+cBPd1WAXw4Ge1 +CB5ixhLgt9phBRQVZx+FacY6JWxQF6kgdf9fDpy6IVVDifN6AXzzcWYZal3U +KhyNKCOwVttAdP2c5kjxBxW34uB/2UIzug== + "]], -Cell[BoxData[ - InterpretationBox[Cell["Lie vectors similarity3 can be described as:"], - TextCell["Lie vectors similarity3 can be described as:"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.6443674615039167`*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{"x", ",", "y", ",", "z"}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461512148*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461520136*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"x", ",", "y", ",", "z"}], "}"}], ",", - RowBox[{"{", - RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}], ",", "lambda"}], - "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461527494*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.6443674615362377`*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", - RowBox[{"-", "w3"}], ",", "w2"}], "}"}], ",", - RowBox[{"{", - RowBox[{"w3", ",", "0", ",", - RowBox[{"-", "w1"}]}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{"-", "w2"}], ",", "w1", ",", "0"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461544832*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{"x", ",", "y", ",", "z"}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461553432*^9}], - -Cell[BoxData["lambda"], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461560822*^9}], - -Cell[BoxData[ - TagBox[ - RowBox[{"(", "\[NoBreak]", GridBox[{ - {"0", - RowBox[{"-", "w3"}], "w2", "x"}, - {"w3", "0", - RowBox[{"-", "w1"}], "y"}, - { - RowBox[{"-", "w2"}], "w1", "0", "z"}, - {"0", "0", "0", "lambda"} - }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, - GridBoxSpacings->{"Columns" -> { - Offset[0.27999999999999997`], { - Offset[0.7]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { - Offset[0.2], { - Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], - Function[BoxForm`e$, - MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461569426*^9}], - -Cell[BoxData[ - InterpretationBox[Cell["Exponential map, refer to Ethan Eade"], - TextCell["Exponential map, refer to Ethan Eade"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461580261*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"SkewSym", " ", "w1"}], ",", - RowBox[{"SkewSym", " ", "w2"}], ",", - RowBox[{"SkewSym", " ", "w3"}]}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461588023*^9}], - -Cell[BoxData[ - RowBox[{"sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.6443674615950117`*^9}], - -Cell[BoxData["sin"], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461601419*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461607435*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"1", "-", "sin"}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461613455*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"0.5`", "\[VeryThinSpace]", "-", - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.6443674616197567`*^9}], - -Cell[BoxData[ - FractionBox[ - SuperscriptBox["lambda", "2"], - RowBox[{ - SuperscriptBox["lambda", "2"], "+", - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.64436746162819*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{ - RowBox[{"-", "1"}], "+", "lambda", "-", - RowBox[{"exp", " ", "lambda"}]}], - SuperscriptBox["lambda", "2"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461636879*^9}], - -Cell[BoxData[ - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], "-", - RowBox[{"lambda", ".", - FractionBox[ - RowBox[{"1", "-", "sin"}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461644608*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"1", "-", "lambda", "+", - RowBox[{"exp", " ", "lambda"}], "+", - RowBox[{"0.5`", " ", - SuperscriptBox["lambda", "2"]}]}], - SuperscriptBox["lambda", "2"]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.6443674616517467`*^9}], - -Cell[BoxData[ - RowBox[{ - FractionBox[ - RowBox[{"1", "-", "sin"}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], "-", - FractionBox[ - RowBox[{"lambda", " ", - RowBox[{"(", - RowBox[{"0.5`", "\[VeryThinSpace]", "-", - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}], ")"}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461658148*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"1", "+", - RowBox[{"exp", " ", "lambda"}]}], "lambda"]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461664322*^9}], - -Cell[BoxData[ - RowBox[{"lambda", "+", - RowBox[{ - FractionBox[ - SuperscriptBox["lambda", "2"], - RowBox[{ - SuperscriptBox["lambda", "2"], "+", - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "lambda"}], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", "1"}], "+", "lambda", "-", - RowBox[{"exp", " ", "lambda"}]}], - SuperscriptBox["lambda", "2"]]}], ")"}]}]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461671301*^9}], - -Cell[BoxData[ - RowBox[{"nv", "+", - RowBox[{ - FractionBox[ - SuperscriptBox["lambda", "2"], - RowBox[{ - SuperscriptBox["lambda", "2"], "+", - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", - RowBox[{"(", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", "lambda", "+", - RowBox[{"exp", " ", "lambda"}], "+", - RowBox[{"0.5`", " ", - SuperscriptBox["lambda", "2"]}]}], - SuperscriptBox["lambda", "2"]], "-", "nv"}], ")"}]}]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461677743*^9}], - -Cell[BoxData["sin"], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461684054*^9}], - -Cell[BoxData[ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461690474*^9}], - -Cell[BoxData[ - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{ - RowBox[{"1", "+", - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}]}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{"1", "+", - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}]}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{ - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}], ",", - RowBox[{"1", "+", - RowBox[{"sin", ".", "v_skew"}], "+", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", - RowBox[{"cos", " ", "sqrt", " ", "Transpose", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}]}]}], - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]], ".", "v_skew", ".", - "v_skew"}]}]}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.64436746170076*^9}], - -Cell[BoxData[ - RowBox[{ - RowBox[{ - FractionBox[ - RowBox[{"1", "+", - RowBox[{"exp", " ", "lambda"}]}], "lambda"], ".", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{"lambda", "+", - RowBox[{ - FractionBox[ - SuperscriptBox["lambda", "2"], - RowBox[{ - SuperscriptBox["lambda", "2"], "+", - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "lambda"}], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", "1"}], "+", "lambda", "-", - RowBox[{"exp", " ", "lambda"}]}], - SuperscriptBox["lambda", "2"]]}], ")"}]}]}], ")"}], ".", "v_skew"}], - "+", - RowBox[{ - RowBox[{"(", - RowBox[{"nv", "+", - RowBox[{ - FractionBox[ - SuperscriptBox["lambda", "2"], - RowBox[{ - SuperscriptBox["lambda", "2"], "+", - RowBox[{ - SuperscriptBox["sqrt", "2"], " ", - SuperscriptBox["Transpose", "2"], " ", - SuperscriptBox[ - RowBox[{"(", - RowBox[{ - SuperscriptBox["w1", "2"], "+", - SuperscriptBox["w2", "2"], "+", - SuperscriptBox["w3", "2"]}], ")"}], "2"]}]}]], ".", - RowBox[{"(", - RowBox[{ - FractionBox[ - RowBox[{"1", "-", "lambda", "+", - RowBox[{"exp", " ", "lambda"}], "+", - RowBox[{"0.5`", " ", - SuperscriptBox["lambda", "2"]}]}], - SuperscriptBox["lambda", "2"]], "-", "nv"}], ")"}]}]}], ")"}], ".", - "v_skew", ".", "v_skew"}]}]], "Output", - CellChangeTimes->{{3.6443419121329803`*^9, 3.644341920689967*^9}, - 3.644342083088463*^9, {3.644344124265246*^9, 3.6443441324042997`*^9}, { - 3.6443441881168823`*^9, 3.6443441950748997`*^9}, {3.644344229943321*^9, - 3.644344245111559*^9}, {3.644344284483355*^9, 3.6443442933267717`*^9}, - 3.6443444220909557`*^9, {3.6443444591254807`*^9, 3.6443445061756277`*^9}, { - 3.6443445415799227`*^9, 3.64434458046883*^9}, 3.644344622442521*^9, { - 3.644344676089139*^9, 3.644344697634576*^9}, {3.644345119918116*^9, - 3.644345130310161*^9}, 3.644345228402564*^9, {3.644345294239305*^9, - 3.6443453137058153`*^9}, {3.64434535496251*^9, 3.644345397456712*^9}, { - 3.644345453674008*^9, 3.6443454741593657`*^9}, 3.644345683963255*^9, { - 3.644345784991262*^9, 3.6443458090169373`*^9}, {3.6443458393601837`*^9, - 3.644345852880629*^9}, 3.644345900196662*^9, 3.644346148937639*^9, - 3.644346465932077*^9, {3.644364351433588*^9, 3.644364361690076*^9}, - 3.644364444175256*^9, {3.644366154194023*^9, 3.6443661742894583`*^9}, - 3.644366333614024*^9, 3.644366562449389*^9, 3.644366750790797*^9, - 3.6443667923118773`*^9, {3.6443668797865257`*^9, 3.644366888321496*^9}, - 3.6443669653333273`*^9, 3.644367012572801*^9, {3.6443670756055727`*^9, - 3.6443671022081423`*^9}, 3.6443671488140087`*^9, 3.644367201888232*^9, { - 3.644367248576871*^9, 3.644367277024843*^9}, 3.644367338911738*^9, - 3.644367373557184*^9, 3.644367461708905*^9}] -}, Open ]] +Cell[BoxData[""], "Input", + CellChangeTimes->{{3.644369188660811*^9, 3.644369188678254*^9}}] }, Open ]] }, WindowSize->{812, 579}, -WindowMargins->{{Automatic, -114}, {Automatic, 30}}, +WindowMargins->{{Automatic, 17}, {Automatic, 31}}, FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (December 4, \ 2014)", StyleDefinitions->"Default.nb" @@ -1645,49 +372,10 @@ CellTagsIndex->{} Notebook[{ Cell[CellGroupData[{ Cell[580, 22, 808, 14, 224, "Title"], -Cell[CellGroupData[{ -Cell[1413, 40, 3597, 96, 166, "Input"], -Cell[5013, 138, 263, 9, 60, "Output"], -Cell[5279, 149, 204, 3, 31, "Output"], -Cell[5486, 154, 411, 11, 28, "Output"], -Cell[5900, 167, 413, 11, 28, "Output"], -Cell[6316, 180, 410, 11, 28, "Output"], -Cell[6729, 193, 432, 12, 28, "Output"], -Cell[7164, 207, 432, 12, 28, "Output"], -Cell[7599, 221, 432, 12, 28, "Output"], -Cell[8034, 235, 434, 12, 28, "Output"] -}, Open ]], -Cell[CellGroupData[{ -Cell[8505, 252, 7181, 205, 645, "Input"], -Cell[15689, 459, 1608, 22, 31, "Output"], -Cell[17300, 483, 1531, 22, 28, "Output"], -Cell[18834, 507, 1534, 22, 28, "Output"], -Cell[20371, 531, 1660, 27, 28, "Output"], -Cell[22034, 560, 1819, 30, 28, "Output"], -Cell[23856, 592, 1777, 31, 28, "Output"], -Cell[25636, 625, 1531, 22, 28, "Output"], -Cell[27170, 649, 1481, 20, 28, "Output"], -Cell[28654, 671, 2184, 42, 92, "Output"], -Cell[30841, 715, 1590, 22, 31, "Output"], -Cell[32434, 739, 1624, 25, 28, "Output"], -Cell[34061, 766, 1659, 26, 35, "Output"], -Cell[35723, 794, 1478, 20, 28, "Output"], -Cell[37204, 816, 1991, 37, 59, "Output"], -Cell[39198, 855, 1788, 31, 55, "Output"], -Cell[40989, 888, 2360, 48, 79, "Output"], -Cell[43352, 938, 1853, 33, 57, "Output"], -Cell[45208, 973, 1613, 25, 49, "Output"], -Cell[46824, 1000, 2392, 50, 59, "Output"], -Cell[49219, 1052, 1664, 26, 51, "Output"], -Cell[50886, 1080, 2821, 62, 79, "Output"], -Cell[53710, 1144, 1555, 23, 48, "Output"], -Cell[55268, 1169, 2162, 43, 57, "Output"], -Cell[57433, 1214, 2186, 43, 57, "Output"], -Cell[59622, 1259, 1478, 20, 28, "Output"], -Cell[61103, 1281, 1991, 37, 59, "Output"], -Cell[63097, 1320, 8325, 217, 475, "Output"], -Cell[71425, 1539, 3524, 85, 186, "Output"] -}, Open ]] +Cell[1391, 38, 3597, 96, 166, "Input"], +Cell[4991, 136, 92, 1, 28, InheritFromParent], +Cell[5086, 139, 7409, 210, 696, "Input"], +Cell[12498, 351, 92, 1, 28, InheritFromParent] }, Open ]] } ] From 95f4d14d5e1073c9f28642d6703543c14bdc464f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 13:00:49 -0800 Subject: [PATCH 19/22] Fixed static Rot3 methods --- .../geometry/tests/testSimilarity3.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 955d09e89..84aa14420 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -40,11 +40,11 @@ using symbol_shorthand::X; GTSAM_CONCEPT_TESTABLE_INST(Similarity3) static Point3 P(0.2, 0.7, -2); -static Rot3 R = Rot3::rodriguez(0.3, 0, 0); +static Rot3 R = Rot3::Rodrigues(0.3, 0, 0); static double s = 4; static Similarity3 T_default(R, Point3(3.5, -8.2, 4.2), 1); -static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); -static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 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 Similarity3 T4(R, P, s); static Similarity3 T5(R, P, 10); static Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform @@ -71,15 +71,15 @@ TEST(Similarity3, Getters) { 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())); + 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, AdjointMap) { - Similarity3 test(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + 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, @@ -93,7 +93,7 @@ TEST(Similarity3, AdjointMap) { //****************************************************************************** TEST(Similarity3, inverse) { - Similarity3 sim3(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + 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); @@ -109,8 +109,8 @@ TEST(Similarity3, inverse) { //****************************************************************************** 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); + 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; Vector3 te(-13.6797, 3.2441, -5.7794); @@ -137,14 +137,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.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::rodriguez(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); @@ -284,7 +284,7 @@ TEST(Similarity3, GroupAction) { // 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); + 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); PriorFactor factor(key, prior, model); @@ -311,13 +311,13 @@ TEST(Similarity3, Optimization) { // 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), + 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), + 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), + 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), + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(6 * 0.7, 0, 0), 1.0); Similarity3 loop = Similarity3(1.42); @@ -348,13 +348,13 @@ TEST(Similarity3, Optimization2) { 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)); + 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)); + 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)); + 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)); + Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); //initial.print("Initial Estimate\n"); From d7ed19dc2123bee1e28e4c42e24a364e462ec887 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 14:05:59 -0800 Subject: [PATCH 20/22] Fixed equality, transform_from, AdjointMap, and added wedge (for testing AdjointMap) --- gtsam_unstable/geometry/Similarity3.cpp | 79 ++++++++++--------- gtsam_unstable/geometry/Similarity3.h | 18 ++++- .../geometry/tests/testSimilarity3.cpp | 78 +++++++----------- 3 files changed, 86 insertions(+), 89 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index f0f673bca..dd4ca3a31 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -38,13 +38,13 @@ Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : R_(R), t_(t), s_(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); +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 equals(other, 1e-9); + return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_; } void Similarity3::print(const std::string& s) const { @@ -70,40 +70,47 @@ Similarity3 Similarity3::inverse() const { 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()); - *H1 << DR, R, R * p.vector(); - print("From Derivative"); + // 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 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 + 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 { -// ToDo: This adjoint might not be correct, it is based on delta = [u, w, lambda] -// However, we use the convention delta = [w, u, lambda] + // http://www.ethaneade.org/latex2html/lie/node30.html const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; - adj << s_ * R, A, -s_ * t, // 3*7 - Z_3x3, R, Matrix31::Zero(), // 3*7 - Matrix16::Zero(), 1; // 1*7 + adj << + R, Z_3x3, Matrix31::Zero(), // 3*7 + A, s_ * R, -s_ * t, // 3*7 + Matrix16::Zero(), 1; // 1*7 return adj; } -Matrix33 Similarity3::GetV(Vector3 w, double lambda){ - Matrix33 wx = skewSymmetric(w[0], w[1], w[2]); +Matrix3 Similarity3::GetV(Vector3 w, double lambda) { + Matrix3 wx = skewSymmetric(w[0], w[1], w[2]); double lambdasquared = lambda * lambda; double thetasquared = w.transpose() * w; double theta = sqrt(thetasquared); @@ -122,13 +129,12 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){ A = (1 - exp(-lambda)) / lambda; B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; - } - else if(thetasquared <= 1e-9 && lambdasquared > 1e-9) { + } else if (thetasquared <= 1e-9 && lambdasquared > 1e-9) { //Taylor series expansions X = 1; - Y = 0.5-thetasquared/24.0; - Z = 1.0/6.0 - thetasquared/120.0; - W = 1.0/24.0 - thetasquared/720.0; + Y = 0.5 - thetasquared / 24.0; + Z = 1.0 / 6.0 - thetasquared / 120.0; + W = 1.0 / 24.0 - thetasquared / 720.0; alpha = lambdasquared / (lambdasquared + thetasquared); beta = (exp(-lambda) - 1 + lambda) / lambdasquared; gama = Y - (lambda * Z); @@ -138,8 +144,7 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){ A = (1 - exp(-lambda)) / lambda; B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; - } - else if(thetasquared > 1e-9 && lambdasquared <= 1e-9) { + } else if (thetasquared > 1e-9 && lambdasquared <= 1e-9) { X = sin(theta) / theta; Y = (1 - cos(theta)) / thetasquared; Z = (1 - X) / thetasquared; @@ -158,10 +163,9 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){ } B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; - } - else { + } else { X = 1; - Y = 0.5-thetasquared/24.0; + Y = 0.5 - thetasquared / 24.0; Z = 1.0 / 6.0 - thetasquared / 120.0; W = 1.0 / 24.0 - thetasquared / 720.0; alpha = lambdasquared / (lambdasquared + thetasquared); @@ -179,7 +183,7 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){ B = gama; C = upsilon; } - return A * Matrix33::Identity() + B * wx + C * wx * wx; + return A * I_3x3 + B * wx + C * wx * wx; } Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { @@ -196,26 +200,27 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { - Vector3 w(v.head<3>()); + const auto w = v.head<3>(); + const auto u = v.segment<3>(3); double lambda = v[6]; if (Hm) { - Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); + // Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); // incomplete } - return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); + 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; } const Matrix4 Similarity3::matrix() const { Matrix4 T; - T.topRows<3>() << s_ * R_.matrix(), t_.vector(); - T.bottomRows<1>() << 0, 0, 0, 1; + T.topRows<3>() << R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1.0/s_; return T; } diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index c380c4aa7..92082ee02 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -67,7 +67,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 @@ -92,6 +92,7 @@ public: /// @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; @@ -124,11 +125,19 @@ public: } }; + 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; - using LieGroup::inverse; - /// @} /// @name Standard interface /// @{ @@ -152,6 +161,7 @@ public: } /// 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 @@ -170,7 +180,7 @@ public: /// Calculate expmap and logmap coefficients. private: - static Matrix33 GetV(Vector3 w, double lambda); + static Matrix3 GetV(Vector3 w, double lambda); /// @} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 84aa14420..b8db48942 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -42,7 +42,7 @@ GTSAM_CONCEPT_TESTABLE_INST(Similarity3) static Point3 P(0.2, 0.7, -2); static Rot3 R = Rot3::Rodrigues(0.3, 0, 0); static double s = 4; -static Similarity3 T_default(R, Point3(3.5, -8.2, 4.2), 1); +static Similarity3 T1(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 Similarity3 T4(R, P, s); @@ -79,16 +79,13 @@ TEST(Similarity3, Getters) { //****************************************************************************** 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)); } //****************************************************************************** @@ -169,13 +166,13 @@ TEST( Similarity3, retract_first_order) { TEST(Similarity3, localCoordinates_first_order) { Vector d12 = repeat(7, 0.1); d12(6) = 1.0; - Similarity3 t1 = T_default, 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_default; + Similarity3 t1 = T1; Similarity3 t2 = T3; Similarity3 origin; Vector d12 = t1.localCoordinates(t2); @@ -188,10 +185,11 @@ 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 << + 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)); } @@ -226,55 +224,39 @@ TEST(Similarity3, ExpLogMap) { //****************************************************************************** // Group action on Point3 (with simpler transform) TEST(Similarity3, GroupAction) { - EXPECT(assert_equal(Point3(1, 1, 0), T6 * Point3(0, 0, 0))); + 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 actual group action on R^4 + // Test group action on R^4 via matrix representation Vector4 qh; qh << 1, 0, 0, 1; Vector4 ph; - ph << 3, 1, 0, 1; + 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); - - Point3 pa = Point3(1, 0, 0); - Point3 pTa = Point3(2, 2, 3); - Point3 pTb = Point3(3, 2, 3); - - EXPECT(assert_equal(pTa, Ta.transform_from(pa))); - EXPECT(assert_equal(pTb, Tb.transform_from(pa))); + 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); - - Point3 pTc = Point3(1, 3, 3); - Point3 pTd = Point3(1, 4, 3); - - EXPECT(assert_equal(pTc, Tc.transform_from(pa))); - EXPECT(assert_equal(pTd, Td.transform_from(pa))); + 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); - { // T default + Point3 q(1, 2, 3); + for (const auto T : { T1, T2, T3, T4, T5, T6 }) { Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T_default, q); - Matrix H2 = numericalDerivative22(f, T_default, q); + Matrix H1 = numericalDerivative21(f, T1, q); + Matrix H2 = numericalDerivative22(f, T1, q); Matrix actualH1, actualH2; - T_default.transform_from(q, actualH1, actualH2); - EXPECT(assert_equal(H1, actualH1)); - EXPECT(assert_equal(H2, actualH2)); - } - { // T4 - Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T6, q); - Matrix H2 = numericalDerivative22(f, T6, q); - Matrix actualH1, actualH2; - Point3 p = T6.transform_from(q, actualH1, actualH2); - EXPECT(assert_equal(Point3(3, 1, 0), p)); - EXPECT(assert_equal(Point3(3, 1, 0), T6 * q)); + T1.transform_from(q, actualH1, actualH2); EXPECT(assert_equal(H1, actualH1)); EXPECT(assert_equal(H2, actualH2)); } From f64d17f4f07213899f3399d176f0bf90075dcb53 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 15:02:07 -0800 Subject: [PATCH 21/22] Cleaned up and tested exmpap --- gtsam_unstable/geometry/Similarity3.cpp | 132 +++++++++--------- gtsam_unstable/geometry/Similarity3.h | 25 ++-- .../geometry/tests/testSimilarity3.cpp | 63 ++++++--- 3 files changed, 122 insertions(+), 98 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index dd4ca3a31..e93a7953f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -38,6 +38,10 @@ Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : R_(R), t_(t), s_(s) { } +Similarity3::Similarity3(const Matrix4& T) : + R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) { +} + 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); @@ -100,7 +104,7 @@ Matrix7 Similarity3::AdjointMap() const { // http://www.ethaneade.org/latex2html/lie/node30.html const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); - Matrix3 A = s_ * skewSymmetric(t) * R; + const Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << R, Z_3x3, Matrix31::Zero(), // 3*7 @@ -110,91 +114,86 @@ Matrix7 Similarity3::AdjointMap() const { } Matrix3 Similarity3::GetV(Vector3 w, double lambda) { - Matrix3 wx = skewSymmetric(w[0], w[1], w[2]); - double lambdasquared = lambda * lambda; - double thetasquared = w.transpose() * w; - double theta = sqrt(thetasquared); - double X, Y, Z, W, alpha, beta, gama, mu, upsilon, A, B, C; - if (thetasquared > 1e-9 && lambdasquared > 1e-9) { - X = sin(theta) / theta; - Y = (1 - cos(theta)) / thetasquared; - Z = (1 - X) / thetasquared; - W = (0.5 - Y) / thetasquared; - alpha = lambdasquared / (lambdasquared + thetasquared); - beta = (exp(-lambda) - 1 + lambda) / lambdasquared; - gama = Y - (lambda * Z); - mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) - / (lambdasquared * lambda); - upsilon = Z - (lambda * W); + // 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 - gama) + gama; + B = alpha * (beta - gamma) + gamma; C = alpha * (mu - upsilon) + upsilon; - } else if (thetasquared <= 1e-9 && lambdasquared > 1e-9) { + } else if (theta2 <= 1e-9 && lambda2 > 1e-9) { //Taylor series expansions - X = 1; - Y = 0.5 - thetasquared / 24.0; - Z = 1.0 / 6.0 - thetasquared / 120.0; - W = 1.0 / 24.0 - thetasquared / 720.0; - alpha = lambdasquared / (lambdasquared + thetasquared); - beta = (exp(-lambda) - 1 + lambda) / lambdasquared; - gama = Y - (lambda * Z); - mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) - / (lambdasquared * lambda); - upsilon = Z - (lambda * W); + 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 - gama) + gama; + B = alpha * (beta - gamma) + gamma; C = alpha * (mu - upsilon) + upsilon; - } else if (thetasquared > 1e-9 && lambdasquared <= 1e-9) { - X = sin(theta) / theta; - Y = (1 - cos(theta)) / thetasquared; - Z = (1 - X) / thetasquared; - W = (0.5 - Y) / thetasquared; - alpha = lambdasquared / (lambdasquared + thetasquared); - beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 - - (lambda * lambdasquared) / 120; - gama = Y - (lambda * Z); - mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 - - (lambda * lambdasquared) / 720; - upsilon = Z - (lambda * W); + } 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 + lambdasquared / 6.0; + A = 1 - lambda / 2.0 + lambda2 / 6.0; } else { A = (1 - exp(-lambda)) / lambda; } - B = alpha * (beta - gama) + gama; + B = alpha * (beta - gamma) + gamma; C = alpha * (mu - upsilon) + upsilon; } else { - X = 1; - Y = 0.5 - thetasquared / 24.0; - Z = 1.0 / 6.0 - thetasquared / 120.0; - W = 1.0 / 24.0 - thetasquared / 720.0; - alpha = lambdasquared / (lambdasquared + thetasquared); - beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 - - (lambda * lambdasquared) / 120; - gama = Y - (lambda * Z); - mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 - - (lambda * lambdasquared) / 720; - upsilon = Z - (lambda * W); + 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 + lambdasquared / 6.0; + A = 1 - lambda / 2.0 + lambda2 / 6.0; } else { A = (1 - exp(-lambda)) / lambda; } - B = gama; + B = gamma; C = upsilon; } - return A * I_3x3 + B * wx + C * wx * wx; + const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]); + return A * I_3x3 + B * Wx + C * Wx * Wx; } -Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { - // To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org +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 - Vector3 w = Rot3::Logmap(s.R_); - double lambda = log(s.s_); + const Vector3 w = Rot3::Logmap(T.R_); + const double lambda = log(T.s_); Vector7 result; - result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; + result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; if (Hm) { - // incomplete + throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); } return result; } @@ -202,10 +201,9 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { const auto w = v.head<3>(); const auto u = v.segment<3>(3); - double lambda = v[6]; + const double lambda = v[6]; if (Hm) { - // Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); - // incomplete + 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)); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 92082ee02..853261fc2 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -60,6 +60,9 @@ public: /// 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 /// @{ @@ -118,10 +121,10 @@ public: /// Chart at the origin struct ChartAtOrigin { static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { - return Similarity3::Expmap(v); + return Similarity3::Expmap(v, H); } static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { - return Similarity3::Logmap(other); + return Similarity3::Logmap(other, H); } }; @@ -183,15 +186,17 @@ private: static Matrix3 GetV(Vector3 w, double lambda); /// @} - }; template<> -struct traits : public internal::LieGroup { -}; - -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 b8db48942..111d53d59 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -39,15 +40,16 @@ 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 double s = 4; -static Similarity3 T1(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 Similarity3 T4(R, P, s); -static Similarity3 T5(R, P, 10); -static Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform +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) { @@ -197,10 +199,10 @@ TEST(Similarity3, Matrix) { //***************************************************************************** // Exponential and log maps TEST(Similarity3, ExpLogMap) { - Vector7 expected; - expected << 0.1,0.2,0.3,0.4,0.5,0.6,0.7; - Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(expected)); - EXPECT(assert_equal(expected, actual)); + 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; @@ -211,14 +213,8 @@ TEST(Similarity3, ExpLogMap) { Similarity3 ident = Similarity3::identity(); EXPECT(assert_equal(expZero, ident)); - // Compare to matrix exponential calculated using matlab expm - Rot3 Rexpm(0.9358, -0.2832, 0.2102, - 0.3029, 0.9506, -0.0680, - -0.1805, 0.1273, 0.9753); - Point3 texpm(0.2724, 0.3825, 0.4213); - Similarity3 Sexpm(Rexpm, texpm, 2.0138); - Similarity3 Scalc = Similarity3::Expmap(expected); - EXPECT(assert_equal(Sexpm, Scalc, 1e-3)); + // Compare to matrix exponential, using expm in Lie.h + EXPECT(assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); } //****************************************************************************** @@ -387,6 +383,31 @@ TEST(Similarity3, AlignScaledPointClouds) { // 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; From 39024aab1638788cbcc5e2a69fdbfc92f86d9dc6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 19:33:50 -0800 Subject: [PATCH 22/22] Removed obsolete document. --- doc/Mathematica/SimiliarityGroup.nb | 384 ---------------------------- 1 file changed, 384 deletions(-) delete mode 100644 doc/Mathematica/SimiliarityGroup.nb diff --git a/doc/Mathematica/SimiliarityGroup.nb b/doc/Mathematica/SimiliarityGroup.nb deleted file mode 100644 index c4a35eb1c..000000000 --- a/doc/Mathematica/SimiliarityGroup.nb +++ /dev/null @@ -1,384 +0,0 @@ -(* Content-type: application/vnd.wolfram.mathematica *) - -(*** Wolfram Notebook File ***) -(* http://www.wolfram.com/nb *) - -(* CreatedBy='Mathematica 10.0' *) - -(*CacheID: 234*) -(* Internal cache information: -NotebookFileLineBreakTest -NotebookFileLineBreakTest -NotebookDataPosition[ 158, 7] -NotebookDataLength[ 13070, 375] -NotebookOptionsPosition[ 12606, 355] -NotebookOutlinePosition[ 12963, 371] -CellTagsIndexPosition[ 12920, 368] -WindowFrame->Normal*) - -(* Beginning of Notebook Content *) -Notebook[{ - -Cell[CellGroupData[{ -Cell[TextData[{ - "Similarity Group\n", - StyleBox["Representation", "Chapter"], - "\n", - StyleBox["Exponential and Logmap\nRetract and localCoordinate", "Chapter"] -}], "Title", - CellChangeTimes->{{3.6442457705813923`*^9, 3.644245851964954*^9}, { - 3.644245883900199*^9, 3.644245897451631*^9}, {3.644246409411936*^9, - 3.6442464411218433`*^9}, {3.644246632965823*^9, 3.644246721355283*^9}, - 3.644339337635804*^9, {3.6443397539003696`*^9, 3.644339805690949*^9}, { - 3.6443398507824793`*^9, 3.644339940436355*^9}, {3.644339982058689*^9, - 3.64434014492003*^9}, {3.644340183085711*^9, 3.64434030003795*^9}, { - 3.644340442268632*^9, 3.644340457502075*^9}, {3.644340502643257*^9, - 3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}, { - 3.6443655095926237`*^9, 3.644365512168627*^9}}], - -Cell[BoxData[ - RowBox[{"\[IndentingNewLine]", - RowBox[{ - Cell["Lie group generators for similarity transform:"], - "\[IndentingNewLine]", - RowBox[{"G1", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G2", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G3", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G4", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", - RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G5", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{ - RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G6", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", - RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", - RowBox[{"G7", " ", "=", " ", - RowBox[{"{", - RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0", ",", - RowBox[{"-", "1"}]}], "}"}]}], "}"}]}]}]}]], "Input", - CellChangeTimes->{{3.644340621916498*^9, 3.644340623238144*^9}, { - 3.64434081043055*^9, 3.644340996367342*^9}, {3.6443410615657*^9, - 3.644341237564106*^9}, {3.644341297291617*^9, 3.6443413676185513`*^9}, { - 3.644341671605256*^9, 3.64434167330084*^9}, {3.644345191055595*^9, - 3.644345216636923*^9}}], - -Cell[BoxData[""], "Input", - CellChangeTimes->{{3.644369249242889*^9, 3.644369249253479*^9}}], - -Cell[BoxData[{Cell["Lie vectors similarity3 can be described as:"], "\ -\[IndentingNewLine]", - RowBox[{"u", " ", "=", " ", - RowBox[{"{", - RowBox[{"x", ",", "y", ",", "z"}], "}"}]}], "\[IndentingNewLine]", - RowBox[{"v", " ", "=", " ", - RowBox[{"{", - RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simLieVector", " ", "=", " ", - RowBox[{"{", - RowBox[{"u", ",", "v", ",", "lambda"}], "}"}]}], - "\[IndentingNewLine]"}], "\[IndentingNewLine]", - RowBox[{"simMatrix", " ", "=", " ", - RowBox[{"Table", "[", - RowBox[{"0", ",", - RowBox[{"{", - RowBox[{"i", ",", "4"}], "}"}], ",", - RowBox[{"{", - RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simMatrix", "[", - RowBox[{"[", - RowBox[{ - RowBox[{"1", ";;", "3"}], ",", - RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", - RowBox[{"SkewSym", "[", "v", "]"}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simMatrix", "[", - RowBox[{"[", - RowBox[{ - RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", - "u"}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simMatrix", "[", - RowBox[{"[", - RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", - "lambda"}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"MatrixForm", "[", "simMatrix", "]"}], "\[IndentingNewLine]", - RowBox[{"(*", - RowBox[{"simExponetialMap", " ", "=", " ", - RowBox[{"Series", "[", - RowBox[{"simMatrix", ",", - RowBox[{"{", - RowBox[{"u", ",", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", "2"}], "}"}]}], - "]"}]}], "*)"}], - "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["Exponential map, \ -refer to Ethan Eade"], "\[IndentingNewLine]", - RowBox[{"v_skew", " ", "=", " ", - RowBox[{"SkewSym", - RowBox[{"(", "v", ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"theta", " ", "=", - RowBox[{"sqrt", - RowBox[{"(", - RowBox[{"Transpose", - RowBox[{ - RowBox[{"(", "v", ")"}], ".", "v"}]}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"sin", - RowBox[{ - RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", - RowBox[{"Y", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"cos", - RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"Z", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"1", "-", "X"}], ")"}], "/", - RowBox[{"(", - RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"W", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"0.5", " ", "-", " ", "Y"}], ")"}], "/", - RowBox[{"(", - RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"alpha", " ", "=", " ", - RowBox[{ - RowBox[{"lambda", "^", "2"}], " ", "/", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"lambda", "^", "2"}], " ", "+", " ", - RowBox[{"theta", "^", "2"}]}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"beta", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"exp", - RowBox[{"(", - RowBox[{"-", "lambda"}], ")"}]}], "-", "1", "+", "lambda"}], ")"}], - "/", - RowBox[{"(", - RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"gamma", " ", "=", " ", - RowBox[{"Y", " ", "-", " ", - RowBox[{"lambda", ".", "Z"}]}]}], "\[IndentingNewLine]", - RowBox[{"mu", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"1", "-", "lambda", " ", "+", " ", - RowBox[{"0.5", "*", - RowBox[{"lambda", "^", "2"}]}], " ", "-", " ", - RowBox[{"exp", - RowBox[{"(", - RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"nu", " ", "=", " ", - RowBox[{"Z", "-", - RowBox[{"lambda", " ", "W"}]}]}], "\[IndentingNewLine]", - RowBox[{"Av", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"exp", - RowBox[{"(", - RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", - "lambda"}]}], "\[IndentingNewLine]", - RowBox[{"Bv", " ", "=", " ", - RowBox[{ - RowBox[{"alpha", " ", ".", - RowBox[{"(", - RowBox[{"beta", " ", "-", " ", "lambda"}], ")"}]}], " ", "+", " ", - "lambda"}]}], "\[IndentingNewLine]", - RowBox[{"Cv", " ", "=", " ", - RowBox[{ - RowBox[{"alpha", " ", ".", - RowBox[{"(", - RowBox[{"mu", " ", "-", " ", "nv"}], ")"}]}], " ", "+", " ", - "nv"}]}], "\[IndentingNewLine]", - RowBox[{"a", " ", "=", " ", - RowBox[{"sin", - RowBox[{ - RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", - RowBox[{"b", " ", "=", " ", - RowBox[{ - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"cos", - RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{"R", " ", "=", " ", - RowBox[{ - RowBox[{"IdentityMatrix", "[", "3", "]"}], "+", " ", - RowBox[{"a", ".", "v_skew"}], " ", "+", " ", - RowBox[{"b", ".", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", - RowBox[{"V", " ", "=", " ", - RowBox[{ - RowBox[{"Av", ".", - RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "+", - RowBox[{"Bv", ".", "v_skew"}], " ", "+", " ", - RowBox[{ - "Cv", ".", " ", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", - RowBox[{"simExponetialMap", " ", "=", " ", - RowBox[{"Table", "[", - RowBox[{"0", ",", - RowBox[{"{", - RowBox[{"i", ",", "4"}], "}"}], ",", - RowBox[{"{", - RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simExponetialMap", "[", - RowBox[{"[", - RowBox[{ - RowBox[{"1", ";;", "3"}], ",", - RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", - "R"}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simExponetialMap", "[", - RowBox[{"[", - RowBox[{ - RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", - RowBox[{"V", ".", "u"}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"simExponetialMap", "[", - RowBox[{"[", - RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", - RowBox[{"exp", - RowBox[{"(", - RowBox[{"-", "lambda"}], ")"}]}]}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"MatrixForm", "[", "simExponetialMap", "]"}], - "\[IndentingNewLine]"}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"f", "[", "lambda_", "]"}], " ", "=", " ", - "simExponetialMap"}], "\[IndentingNewLine]", - RowBox[{ - RowBox[{"f", "'"}], "[", "lambda", "]"}], "\[IndentingNewLine]"}], "Input", - CellChangeTimes->CompressedData[" -1:eJwl0l1Ik3EUBvCZsrU2x7CtlFpoGaN3iiauLlpgBGJtqFntInGshlqBTX2x -bOUQsUnsQiizBB312sfLZvkxRSM0yYkxyJwSFtXaxvADIW3pkqHYnr8Xhx8H -DufhwEm5Yiwq3cHhcNKjBeN9Oc08+dIpk970HO4fNzggK54agbreFxNwReL/ -BGc7XR44J3r4ZdurXpjPyhZg753quZ1R7eaSRWjlr/2GudzqMHzrexeBef4n -G3DoYh2Xj5zJzL0wKGvaB9lK5jBsLNVSUJV1IAP+Cf1SQrfQeAy2D9IG2M1z -lsNLKnkVfGTOJPadCdfArXvBZjhEfW+BTk9qG7xJF9qguFzzFE7ffvyK5Nvd -DpJrOzIAhauGcZi2RgehyGQhMoHLgl3Iq1cIodj2/nVC1NEPxYMwQJ2cgKr0 -eR8sKzs9D9mBkiXo3sxbgXYlG4Ipz+6GYfxGfszuqJPtXi482+Hgk/5lbCLU -NYaIe9alMvhgrDAZ9l04r9ie8+RCTUu2Bcp5gSaYulVvg5ITGgYyumsOskff -Skyixf3Qb+2ZgXoL/RU6zwl+wOSu439hYqyZyPZXrMPKVm+cBPd1WAXw4Ge1 -CB5ixhLgt9phBRQVZx+FacY6JWxQF6kgdf9fDpy6IVVDifN6AXzzcWYZal3U -KhyNKCOwVttAdP2c5kjxBxW34uB/2UIzug== - "]], - -Cell[BoxData[""], "Input", - CellChangeTimes->{{3.644369188660811*^9, 3.644369188678254*^9}}] -}, Open ]] -}, -WindowSize->{812, 579}, -WindowMargins->{{Automatic, 17}, {Automatic, 31}}, -FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (December 4, \ -2014)", -StyleDefinitions->"Default.nb" -] -(* End of Notebook Content *) - -(* Internal cache information *) -(*CellTagsOutline -CellTagsIndex->{} -*) -(*CellTagsIndex -CellTagsIndex->{} -*) -(*NotebookFileOutline -Notebook[{ -Cell[CellGroupData[{ -Cell[580, 22, 808, 14, 224, "Title"], -Cell[1391, 38, 3597, 96, 166, "Input"], -Cell[4991, 136, 92, 1, 28, InheritFromParent], -Cell[5086, 139, 7409, 210, 696, "Input"], -Cell[12498, 351, 92, 1, 28, InheritFromParent] -}, Open ]] -} -] -*) - -(* End of internal cache information *)