diff --git a/.cproject b/.cproject index 137f96e4d..ed9f7c34a 100644 --- a/.cproject +++ b/.cproject @@ -2862,6 +2862,14 @@ true true + + make + -j4 + testBatchFixedLagSmoother.run + true + true + true + make -j2 diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b322a40ab..18f311a23 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))){ // if it is a stereo point and the right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; } return b; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 93d2ff218..57a53daa3 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -35,6 +35,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 @@ -202,7 +212,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); @@ -210,9 +220,17 @@ public: Fs->at(i) = Fs->at(i) * J; } } + 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/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d34ba11e3..b015f1d7d 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 21b0c5eb7..56b5d85de 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 { @@ -119,8 +109,6 @@ enum DegeneracyMode { } }; - - /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with StereoCamera. This factor requires that values @@ -155,14 +143,19 @@ 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 */ 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()) { } @@ -240,75 +233,28 @@ 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])); + // 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++) { + 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())); + if(!std::isnan(zi.uR())){ // if right point is valid + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); } - - 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::FarPoint(); - 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::Outlier(); - 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 + } + if (retriangulate) + result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, + params_.triangulation); return result_; - } /// triangulate @@ -570,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_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 65134cb96..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 */ @@ -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) */ @@ -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 8051e238a..b17347edb 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)); @@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static double missing_uR = std::numeric_limits::quiet_NaN(); + vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -151,6 +151,60 @@ 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); + 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) @@ -248,8 +302,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 +325,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 +387,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(); @@ -344,7 +396,192 @@ 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 + + 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, 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 ) { @@ -411,6 +648,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 ) { @@ -562,7 +871,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()); @@ -1039,7 +1348,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { vector views; views.push_back(x1); @@ -1072,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; @@ -1082,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(), @@ -1098,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 } /* ************************************************************************* */