removed nontemplated triangulation functions. Improved unit tests and documentation

gtsam_unstable/geometry/tests/testTriangulation.cpp

@@ -79,6 +79,9 @@ TEST( triangulation, twoPoses) {
   boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, K);
   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
 
+  // Again with nonlinear optimization
+  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, measurements, K, 1e-9, true);
+  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1));

gtsam_unstable/geometry/triangulation.cpp

@@ -1,113 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file triangulation.cpp
- * @brief Functions for triangulation
- * @author Chris Beall
- */
-
-#include <gtsam_unstable/geometry/triangulation.h>
-#include <gtsam/geometry/SimpleCamera.h>
-#include <boost/foreach.hpp>
-#include <boost/assign.hpp>
-#include <boost/assign/std/vector.hpp>
-
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-
-#include <gtsam/slam/ProjectionFactor.h>
-
-using namespace std;
-using namespace boost::assign;
-
-namespace gtsam {
-
-using symbol_shorthand::X;
-using symbol_shorthand::L;
-
-typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor;
-typedef PriorFactor<Pose3> Pose3Prior;
-
-/* ************************************************************************* */
-// See Hartley and Zisserman, 2nd Ed., page 312
-Point3 triangulateDLT(const std::vector<Pose3>& poses, const vector<Matrix>& projection_matrices,
-    const vector<Point2>& measurements, const Cal3_S2 &K, double rank_tol, bool optimize) {
-
-  Matrix A = zeros(projection_matrices.size() *2, 4);
-
-  for(size_t i=0; i< projection_matrices.size(); i++) {
-    size_t row = i*2;
-    const Matrix& projection = projection_matrices.at(i);
-    const Point2& p = measurements.at(i);
-
-    // build system of equations
-    A.row(row) = p.x() * projection.row(2) - projection.row(0);
-    A.row(row+1) = p.y() * projection.row(2) - projection.row(1);
-  }
-  int rank;
-  double error;
-  Vector v;
-  boost::tie(rank, error, v) = DLT(A, rank_tol);
-  //  std::cout << "s " << s.transpose() << std:endl;
-
-  if(rank < 3)
-    throw(TriangulationUnderconstrainedException());
-
-  Point3 point = Point3(sub( (v / v(3)),0,3));
-  if (optimize) {
-      NonlinearFactorGraph graph;
-      gtsam::Values::shared_ptr values(new gtsam::Values());
-      static SharedNoiseModel noise(noiseModel::Unit::Create(2));
-      static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6)));
-      int ij = 0;
-      BOOST_FOREACH(const Point2 &measurement, measurements) {
-          // Factor for pose i
-          ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), boost::make_shared<Cal3_S2>(K));
-          graph.push_back( boost::make_shared<ProjectionFactor>(*projectionFactor) );
-
-          // Prior on pose
-          graph.push_back(Pose3Prior(X(ij), poses[ij], prior_model));
-
-          // Initial pose values
-          values->insert( X(ij), poses[ij]);
-
-          ij++;
-      }
-
-      // Initial landmark value
-      values->insert(L(0), point);
-
-      // Optimize
-      LevenbergMarquardtParams params;
-      params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
-      params.verbosity = NonlinearOptimizerParams::ERROR;
-      params.lambdaInitial = 1;
-      params.lambdaFactor = 10;
-      params.maxIterations = 100;
-      params.absoluteErrorTol = 1.0;
-      params.verbosityLM = LevenbergMarquardtParams::SILENT;
-      params.verbosity = NonlinearOptimizerParams::SILENT;
-      params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
-      LevenbergMarquardtOptimizer optimizer(graph, *values, params);
-      Values result = optimizer.optimize();
-
-      point = result.at<Point3>(L(0));
-
-  }
-  return point;
-}
-
-/* ************************************************************************* */
-
-} // namespace gtsam

gtsam_unstable/geometry/triangulation.h

@@ -54,9 +54,6 @@ public:
   }
 };
 
-Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>& projection_matrices,
-    const std::vector<Point2>& measurements, const Cal3_S2 &K, double rank_tol, bool optimize);
-
 /* ************************************************************************* */
 // See Hartley and Zisserman, 2nd Ed., page 312
 template<class CALIBRATION>
