12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Additionally templated JacobianFactorSVD on measurement type, and Jacobian_SVD unit tests now pass for SmartStereoFactor

release/4.3a0
cbeall3 2014-07-15 14:43:01 -04:00
parent d68e6b9add
commit 089ac4e743
6 changed files with 214 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/JacobianFactorSVD.h
View File

@ -11,12 +11,12 @@ namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D> template<size_t D, class Z>
class JacobianFactorSVD: public JacobianSchurFactor<D> { class JacobianFactorSVD: public JacobianSchurFactor<D> {
public: public:
typedef Eigen::Matrix<double, 2, D> Matrix2D; typedef Eigen::Matrix<double, Z::dimension, D> Matrix2D; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, Matrix2D> KeyMatrix2D; typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix; typedef std::pair<Key, Matrix> KeyMatrix;
@ -38,8 +38,8 @@ public:
/// Constructor /// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, const Matrix& Enull, const Vector& b, JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() { const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
size_t numKeys = Enull.rows() / 2; size_t numKeys = Enull.rows() / Z::Dim();
size_t j = 0, m2 = 2*numKeys-3; size_t j = 0, m2 = Z::Dim()*numKeys-3;
// PLAIN NULL SPACE TRICK // PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose(); // Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
@ -48,7 +48,7 @@ public:
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(numKeys); QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) 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); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
} }
}; };

2
gtsam/slam/SmartFactorBase.h
View File

@ -660,7 +660,7 @@ public:
Vector b; Vector b;
Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); 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: private:

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -444,7 +444,7 @@ public:
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorSVD<D> >(this->keys_); return boost::make_shared< JacobianFactorSVD<D, Point2> >(this->keys_);
} }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate

16
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -473,14 +473,14 @@ public:
// return boost::make_shared< JacobianFactorQ<D> >(this->keys_); // return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
// } // }
// //
// /// different (faster) way to compute Jacobian factor /// different (faster) way to compute Jacobian factor
// boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
// double lambda) const { double lambda) const {
// if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
// return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, point_, lambda);
// else else
// return boost::make_shared< JacobianFactorSVD<D> >(this->keys_); return boost::make_shared< JacobianFactorSVD<D, StereoPoint2> >(this->keys_);
// } }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values, bool computeCamerasAndTriangulate(const Values& values,

3
gtsam/slam/SmartStereoProjectionPoseFactor.h
View File

@ -175,8 +175,7 @@ public:
// depending on flag set on construction we may linearize to different linear factors // depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){ switch(linearizeTo_){
case JACOBIAN_SVD : case JACOBIAN_SVD :
throw("JacobianSVD not working yet!"); return this->createJacobianSVDFactor(cameras(values), 0.0);
// return this->createJacobianSVDFactor(cameras(values), 0.0);
break; break;
case JACOBIAN_Q : case JACOBIAN_Q :
throw("JacobianQ not working yet!"); throw("JacobianQ not working yet!");

