From 4c7f0eba004eaf6e527d08096745ba7eea6fb62b Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 23 Feb 2015 12:41:14 +0100 Subject: [PATCH] Added some templates with whole cameras --- gtsam/geometry/triangulation.h | 58 ++++++++++++++++++++++---------- gtsam/slam/TriangulationFactor.h | 20 +++++------ 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ce83f780b..164bfa881 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -54,7 +54,6 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); -/// /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -76,8 +75,8 @@ 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]; - PinholeCamera camera_i(pose_i, *sharedCal); - graph.push_back(TriangulationFactor // + PinholePose camera_i(pose_i, sharedCal); + graph.push_back(TriangulationFactor > // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -92,25 +91,32 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector >& cameras, - const std::vector& measurements, Key landmarkKey, - const Point3& initialEstimate) { + const std::vector& cameras, const std::vector& measurements, + Key landmarkKey, const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); 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]; - graph.push_back(TriangulationFactor // + const CAMERA& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } -/// +/// PinholeCamera specific version +template +std::pair triangulationGraph( + const std::vector >& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + return triangulationGraph(cameras, measurements, landmarkKey, initialEstimate); +} + /** * Optimize for triangulation * @param graph nonlinear factors for projection @@ -150,9 +156,8 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template -Point3 triangulateNonlinear( - const std::vector >& cameras, +template +Point3 triangulateNonlinear(const std::vector& cameras, const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values @@ -164,6 +169,14 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } +/// PinholeCamera specific version +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + return triangulateNonlinear(cameras, measurements, initialEstimate); +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -223,9 +236,9 @@ Point3 triangulatePoint3(const std::vector& poses, * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template +template Point3 triangulatePoint3( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { @@ -236,9 +249,8 @@ Point3 triangulatePoint3( throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - typedef PinholeCamera Camera; std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) { + 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)) ); } @@ -250,7 +262,7 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Camera& camera, cameras) { + BOOST_FOREACH(const CAMERA& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); @@ -260,5 +272,15 @@ Point3 triangulatePoint3( return point; } +/// Pinhole-specific version +template +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); +} + } // \namespace gtsam diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index b7f6a20dc..895d00f4c 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -27,16 +27,22 @@ namespace gtsam { * The calibration and pose are assumed known. * @addtogroup SLAM */ -template +template > class TriangulationFactor: public NoiseModelFactor1 { public: /// Camera type - typedef PinholeCamera Camera; + typedef CAMERA Camera; protected: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + // Keep a copy of measurement and calibration for I/O const Camera camera_; ///< Camera in which this landmark was seen const Point2 measured_; ///< 2D measurement @@ -47,12 +53,6 @@ protected: public: - /// shorthand for base class type - typedef NoiseModelFactor1 Base; - - /// shorthand for this class - typedef TriangulationFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -114,7 +114,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); + Point2 error(camera_.project2(point, boost::none, H2) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -154,7 +154,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); + b = -(camera_.project2(point, boost::none, A) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b);