diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index eae806607..15c009036 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include @@ -27,17 +27,15 @@ using namespace gtsam; /* ************************************************************************* */ void createExampleBALFile(const string& filename, const vector& P, - const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = - Cal3Bundler()) { - + const Pose3& pose1, const Pose3& pose2, + const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data SfmData data; // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); - for(const Point3& p: P) { - + for (const Point3& p : P) { // Create the track SfmTrack track; track.p = p; @@ -47,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector& P, // 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))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); @@ -59,7 +57,6 @@ void createExampleBALFile(const string& filename, const vector& P, /* ************************************************************************* */ void create5PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); @@ -67,8 +64,8 @@ void create5PointExample1() { // 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); + 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); // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample1.txt"; @@ -78,7 +75,6 @@ void create5PointExample1() { /* ************************************************************************* */ void create5PointExample2() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); @@ -86,20 +82,19 @@ void create5PointExample2() { // Create test data, we need at least 5 points vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); + P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), + Point3(20, -50, 80); // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2,K); + createExampleBALFile(filename, P, pose1, pose2, K); } - /* ************************************************************************* */ void create18PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); @@ -107,11 +102,11 @@ void create18PointExample1() { // Create test data, we need 15 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), Point3(-1, -0.5, 2), // - Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5),// - Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // - Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // + 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), Point3(-1, -0.5, 2), // + Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5), // + Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // + Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples @@ -119,8 +114,6 @@ void create18PointExample1() { createExampleBALFile(filename, P, pose1, pose2); } -/* ************************************************************************* */ - int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); @@ -129,4 +122,3 @@ int main(int argc, char* argv[]) { } /* ************************************************************************* */ - diff --git a/examples/data/18pointExample1.txt b/examples/data/18pointExample1.txt deleted file mode 100644 index e7d186294..000000000 --- a/examples/data/18pointExample1.txt +++ /dev/null @@ -1,131 +0,0 @@ -2 18 36 - -0 0 -0.10000000000000000555 0.5 -1 0 -0.5 -0.19999999999999998335 -0 1 -0.10000000000000000555 -0 -1 1 -1.2246467991473532688e-17 -0.2000000000000000111 -0 2 -0.10000000000000000555 -0.5 -1 2 0.5 -0.20000000000000003886 -0 3 0 0.5 -1 3 -0.5 -0.099999999999999977796 -0 4 0 -0 -1 4 -6.123233995736766344e-18 -0.10000000000000000555 -0 5 0 -0.5 -1 5 0.5 -0.10000000000000003331 -0 6 0.10000000000000000555 0.5 -1 6 -0.5 3.0616169978683830179e-17 -0 7 0.10000000000000000555 -0 -1 7 0 -0 -0 8 0.10000000000000000555 -0.5 -1 8 0.5 -3.0616169978683830179e-17 -0 9 -0.2000000000000000111 1 -1 9 -1 -0.39999999999999996669 -0 10 -0.2000000000000000111 -0 -1 10 -2.4492935982947065376e-17 -0.4000000000000000222 -0 11 -0.2000000000000000111 -1 -1 11 1 -0.40000000000000007772 -0 12 0 1 -1 12 -1 -0.19999999999999995559 -0 13 0 -0 -1 13 -1.2246467991473532688e-17 -0.2000000000000000111 -0 14 0 -1 -1 14 1 -0.20000000000000006661 -0 15 0.2000000000000000111 1 -1 15 -1 6.1232339957367660359e-17 -0 16 0.2000000000000000111 -0 -1 16 0 -0 -0 17 0.2000000000000000111 -1 -1 17 1 -6.1232339957367660359e-17 - -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.10000000000000000555 --0.5 -1 - --0.10000000000000000555 -0 -1 - --0.10000000000000000555 -0.5 -1 - -0 --0.5 -1 - -0 -0 -1 - -0 -0.5 -1 - -0.10000000000000000555 --0.5 -1 - -0.10000000000000000555 -0 -1 - -0.10000000000000000555 -0.5 -1 - --0.10000000000000000555 --0.5 -0.5 - --0.10000000000000000555 -0 -0.5 - --0.10000000000000000555 -0.5 -0.5 - -0 --0.5 -0.5 - -0 -0 -0.5 - -0 -0.5 -0.5 - -0.10000000000000000555 --0.5 -0.5 - -0.10000000000000000555 -0 -0.5 - -0.10000000000000000555 -0.5 -0.5 - diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index b58d4f9d3..89ed8f0ae 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -5,26 +5,24 @@ * @date December 17, 2013 */ -#include - -#include -#include -#include -#include -#include -#include -#include +#include #include #include - -#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); +noiseModel::Isotropic::shared_ptr model1 = + noiseModel::Isotropic::Sigma(1, 0.01); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -45,30 +43,22 @@ PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); -double baseline = 0.1; // actual baseline of the camera +double baseline = 0.1; // actual baseline of the camera -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)); -} +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 (EssentialMatrixFactor, 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; - Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) - * c1Rc2.matrix(); + Matrix aEb_matrix = + skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections @@ -90,7 +80,7 @@ TEST (EssentialMatrixFactor, testData) { } //************************************************************************* -TEST (EssentialMatrixFactor, factor) { +TEST(EssentialMatrixFactor, factor) { Key key(1); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor factor(key, pA(i), pB(i), model1); @@ -104,10 +94,11 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - typedef Eigen::Matrix Vector1; + typedef Eigen::Matrix Vector1; Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), trueE); + boost::none), + trueE); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); @@ -118,10 +109,10 @@ TEST (EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - Expression E_(key); // leaf expression - Expression expr(f, E_); // unary expression + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression // Test the derivatives using Paul's magic Values values; @@ -144,13 +135,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); - Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression E_(&EssentialMatrix::FromRotationAndDirection, + R_, d_); Expression expr(f, E_); // Test the derivatives using Paul's magic @@ -171,7 +165,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { } //************************************************************************* -TEST (EssentialMatrixFactor, minimization) { +TEST(EssentialMatrixFactor, minimization) { // 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 @@ -190,8 +184,8 @@ TEST (EssentialMatrixFactor, minimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); @@ -214,11 +208,10 @@ TEST (EssentialMatrixFactor, minimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, factor) { +TEST(EssentialMatrixFactor2, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); @@ -234,11 +227,13 @@ TEST (EssentialMatrixFactor2, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, trueE, d); + Hexpected2 = + numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,7 +242,7 @@ TEST (EssentialMatrixFactor2, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor2, minimization) { +TEST(EssentialMatrixFactor2, minimization) { // Here we want to optimize for E and inverse depths at the same time // We start with a factor graph and add constraints to it @@ -290,8 +285,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix bodyE = cRb.inverse() * trueE; //************************************************************************* -TEST (EssentialMatrixFactor3, factor) { - +TEST(EssentialMatrixFactor3, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2); @@ -307,11 +301,13 @@ TEST (EssentialMatrixFactor3, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, bodyE, d); + Hexpected2 = + numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -320,13 +316,13 @@ TEST (EssentialMatrixFactor3, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor3, minimization) { - +TEST(EssentialMatrixFactor3, minimization) { // As before, we start with a factor graph and add constraints to it NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, + model2); // Check error at ground truth Values truth; @@ -368,7 +364,6 @@ TEST(EssentialMatrixFactor4, factor) { Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian Matrix HEexpected; Matrix HKexpected; @@ -445,13 +440,14 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); - initial.insert(2, trueK); + initial.insert(2, trueK); // add prior factor for calibration Vector5 priorNoiseModelSigma; priorNoiseModelSigma << 10, 10, 10, 10, 10; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -476,8 +472,8 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { //************************************************************************* TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { // We need 7 points here as the prior on the focal length parameters is weak - // and the initialization is noisy. So in total we are trying to optimize 7 DOF, - // with a strong prior on the remaining 3 DOF. + // and the initialization is noisy. So in total we are trying to optimize 7 + // DOF, with a strong prior on the remaining 3 DOF. NonlinearFactorGraph graph; for (size_t i = 0; i < 7; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), @@ -501,8 +497,9 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { // add prior factor for calibration Vector5 priorNoiseModelSigma; priorNoiseModelSigma << 20, 20, 1, 1, 1; - graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -528,8 +525,8 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.emplace_shared>(1, 2, pA(i), pB(i), - model1); + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); // Check error at ground truth Values truth; @@ -548,8 +545,9 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { // add prior factor for calibration Vector3 priorNoiseModelSigma; priorNoiseModelSigma << 0.1, 0.1, 0.1; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -571,7 +569,6 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } - } // namespace example1 //************************************************************************* @@ -585,14 +582,10 @@ Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); -double baseline = 10; // actual baseline of the camera +double baseline = 10; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } Cal3Bundler trueK = Cal3Bundler(500, 0, 0); boost::shared_ptr K = boost::make_shared(trueK); @@ -622,8 +615,8 @@ TEST(EssentialMatrixFactor, extraMinimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -647,11 +640,10 @@ TEST(EssentialMatrixFactor, extraMinimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, extraTest) { +TEST(EssentialMatrixFactor2, extraTest) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); @@ -667,11 +659,13 @@ TEST (EssentialMatrixFactor2, extraTest) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, trueE, d); + Hexpected2 = + numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -680,14 +674,15 @@ TEST (EssentialMatrixFactor2, extraTest) { } //************************************************************************* -TEST (EssentialMatrixFactor2, extraMinimization) { +TEST(EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.emplace_shared(100, i, pA(i), pB(i), model2, K); + graph.emplace_shared(100, i, pA(i), pB(i), model2, + K); // Check error at ground truth Values truth; @@ -715,8 +710,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { } //************************************************************************* -TEST (EssentialMatrixFactor3, extraTest) { - +TEST(EssentialMatrixFactor3, extraTest) { // The "true E" in the body frame is EssentialMatrix bodyE = cRb.inverse() * trueE; @@ -735,11 +729,13 @@ TEST (EssentialMatrixFactor3, extraTest) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, bodyE, d); + Hexpected2 = + numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -755,4 +751,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ -