diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 164bfa881..0a0508efc 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,7 +18,6 @@ #pragma once - #include #include #include @@ -114,7 +113,8 @@ std::pair triangulationGraph( const std::vector >& cameras, const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { - return triangulationGraph(cameras, measurements, landmarkKey, initialEstimate); + return triangulationGraph > // + (cameras, measurements, landmarkKey, initialEstimate); } /** @@ -143,8 +143,8 @@ Point3 triangulateNonlinear(const std::vector& poses, // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } @@ -163,8 +163,8 @@ Point3 triangulateNonlinear(const std::vector& cameras, // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(cameras, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } @@ -174,7 +174,8 @@ template Point3 triangulateNonlinear( const std::vector >& cameras, const std::vector& measurements, const Point3& initialEstimate) { - return triangulateNonlinear(cameras, measurements, initialEstimate); + return triangulateNonlinear > // + (cameras, measurements, initialEstimate); } /** @@ -203,21 +204,22 @@ Point3 triangulatePoint3(const std::vector& poses, std::vector projection_matrices; BOOST_FOREACH(const Pose3& pose, poses) { projection_matrices.push_back( - sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); + sharedCal->K() * (pose.inverse().matrix()).block<3, 4>(0, 0)); } // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(poses, sharedCal, measurements, point); + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -237,8 +239,7 @@ Point3 triangulatePoint3(const std::vector& poses, * @return Returns a Point3 */ template -Point3 triangulatePoint3( - const std::vector& cameras, +Point3 triangulatePoint3(const std::vector& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { @@ -251,21 +252,22 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration std::vector projection_matrices; BOOST_FOREACH(const CAMERA& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); - projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); - } + Matrix P_ = (camera.pose().inverse().matrix()); + projection_matrices.push_back( + camera.calibration().K() * (P_.block<3, 4>(0, 0))); + } Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const CAMERA& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -278,8 +280,8 @@ Point3 triangulatePoint3( const std::vector >& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { - return triangulatePoint3 >(cameras, measurements, - rank_tol, optimize); + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); } } // \namespace gtsam