@@ -84,9 +81,10 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>
     throw(TriangulationUnderconstrainedException());
 
   Point3 point = Point3(sub( (v / v(3)),0,3));
+
   if (optimize) {
     NonlinearFactorGraph graph;
-    gtsam::Values::shared_ptr values(new gtsam::Values());
+    gtsam::Values values;
     static SharedNoiseModel noise(noiseModel::Unit::Create(2));
     static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6)));
     int ij = 0;
@@ -99,20 +97,18 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>
       Key poseKey = Symbol('x',ij);
       boost::shared_ptr<ProjectionFactor> projectionFactor(new ProjectionFactor(measurement, noise, poseKey, landmarkKey, Ks[ij]));
       graph.push_back(projectionFactor);
-      //ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), Ks[ij]);
-      //graph.push_back( boost::make_shared<ProjectionFactor>(*projectionFactor) );
 
       // Prior on pose
       graph.push_back(Pose3Prior(poseKey, poses[ij], prior_model));
 
       // Initial pose values
-      values->insert( poseKey, poses[ij]);
+      values.insert( poseKey, poses[ij]);
 
       ij++;
     }
 
     // Initial landmark value
-    values->insert(landmarkKey, point);
+    values.insert(landmarkKey, point);
 
     // Optimize
     LevenbergMarquardtParams params;
@@ -125,7 +121,7 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>
     params.verbosityLM = LevenbergMarquardtParams::SILENT;
     params.verbosity = NonlinearOptimizerParams::SILENT;
     params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
-    LevenbergMarquardtOptimizer optimizer(graph, *values, params);
+    LevenbergMarquardtOptimizer optimizer(graph, values, params);
     Values result = optimizer.optimize();
 
     point = result.at<Point3>(landmarkKey);
@@ -142,13 +138,15 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>
  * to verify the quality of the triangulation.
  * @param poses A vector of camera poses
  * @param measurements A vector of camera measurements
- * @param K The camera calibration
+ * @param K The camera calibration (Same for all cameras involved)
  * @param rank tolerance, default 1e-9
+ * @param optimize Flag to turn on nonlinear refinement of triangulation
  * @return Returns a Point3 on success, boost::none otherwise.
  */
 template<class CALIBRATION>
 GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
-    const std::vector<Point2>& measurements, const CALIBRATION& K, double rank_tol =  1e-9, bool optimize = false){
+    const std::vector<Point2>& measurements, const CALIBRATION& K,
+    double rank_tol = 1e-9, bool optimize = false) {
 
   assert(poses.size() == measurements.size());
 
@@ -164,7 +162,11 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     // std::cout << "rank_tol i \n" << rank_tol << std::endl;
   }
 
-  Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, K, rank_tol, optimize);
+  // create vector with shared pointer to calibration (all the same in this case)
+  boost::shared_ptr<CALIBRATION> sharedK = boost::make_shared<CALIBRATION>(K);
+  std::vector<boost::shared_ptr<CALIBRATION> > Ks(poses.size(), sharedK);
+
+  Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, Ks, rank_tol, optimize);
 
   // verify that the triangulated point lies infront of all cameras
   BOOST_FOREACH(const Pose3& pose, poses) {
@@ -176,11 +178,24 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   return triangulated_point;
 }
 
-/* ************************************************************************* */
+/**
+ * Function to triangulate 3D landmark point from an arbitrary number
+ * of poses (at least 2) using the DLT. This function is similar to the one
+ * above, except that each camera has its own calibration. The function
+ * checks that the resulting point lies in front of all cameras, but has
+ * no other checks to verify the quality of the triangulation.
+ * @param poses A vector of camera poses
+ * @param measurements A vector of camera measurements
+ * @param Ks Vector of camera calibrations
+ * @param rank tolerance, default 1e-9
+ * @param optimize Flag to turn on nonlinear refinement of triangulation
+ * @return Returns a Point3 on success, boost::none otherwise.
+ */
 template<class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
-    const  std::vector<Point2>& measurements, const  std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
+    const std::vector<Point2>& measurements,
-    bool optimize = false) {
+    const std::vector<boost::shared_ptr<CALIBRATION> >& Ks,
+    double rank_tol = 1e-9, bool optimize = false) {
 
   assert(poses.size() == measurements.size());
   assert(poses.size() == Ks.size());