diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 3f46ae8b4..f8f625b17 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -32,11 +32,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Class that will gather all data SfmData data; - - // Create two cameras - Rot3 aRb = Rot3::Yaw(M_PI_2); - Point3 aTb(0.1, 0, 0); - Pose3 identity, aPb(aRb, aTb); + // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); @@ -75,7 +71,7 @@ void create5PointExample1() { 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"; + const string filename = "../../examples/Data/5pointExample1.txt"; createExampleBALFile(filename, P, pose1, pose2); } @@ -94,7 +90,7 @@ void create5PointExample2() { 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"; + const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); createExampleBALFile(filename, P, pose1, pose2,K); } @@ -111,20 +107,20 @@ void create18PointExample1() { // Create test data, we need 15 points vector P; - P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), // - Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), // - Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), // - Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), // - Point3(0, -0.5, 0.5), Point3(0, 0, 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), 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 - const string filename = "../../examples/data/18pointExample1.txt"; + const string filename = "../../examples/Data/18pointExample1.txt"; createExampleBALFile(filename, P, pose1, pose2); } /* ************************************************************************* */ -void create11PointExample2() { +void create11PointExample1() { // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); @@ -132,14 +128,13 @@ void create11PointExample2() { // Create test data, we need 11 points vector P; - P += Point3(0, 0, 100), Point3(0, 0, 100), Point3(0, 0, 100), // - Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), // - Point3(-10, 50, 50), Point3(10, -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), Point3(0, 0, 100), Point3(0, 0, 100), // + Point3(-10, 50, 50), Point3(10, -50, 50); // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/11pointExample2.txt"; + const string filename = "../../examples/Data/11pointExample1.txt"; Cal3Bundler K(500, 0, 0); createExampleBALFile(filename, P, pose1, pose2, K); } @@ -150,7 +145,7 @@ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); create18PointExample1(); - create11PointExample2(); + create11PointExample1(); return 0; } diff --git a/examples/Data/18pointExample1.txt b/examples/Data/18pointExample1.txt new file mode 100644 index 000000000..484a7944b --- /dev/null +++ b/examples/Data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +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 +0 5 -0.5 0.25 +1 5 -0.25000000000000005551 -0.55000000000000004441 +0 6 -0.5 -0.25 +1 6 0.24999999999999997224 -0.55000000000000004441 +0 7 0.16666666666666665741 0.33333333333333331483 +1 7 -0.33333333333333331483 0.10000000000000000555 +0 8 0.16666666666666665741 -0.33333333333333331483 +1 8 0.33333333333333331483 0.099999999999999977796 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 0.10000000000000000555 0.5 +1 10 -0.5 3.0616169978683830179e-17 +0 11 0.10000000000000000555 -0.5 +1 11 0.5 -3.0616169978683830179e-17 +0 12 -0.2000000000000000111 -0 +1 12 -2.4492935982947065376e-17 -0.4000000000000000222 +0 13 -0.2000000000000000111 -1 +1 13 1 -0.40000000000000007772 +0 14 0 -0 +1 14 -1.2246467991473532688e-17 -0.2000000000000000111 +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 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + +-1 +-0.5 +2 + +-1 +0.5 +2 + +0.25 +-0.5 +1.5 + +0.25 +0.5 +1.5 + +-0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +0 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 + diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cf8f2a4b8..020c94fdb 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,80 +101,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> HE, - OptionalJacobian<1, 3> HvA, - OptionalJacobian<1, 3> HvB) const { - // compute the epipolar lines - Point3 lB = E_ * vB; - Point3 lA = E_.transpose() * vA; - - // compute the algebraic error as a simple dot product. - double algebraic_error = dot(vA, lB); - - // compute the line-norms for the two lines - Matrix23 I; - I.setIdentity(); - Matrix21 nA = I * lA; - Matrix21 nB = I * lB; - double nA_sq_norm = nA.squaredNorm(); - double nB_sq_norm = nB.squaredNorm(); - - // compute the normalizing denominator and finally the sampson error - double denominator = sqrt(nA_sq_norm + nB_sq_norm); - double sampson_error = algebraic_error / denominator; - - if (HE) { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { + if (H) { // See math.lyx - // computing the derivatives of the numerator w.r.t. E - Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 numerator_H_D = vA.transpose() * - skewSymmetric(-rotation().matrix() * vB) * - direction().basis(); - - // computing the derivatives of the denominator w.r.t. E - Matrix12 denominator_H_nA = nA.transpose() / denominator; - Matrix12 denominator_H_nB = nB.transpose() / denominator; - Matrix13 denominator_H_lA = denominator_H_nA * I; - Matrix13 denominator_H_lB = denominator_H_nB * I; - Matrix33 lB_H_R = E_ * skewSymmetric(-vB); - Matrix32 lB_H_D = - skewSymmetric(-rotation().matrix() * vB) * direction().basis(); - Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA); - Matrix32 lA_H_D = - rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); - - Matrix13 denominator_H_R = - denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; - Matrix12 denominator_H_D = - denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; - - Matrix15 denominator_H; - denominator_H << denominator_H_R, denominator_H_D; - Matrix15 numerator_H; - numerator_H << numerator_H_R, numerator_H_D; - - *HE = 2 * sampson_error * (numerator_H / denominator - - algebraic_error * denominator_H / (denominator * denominator)); + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); + *H << HR, HD; } - - if (HvA){ - Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); - Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; - - *HvA = 2 * sampson_error * (numerator_H_vA / denominator - - algebraic_error * denominator_H_vA / (denominator * denominator)); - } - - if (HvB){ - Matrix13 numerator_H_vB = vA.transpose() * matrix(); - Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; - - *HvB = 2 * sampson_error * (numerator_H_vB / denominator - - algebraic_error * denominator_H_vB / (denominator * denominator)); - } - - return sampson_error * sampson_error; + return dot(vA, E_ * vB); } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 5293f1c7f..909576aa0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,11 +159,9 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, sampson squared + /// epipolar error, algebraic GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> HE = boost::none, - OptionalJacobian<1, 3> HvA = boost::none, - OptionalJacobian<1, 3> HvB = boost::none) const; + OptionalJacobian<1, 5> H = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 999bf34b9..86a498cdc 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,70 +241,10 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } -//************************************************************************* -TEST(EssentialMatrix, errorValue) { - // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); - - // compute the expected error - // E = [0, 0, 0; 0, 0, -1; 1, 0, 0] - // line for b = [0, -1, 3] - // line for a = [1, 0, 2] - // algebraic error = 5 - // norm of line for b = 1 - // norm of line for a = 1 - // sampson error = 5^2 / 1^2 + 1^2 - double expected = 12.5; - - // check the error - double actual = trueE.error(a, b); - EXPECT(assert_equal(expected, actual, 1e-6)); -} - -//************************************************************************* -double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { - EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); - return E.error(a, b); -} -TEST(EssentialMatrix, errorJacobians) { - // Use two points to get error - Point3 vA(1, -2, 1); - Point3 vB(3, 1, 1); - - Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); - Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); - - // Use numerical derivatives to calculate the expected Jacobian - Matrix13 HRexpected; - Matrix12 HDexpected; - Matrix13 HvAexpected; - Matrix13 HvBexpected; - HRexpected = numericalDerivative41(error_, E.rotation(), - E.direction(), vA, vB); - HDexpected = numericalDerivative42(error_, E.rotation(), - E.direction(), vA, vB); - HvAexpected = numericalDerivative43(error_, E.rotation(), - E.direction(), vA, vB); - HvBexpected = numericalDerivative44(error_, E.rotation(), - E.direction(), vA, vB); - Matrix15 HEexpected; - HEexpected << HRexpected, HDexpected; - - Matrix15 HEactual; - Matrix13 HvAactual, HvBactual; - E.error(vA, vB, HEactual, HvAactual, HvBactual); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); - EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); - EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ + diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index ec4351b86..3e8c45ece 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -80,7 +80,7 @@ class EssentialMatrixFactor : public NoiseModelFactor1 { const EssentialMatrix& E, boost::optional H = boost::none) const override { Vector error(1); - error << E.error(vA_, vB_, H, boost::none, boost::none); + error << E.error(vA_, vB_, H); return error; } @@ -367,22 +367,20 @@ class EssentialMatrixFactor4 Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - - Matrix13 error_H_vA, error_H_vB; - Vector error(1); - error << E.error(vA, vB, H1, H2 ? &error_H_vA : 0, H2 ? &error_H_vB : 0); - if (H2) { // compute the jacobian of error w.r.t K - // error function f - // H2 = df/dK = df/dvA * dvA/dK + df/dvB * dvB/dK + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = error_H_vA.leftCols<2>() * cA_H_K - + error_H_vB.leftCols<2>() * cB_H_K; + *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } - + + Vector error(1); + error << E.error(vA, vB, H1); + return error; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2d2a25cd7..563482651 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -12,7 +12,6 @@ #include #include #include -#include #include #include #include @@ -25,7 +24,7 @@ using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 1e-4); + 0.01); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -73,13 +72,13 @@ TEST (EssentialMatrixFactor, testData) { EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections - EXPECT(assert_equal(Point2(-0.1, -0.5), pA(0), 1e-8)); - EXPECT(assert_equal(Point2(-0.5, 0.2), pB(0), 1e-8)); - EXPECT(assert_equal(Point2(0, 0), pA(4), 1e-8)); - EXPECT(assert_equal(Point2(0, 0.1), pB(4), 1e-8)); + 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(Vector3(0, 0.1, 1), vB(4), 1e-8)); + EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) @@ -120,8 +119,7 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, - boost::none); + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -147,12 +145,9 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, - boost::none); - boost::function, - OptionalJacobian<5, 2>)> - g; + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::function, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); @@ -199,9 +194,8 @@ TEST (EssentialMatrixFactor, minimization) { (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(18161.79, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else - // TODO : redo this error EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif @@ -374,6 +368,7 @@ 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; @@ -393,7 +388,7 @@ TEST(EssentialMatrixFactor4, factor) { } //************************************************************************* -TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) { Key keyE(1); Key keyK(2); // initialize essential matrix @@ -409,15 +404,33 @@ TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { Point2 pB(12.0, 15.0); EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 3e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); } //************************************************************************* -TEST(EssentialMatrixFactor4, minimization) { - // As before, we start with a factor graph and add constraints to it +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2)); + Unit3 t(Point3(0.1, 0, 0)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3Bundler K; + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(-0.1, 0.5); + Point2 pB(-0.5, -0.2); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { NonlinearFactorGraph graph; - size_t numberPoints = 11; - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -427,57 +440,145 @@ TEST(EssentialMatrixFactor4, minimization) { truth.insert(2, trueK); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + // Initialization + Values initial; + 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); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 10, 10, 10, 10, 10; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // 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, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +//************************************************************************* +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. + NonlinearFactorGraph graph; + for (size_t i = 0; i < 7; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 20, 20, 1, 1, 1; + graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 7; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + Cal3Bundler trueK; + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + 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).finished()); - Cal3_S2 initialK = - trueK.retract((Vector(5) << 0.1, -0.08, 0.01, -0.05, 0.06).finished()); + Cal3Bundler initialK = trueK; initial.insert(1, initialE); initial.insert(2, initialK); -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(6246.35, graph.error(initial), 1e-2); -#else - // TODO: update this value too - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); -#endif // add prior factor for calibration - Vector5 priorNoiseModelSigma; - priorNoiseModelSigma << 0.3, 0.3, 0.05, 0.3, 0.3; - graph.emplace_shared>( - 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - - // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Vector3 priorNoiseModelSigma; + priorNoiseModelSigma << 1, 1, 1; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); // Check result EssentialMatrix actualE = result.at(1); - Cal3_S2 actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); // Check error at result - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.3); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), - 1e-5); + 1e-6); } + } // namespace example1 //************************************************************************* namespace example2 { -const string filename = findExampleDataFile("11pointExample2.txt"); +const string filename = findExampleDataFile("5pointExample2.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); @@ -526,10 +627,9 @@ TEST(EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(7850.88, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else - // TODO: redo this error - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // Optimize @@ -647,13 +747,12 @@ TEST (EssentialMatrixFactor3, extraTest) { } } -//************************************************************************* -TEST(EssentialMatrixFactor4, extraMinimization) { +/* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { // Additional test with camera moving in positive X direction - size_t numberPoints = 11; NonlinearFactorGraph graph; - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -667,47 +766,43 @@ TEST(EssentialMatrixFactor4, extraMinimization) { Values initial; EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); - Cal3Bundler initialK = - trueK.retract((Vector(3) << -50, -0.003, 0.003).finished()); + Cal3Bundler initialK = trueK.retract((Vector(3) << 0.1, 0.1, 0.1).finished()); initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(242630.09, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); #else - // TODO: redo this error - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif // add prior factor on calibration Vector3 priorNoiseModelSigma; - priorNoiseModelSigma << 100, 0.01, 0.01; - // TODO: fix this to work with the prior on initialK - graph.emplace_shared>( - 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + priorNoiseModelSigma << 1, 1, 1; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); // Check result EssentialMatrix actualE = result.at(1); Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), 1e-6); } +*/ } // namespace example2