diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 09654d3f1..3b4bdad17 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -31,7 +31,7 @@ namespace gtsam { Point3 triangulateDLT(const vector& projection_matrices, const vector& measurements, double rank_tol) { - Matrix A = Matrix_(projection_matrices.size() *2, 4); + Matrix A = zeros(projection_matrices.size() *2, 4); for(size_t i=0; i< projection_matrices.size(); i++) { size_t row = i*2; @@ -47,6 +47,7 @@ Point3 triangulateDLT(const vector& projection_matrices, Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); + if(rank < 3) throw(TriangulationUnderconstrainedException());