12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

all jacobian tests for essential matrix use macro

release/4.3a0
Akshay Krishnan 2021-06-21 05:21:19 +00:00
parent a69f9e59b4
commit 119e43aeac
1 changed files with 22 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -7,7 +7,6 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/nonlinear/ExpressionFactor.h> #include <gtsam/nonlinear/ExpressionFactor.h>
@ -89,20 +88,12 @@ TEST(EssentialMatrixFactor, factor) {
// Check evaluation // Check evaluation
Vector expected(1); Vector expected(1);
expected << 0; expected << 0;
Matrix Hactual; Vector actual = factor.evaluateError(trueE);
Vector actual = factor.evaluateError(trueE, Hactual);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian Values val;
Matrix Hexpected; val.insert(key, trueE);
typedef Eigen::Matrix<double, 1, 1> Vector1; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none),
trueE);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
} }
} }
@ -226,19 +217,10 @@ TEST(EssentialMatrixFactor2, factor) {
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian Values val;
Matrix Hexpected1, Hexpected2; val.insert(100, trueE);
boost::function<Vector(const EssentialMatrix &, double)> f = val.insert(i, d);
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
boost::none, boost::none);
Hexpected1 =
numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
} }
} }
@ -300,19 +282,10 @@ TEST(EssentialMatrixFactor3, factor) {
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian Values val;
Matrix Hexpected1, Hexpected2; val.insert(100, bodyE);
boost::function<Vector(const EssentialMatrix &, double)> f = val.insert(i, d);
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7);
boost::none, boost::none);
Hexpected1 =
numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
} }
} }
@ -640,24 +613,14 @@ TEST(EssentialMatrixFactor2, extraTest) {
const Point2 pi = camera2.project(P1); const Point2 pi = camera2.project(P1);
Point2 expected(pi - pB(i)); Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian Values val;
Matrix Hexpected1, Hexpected2; val.insert(100, trueE);
boost::function<Vector(const EssentialMatrix &, double)> f = val.insert(i, d);
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
boost::none, boost::none);
Hexpected1 =
numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
} }
} }
@ -710,24 +673,14 @@ TEST(EssentialMatrixFactor3, extraTest) {
const Point2 pi = camera2.project(P1); const Point2 pi = camera2.project(P1);
Point2 expected(pi - pB(i)); Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian Values val;
Matrix Hexpected1, Hexpected2; val.insert(100, bodyE);
boost::function<Vector(const EssentialMatrix &, double)> f = val.insert(i, d);
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
boost::none, boost::none);
Hexpected1 =
numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
} }
} }