From e98c80aad0e4967cdc3d3330323f7c38d5addf4e Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 22 Oct 2013 15:31:46 +0000 Subject: [PATCH] removed nontemplated triangulation functions. Improved unit tests and documentation --- .../geometry/tests/testTriangulation.cpp | 3 + gtsam_unstable/geometry/triangulation.cpp | 113 ------------------ gtsam_unstable/geometry/triangulation.h | 45 ++++--- 3 files changed, 33 insertions(+), 128 deletions(-) delete mode 100644 gtsam_unstable/geometry/triangulation.cpp diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index fbbc62991..9075803ea 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -79,6 +79,9 @@ TEST( triangulation, twoPoses) { boost::optional triangulated_3cameras = triangulatePoint3(poses, measurements, K); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + // Again with nonlinear optimization + boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, measurements, K, 1e-9, true); + EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1)); diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp deleted file mode 100644 index eb0c07009..000000000 --- a/gtsam_unstable/geometry/triangulation.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file triangulation.cpp - * @brief Functions for triangulation - * @author Chris Beall - */ - -#include -#include -#include -#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 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); - - 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), 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; -} - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index cb2bee2c4..f190cc30e 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -54,9 +54,6 @@ public: } }; -Point3 triangulateDLT(const std::vector& poses, const std::vector& projection_matrices, - const std::vector& measurements, const Cal3_S2 &K, double rank_tol, bool optimize); - /* ************************************************************************* */ // See Hartley and Zisserman, 2nd Ed., page 312 template @@ -84,9 +81,10 @@ Point3 triangulateDLT(const std::vector& poses, const std::vector throw(TriangulationUnderconstrainedException()); Point3 point = Point3(sub( (v / v(3)),0,3)); + if (optimize) { NonlinearFactorGraph graph; - gtsam::Values::shared_ptr values(new gtsam::Values()); + gtsam::Values 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; @@ -99,20 +97,18 @@ Point3 triangulateDLT(const std::vector& poses, const std::vector Key poseKey = Symbol('x',ij); boost::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise, poseKey, landmarkKey, Ks[ij])); graph.push_back(projectionFactor); - //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(poseKey, poses[ij], prior_model)); // Initial pose values - values->insert( poseKey, poses[ij]); + values.insert( poseKey, poses[ij]); ij++; } // Initial landmark value - values->insert(landmarkKey, point); + values.insert(landmarkKey, point); // Optimize LevenbergMarquardtParams params; @@ -125,7 +121,7 @@ Point3 triangulateDLT(const std::vector& poses, const std::vector params.verbosityLM = LevenbergMarquardtParams::SILENT; params.verbosity = NonlinearOptimizerParams::SILENT; params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; - LevenbergMarquardtOptimizer optimizer(graph, *values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); point = result.at(landmarkKey); @@ -142,13 +138,15 @@ Point3 triangulateDLT(const std::vector& poses, const std::vector * to verify the quality of the triangulation. * @param poses A vector of camera poses * @param measurements A vector of camera measurements - * @param K The camera calibration + * @param K The camera calibration (Same for all cameras involved) * @param rank tolerance, default 1e-9 + * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 on success, boost::none otherwise. */ template GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const CALIBRATION& K, double rank_tol = 1e-9, bool optimize = false){ + const std::vector& measurements, const CALIBRATION& K, + double rank_tol = 1e-9, bool optimize = false) { assert(poses.size() == measurements.size()); @@ -164,7 +162,11 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, // std::cout << "rank_tol i \n" << rank_tol << std::endl; } - Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, K, rank_tol, optimize); + // create vector with shared pointer to calibration (all the same in this case) + boost::shared_ptr sharedK = boost::make_shared(K); + std::vector > Ks(poses.size(), sharedK); + + 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) { @@ -176,11 +178,24 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, return triangulated_point; } -/* ************************************************************************* */ +/** + * Function to triangulate 3D landmark point from an arbitrary number + * of poses (at least 2) using the DLT. This function is similar to the one + * above, except that each camera has its own calibration. The function + * checks that the resulting point lies in front of all cameras, but has + * no other checks to verify the quality of the triangulation. + * @param poses A vector of camera poses + * @param measurements A vector of camera measurements + * @param Ks Vector of camera calibrations + * @param rank tolerance, default 1e-9 + * @param optimize Flag to turn on nonlinear refinement of triangulation + * @return Returns a Point3 on success, boost::none otherwise. + */ template Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const std::vector >& Ks, double rank_tol, - bool optimize = false) { + const std::vector& measurements, + const std::vector >& Ks, + double rank_tol = 1e-9, bool optimize = false) { assert(poses.size() == measurements.size()); assert(poses.size() == Ks.size());