397
gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -298,206 +298,205 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
} }
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
//
// std::vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
// vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
// vector<StereoPoint2> 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<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3)));
//}
///* *************************************************************************/ std::vector<Key> views;
//TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ views.push_back(x1);
// views.push_back(x2);
// double excludeLandmarksFutherThanDist = 2; views.push_back(x3);
//
// std::vector<Key> views; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// views.push_back(x1); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// views.push_back(x2); StereoCamera cam1(pose1, K);
// views.push_back(x3); // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) StereoCamera cam2(pose2, K);
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // create third camera 1 meter above the first camera
// StereoCamera cam1(pose1, K); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
// // create second camera 1 meter to the right of first camera StereoCamera cam3(pose3, K);
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// StereoCamera cam2(pose2, K); // three landmarks ~5 meters infront of camera
// // create third camera 1 meter above the first camera Point3 landmark1(5, 0.5, 1.2);
// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Point3 landmark2(5, -0.5, 1.2);
// StereoCamera cam3(pose3, K); Point3 landmark3(3, 0, 3.0);
//
// // three landmarks ~5 meters infront of camera // 1. Project three landmarks into three cameras and triangulate
// Point3 landmark1(5, 0.5, 1.2); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
// Point3 landmark2(5, -0.5, 1.2); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
// Point3 landmark3(3, 0, 3.0); vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
//
// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3; SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
// smartFactor1->add(measurements_cam1, views, model, K);
// // 1. Project three landmarks into three cameras and triangulate
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); smartFactor2->add(measurements_cam2, views, model, K);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K);
// smartFactor1->add(measurements_cam1, views, model, K);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
// smartFactor2->add(measurements_cam2, views, model, K); NonlinearFactorGraph graph;
// graph.push_back(smartFactor1);
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); graph.push_back(smartFactor2);
// smartFactor3->add(measurements_cam3, views, model, K); graph.push_back(smartFactor3);
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
//
// NonlinearFactorGraph graph; // 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
// graph.push_back(smartFactor1); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
// graph.push_back(smartFactor2); Values values;
// graph.push_back(smartFactor3); values.insert(x1, pose1);
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); values.insert(x2, pose2);
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); values.insert(x3, pose3*noise_pose);
//
// // 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 LevenbergMarquardtParams params;
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values result;
// Values values; LevenbergMarquardtOptimizer optimizer(graph, values, params);
// values.insert(x1, pose1); result = optimizer.optimize();
// values.insert(x2, pose2); EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// values.insert(x3, pose3*noise_pose); }
//
// // All factors are disabled and pose should remain where it is /* *************************************************************************/
// LevenbergMarquardtParams params; TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params); double excludeLandmarksFutherThanDist = 2;
// result = optimizer.optimize();
// EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3))); std::vector<Key> views;
//} views.push_back(x1);
// views.push_back(x2);
///* *************************************************************************/ views.push_back(x3);
//TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// double excludeLandmarksFutherThanDist = 1e10; Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error StereoCamera cam1(pose1, K);
// // create second camera 1 meter to the right of first camera
// std::vector<Key> views; Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// views.push_back(x1); StereoCamera cam2(pose2, K);
// views.push_back(x2); // create third camera 1 meter above the first camera
// views.push_back(x3); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
// StereoCamera cam3(pose3, K);
// // 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)); // three landmarks ~5 meters infront of camera
// StereoCamera cam1(pose1, K); Point3 landmark1(5, 0.5, 1.2);
// // create second camera 1 meter to the right of first camera Point3 landmark2(5, -0.5, 1.2);
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Point3 landmark3(3, 0, 3.0);
// StereoCamera cam2(pose2, K);
// // create third camera 1 meter above the first camera // 1. Project three landmarks into three cameras and triangulate
// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
// StereoCamera cam3(pose3, K); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
// vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
// // three landmarks ~5 meters infront of camera
// Point3 landmark1(5, 0.5, 1.2);
// Point3 landmark2(5, -0.5, 1.2); SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
// Point3 landmark3(3, 0, 3.0); smartFactor1->add(measurements_cam1, views, model, K);
// Point3 landmark4(5, -0.5, 1);
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; smartFactor2->add(measurements_cam2, views, model, K);
//
// // 1. Project three landmarks into three cameras and triangulate SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); smartFactor3->add(measurements_cam3, views, model, K);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
// measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier NonlinearFactorGraph graph;
// graph.push_back(smartFactor1);
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, graph.push_back(smartFactor2);
// JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); graph.push_back(smartFactor3);
// smartFactor1->add(measurements_cam1, views, model, K); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); // 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
// smartFactor2->add(measurements_cam2, views, model, K); 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;
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, values.insert(x1, pose1);
// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); values.insert(x2, pose2);
// smartFactor3->add(measurements_cam3, views, model, K); values.insert(x3, pose3*noise_pose);
//
// SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, // All factors are disabled and pose should remain where it is
// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); LevenbergMarquardtParams params;
// smartFactor4->add(measurements_cam4, views, model, K); Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); result = optimizer.optimize();
// EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
// NonlinearFactorGraph graph; }
// graph.push_back(smartFactor1);
// graph.push_back(smartFactor2); /* *************************************************************************/
// graph.push_back(smartFactor3); TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// graph.push_back(smartFactor4);
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); double excludeLandmarksFutherThanDist = 1e10;
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise std::vector<Key> views;
// Values values; views.push_back(x1);
// values.insert(x1, pose1); views.push_back(x2);
// values.insert(x2, pose2); views.push_back(x3);
// values.insert(x3, pose3);
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// // All factors are disabled and pose should remain where it is Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// LevenbergMarquardtParams params; StereoCamera cam1(pose1, K);
// Values result; // create second camera 1 meter to the right of first camera
// LevenbergMarquardtOptimizer optimizer(graph, values, params); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// result = optimizer.optimize(); StereoCamera cam2(pose2, K);
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
vector<StereoPoint2> 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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3)));
}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){