From 55ccc66de2785f12a9ef5c7cf1dc5a2d92152af3 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:43:35 -0400 Subject: [PATCH 1/3] added comment --- gtsam/geometry/CameraSet.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; From 9336c0b8c057e1828bc0274c7c402998cf8ec5c1 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:47:30 -0400 Subject: [PATCH 2/3] added comment and made variables const when possible --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 095094502..328256571 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())); From 938454916f539ba74641f2923b5f381e7e0c0ac9 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 20:10:31 -0400 Subject: [PATCH 3/3] improved test, and slightly loosened tolerance when using MKL --- .../tests/testSmartStereoProjectionPoseFactor.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 } /* ************************************************************************* */