diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index bb149636b..18f311a23 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -57,7 +57,7 @@ protected: Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { Vector bi = traits::Local(measured[i], predicted[i]); - if(ZDim==3 && std::isnan(bi(1))){ // compensate for the case in which the right pixel in a stereoPoint is missing (nan) + if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) bi(1) = 0; } b.segment(row) = bi; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index d46aef535..006ff7bdf 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -233,15 +233,16 @@ public: size_t m = cameras.size(); bool retriangulate = decideIfTriangulate(cameras); + // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras MonoCameras monoCameras; MonoMeasurements monoMeasured; for(size_t i = 0; i < m; i++) { - Pose3 leftPose = cameras[i].pose(); - Cal3_S2 monoCal = cameras[i].calibration().calibration(); - MonoCamera leftCamera_i(leftPose,monoCal); - Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); - Pose3 rightPose = leftPose.compose( left_Pose_right ); - MonoCamera rightCamera_i(rightPose,monoCal); + const Pose3 leftPose = cameras[i].pose(); + const Cal3_S2 monoCal = cameras[i].calibration().calibration(); + const MonoCamera leftCamera_i(leftPose,monoCal); + const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); + const Pose3 rightPose = leftPose.compose( left_Pose_right ); + const MonoCamera rightCamera_i(rightPose,monoCal); const StereoPoint2 zi = measured_[i]; monoCameras.push_back(leftCamera_i); monoMeasured.push_back(Point2(zi.uL(),zi.v())); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index c4fb32dd7..b17347edb 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 } /* ************************************************************************* */