diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 0d3c04de1..8bed77d0f 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -11,12 +11,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template +template class JacobianFactorSVD: public JacobianSchurFactor { public: - typedef Eigen::Matrix Matrix2D; + typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; @@ -38,8 +38,8 @@ public: /// Constructor JacobianFactorSVD(const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - size_t numKeys = Enull.rows() / 2; - size_t j = 0, m2 = 2*numKeys-3; + size_t numKeys = Enull.rows() / Z::Dim(); + size_t j = 0, m2 = Z::Dim()*numKeys-3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) @@ -48,7 +48,7 @@ public: std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second)); + QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, Z::Dim() * j++, m2, Z::Dim()) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ae2dc8b0f..03bd92f21 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -660,7 +660,7 @@ public: Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, Z> >(Fblocks, Enull, b); } private: diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 04112e239..a08b79953 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -444,7 +444,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index c79ad1a89..71f5e11b3 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -473,14 +473,14 @@ public: // return boost::make_shared< JacobianFactorQ >(this->keys_); // } // -// /// different (faster) way to compute Jacobian factor -// boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, -// double lambda) const { -// if (triangulateForLinearize(cameras)) -// return Base::createJacobianSVDFactor(cameras, point_, lambda); -// else -// return boost::make_shared< JacobianFactorSVD >(this->keys_); -// } + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); + } /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 60992281e..9448f6498 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -175,8 +175,7 @@ public: // depending on flag set on construction we may linearize to different linear factors switch(linearizeTo_){ case JACOBIAN_SVD : - throw("JacobianSVD not working yet!"); -// return this->createJacobianSVDFactor(cameras(values), 0.0); + return this->createJacobianSVDFactor(cameras(values), 0.0); break; case JACOBIAN_Q : throw("JacobianQ not working yet!"); diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0b9f55d0b..d9b86e158 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -298,206 +298,205 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ } -///* *************************************************************************/ -//TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ -// -// std::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), gtsam::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); -// -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); -// smartFactor1->add(measurements_cam1, views, model, K); -// -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); -// smartFactor2->add(measurements_cam2, views, model, K); -// -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); -// smartFactor3->add(measurements_cam3, views, model, 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), gtsam::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), gtsam::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); -// -// LevenbergMarquardtParams params; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, params); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose3,result.at(x3))); -//} +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ -///* *************************************************************************/ -//TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ -// -// double excludeLandmarksFutherThanDist = 2; -// -// std::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), gtsam::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); -// -// vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // 1. Project three landmarks into three cameras and triangulate -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); -// smartFactor1->add(measurements_cam1, views, model, K); -// -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); -// smartFactor2->add(measurements_cam2, views, model, K); -// -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); -// smartFactor3->add(measurements_cam3, views, model, 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), gtsam::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), gtsam::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); -// -// // All factors are disabled and pose should remain where it is -// LevenbergMarquardtParams params; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, params); -// result = optimizer.optimize(); -// EXPECT(assert_equal(values.at(x3),result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ -// -// double excludeLandmarksFutherThanDist = 1e10; -// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error -// -// std::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), gtsam::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); -// Point3 landmark4(5, -0.5, 1); -// -// vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; -// -// // 1. Project three landmarks into three cameras and triangulate -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); -// measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier -// -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, -// JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); -// smartFactor1->add(measurements_cam1, views, model, K); -// -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, -// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); -// smartFactor2->add(measurements_cam2, views, model, K); -// -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, -// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); -// smartFactor3->add(measurements_cam3, views, model, K); -// -// SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, -// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); -// smartFactor4->add(measurements_cam4, views, model, 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(smartFactor4); -// graph.push_back(PriorFactor(x1, pose1, noisePrior)); -// graph.push_back(PriorFactor(x2, pose2, noisePrior)); -// -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// values.insert(x3, pose3); -// -// // All factors are disabled and pose should remain where it is -// LevenbergMarquardtParams params; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, params); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose3,result.at(x3))); -//} + std::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), gtsam::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); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, 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), gtsam::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), gtsam::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); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::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), gtsam::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); + + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, 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), gtsam::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), gtsam::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); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::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), gtsam::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); + Point3 landmark4(5, -0.5, 1); + + // 1. Project four 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); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + + + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, 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(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){