diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index d0139d850..4a3985842 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -436,8 +436,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - vector views; views.push_back(x1); views.push_back(x2); @@ -473,7 +471,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setDynamicOutlierRejectionThreshold(1); SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); @@ -488,6 +486,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartFactor::shared_ptr smartFactor4(new SmartFactor(params)); smartFactor4->add(measurements_cam4, views, model, K); + // same as factor 4, but dynamic outlier rejection is off + SmartFactor::shared_ptr smartFactor4b(new SmartFactor()); + smartFactor4b->add(measurements_cam4, views, model, K); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph; @@ -505,7 +507,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { values.insert(x2, pose2); values.insert(x3, pose3); - // All factors are disabled and pose should remain where it is + EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9); + // zero error due to dynamic outlier rejection + EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); + + // dynamic outlier rejection is off + EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + + // Factors 1-3 should have valid point, factor 4 should not + EXPECT(smartFactor1->point()); + EXPECT(smartFactor2->point()); + EXPECT(smartFactor3->point()); + EXPECT(smartFactor4->point().degenerate()); + EXPECT(smartFactor4b->point()); + + // Factor 4 is disabled, pose 3 stays put Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize();