diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 118673831..9d7cf72a2 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,19 +16,12 @@ * Author: cbeall3 */ -#include - -#include -#include -#include -#include - -#include #include +#include +#include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 559871059..3017fdf7a 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -18,6 +18,9 @@ #include +#include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 0cb9d4838..49998d117 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,19 +18,11 @@ #pragma once -#include -#include -#include -#include -#include -#include #include -#include +#include +#include #include - -#include -#include -#include +#include #include @@ -60,7 +52,9 @@ public: * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); +GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol); // Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem // We should have a projectionfactor that knows pose is fixed @@ -135,7 +129,8 @@ std::pair triangulationGraph( * @param landmarkKey to refer to landmark * @return refined Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); +GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, + const Values& values, Key landmarkKey); /** * Given an initial estimate , refine a point using measurements in several cameras @@ -221,7 +216,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -256,9 +251,9 @@ Point3 triangulatePoint3( typedef PinholeCamera Camera; std::vector projection_matrices; BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); + projection_matrices.push_back( + camera.calibration().K() + * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -271,7 +266,7 @@ Point3 triangulatePoint3( 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