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

changing to macro EssenstialMatrixfactor4

release/4.3a0
Akshay Krishnan 2021-06-21 03:47:44 +00:00
parent e3b6c8308a
commit a69f9e59b4
1 changed files with 6 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,6 +14,7 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
@ -359,26 +360,13 @@ TEST(EssentialMatrixFactor4, factor) {
// Check evaluation
Vector1 expected;
expected << 0;
Matrix HEactual;
Matrix HKactual;
Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual);
Vector actual = factor.evaluateError(trueE, trueK);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix HEexpected;
Matrix HKexpected;
typedef Eigen::Matrix<double, 1, 1> Vector1;
boost::function<Vector(const EssentialMatrix &, const Cal3_S2 &)> f =
boost::bind(&EssentialMatrixFactor4<Cal3_S2>::evaluateError, factor, _1,
_2, boost::none, boost::none);
HEexpected = numericalDerivative21<Vector1, EssentialMatrix, Cal3_S2>(
f, trueE, trueK);
HKexpected = numericalDerivative22<Vector1, EssentialMatrix, Cal3_S2>(
f, trueE, trueK);
// Verify the Jacobian is correct
EXPECT(assert_equal(HEexpected, HEactual, 1e-8));
EXPECT(assert_equal(HKexpected, HKactual, 1e-8));
Values truth;
truth.insert(keyE, trueE);
truth.insert(keyK, trueK);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
}
}