From 3c15ef5d1ed349058514b907542e59bb9289e338 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 16:11:07 -0400 Subject: [PATCH 01/15] great simplification in stereo triangulation: converting stereo into a set of monocular cameras, then proceed as in the monocular case --- .../slam/SmartStereoProjectionFactor.h | 91 +++++-------------- .../slam/SmartStereoProjectionPoseFactor.h | 2 +- .../testSmartStereoProjectionPoseFactor.cpp | 8 +- 3 files changed, 27 insertions(+), 74 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index bede012d8..9d9e65af6 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -155,6 +155,11 @@ public: /// Vector of cameras typedef CameraSet Cameras; + /// Vector of monocular cameras (stereo treated as 2 monocular) + typedef PinholeCamera MonoCamera; + typedef CameraSet MonoCameras; + typedef std::vector MonoMeasurements; + /** * Constructor * @param params internal parameters of the smart factors @@ -240,75 +245,25 @@ public: size_t m = cameras.size(); bool retriangulate = decideIfTriangulate(cameras); -// if(!retriangulate) -// std::cout << "retriangulate = false" << std::endl; -// -// bool retriangulate = true; - - if (retriangulate) { -// std::cout << "Retriangulate " << std::endl; - std::vector reprojections; - reprojections.reserve(m); - for(size_t i = 0; i < m; i++) { - reprojections.push_back(cameras[i].backproject(measured_[i])); - } - - Point3 pw_sum(0,0,0); - for(const Point3& pw: reprojections) { - pw_sum = pw_sum + pw; - } - // average reprojected landmark - Point3 pw_avg = pw_sum / double(m); - - double totalReprojError = 0; - - // check if it lies in front of all cameras - for(size_t i = 0; i < m; i++) { - const Pose3& pose = cameras[i].pose(); - const Point3& pl = pose.transform_to(pw_avg); - if (pl.z() <= 0) { - result_ = TriangulationResult::BehindCamera(); - return result_; - } - - // check landmark distance - if (params_.triangulation.landmarkDistanceThreshold > 0 && - pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - result_ = TriangulationResult::Degenerate(); - return result_; - } - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { - const StereoPoint2& zi = measured_[i]; - StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); - totalReprojError += reprojectionError.vector().norm(); - } - } // for - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { - result_ = TriangulationResult::Degenerate(); - return result_; - } - - if(params_.triangulation.enableEPI) { - try { - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); - } catch(StereoCheiralityException& e) { - if(params_.verboseCheirality) - std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; - if(params_.throwCheirality) - throw; - result_ = TriangulationResult::BehindCamera(); - return TriangulationResult::BehindCamera(); - } - } - - result_ = TriangulationResult(pw_avg); - - } // if retriangulate + 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 StereoPoint2 zi = measured_[i]; + monoCameras.push_back(leftCamera_i); + monoMeasured.push_back(Point2(zi.uL(),zi.v())); + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); + } + if (retriangulate) + result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, + params_.triangulation); return result_; - } /// triangulate diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 65134cb96..8d3fd84f5 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -102,7 +102,7 @@ public: /** * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param K the (known) camera calibration (same for all measurements) */ diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 5bad0e171..c58a81dae 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -248,8 +248,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -273,7 +271,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error // get triangulated landmarks from smart factors Point3 landmark1_smart = *smartFactor1->point(); @@ -335,7 +333,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); Values result2 = optimizer2.optimize(); @@ -562,7 +560,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); From f2bec78a58281ee52b9c58f03343cac048995179 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 18:27:20 -0400 Subject: [PATCH 02/15] first implementation of smartStereo with possibly left-only pixel measurements --- gtsam/slam/SmartFactorBase.h | 22 +++++++++++++++++++ .../slam/SmartStereoProjectionFactor.h | 6 +++-- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 93d2ff218..fad49a339 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -210,6 +211,27 @@ public: Fs->at(i) = Fs->at(i) * J; } } + + static const int Np = FixedDimension::value; // 2 (Unit3) or 3 (Point3) + + // when using stereo cameras, some of the measurements might be missing: + for(size_t i=0; i < Fs->size(); i++){ + if(ZDim == 3){ // it's a stereo point .. + Z z3 = measured_.at(i); + if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid + // delete influence of right point on jacobian Fs + std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl; + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iiblock<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); + // set to zero entry from vector ue + ue(ZDim * i + 1) = 0.0; // this should not matter anyway + } + } + } return ue; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 9d9e65af6..e2b35ad82 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -257,8 +257,10 @@ public: const StereoPoint2 zi = measured_[i]; monoCameras.push_back(leftCamera_i); monoMeasured.push_back(Point2(zi.uL(),zi.v())); - monoCameras.push_back(rightCamera_i); - monoMeasured.push_back(Point2(zi.uR(),zi.v())); + if(!isnan(zi.uR())){ // if right point is valid + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); + } } if (retriangulate) result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, From cd9b4cd5ab3cc15c654904e71e269b65878cd49a Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 18:31:55 -0400 Subject: [PATCH 03/15] moved common definitions to base class --- gtsam/slam/SmartFactorBase.h | 10 ++++++++++ gtsam/slam/SmartProjectionFactor.h | 10 ---------- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 14 ++------------ .../tests/testSmartStereoProjectionPoseFactor.cpp | 4 +--- 4 files changed, 13 insertions(+), 25 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index fad49a339..160395997 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -36,6 +36,16 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + /** * @brief Base class for smart factors * This base class has no internal point, but it has a measurement, noise model diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 64b300b86..b1e2870dd 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -31,16 +31,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - /* * Parameters for the smart projection factors */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e2b35ad82..778829765 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -35,17 +35,7 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - - /* + /* * Parameters for the smart stereo projection factors */ struct GTSAM_EXPORT SmartStereoProjectionParams { @@ -257,7 +247,7 @@ public: const StereoPoint2 zi = measured_[i]; monoCameras.push_back(leftCamera_i); monoMeasured.push_back(Point2(zi.uL(),zi.v())); - if(!isnan(zi.uR())){ // if right point is valid + if(!std::isnan(zi.uR())){ // if right point is valid monoCameras.push_back(rightCamera_i); monoMeasured.push_back(Point2(zi.uR(),zi.v())); } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index c58a81dae..3acfb85de 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -// TODO #include +#include #include #include #include @@ -33,8 +33,6 @@ using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); From b90e224f5908faa950c49b3c47e9434b3ed06502 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 18:56:04 -0400 Subject: [PATCH 04/15] added tests for error and triangulation --- gtsam/geometry/CameraSet.h | 6 +- .../testSmartStereoProjectionPoseFactor.cpp | 55 +++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b322a40ab..bb149636b 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -56,7 +56,11 @@ protected: // Project and fill error vector Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - b.segment(row) = traits::Local(measured[i], predicted[i]); + 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) + bi(1) = 0; + } + b.segment(row) = bi; } return b; } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 3acfb85de..ef2f80c31 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -149,6 +149,61 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { //EXPECT(assert_equal(zero(4),actual,1e-8)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters in front of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + double missing_uR = std::numeric_limits::quiet_NaN(); + StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right_missing, x2, K2); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing: + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + CameraSet cams; + cams += level_camera; + cams += level_camera_right; + TriangulationResult result = factor1.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); + + // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing: + SmartStereoProjectionPoseFactor factor2(model); + StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v()); + factor2.add(level_uv_missing, x1, K2); + factor2.add(level_uv_right_missing, x2, K2); + result = factor2.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) From 6c163b0a4d6a516bfd7f8ca385ea4b0ab6379d2e Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 19:07:00 -0400 Subject: [PATCH 05/15] added test which optimize smartStereoFactor with missing measurements (uR) --- gtsam/slam/SmartFactorBase.h | 1 - .../testSmartStereoProjectionPoseFactor.cpp | 75 ++++++++++++++++++- 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 160395997..eacca44b5 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -230,7 +230,6 @@ public: Z z3 = measured_.at(i); if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid // delete influence of right point on jacobian Fs - std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl; MatrixZD& Fi = Fs->at(i); for(size_t ii=0; ii::quiet_NaN(); + vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -167,7 +169,6 @@ TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { // 1. Project two landmarks into two cameras and triangulate StereoPoint2 level_uv = level_camera.project(landmark); StereoPoint2 level_uv_right = level_camera_right.project(landmark); - double missing_uR = std::numeric_limits::quiet_NaN(); StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); Values values; @@ -462,6 +463,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { EXPECT(assert_equal(pose3, result.at(x3))); } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // DELETE SOME MEASUREMENTS + StereoPoint2 sp = measurements_cam1[1]; + measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + sp = measurements_cam2[2]; + measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3),1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { From a5138bfb463251b1ee42d1f768bd18a726d4fc68 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 25 Jul 2016 22:13:25 -0400 Subject: [PATCH 06/15] included body_P_sensor in smartStereoProjectionPoseFactor! --- .../slam/SmartStereoProjectionFactor.h | 8 +- .../slam/SmartStereoProjectionPoseFactor.h | 12 ++- .../testSmartStereoProjectionPoseFactor.cpp | 89 +++++++++++++++++++ 3 files changed, 100 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 778829765..095094502 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -109,8 +109,6 @@ namespace gtsam { } }; - - /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with StereoCamera. This factor requires that values @@ -155,9 +153,9 @@ public: * @param params internal parameters of the smart factors */ SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel), // + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, body_P_sensor), // params_(params), // result_(TriangulationResult::Degenerate()) { } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 8d3fd84f5..3b4c5e0db 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -66,9 +66,9 @@ public: * @param params internal parameters of the smart factors */ SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel, params) { + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, params, body_P_sensor) { } /** Virtual destructor */ @@ -161,7 +161,11 @@ public: Base::Cameras cameras; size_t i=0; for(const Key& k: this->keys_) { - const Pose3& pose = values.at(k); + Pose3 pose = values.at(k); + + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4401f241c..098a7a95c 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -396,7 +396,96 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { + // camera has some displacement + Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1.compose(body_P_sensor), K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2.compose(body_P_sensor), K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3.compose(body_P_sensor), K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_l1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor1->add(measurements_l1, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor2->add(measurements_l2, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor3->add(measurements_l3, views, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error + + // get triangulated landmarks from smart factors + Point3 landmark1_smart = *smartFactor1->point(); + Point3 landmark2_smart = *smartFactor2->point(); + Point3 landmark3_smart = *smartFactor3->point(); + + Values result; + gttic_(SmartStereoProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionPoseFactor); + tictoc_finishedIteration_(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + EXPECT(assert_equal(pose3, result.at(x3))); +} /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { From 50d6532fe130869082f0421de6a10b5d3bb7d9ee Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 27 Jul 2016 23:48:58 -0400 Subject: [PATCH 07/15] the ultimate test: smartStereoFactors generalize smartFactors in that they work in the purely monocular case! --- .../testSmartStereoProjectionPoseFactor.cpp | 101 ++++++++++++++++++ 1 file changed, 101 insertions(+) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 098a7a95c..015af822f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -487,6 +487,107 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + + SimpleCamera cam1(cameraPose1, *K); // with camera poses + SimpleCamera cam2(cameraPose2, *K); + SimpleCamera cam3(cameraPose3, *K); + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN) + vector measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo; + for(size_t k=0; k(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} +/* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { vector views; From bb32cadfb0896a6019651aea737bc8a6038f9263 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 28 Jul 2016 17:20:34 -0400 Subject: [PATCH 08/15] minor fix --- gtsam/slam/SmartFactorBase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index eacca44b5..c3c3c8c30 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -228,7 +228,7 @@ public: for(size_t i=0; i < Fs->size(); i++){ if(ZDim == 3){ // it's a stereo point .. Z z3 = measured_.at(i); - if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid + if(std::isnan(z3.vector()[1])){ // .. and the right pixel is invalid // delete influence of right point on jacobian Fs MatrixZD& Fi = Fs->at(i); for(size_t ii=0; ii Date: Fri, 29 Jul 2016 12:14:09 -0400 Subject: [PATCH 09/15] fixed bug with smart measurement calling .vector(): Point2 does not admit .vector() for some cmake configuration. Using using casting: more elegant now! --- gtsam/slam/SmartFactorBase.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index c3c3c8c30..7bb3e3322 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -226,19 +226,19 @@ public: // when using stereo cameras, some of the measurements might be missing: for(size_t i=0; i < Fs->size(); i++){ - if(ZDim == 3){ // it's a stereo point .. - Z z3 = measured_.at(i); - if(std::isnan(z3.vector()[1])){ // .. and the right pixel is invalid - // delete influence of right point on jacobian Fs - MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; ii(&measured_.at(i)); + if(z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid + { + // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iiblock<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); - // set to zero entry from vector ue - ue(ZDim * i + 1) = 0.0; // this should not matter anyway - } + // delete influence of right point on jacobian E + E->block<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); + + // set to zero entry from vector ue + ue(ZDim * i + 1) = 0.0; } } return ue; From 20c13580cc734ef0b756740157aa4ce063d7c164 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 29 Jul 2016 16:34:18 -0400 Subject: [PATCH 10/15] deleted unused variables in unit test --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 015af822f..c872e791c 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -469,11 +469,6 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error - // get triangulated landmarks from smart factors - Point3 landmark1_smart = *smartFactor1->point(); - Point3 landmark2_smart = *smartFactor2->point(); - Point3 landmark3_smart = *smartFactor3->point(); - Values result; gttic_(SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); From c9e43e7435bf5def8bfc633e0fe17b3072eafe62 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 29 Jul 2016 16:40:30 -0400 Subject: [PATCH 11/15] bug fix: added check on whether to modify Jacobians, depending on whether they are boost::none or not --- gtsam/slam/SmartFactorBase.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 7bb3e3322..1d33d16a8 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -213,7 +213,7 @@ public: boost::optional Fs = boost::none, // boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); - if(body_P_sensor_){ + if(body_P_sensor_ && Fs){ for(size_t i=0; i < Fs->size(); i++){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); @@ -225,17 +225,17 @@ public: static const int Np = FixedDimension::value; // 2 (Unit3) or 3 (Point3) // when using stereo cameras, some of the measurements might be missing: - for(size_t i=0; i < Fs->size(); i++){ + for(size_t i=0; i < cameras.size(); i++){ const StereoPoint2 * z3 = reinterpret_cast(&measured_.at(i)); - if(z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid + if(ZDim==3 && z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid { - // delete influence of right point on jacobian Fs - MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iiblock<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); + if(Fs){ // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iiblock<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); // set to zero entry from vector ue ue(ZDim * i + 1) = 0.0; From 55ccc66de2785f12a9ef5c7cf1dc5a2d92152af3 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:43:35 -0400 Subject: [PATCH 12/15] 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 13/15] 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 14/15] 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 } /* ************************************************************************* */ From e0869719fa5b3ce867c2cfad1a5ec040ea18beb7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 2 Aug 2016 22:56:48 -0400 Subject: [PATCH 15/15] now SmartFactorBase doesn't know about stereoPoint2 (removed also header). The functionality to check if the right pixel is missing has been moved to SmartStereoProjectionFactor, removing the casting. --- gtsam/slam/SmartFactorBase.h | 29 +++++-------------- .../slam/SmartStereoProjectionFactor.h | 26 +++++++++++++++++ 2 files changed, 34 insertions(+), 21 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1d33d16a8..57a53daa3 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -27,7 +27,6 @@ #include #include #include -#include #include #include @@ -221,29 +220,17 @@ public: Fs->at(i) = Fs->at(i) * J; } } - - static const int Np = FixedDimension::value; // 2 (Unit3) or 3 (Point3) - - // when using stereo cameras, some of the measurements might be missing: - for(size_t i=0; i < cameras.size(); i++){ - const StereoPoint2 * z3 = reinterpret_cast(&measured_.at(i)); - if(ZDim==3 && z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid - { - if(Fs){ // delete influence of right point on jacobian Fs - MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iiblock<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np); - - // set to zero entry from vector ue - ue(ZDim * i + 1) = 0.0; - } - } + correctForMissingMeasurements(cameras, ue, Fs, E); return ue; } + /** + * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) + * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} + /** * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Noise model applied diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 006ff7bdf..56b5d85de 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -516,6 +516,32 @@ public: } } + /** + * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const + { + // when using stereo cameras, some of the measurements might be missing: + for(size_t i=0; i < cameras.size(); i++){ + const StereoPoint2& z = measured_.at(i); + if(std::isnan(z.uR())) // if the right pixel is invalid + { + if(Fs){ // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); + + // set the corresponding entry of vector ue to zero + ue(ZDim * i + 1) = 0.0; + } + } + } + /** return the landmark */ TriangulationResult point() const { return result_;