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

adding prior on calibrations

release/4.3a0
Ayush Baid 2021-04-28 18:49:07 -04:00
parent 2cf76daf3c
commit 4572282deb
2 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/slam/EssentialMatrixFactor.h
View File

@ -297,6 +297,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
* Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given
* essential matrix and calibration. The calibration is shared between two * essential matrix and calibration. The calibration is shared between two
* images. * images.
*
* Note: as correspondences between 2d coordinates can only recover 7 DoF,
* this factor should always be used with a prior factor on calibration.
*/ */
template <class CALIBRATION> template <class CALIBRATION>
class EssentialMatrixFactor4 class EssentialMatrixFactor4
@ -357,8 +360,8 @@ class EssentialMatrixFactor4
// converting from pixel coordinates to normalized coordinates cA and cB // converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0); Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
// convert to homogeneous coordinates // convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vA = EssentialMatrix::Homogeneous(cA);

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

@ -425,9 +425,9 @@ TEST(EssentialMatrixFactor4, minimization) {
EssentialMatrix initialE = EssentialMatrix initialE =
trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
Cal3_S2 initialK = Cal3_S2 initialK =
trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); trueK.retract((Vector(5) << 0.1, -0.08, 0.01, -0.05, 0.06).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
initial.insert(2, trueK); initial.insert(2, initialK);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else #else
@ -435,6 +435,11 @@ TEST(EssentialMatrixFactor4, minimization) {
1e-2); // TODO: update this value too 1e-2); // TODO: update this value too
#endif #endif
// add prior factor for calibration
Vector5 priorNoiseModelSigma;
priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1;
graph.emplace_shared<PriorFactor<Cal3_S2>>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
// Optimize // Optimize
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
@ -444,7 +449,7 @@ TEST(EssentialMatrixFactor4, minimization) {
EssentialMatrix actualE = result.at<EssentialMatrix>(1); EssentialMatrix actualE = result.at<EssentialMatrix>(1);
Cal3_S2 actualK = result.at<Cal3_S2>(2); Cal3_S2 actualK = result.at<Cal3_S2>(2);
EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance
EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance EXPECT(assert_equal(trueK, actualK, 1e-2)); // TODO: fix the tolerance
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -652,16 +657,21 @@ TEST(EssentialMatrixFactor4, extraMinimization) {
EssentialMatrix initialE = EssentialMatrix initialE =
trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
Cal3Bundler initialK = Cal3Bundler initialK =
trueK.retract((Vector(3) << 0.1, -0.02, 0.03).finished()); trueK.retract((Vector(3) << 0.1, -0.01, 0.01).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
initial.insert(2, initialK); initial.insert(2, initialK);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(633.71, graph.error(initial), 1e-2); EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2);
#else #else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this
#endif #endif
// add prior factor on calibration
Vector3 priorNoiseModelSigma;
priorNoiseModelSigma << 0.3, 0.03, 0.03;
graph.emplace_shared<PriorFactor<Cal3Bundler>>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
// Optimize // Optimize
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);