diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index ce0db7cbc..2d31e1b45 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -22,8 +22,12 @@ #include #include #include +#include +#include +#include #include + namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 @@ -58,6 +62,39 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, const std::vector& measurements, const Cal3_S2& K, double rank_tol = 1e-9); +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol); + +/* ************************************************************************* */ +template +Point3 triangulatePoint3(const std::vector& poses, + const std::vector& measurements, const std::vector >& Ks, double rank_tol) { + + assert(poses.size() == measurements.size()); + assert(poses.size() == Ks.size()); + + if(poses.size() < 2) + throw(TriangulationUnderconstrainedException()); + + std::vector projection_matrices; + + // construct projection matrices from poses & calibration + for(size_t i = 0; imatrix() * sub(poses.at(i).inverse().matrix(),0,3,0,4) ); + } + + Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // verify that the triangulated point lies infront of all cameras + BOOST_FOREACH(const Pose3& pose, poses) { + const Point3& p_local = pose.transform_to(triangulated_point); + if(p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } + + return triangulated_point; +} + } // \namespace gtsam