diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 8fa29b231..d5c0ce79b 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -21,15 +21,28 @@ #include #include +#include +#include +#include +#include + +#include + using namespace std; using namespace boost::assign; namespace gtsam { +using symbol_shorthand::X; +using symbol_shorthand::L; + +typedef GenericProjectionFactor ProjectionFactor; +typedef PriorFactor Pose3Prior; + /* ************************************************************************* */ // See Hartley and Zisserman, 2nd Ed., page 312 -Point3 triangulateDLT(const vector& projection_matrices, - const vector& measurements, double rank_tol) { +Point3 triangulateDLT(const std::vector& poses, const vector& projection_matrices, + const vector& measurements, const Cal3_S2 &K, double rank_tol, bool optimize) { Matrix A = zeros(projection_matrices.size() *2, 4); @@ -51,7 +64,117 @@ Point3 triangulateDLT(const vector& projection_matrices, if(rank < 3) throw(TriangulationUnderconstrainedException()); - return Point3(sub( (v / v(3)),0,3)); + Point3 point = Point3(sub( (v / v(3)),0,3)); + if (optimize) { + NonlinearFactorGraph graph; + gtsam::Values::shared_ptr values(new gtsam::Values()); + static SharedNoiseModel noise(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6))); + int ij = 0; + BOOST_FOREACH(const Point2 &measurement, measurements) { + // Factor for pose i + ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), boost::make_shared(K)); + graph.push_back( boost::make_shared(*projectionFactor) ); + + // Prior on pose + graph.push_back(Pose3Prior(X(ij), poses[ij], prior_model)); + + // Initial pose values + values->insert( X(ij), poses[ij]); + + ij++; + } + + // Initial landmark value + values->insert(L(0), point); + + // Optimize + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + params.lambdaInitial = 1; + params.lambdaFactor = 10; + params.maxIterations = 100; + params.absoluteErrorTol = 1.0; + params.verbosityLM = LevenbergMarquardtParams::SILENT; + params.verbosity = NonlinearOptimizerParams::SILENT; + params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + LevenbergMarquardtOptimizer optimizer(graph, *values, params); + Values result = optimizer.optimize(); + + point = result.at(L(0)); + + } + return point; +} + +/* ************************************************************************* */ +// See Hartley and Zisserman, 2nd Ed., page 312 +Point3 triangulateDLT(const std::vector& poses, const vector& projection_matrices, + const vector& measurements, const vector &Ks, double rank_tol, bool optimize) { + + Matrix A = zeros(projection_matrices.size() *2, 4); + + for(size_t i=0; i< projection_matrices.size(); i++) { + size_t row = i*2; + const Matrix& projection = projection_matrices.at(i); + const Point2& p = measurements.at(i); + + // build system of equations + A.row(row) = p.x() * projection.row(2) - projection.row(0); + A.row(row+1) = p.y() * projection.row(2) - projection.row(1); + } + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + // std::cout << "s " << s.transpose() << std:endl; + + if(rank < 3) + throw(TriangulationUnderconstrainedException()); + + Point3 point = Point3(sub( (v / v(3)),0,3)); + if (optimize) { + NonlinearFactorGraph graph; + gtsam::Values::shared_ptr values(new gtsam::Values()); + static SharedNoiseModel noise(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6))); + int ij = 0; + BOOST_FOREACH(const Point2 &measurement, measurements) { + // Factor for pose i + ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), Ks[ij]); + graph.push_back( boost::make_shared(*projectionFactor) ); + + // Prior on pose + graph.push_back(Pose3Prior(X(ij), poses[ij], prior_model)); + + // Initial pose values + values->insert( X(ij), poses[ij]); + + ij++; + } + + // Initial landmark value + values->insert(L(0), point); + + // Optimize + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + params.lambdaInitial = 1; + params.lambdaFactor = 10; + params.maxIterations = 100; + params.absoluteErrorTol = 1.0; + params.verbosityLM = LevenbergMarquardtParams::SILENT; + params.verbosity = NonlinearOptimizerParams::SILENT; + params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + LevenbergMarquardtOptimizer optimizer(graph, *values, params); + Values result = optimizer.optimize(); + + point = result.at(L(0)); + } + + return point; } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 9210e1fd7..3be9c4d9e 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -47,8 +47,12 @@ public: } }; -Point3 triangulateDLT(const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); +Point3 triangulateDLT(const std::vector& poses, const std::vector& projection_matrices, + const std::vector& measurements, const Cal3_S2 &K, double rank_tol, bool optimize); + +Point3 triangulateDLT(const std::vector& poses, const std::vector& projection_matrices, + const std::vector& measurements, const std::vector &Ks, double rank_tol, bool optimize); + /** * Function to triangulate 3D landmark point from an arbitrary number @@ -63,7 +67,7 @@ Point3 triangulateDLT(const std::vector& projection_matrices, */ template GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const CALIBRATION& K, double rank_tol = 1e-9){ + const std::vector& measurements, const CALIBRATION& K, double rank_tol = 1e-9, bool optimize = false){ assert(poses.size() == measurements.size()); @@ -79,7 +83,7 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, // std::cout << "rank_tol i \n" << rank_tol << std::endl; } - Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); + Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, K, rank_tol, optimize); // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) { @@ -94,7 +98,8 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, /* ************************************************************************* */ template Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const std::vector >& Ks, double rank_tol) { + const std::vector& measurements, const std::vector >& Ks, double rank_tol, + bool optimize = false) { assert(poses.size() == measurements.size()); assert(poses.size() == Ks.size()); @@ -111,7 +116,7 @@ Point3 triangulatePoint3(const std::vector& poses, // std::cout << "2rank_tol i \n" << rank_tol << std::endl; } - Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); + Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, Ks, rank_tol, optimize); // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) {