diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index c872e791c..7e8d21580 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -1348,7 +1348,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { vector views; views.push_back(x1); @@ -1381,6 +1381,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; @@ -1391,6 +1394,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), @@ -1407,10 +1413,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case + // Hessian is invariant to rotations and translations in the degenerate case EXPECT( assert_equal(hessianFactor->information(), +#ifdef GTSAM_USE_EIGEN_MKL + hessianFactorRotTran->information(), 1e-5)); +#else hessianFactorRotTran->information(), 1e-6)); +#endif } /* ************************************************************************* */