From 1e58c0b0a2c013cd49d499b9f2d4441ac8b2a574 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 19:34:33 -0800 Subject: [PATCH] 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");