diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index f2c957835..8d45311f1 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,7 +16,6 @@ * Author: cbeall3 */ -#include #include #include #include diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 49998d117..f767514c1 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,10 +18,10 @@ #pragma once -#include +#include +#include #include #include -#include #include #include @@ -56,9 +56,6 @@ 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 - /// /** * Create a factor graph with projection factors from poses and one calibration @@ -81,10 +78,9 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; - graph.push_back(GenericProjectionFactor // - (measurements[i], unit2, i, landmarkKey, sharedCal)); - graph.push_back(PriorFactor(i, pose_i, prior_model)); - values.insert(i, pose_i); + PinholeCamera camera_i(pose_i, *sharedCal); + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } @@ -110,13 +106,8 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const PinholeCamera& camera_i = cameras[i]; - boost::shared_ptr // Seems wasteful to create new object - sharedCal(new CALIBRATION(camera_i.calibration())); - graph.push_back(GenericProjectionFactor // - (measurements[i], unit2, i, landmarkKey, sharedCal)); - const Pose3& pose_i = camera_i.pose(); - graph.push_back(PriorFactor(i, pose_i, prior_model)); - values.insert(i, pose_i); + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } @@ -251,9 +242,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);