diff --git a/.cproject b/.cproject index 19005c63f..b5d5685ab 100644 --- a/.cproject +++ b/.cproject @@ -542,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -576,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,11 +656,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -760,22 +766,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -792,6 +782,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -816,42 +822,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -912,34 +902,42 @@ true true - + make -j5 - testRotateFactor.run + testValues.run true true true - + make - -j2 - all + -j5 + testOrdering.run true true true - + make - -j2 - check + -j5 + testKey.run true true true - + make - -j2 - clean + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -1330,6 +1328,7 @@ make + testGraph.run true false @@ -1337,6 +1336,7 @@ make + testJunctionTree.run true false @@ -1344,6 +1344,7 @@ make + testSymbolicBayesNetB.run true false @@ -1511,6 +1512,7 @@ make + testErrors.run true false @@ -1556,22 +1558,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1652,6 +1638,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1916,22 +1910,6 @@ true true - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - make -j2 @@ -2014,7 +1992,6 @@ make - testSimulated2DOriented.run true false @@ -2054,7 +2031,6 @@ make - testSimulated2D.run true false @@ -2062,7 +2038,6 @@ make - testSimulated3D.run true false @@ -2172,14 +2147,6 @@ true true - - make - -j5 - testSampler.run - true - true - true - make -j2 @@ -2332,6 +2299,14 @@ true true + + make + -j5 + CreateSFMExampleData.run + true + true + true + make -j4 @@ -2350,7 +2325,6 @@ make - tests/testGaussianISAM2 true false @@ -2372,102 +2346,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2669,6 +2547,7 @@ cpack + -G DEB true false @@ -2676,6 +2555,7 @@ cpack + -G RPM true false @@ -2683,6 +2563,7 @@ cpack + -G TGZ true false @@ -2690,6 +2571,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2855,34 +2737,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2926,6 +2872,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp new file mode 100644 index 000000000..b62b1c6fa --- /dev/null +++ b/examples/CreateSFMExampleData.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CreateSFMExampleData.cpp + * @brief Create some example data that for inclusion in the data folder + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace boost::assign; +using namespace std; +using namespace gtsam; + +void create5PointExample1() { + + // Class that will gather all data + SfM_data data; + + // Create two cameras and corresponding essential matrix E + Rot3 aRb = Rot3::yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 identity, aPb(aRb, aTb); + data.cameras.push_back(SfM_Camera(identity)); + data.cameras.push_back(SfM_Camera(aPb)); + + // Create test data, we need at least 5 points + vector P; + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + + BOOST_FOREACH(const Point3& p, P) { + + // Create the track + SfM_Track track; + track.p = p; + track.r = 1; + track.g = 1; + track.b = 1; + + // Project points in both cameras + for (size_t i = 0; i < 2; i++) + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + + // Add track to data + data.tracks.push_back(track); + } + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/5pointExample1.txt"; + writeBAL(filename, data); +} + +int main(int argc, char* argv[]) { + create5PointExample1(); + return 0; +} + +/* ************************************************************************* */ + diff --git a/examples/Data/5pointExample1.txt b/examples/Data/5pointExample1.txt new file mode 100644 index 000000000..212f75aec --- /dev/null +++ b/examples/Data/5pointExample1.txt @@ -0,0 +1,53 @@ +2 5 10 + +0 0 0 -0 +1 0 -6.123233995736766344e-18 -0.10000000000000000555 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 0.10000000000000000555 -0 +1 2 0 -0 +0 3 0 -1 +1 3 1 -0.20000000000000006661 +0 4 0 1 +1 4 -1 -0.19999999999999995559 + +3.141592653589793116 +0 +0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 +0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +0 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1247f6ddf..cb8cd8bfd 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -6,11 +6,13 @@ */ #include -#include -#include +#include #include #include +#include +#include #include + #include #include @@ -21,75 +23,68 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -//************************************************************************* -// Create two cameras and corresponding essential matrix E -Rot3 aRb = Rot3::yaw(M_PI_2); -Point3 aTb(0.1, 0, 0); -Pose3 identity, aPb(aRb, aTb); -typedef CalibratedCamera Cam; -Cam cameraA(identity), cameraB(aPb); -Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix(); +const string filename = findExampleDataFile("5pointExample1.txt"); +SfM_data data; +bool readOK = readBAL(filename, data); +Rot3 aRb = data.cameras[1].pose().rotation(); +Point3 aTb = data.cameras[1].pose().translation(); -// Create test data, we need at least 5 points -Point3 P[5] = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // -Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) }; - -// Project points in both cameras -vector pA(5), pB(5); -vector::iterator // -it1 = std::transform(P, P + 5, pA.begin(), - boost::bind(&Cam::project, &cameraA, _1, boost::none, boost::none)), // -it2 = std::transform(P, P + 5, pB.begin(), - boost::bind(&Cam::project, &cameraB, _1, boost::none, boost::none)); - -// Convert to homogeneous coordinates -vector vA(5), vB(5); -vector::iterator // -it3 = std::transform(pA.begin(), pA.end(), vA.begin(), - &EssentialMatrix::Homogeneous), // -it4 = std::transform(pB.begin(), pB.end(), vB.begin(), - &EssentialMatrix::Homogeneous); +Point2 pA(size_t i) { + return data.tracks[i].measurements[0].second; +} +Point2 pB(size_t i) { + return data.tracks[i].measurements[1].second; +} +Vector vA(size_t i) { + return EssentialMatrix::Homogeneous(pA(i)); +} +Vector vB(size_t i) { + return EssentialMatrix::Homogeneous(pB(i)); +} //************************************************************************* -TEST (EssentialMatrix, testData) { +TEST (EssentialMatrixFactor, testData) { + CHECK(readOK); + // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - EXPECT(assert_equal(expected, aEb_matrix)); + Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix(); + EXPECT(assert_equal(expected, aEb_matrix,1e-8)); // Check some projections - EXPECT(assert_equal(Point2(0,0),pA[0])); - EXPECT(assert_equal(Point2(0,0.1),pB[0])); - EXPECT(assert_equal(Point2(0,-1),pA[4])); - EXPECT(assert_equal(Point2(-1,0.2),pB[4])); + EXPECT(assert_equal(Point2(0,0),pA(0),1e-8)); + EXPECT(assert_equal(Point2(0,0.1),pB(0),1e-8)); + EXPECT(assert_equal(Point2(0,-1),pA(4),1e-8)); + EXPECT(assert_equal(Point2(-1,0.2),pB(4),1e-8)); // Check homogeneous version - EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB[4])); + EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB(4),1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, vA[i].transpose() * aEb_matrix * vB[i], 1e-8); + EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8); // Check epipolar constraint EssentialMatrix trueE(aRb, aTb); for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, trueE.error(vA[i],vB[i]), 1e-8); + EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i),vB(i)), 1e-7); } //************************************************************************* -TEST (EssentialMatrix, factor) { +TEST (EssentialMatrixFactor, factor) { EssentialMatrix trueE(aRb, aTb); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor factor(1, pA[i], pB[i], model); + EssentialMatrixFactor factor(1, pA(i), pB(i), model); // Check evaluation Vector expected(1); expected << 0; Matrix HActual; Vector actual = factor.evaluateError(trueE, HActual); - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix HExpected; @@ -98,12 +93,12 @@ TEST (EssentialMatrix, factor) { boost::none), trueE); // Verify the Jacobian is correct - EXPECT(assert_equal(HExpected, HActual, 1e-9)); + EXPECT(assert_equal(HExpected, HActual, 1e-8)); } } //************************************************************************* -TEST (EssentialMatrix, fromConstraints) { +TEST (EssentialMatrixFactor, fromConstraints) { // Here we want to optimize directly on essential matrix constraints // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, // but GTSAM does the equivalent anyway, provided we give the right @@ -112,9 +107,10 @@ TEST (EssentialMatrix, fromConstraints) { // We start with a factor graph and add constraints to it // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1,0.01); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, + 0.01); for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA[i], pB[i], model)); + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model)); // Check error at ground truth Values truth; @@ -124,7 +120,8 @@ TEST (EssentialMatrix, fromConstraints) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); + EssentialMatrix initialE = trueE.retract( + (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2); @@ -142,7 +139,7 @@ TEST (EssentialMatrix, fromConstraints) { // Check errors individually for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, actual.error(vA[i],vB[i]), 1e-6); + EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6); }