diff --git a/.cproject b/.cproject index 2e23fe9ec..2dbaf2d0f 100644 --- a/.cproject +++ b/.cproject @@ -904,6 +904,14 @@ true true + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + make -j2 diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index aca2fd96d..ed1f9b78a 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 70027cc28..3f0cb07f7 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -8,115 +8,21 @@ #include #include #include -#include -#include -#include + #include -#include -#include -#include - using namespace std; -using namespace boost::assign; using namespace gtsam; -/** - * Factor that evaluates epipolar error p'Ep for given essential matrix - */ -class EssentialMatrixFactor: public NoiseModelFactor1 { - - Point2 pA_, pB_; ///< Measurements in image A and B - Vector vA_, vB_; ///< Homogeneous versions - - typedef NoiseModelFactor1 Base; - -public: - - /// Constructor - EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key), pA_(pA), pB_(pB), // - vA_(EssentialMatrix::Homogeneous(pA)), // - vB_(EssentialMatrix::Homogeneous(pB)) { - } - - /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - Base::print(s); - std::cout << " EssentialMatrixFactor with measurements\n (" - << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() - << ")'" << endl; - } - - /// vector of errors returns 1D vector - Vector evaluateError(const EssentialMatrix& E, boost::optional H = - boost::none) const { - return (Vector(1) << E.error(vA_, vB_, H)); - } - -}; - //************************************************************************* // 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(); - -// 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)); - -// Converto to homogenous 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); - -//************************************************************************* -TEST (EssentialMatrix, testData) { - // 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)); - - // 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])); - - // Check homogeneous version - EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB[4])); - - // Check epipolar constraint - for (size_t i = 0; i < 5; i++) - 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); -} //************************************************************************* TEST (EssentialMatrix, equality) { -// EssentialMatrix actual, expected; -// EXPECT(assert_equal(expected, actual)); + EssentialMatrix actual(aRb, aTb), expected(aRb, aTb); + EXPECT(assert_equal(expected, actual)); } //************************************************************************* @@ -135,76 +41,6 @@ TEST (EssentialMatrix, retract2) { EXPECT(assert_equal(expected, actual)); } -//************************************************************************* -TEST (EssentialMatrix, 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); - - // Check evaluation - Vector expected(1); - expected << 0; - Matrix HActual; - Vector actual = factor.evaluateError(trueE, HActual); - EXPECT(assert_equal(expected, actual, 1e-8)); - - // Use numerical derivatives to calculate the expected Jacobian - Matrix HExpected; - HExpected = numericalDerivative11( - boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), trueE); - - // Verify the Jacobian is correct - CHECK(assert_equal(HExpected, HActual, 1e-9)); - } -} - -//************************************************************************* -TEST (EssentialMatrix, 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 - // factors. In this case, the factors are the constraints. - - // 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); - for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA[i], pB[i], model)); - - // Check error at ground truth - Values truth; - EssentialMatrix trueE(aRb, aTb); - truth.insert(1, trueE); - EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); - - // Check error at initial estimate - Values initial; - 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); - - // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); - Values result = optimizer.optimize(); - - // Check result - EssentialMatrix actual = result.at(1); - EXPECT(assert_equal(trueE, actual,1e-1)); - - // Check error at result - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); - - // Check errors individually - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, actual.error(vA[i],vB[i]), 1e-6); - -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h new file mode 100644 index 000000000..5e07cabc0 --- /dev/null +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -0,0 +1,54 @@ +/* + * @file EssentialMatrixFactor.cpp + * @brief EssentialMatrixFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Factor that evaluates epipolar error p'Ep for given essential matrix + */ +class EssentialMatrixFactor: public NoiseModelFactor1 { + + Point2 pA_, pB_; ///< Measurements in image A and B + Vector vA_, vB_; ///< Homogeneous versions + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) : + Base(model, key), pA_(pA), pB_(pB), // + vA_(EssentialMatrix::Homogeneous(pA)), // + vB_(EssentialMatrix::Homogeneous(pB)) { + } + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(s); + std::cout << " EssentialMatrixFactor with measurements\n (" + << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() + << ")'" << std::endl; + } + + /// vector of errors returns 1D vector + Vector evaluateError(const EssentialMatrix& E, boost::optional H = + boost::none) const { + return (Vector(1) << E.error(vA_, vB_, H)); + } + +}; + +} // gtsam + diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp new file mode 100644 index 000000000..5a5753589 --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -0,0 +1,155 @@ +/* + * @file testEssentialMatrixFactor.cpp + * @brief Test EssentialMatrixFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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(); + +// 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)); + +// Converto to homogenous 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); + +//************************************************************************* +TEST (EssentialMatrix, testData) { + // 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)); + + // 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])); + + // Check homogeneous version + EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB[4])); + + // Check epipolar constraint + for (size_t i = 0; i < 5; i++) + 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); +} + +//************************************************************************* +TEST (EssentialMatrix, 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); + + // Check evaluation + Vector expected(1); + expected << 0; + Matrix HActual; + Vector actual = factor.evaluateError(trueE, HActual); + EXPECT(assert_equal(expected, actual, 1e-8)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix HExpected; + HExpected = numericalDerivative11( + boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, + boost::none), trueE); + + // Verify the Jacobian is correct + CHECK(assert_equal(HExpected, HActual, 1e-9)); + } +} + +//************************************************************************* +TEST (EssentialMatrix, 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 + // factors. In this case, the factors are the constraints. + + // 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); + for (size_t i = 0; i < 5; i++) + graph.add(EssentialMatrixFactor(1, pA[i], pB[i], model)); + + // Check error at ground truth + Values truth; + EssentialMatrix trueE(aRb, aTb); + truth.insert(1, trueE); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + 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); + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(1); + EXPECT(assert_equal(trueE, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, actual.error(vA[i],vB[i]), 1e-6); + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +