Merge branch 'TriangulationResult' into feature/SmartFactors3

.cproject

@@ -1309,6 +1309,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1348,6 +1349,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1355,6 +1357,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1458,7 +1461,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1536,10 +1538,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRegularImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j4</buildArguments>
-				<buildTarget>testImplicitSchurFactor.run</buildTarget>
+				<buildTarget>testRegularImplicitSchurFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1793,7 +1795,6 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1801,7 +1802,6 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1809,7 +1809,6 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1817,7 +1816,6 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2009,6 +2007,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2160,7 +2159,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2168,7 +2166,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2216,7 +2213,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2224,7 +2220,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2232,7 +2227,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2248,7 +2242,6 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3392,7 +3385,6 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3400,7 +3392,6 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3408,7 +3399,6 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam/geometry/tests/testPinholeSet.cpp

@@ -129,7 +129,7 @@ TEST(PinholeSet, Pinhole) {
   // Instantiate triangulateSafe
   TriangulationParameters params;
   TriangulationResult actual = set.triangulateSafe(z,params);
-  CHECK(actual.degenerate);
+  CHECK(actual.degenerate());
 }
 
 /* ************************************************************************* */

gtsam/geometry/triangulation.h

@@ -316,12 +316,57 @@ struct TriangulationParameters {
           _landmarkDistanceThreshold), dynamicOutlierRejectionThreshold(
           _dynamicOutlierRejectionThreshold) {
   }
+
+  // stream to output
+  friend std::ostream &operator<<(std::ostream &os,
+      const TriangulationParameters& p) {
+    os << "rankTolerance = " << p.rankTolerance << std::endl;
+    os << "enableEPI = " << p.enableEPI << std::endl;
+    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
+        << std::endl;
+    os << "dynamicOutlierRejectionThreshold = "
+        << p.dynamicOutlierRejectionThreshold << std::endl;
+    return os;
+  }
 };
 
-struct TriangulationResult {
+/**
-  Point3 point;
+ * TriangulationResult is an optional point, along with the reasons why it is invalid.
-  bool degenerate;
+ */
-  bool cheiralityException;
+class TriangulationResult: public boost::optional<Point3> {
+  enum Status {
+    VALID, DEGENERATE, BEHIND_CAMERA
+  };
+  Status status_;
+  TriangulationResult(Status s) :
+      status_(s) {
+  }
+public:
+  TriangulationResult(const Point3& p) :
+      status_(VALID) {
+    reset(p);
+  }
+  static TriangulationResult Degenerate() {
+    return TriangulationResult(DEGENERATE);
+  }
+  static TriangulationResult BehindCamera() {
+    return TriangulationResult(BEHIND_CAMERA);
+  }
+  bool degenerate() const {
+    return status_ == DEGENERATE;
+  }
+  bool behindCamera() const {
+    return status_ == BEHIND_CAMERA;
+  }
+  // stream to output
+  friend std::ostream &operator<<(std::ostream &os,
+      const TriangulationResult& result) {
+    if (result)
+      os << "point = " << *result << std::endl;
+    else
+      os << "no point, status = " << result.status_ << std::endl;
+    return os;
+  }
 };
 
 /// triangulateSafe: extensive checking of the outcome
@@ -330,62 +375,53 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
     const std::vector<Point2>& measured,
     const TriangulationParameters& params) {
 
-  TriangulationResult result;
-
   size_t m = cameras.size();
 
   // if we have a single pose the corresponding factor is uninformative
-  if (m < 2) {
+  if (m < 2)
-    result.degenerate = true;
+    return TriangulationResult::Degenerate();
-    return result;
+  else
-  }
+    // We triangulate the 3D position of the landmark
+    try {
+      Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
+          params.rankTolerance, params.enableEPI);
 
-  // We triangulate the 3D position of the landmark
+      // Check landmark distance and reprojection errors to avoid outliers
-  try {
+      size_t i = 0;
-    // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
+      double totalReprojError = 0.0;
-    result.point = triangulatePoint3<CAMERA>(cameras, measured,
+      BOOST_FOREACH(const CAMERA& camera, cameras) {
-        params.rankTolerance, params.enableEPI);
+        // we discard smart factors corresponding to points that are far away
-    result.degenerate = false;
+        Point3 cameraTranslation = camera.pose().translation();
-    result.cheiralityException = false;
+        if (cameraTranslation.distance(point) > params.landmarkDistanceThreshold)
+        return TriangulationResult::Degenerate();
+        // Also flag if point is behind any of the cameras
+        try {
+          const Point2& zi = measured.at(i);
+          Point2 reprojectionError(camera.project(point) - zi);
+          totalReprojError += reprojectionError.vector().norm();
+        } catch (CheiralityException) {
+          return TriangulationResult::BehindCamera();
+        }
+        i += 1;
+      }
+      // we discard smart factors that have large reprojection error
+      if (params.dynamicOutlierRejectionThreshold > 0
+          && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
+        return TriangulationResult::Degenerate();
 
-    // Check landmark distance and reprojection errors to avoid outliers
+      // all good!
-    double totalReprojError = 0.0;
+      return TriangulationResult(point);
-    size_t i = 0;
+    } catch (TriangulationUnderconstrainedException&) {
-    BOOST_FOREACH(const CAMERA& camera, cameras) {
+      // if TriangulationUnderconstrainedException can be
-      Point3 cameraTranslation = camera.pose().translation();
+      // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
-      // we discard smart factors corresponding to points that are far away
+      // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
-      if (cameraTranslation.distance(result.point)
+      // in the second case we want to use a rotation-only smart factor
-          > params.landmarkDistanceThreshold) {
+      return TriangulationResult::Degenerate();
-        result.degenerate = true;
+    } catch (TriangulationCheiralityException&) {
-        break;
+      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
-      }
+      // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
-      const Point2& zi = measured.at(i);
+      return TriangulationResult::BehindCamera();
-      try {
-        Point2 reprojectionError(camera.project(result.point) - zi);
-        totalReprojError += reprojectionError.vector().norm();
-      } catch (CheiralityException) {
-        result.cheiralityException = true;
-      }
-      i += 1;
     }
-    // we discard smart factors that have large reprojection error
-    if (params.dynamicOutlierRejectionThreshold > 0
-        && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
-      result.degenerate = true;
-
-  } catch (TriangulationUnderconstrainedException&) {
-    // if TriangulationUnderconstrainedException can be
-    // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
-    // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
-    // in the second case we want to use a rotation-only smart factor
-    result.degenerate = true;
-    result.cheiralityException = false;
-  } catch (TriangulationCheiralityException&) {
-    // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
-    // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
-    result.cheiralityException = true;
-  }
-  return result;
 }
 
 } // \namespace gtsam

gtsam/slam/RegularImplicitSchurFactor.h

@@ -9,6 +9,7 @@
 
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
 #include <boost/foreach.hpp>
 #include <iosfwd>
 
@@ -17,7 +18,7 @@ namespace gtsam {
 /**
  * RegularImplicitSchurFactor
  */
-template<size_t D> //
+template<size_t D, size_t Z = 2> //
 class RegularImplicitSchurFactor: public GaussianFactor {
 
 public:
@@ -26,12 +27,12 @@ public:
 
 protected:
 
-  typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
+  typedef Eigen::Matrix<double, Z, D> Matrix2D; ///< type of an F block
   typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
   typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
 
-  const std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
+  const std::vector<KeyMatrix2D> Fblocks_; ///< All Z*D F blocks (one for each camera)
-  const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
+  const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate)
   const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
   const Vector b_; ///< 2m-dimensional RHS vector
 
@@ -122,15 +123,141 @@ public:
         "RegularImplicitSchurFactor::jacobian non implemented");
     return std::make_pair(Matrix(), Vector());
   }
-  virtual Matrix augmentedInformation() const {
+
-    throw std::runtime_error(
+  /**
-        "RegularImplicitSchurFactor::augmentedInformation non implemented");
+   * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
-    return Matrix();
+   * Fast version - works on with sparsity
+   */
+  static void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
+      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
+    // Schur complement trick
+    // G = F' * F - F' * E * P * E' * F
+    // g = F' * (b - E * P * E' * b)
+
+    // a single point is observed in m cameras
+    size_t m = Fblocks.size();
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera
+
+      const Matrix2D& Fi = Fblocks.at(i).second;
+      const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
+
+      // D = (Dx2) * (Z)
+      augmentedHessian(i, m) = Fi.transpose() * b.segment<Z>(Z * i) // F' * b
+      - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      augmentedHessian(i, i) = Fi.transpose()
+          * (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi);
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const Matrix2D& Fj = Fblocks.at(j).second;
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian(i, j) = -Fi.transpose()
+            * (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
+      }
+    } // end of for over cameras
   }
+
+  /**
+   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
+   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
+   */
+  static void updateSparseSchurComplement(
+      const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+      const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
+      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
+
+    FastMap<Key, size_t> KeySlotMap;
+    for (size_t slot = 0; slot < allKeys.size(); slot++)
+      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
+    // Schur complement trick
+    // G = F' * F - F' * E * P * E' * F
+    // g = F' * (b - E * P * E' * b)
+
+    MatrixDD matrixBlock;
+    typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
+
+    // a single point is observed in m cameras
+    size_t m = Fblocks.size(); // cameras observing current point
+    size_t aug_m = (augmentedHessian.rows() - 1) / D; // all cameras in the group
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera in the current factor
+
+      const Matrix2D& Fi = Fblocks.at(i).second;
+      const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
+
+      // D = (DxZDim) * (Z)
+      // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
+      // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
+      // Key cameraKey_i = this->keys_[i];
+      DenseIndex aug_i = KeySlotMap.at(keys[i]);
+
+      // information vector - store previous vector
+      // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
+      // add contribution of current factor
+      augmentedHessian(aug_i, aug_m) =
+          augmentedHessian(aug_i, aug_m).knownOffDiagonal()
+              + Fi.transpose() * b.segment<Z>(Z * i) // F' * b
+          - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
+
+          // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+          // main block diagonal - store previous block
+      matrixBlock = augmentedHessian(aug_i, aug_i);
+      // add contribution of current factor
+      augmentedHessian(aug_i, aug_i) = matrixBlock
+          + (Fi.transpose()
+              * (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi));
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const Matrix2D& Fj = Fblocks.at(j).second;
+
+        //Key cameraKey_j = this->keys_[j];
+        DenseIndex aug_j = KeySlotMap.at(keys[j]);
+
+        // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
+        // off diagonal block - store previous block
+        // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
+        // add contribution of current factor
+        augmentedHessian(aug_i, aug_j) =
+            augmentedHessian(aug_i, aug_j).knownOffDiagonal()
+                - Fi.transpose()
+                    * (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
+      }
+    } // end of for over cameras
+
+    augmentedHessian(aug_m, aug_m)(0, 0) += f;
+  }
+
+  /// *Compute* full augmented information matrix
+  virtual Matrix augmentedInformation() const {
+
+    // Create a SymmetricBlockMatrix
+    int m = this->keys_.size();
+    size_t M1 = D * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, D);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+
+    // Do the Schur complement
+    sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
+    return augmentedHessian.matrix();
+  }
+
+  /// *Compute* full information matrix
   virtual Matrix information() const {
-    throw std::runtime_error(
+    Matrix augmented = augmentedInformation();
-        "RegularImplicitSchurFactor::information non implemented");
+    int m = this->keys_.size();
-    return Matrix();
+    size_t M = D * m;
+    return augmented.block(0,0,M,M);
   }
 
   /// Return the diagonal of the Hessian for this factor
@@ -142,10 +269,10 @@ public:
       Key j = keys_[pos];
 
       // Calculate Fj'*Ej for the current camera (observing a single point)
-      // D x 3 = (D x 2) * (2 x 3)
+      // D x 3 = (D x Z) * (Z x 3)
       const Matrix2D& Fj = Fblocks_[pos].second;
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<2, 3>(2 * pos, 0);
+          * E_.block<Z, 3>(Z * pos, 0);
 
       Eigen::Matrix<double, D, 1> dj;
       for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@@ -174,10 +301,10 @@ public:
       Key j = keys_[pos];
 
       // Calculate Fj'*Ej for the current camera (observing a single point)
-      // D x 3 = (D x 2) * (2 x 3)
+      // D x 3 = (D x Z) * (Z x 3)
       const Matrix2D& Fj = Fblocks_[pos].second;
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<2, 3>(2 * pos, 0);
+          * E_.block<Z, 3>(Z * pos, 0);
 
       DVector dj;
       for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@@ -195,28 +322,28 @@ public:
     // F'*(I - E*P*E')*F
     for (size_t pos = 0; pos < size(); ++pos) {
       Key j = keys_[pos];
-      // F'*F - F'*E*P*E'*F   (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
+      // F'*F - F'*E*P*E'*F  e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
       const Matrix2D& Fj = Fblocks_[pos].second;
       //      Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-      //          * E_.block<2, 3>(2 * pos, 0);
+      //          * E_.block<Z, 3>(Z * pos, 0);
       //      blocks[j] = Fj.transpose() * Fj
       //          - FtE * PointCovariance_ * FtE.transpose();
 
-      const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
+      const Matrix23& Ej = E_.block<Z, 3>(Z * pos, 0);
       blocks[j] = Fj.transpose()
           * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
 
       // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
-      //      static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
+      //      static const Eigen::Matrix<double, Z, Z> I2 = eye(Z);
       //      Matrix2 Q = //
-      //          I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
+      //          I2 - E_.block<Z, 3>(Z * pos, 0) * PointCovariance_ * E_.block<Z, 3>(Z * pos, 0).transpose();
       //      blocks[j] = Fj.transpose() * Q * Fj;
     }
     return blocks;
   }
 
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
         PointCovariance_, E_, b_);
     throw std::runtime_error(
         "RegularImplicitSchurFactor::clone non implemented");
@@ -226,7 +353,7 @@ public:
   }
 
   virtual GaussianFactor::shared_ptr negate() const {
-    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
         PointCovariance_, E_, b_);
     throw std::runtime_error(
         "RegularImplicitSchurFactor::negate non implemented");
@@ -247,23 +374,23 @@ public:
   typedef std::vector<Vector2> Error2s;
 
   /**
-   * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
+   * @brief Calculate corrected error Q*(e-Z*b) = (I - E*P*E')*(e-Z*b)
    */
   void projectError2(const Error2s& e1, Error2s& e2) const {
 
-    // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
+    // d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m
     Vector3 d1;
     d1.setZero();
     for (size_t k = 0; k < size(); k++)
-      d1 += E_.block<2, 3>(2 * k, 0).transpose()
+      d1 += E_.block<Z, 3>(Z * k, 0).transpose()
-          * (e1[k] - 2 * b_.segment<2>(k * 2));
+          * (e1[k] - Z * b_.segment<Z>(k * Z));
 
     // d2 = E.transpose() * e1 = (3*2m)*2m
     Vector3 d2 = PointCovariance_ * d1;
 
     // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
     for (size_t k = 0; k < size(); k++)
-      e2[k] = e1[k] - 2 * b_.segment<2>(k * 2) - E_.block<2, 3>(2 * k, 0) * d2;
+      e2[k] = e1[k] - Z * b_.segment<Z>(k * Z) - E_.block<Z, 3>(Z * k, 0) * d2;
   }
 
   /*
@@ -305,7 +432,7 @@ public:
 
     // e1 = F * x - b = (2m*dm)*dm
     for (size_t k = 0; k < size(); ++k)
-      e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<2>(k * 2);
+      e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<Z>(k * Z);
     projectError(e1, e2);
 
     double result = 0;
@@ -324,14 +451,14 @@ public:
     Vector3 d1;
     d1.setZero();
     for (size_t k = 0; k < size(); k++)
-      d1 += E_.block<2, 3>(2 * k, 0).transpose() * e1[k];
+      d1 += E_.block<Z, 3>(Z * k, 0).transpose() * e1[k];
 
     // d2 = E.transpose() * e1 = (3*2m)*2m
     Vector3 d2 = PointCovariance_ * d1;
 
     // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
     for (size_t k = 0; k < size(); k++)
-      e2[k] = e1[k] - E_.block<2, 3>(2 * k, 0) * d2;
+      e2[k] = e1[k] - E_.block<Z, 3>(Z * k, 0) * d2;
   }
 
   /// Scratch space for multiplyHessianAdd
@@ -426,7 +553,7 @@ public:
     e1.resize(size());
     e2.resize(size());
     for (size_t k = 0; k < size(); k++)
-      e1[k] = b_.segment<2>(2 * k);
+      e1[k] = b_.segment<Z>(Z * k);
     projectError(e1, e2);
 
     // g = F.transpose()*e2
@@ -453,7 +580,7 @@ public:
     e1.resize(size());
     e2.resize(size());
     for (size_t k = 0; k < size(); k++)
-      e1[k] = b_.segment<2>(2 * k);
+      e1[k] = b_.segment<Z>(Z * k);
     projectError(e1, e2);
 
     for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
@@ -472,8 +599,8 @@ public:
 // end class RegularImplicitSchurFactor
 
 // traits
-template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
+template<size_t Z, size_t D> struct traits<RegularImplicitSchurFactor<D, Z> > : public Testable<
-    RegularImplicitSchurFactor<D> > {
+    RegularImplicitSchurFactor<D, Z> > {
 };
 
 }

gtsam/slam/SmartFactorBase.h

@@ -337,14 +337,14 @@ public:
     double f = computeJacobians(Fblocks, E, b, cameras, point);
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
 
-    // we create directly a SymmetricBlockMatrix
+    // Create a SymmetricBlockMatrix
     size_t M1 = Dim * m + 1;
     std::vector<DenseIndex> dims(m + 1); // this also includes the b term
     std::fill(dims.begin(), dims.end() - 1, Dim);
     dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
 
     // build augmented hessian
-    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
     sparseSchurComplement(Fblocks, E, P, b, augmentedHessian);
     augmentedHessian(m, m)(0, 0) = f;
 
@@ -352,121 +352,6 @@ public:
         augmentedHessian);
   }
 
-  /**
-   * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
-   * Fast version - works on with sparsity
-   */
-  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size();
-
-    // Blockwise Schur complement
-    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
-
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
-
-      // Dim = (Dx2) * (2)
-      // (augmentedHessian.matrix()).block<Dim,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
-      augmentedHessian(i1, numKeys) = Fi1.transpose()
-          * b.segment<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-      augmentedHessian(i1, i1) = Fi1.transpose()
-          * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
-
-      // upper triangular part of the hessian
-      for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        augmentedHessian(i1, i2) = -Fi1.transpose()
-            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-      }
-    } // end of for over cameras
-  }
-
-  /**
-   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
-   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
-   */
-  void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      const double f, const FastVector<Key> allKeys,
-      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    MatrixDD matrixBlock;
-    typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
-
-    FastMap<Key, size_t> KeySlotMap;
-    for (size_t slot = 0; slot < allKeys.size(); slot++)
-      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size(); // cameras observing current point
-    size_t aug_numKeys = (augmentedHessian.rows() - 1) / Dim; // all cameras in the group
-
-    // Blockwise Schur complement
-    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
-
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
-
-      // Dim = (DxZDim) * (ZDim)
-      // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
-      // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
-      // Key cameraKey_i1 = this->keys_[i1];
-      DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
-
-      // information vector - store previous vector
-      // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
-      // add contribution of current factor
-      augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
-          aug_numKeys).knownOffDiagonal()
-          + Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-      // main block diagonal - store previous block
-      matrixBlock = augmentedHessian(aug_i1, aug_i1);
-      // add contribution of current factor
-      augmentedHessian(aug_i1, aug_i1) =
-          matrixBlock
-              + (Fi1.transpose()
-                  * (Fi1
-                      - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1));
-
-      // upper triangular part of the hessian
-      for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        //Key cameraKey_i2 = this->keys_[i2];
-        DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
-
-        // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
-        // off diagonal block - store previous block
-        // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
-        // add contribution of current factor
-        augmentedHessian(aug_i1, aug_i2) =
-            augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
-                - Fi1.transpose()
-                    * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-      }
-    } // end of for over cameras
-
-    augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
-  }
-
   /**
    * Add the contribution of the smart factor to a pre-allocated Hessian,
    * using sparse linear algebra. More efficient than the creation of the
@@ -476,33 +361,38 @@ public:
       const double lambda, bool diagonalDamping,
       SymmetricBlockMatrix& augmentedHessian,
       const FastVector<Key> allKeys) const {
-
-    // int numKeys = this->keys_.size();
-
-    std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
     Vector b;
+    std::vector<KeyMatrix2D> Fblocks;
     double f = computeJacobians(Fblocks, E, b, cameras, point);
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
-    updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ...
+    RegularImplicitSchurFactor<Dim, ZDim>::updateSparseSchurComplement(Fblocks,
+        E, P, b, f, allKeys, keys_, augmentedHessian);
+  }
+
+  /// Whiten the Jacobians computed by computeJacobians using noiseModel_
+  void whitenJacobians(std::vector<KeyMatrix2D>& F, Matrix& E,
+      Vector& b) const {
+    noiseModel_->WhitenSystem(E, b);
+    // TODO make WhitenInPlace work with any dense matrix type
+    BOOST_FOREACH(KeyMatrix2D& Fblock,F)
+      Fblock.second = noiseModel_->Whiten(Fblock.second);
   }
 
   /**
    * Return Jacobians as RegularImplicitSchurFactor with raw access
    */
-  boost::shared_ptr<RegularImplicitSchurFactor<Dim> > createRegularImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<Dim, ZDim> > //
-      const Cameras& cameras, const Point3& point, double lambda = 0.0,
+  createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
-      bool diagonalDamping = false) const {
+      double lambda = 0.0, bool diagonalDamping = false) const {
-    std::vector<KeyMatrix2D> F;
     Matrix E;
     Vector b;
+    std::vector<KeyMatrix2D> F;
     computeJacobians(F, E, b, cameras, point);
-    noiseModel_->WhitenSystem(E, b);
+    whitenJacobians(F, E, b);
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
-    // TODO make WhitenInPlace work with any dense matrix type
+    return boost::make_shared<RegularImplicitSchurFactor<Dim, ZDim> >(F, E, P,
-    BOOST_FOREACH(KeyMatrix2D& Fblock,F)
+        b);
-      Fblock.second = noiseModel_->Whiten(Fblock.second);
-    return boost::make_shared<RegularImplicitSchurFactor<Dim> >(F, E, P, b);
   }
 
   /**
@@ -511,12 +401,11 @@ public:
   boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    std::vector<KeyMatrix2D> F;
     Matrix E;
     Vector b;
+    std::vector<KeyMatrix2D> F;
     computeJacobians(F, E, b, cameras, point);
     const size_t M = b.size();
-    std::cout << M << std::endl;
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
     SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
     return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(F, E, P, b, n);

gtsam/slam/SmartProjectionCameraFactor.h

@@ -15,6 +15,7 @@
  * @author Luca Carlone
  * @author Chris Beall
  * @author Zsolt Kira
+ * @author Frank Dellaert
  */
 
 #pragma once

gtsam/slam/SmartProjectionFactor.h

@@ -35,14 +35,8 @@ namespace gtsam {
  * Structure for storing some state memory, used to speed up optimization
  * @addtogroup SLAM
  */
-class SmartProjectionFactorState {
+struct SmartProjectionFactorState {
 
-protected:
-
-public:
-
-  SmartProjectionFactorState() {
-  }
   // Hessian representation (after Schur complement)
   bool calculatedHessian;
   Matrix H;
@@ -68,38 +62,31 @@ private:
 
 protected:
 
-  // Some triangulation parameters
+  /// @name Caching triangulation
-  const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
+  /// @{
+  const TriangulationParameters parameters_;
+  mutable TriangulationResult result_; ///< result from triangulateSafe
+
   const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
   mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
+  /// @}
 
+  /// @name Parameters governing how triangulation result is treated
+  /// @{
   const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
+  const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+  const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+  /// @}
 
-  const bool enableEPI_; ///< if set to true, will refine triangulation using LM
+  /// @name Caching linearization
+  /// @{
+  /// shorthand for smart projection factor state variable
+  typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
+  SmartFactorStatePtr state_; ///< cached linearization
 
   const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
   mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
-
+  /// @}
-  mutable Point3 point_; ///< Current estimate of the 3D point
-
-  mutable bool degenerate_;
-  mutable bool cheiralityException_;
-
-  // verbosity handling for Cheirality Exceptions
-  const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-  const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-
-  boost::shared_ptr<SmartProjectionFactorState> state_;
-
-  /// shorthand for smart projection factor state variable
-  typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
-
-  double landmarkDistanceThreshold_; // if the landmark is triangulated at a
-  // distance larger than that the factor is considered degenerate
-
-  double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
-  // average reprojection error is smaller than this threshold after triangulation,
-  // and the factor is disregarded if the error is large
 
 public:
 
@@ -117,17 +104,18 @@ public:
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
    */
-  SmartProjectionFactor(const double rankTol, const double linThreshold,
+  SmartProjectionFactor(const double rankTolerance, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
       double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
           SmartFactorStatePtr(new SmartProjectionFactorState())) :
-      rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
+      parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
-          manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
+          dynamicOutlierRejectionThreshold), //
-          linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
+      result_(TriangulationResult::Degenerate()), //
-          false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
+      retriangulationThreshold_(1e-5), //
-          landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
+      manageDegeneracy_(manageDegeneracy), //
-          dynamicOutlierRejectionThreshold) {
+      throwCheirality_(false), verboseCheirality_(false), //
+      state_(state), linearizationThreshold_(linThreshold) {
   }
 
   /** Virtual destructor */
@@ -141,24 +129,23 @@ public:
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
-    std::cout << s << "SmartProjectionFactor, z = \n";
+    std::cout << s << "SmartProjectionFactor\n";
-    std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+    std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
-    std::cout << "degenerate_ = " << degenerate_ << std::endl;
+    std::cout << "result:\n" << result_ << std::endl;
-    std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
     Base::print("", keyFormatter);
   }
 
-  /// Check if the new linearization point_ is the same as the one used for previous triangulation
+  /// Check if the new linearization point is the same as the one used for previous triangulation
   bool decideIfTriangulate(const Cameras& cameras) const {
-    // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
+    // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
     // Note that this is not yet "selecting linearization", that will come later, and we only check if the
-    // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
+    // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
 
     size_t m = cameras.size();
 
     bool retriangulate = false;
 
-    // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
+    // if we do not have a previous linearization point or the new linearization point includes more poses
     if (cameraPosesTriangulation_.empty()
         || cameras.size() != cameraPosesTriangulation_.size())
       retriangulate = true;
@@ -181,19 +168,19 @@ public:
         cameraPosesTriangulation_.push_back(cameras[i].pose());
     }
 
-    return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
+    return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation
   }
 
-  /// This function checks if the new linearization point_ is 'close'  to the previous one used for linearization
+  /// This function checks if the new linearization point is 'close'  to the previous one used for linearization
   bool decideIfLinearize(const Cameras& cameras) const {
     // "selective linearization"
     // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
     // (we only care about the "rigidity" of the poses, not about their absolute pose)
 
-    if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
+    if (linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
       return true;
 
-    // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
+    // if we do not have a previous linearization point or the new linearization point includes more poses
     if (cameraPosesLinearization_.empty()
         || (cameras.size() != cameraPosesLinearization_.size()))
       return true;
@@ -211,100 +198,29 @@ public:
       Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
       Pose3 localCameraPoseOld = firstCameraPoseOld.between(
           cameraPosesLinearization_[i]);
-      if (!localCameraPose.equals(localCameraPoseOld,
+      if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold_))
-          this->linearizationThreshold_))
         return true; // at least two "relative" poses are different, hence we re-linearize
     }
-    return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
+    return false; // if we arrive to this point all poses are the same and we don't need re-linearize
   }
 
   /// triangulateSafe
-  size_t triangulateSafe(const Values& values) const {
+  TriangulationResult triangulateSafe(const Cameras& cameras) const {
-    return triangulateSafe(this->cameras(values));
-  }
-
-  /// triangulateSafe
-  size_t triangulateSafe(const Cameras& cameras) const {
 
     size_t m = cameras.size();
-    if (m < 2) { // if we have a single pose the corresponding factor is uninformative
+    if (m < 2) // if we have a single pose the corresponding factor is uninformative
-      degenerate_ = true;
+      return TriangulationResult::Degenerate();
-      return m;
+
-    }
     bool retriangulate = decideIfTriangulate(cameras);
-
+    if (retriangulate)
-    if (retriangulate) {
+      result_ = gtsam::triangulateSafe(cameras, this->measured_, parameters_);
-      // We triangulate the 3D position of the landmark
+    return result_;
-      try {
-        // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
-        point_ = triangulatePoint3<CAMERA>(cameras, this->measured_,
-            rankTolerance_, enableEPI_);
-        degenerate_ = false;
-        cheiralityException_ = false;
-
-        // Check landmark distance and reprojection errors to avoid outliers
-        double totalReprojError = 0.0;
-        size_t i = 0;
-        BOOST_FOREACH(const CAMERA& camera, cameras) {
-          Point3 cameraTranslation = camera.pose().translation();
-          // we discard smart factors corresponding to points that are far away
-          if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
-            degenerate_ = true;
-            break;
-          }
-          const Point2& zi = this->measured_.at(i);
-          try {
-            Point2 reprojectionError(camera.project(point_) - zi);
-            totalReprojError += reprojectionError.vector().norm();
-          } catch (CheiralityException) {
-            cheiralityException_ = true;
-          }
-          i += 1;
-        }
-        // we discard smart factors that have large reprojection error
-        if (dynamicOutlierRejectionThreshold_ > 0
-            && totalReprojError / m > dynamicOutlierRejectionThreshold_)
-          degenerate_ = true;
-
-      } catch (TriangulationUnderconstrainedException&) {
-        // if TriangulationUnderconstrainedException can be
-        // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
-        // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
-        // in the second case we want to use a rotation-only smart factor
-        degenerate_ = true;
-        cheiralityException_ = false;
-      } catch (TriangulationCheiralityException&) {
-        // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
-        // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
-        cheiralityException_ = true;
-      }
-    }
-    return m;
   }
 
   /// triangulate
   bool triangulateForLinearize(const Cameras& cameras) const {
-
+    triangulateSafe(cameras); // imperative, might reset result_
-    bool isDebug = false;
+    return (manageDegeneracy_ || result_);
-    size_t nrCameras = this->triangulateSafe(cameras);
-
-    if (nrCameras < 2
-        || (!this->manageDegeneracy_
-            && (this->cheiralityException_ || this->degenerate_))) {
-      if (isDebug) {
-        std::cout
-            << "createRegularImplicitSchurFactor: degenerate configuration"
-            << std::endl;
-      }
-      return false;
-    } else {
-
-      // instead, if we want to manage the exception..
-      if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
-        this->degenerate_ = true;
-      }
-      return true;
-    }
   }
 
   /// linearize returns a Hessianfactor that is an approximation of error(p)
@@ -324,12 +240,10 @@ public:
       exit(1);
     }
 
-    this->triangulateSafe(cameras);
+    triangulateSafe(cameras);
 
-    if (numKeys < 2
+    if (!manageDegeneracy_ && !result_) {
-        || (!this->manageDegeneracy_
+      // put in "empty" Hessian
-            && (this->cheiralityException_ || this->degenerate_))) {
-      // std::cout << "In linearize: exception" << std::endl;
       BOOST_FOREACH(Matrix& m, Gs)
         m = zeros(Base::Dim, Base::Dim);
       BOOST_FOREACH(Vector& v, gs)
@@ -338,23 +252,19 @@ public:
           Gs, gs, 0.0);
     }
 
-    // instead, if we want to manage the exception..
+    // decide whether to re-linearize
-    if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
-      this->degenerate_ = true;
-    }
-
     bool doLinearize = this->decideIfLinearize(cameras);
 
-    if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
+    if (linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
       for (size_t i = 0; i < cameras.size(); i++)
         this->cameraPosesLinearization_[i] = cameras[i].pose();
 
     if (!doLinearize) { // return the previous Hessian factor
       std::cout << "=============================" << std::endl;
       std::cout << "doLinearize " << doLinearize << std::endl;
-      std::cout << "this->linearizationThreshold_ "
+      std::cout << "linearizationThreshold_ " << linearizationThreshold_
-          << this->linearizationThreshold_ << std::endl;
+          << std::endl;
-      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+      std::cout << "valid: " << isValid() << std::endl;
       std::cout
           << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
           << std::endl;
@@ -370,6 +280,7 @@ public:
     {
       std::vector<typename Base::KeyMatrix2D> Fblocks;
       f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+      Base::whitenJacobians(Fblocks,E,b);
       Base::FillDiagonalF(Fblocks, F); // expensive !
     }
 
@@ -404,7 +315,7 @@ public:
       }
     }
     // ==================================================================
-    if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
+    if (linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
       this->state_->Gs = Gs;
       this->state_->gs = gs;
       this->state_->f = f;
@@ -417,7 +328,7 @@ public:
   boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> > createRegularImplicitSchurFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
+      return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
     else
       return boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> >();
   }
@@ -426,7 +337,7 @@ public:
   boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createJacobianQFactor(cameras, point_, lambda);
+      return Base::createJacobianQFactor(cameras, *result_, lambda);
     else
       return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
   }
@@ -434,63 +345,27 @@ public:
   /// Create a factor, takes values
   boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
       const Values& values, double lambda) const {
-    Cameras cameras;
+    return createJacobianQFactor(this->cameras(values), lambda);
-    // TODO triangulate twice ??
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-    if (nonDegenerate)
-      return createJacobianQFactor(cameras, lambda);
-    else
-      return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
   }
 
   /// different (faster) way to compute Jacobian factor
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createJacobianSVDFactor(cameras, point_, lambda);
+      return Base::createJacobianSVDFactor(cameras, *result_, lambda);
     else
       return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
   }
 
-  /// Returns true if nonDegenerate
-  bool computeCamerasAndTriangulate(const Values& values,
-      Cameras& cameras) const {
-    Values valuesFactor;
-
-    // Select only the cameras
-    BOOST_FOREACH(const Key key, this->keys_)
-      valuesFactor.insert(key, values.at(key));
-
-    cameras = this->cameras(valuesFactor);
-    size_t nrCameras = this->triangulateSafe(cameras);
-
-    if (nrCameras < 2
-        || (!this->manageDegeneracy_
-            && (this->cheiralityException_ || this->degenerate_)))
-      return false;
-
-    // instead, if we want to manage the exception..
-    if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
-      this->degenerate_ = true;
-
-    if (this->degenerate_) {
-      std::cout << "SmartProjectionFactor: this is not ready" << std::endl;
-      std::cout << "this->cheiralityException_ " << this->cheiralityException_
-          << std::endl;
-      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
-    }
-    return true;
-  }
-
   /**
    * Triangulate and compute derivative of error with respect to point
    * @return whether triangulation worked
    */
   bool triangulateAndComputeE(Matrix& E, const Values& values) const {
-    Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      cameras.project2(point_, boost::none, E);
+      cameras.project2(*result_, boost::none, E);
     return nonDegenerate;
   }
 
@@ -501,31 +376,18 @@ public:
       std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
       const Cameras& cameras) const {
 
-    if (this->degenerate_) {
+    if (!result_) {
-      std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
+      // TODO Luca clarify whether this works or not
-      std::cout << "point " << point_ << std::endl;
+      result_ = TriangulationResult(
-      std::cout
+          cameras[0].backprojectPointAtInfinity(this->measured_.at(0)));
-          << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used"
-          << std::endl;
-      if (Base::Dim > 6) {
-        std::cout
-            << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
-            << std::endl;
-      }
-
       // TODO replace all this by Call to CameraSet
       int m = this->keys_.size();
       E = zeros(2 * m, 2);
       b = zero(2 * m);
       double f = 0;
       for (size_t i = 0; i < this->measured_.size(); i++) {
-        if (i == 0) { // first pose
-          this->point_ = cameras[i].backprojectPointAtInfinity(
-              this->measured_.at(i));
-          // 3D parametrization of point at infinity: [px py 1]
-        }
         Matrix Fi, Ei;
-        Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
+        Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei)
             - this->measured_.at(i)).vector();
 
         f += bi.squaredNorm();
@@ -535,17 +397,17 @@ public:
       }
       return f;
     } else {
-      // nondegenerate: just return Base version
+      // valid result: just return Base version
-      return Base::computeJacobians(Fblocks, E, b, cameras, point_);
+      return Base::computeJacobians(Fblocks, E, b, cameras, *result_);
-    } // end else
+    }
   }
 
   /// Version that takes values, and creates the point
   bool triangulateAndComputeJacobians(
       std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
       const Values& values) const {
-    Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
       computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
     return nonDegenerate;
@@ -555,19 +417,19 @@ public:
   bool triangulateAndComputeJacobiansSVD(
       std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
       Vector& b, const Values& values) const {
-    typename Base::Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    double good = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
-    if (good)
+    if (nonDegenerate)
-      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
+      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
-    return true;
+    return nonDegenerate;
   }
 
   /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionErrorAfterTriangulation(const Values& values) const {
-    Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      return Base::reprojectionError(cameras, point_);
+      return Base::reprojectionError(cameras, *result_);
     else
       return zero(cameras.size() * 2);
   }
@@ -581,65 +443,61 @@ public:
   double totalReprojectionError(const Cameras& cameras,
       boost::optional<Point3> externalPoint = boost::none) const {
 
-    size_t nrCameras;
+    if (externalPoint)
-    if (externalPoint) {
+      result_ = TriangulationResult(*externalPoint);
-      nrCameras = this->keys_.size();
+    else
-      point_ = *externalPoint;
+      result_ = triangulateSafe(cameras);
-      degenerate_ = false;
-      cheiralityException_ = false;
-    } else {
-      nrCameras = this->triangulateSafe(cameras);
-    }
 
-    if (nrCameras < 2
+    // if we don't want to manage the exceptions we discard the factor
-        || (!this->manageDegeneracy_
+    if (!manageDegeneracy_ && !result_)
-            && (this->cheiralityException_ || this->degenerate_))) {
-      // if we don't want to manage the exceptions we discard the factor
-      // std::cout << "In error evaluation: exception" << std::endl;
       return 0.0;
-    }
 
-    if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
+    if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors
       std::cout
           << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
           << std::endl;
-      this->degenerate_ = true;
     }
 
-    if (this->degenerate_) {
+    if (isDegenerate()) {
       // return 0.0; // TODO: this maybe should be zero?
       std::cout
           << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
           << std::endl;
       // 3D parameterization of point at infinity
-      const Point2& zi = this->measured_.at(0);
+      const Point2& z0 = this->measured_.at(0);
-      this->point_ = cameras.front().backprojectPointAtInfinity(zi);
+      result_ = TriangulationResult(
-      return Base::totalReprojectionErrorAtInfinity(cameras, this->point_);
+          cameras.front().backprojectPointAtInfinity(z0));
+      return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
     } else {
       // Just use version in base class
-      return Base::totalReprojectionError(cameras, point_);
+      return Base::totalReprojectionError(cameras, *result_);
     }
   }
 
   /** return the landmark */
-  boost::optional<Point3> point() const {
+  TriangulationResult point() const {
-    return point_;
+    return result_;
   }
 
   /** COMPUTE the landmark */
-  boost::optional<Point3> point(const Values& values) const {
+  TriangulationResult point(const Values& values) const {
-    triangulateSafe(values);
+    Cameras cameras = this->cameras(values);
-    return point_;
+    return triangulateSafe(cameras);
+  }
+
+  /// Is result valid?
+  inline bool isValid() const {
+    return result_;
   }
 
   /** return the degenerate state */
   inline bool isDegenerate() const {
-    return (cheiralityException_ || degenerate_);
+    return result_.degenerate();
   }
 
   /** return the cheirality status flag */
   inline bool isPointBehindCamera() const {
-    return cheiralityException_;
+    return result_.behindCamera();
   }
 
   /** return cheirality verbosity */

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
   EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
   EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
   EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
+
+  // augmentedInformation (test just checks diagonals)
+  Matrix actualInfo = factor.augmentedInformation();
+  EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
+
+  // information (test just checks diagonals)
+  Matrix actualInfo2 = factor.information();
+  EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -15,6 +15,7 @@
  *  @author Chris Beall
  *  @author Luca Carlone
  *  @author Zsolt Kira
+ *  @author Frank Dellaert
  *  @date   Sept 2013
  */
 
@@ -133,9 +134,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
 
   using namespace vanilla;
 
-  // 1. Project two landmarks into two cameras and triangulate
+  // Project one landmark into two cameras and add noise on first
-  Point2 pixelError(0.2, 0.2);
+  Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
-  Point2 level_uv = level_camera.project(landmark1) + pixelError;
   Point2 level_uv_right = level_camera_right.project(landmark1);
 
   Values values;
@@ -147,7 +147,24 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   factor1->add(level_uv, c1, unit2);
   factor1->add(level_uv_right, c2, unit2);
 
+  // Point is now uninitialized before a triangulation event
+  EXPECT(!factor1->point());
+
+  double expectedError = 58640;
   double actualError1 = factor1->error(values);
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);
+
+  // Check triangulated point
+  CHECK(factor1->point());
+  EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4));
+
+  // Check whitened errors
+  Vector expected(4);
+  expected << -7, 235, 58, -242;
+  SmartFactor::Cameras cameras1 = factor1->cameras(values);
+  Point3 point1 = *factor1->point();
+  Vector actual = factor1->whitenedErrors(cameras1, point1);
+  EXPECT(assert_equal(expected, actual, 1));
 
   SmartFactor::shared_ptr factor2(new SmartFactor());
   vector<Point2> measurements;
@@ -165,8 +182,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   factor2->add(measurements, views, noises);
 
   double actualError2 = factor2->error(values);
-
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
-  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
 }
 
 /* *************************************************************************/
@@ -174,57 +190,81 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
 
   using namespace vanilla;
 
+  // Project three landmarks into three cameras
   vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
-
-  // 1. Project three landmarks into three cameras and triangulate
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
+  // Create and fill smartfactors
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
   vector<Key> views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
-
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
   smartFactor1->add(measurements_cam1, views, unit2);
-
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
   smartFactor2->add(measurements_cam2, views, unit2);
-
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
   smartFactor3->add(measurements_cam3, views, unit2);
 
-  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
+  // Create factor graph and add priors on two cameras
-
   NonlinearFactorGraph graph;
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
   graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
   graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
 
-  Values values;
+  // Create initial estimate
-  values.insert(c1, cam1);
+  Values initial;
-  values.insert(c2, cam2);
+  initial.insert(c1, cam1);
-  values.insert(c3, perturbCameraPose(cam3));
+  initial.insert(c2, cam2);
+  initial.insert(c3, perturbCameraPose(cam3));
   if (isDebugTest)
-    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+    initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
 
+  // Points are now uninitialized before a triangulation event
+  EXPECT(!smartFactor1->point());
+  EXPECT(!smartFactor2->point());
+  EXPECT(!smartFactor3->point());
+
+  EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
+  EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
+  EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
+
+  // Error should trigger this and initialize the points, abort if not so
+  CHECK(smartFactor1->point());
+  CHECK(smartFactor2->point());
+  CHECK(smartFactor3->point());
+
+  EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4));
+  EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4));
+  EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4));
+
+  // Check whitened errors
+  Vector expected(6);
+  expected << 256, 29, -26, 29, -206, -202;
+  SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
+  Point3 point1 = *smartFactor1->point();
+  Vector actual = smartFactor1->whitenedErrors(cameras1, point1);
+  EXPECT(assert_equal(expected, actual, 1));
+
+  // Optimize
   LevenbergMarquardtParams params;
-  if (isDebugTest)
+  if (isDebugTest) {
     params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
-  if (isDebugTest)
     params.verbosity = NonlinearOptimizerParams::ERROR;
+  }
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
 
-  Values result;
+  EXPECT(assert_equal(landmark1,*smartFactor1->point(), 1e-7));
-  gttic_(SmartProjectionCameraFactor);
+  EXPECT(assert_equal(landmark2,*smartFactor2->point(), 1e-7));
-  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  EXPECT(assert_equal(landmark3,*smartFactor3->point(), 1e-7));
-  result = optimizer.optimize();
-  gttoc_(SmartProjectionCameraFactor);
-  tictoc_finishedIteration_();
 
-  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
   //  VectorValues delta = GFG->optimize();
 
   if (isDebugTest)
@@ -243,8 +283,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
 
   using namespace vanilla;
 
+  // Project three landmarks into three cameras
   vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
-  // 1. Project three landmarks into three cameras and triangulate
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@@ -300,11 +340,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
     params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_(SmartProjectionCameraFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  gttoc_(SmartProjectionCameraFactor);
-  tictoc_finishedIteration_();
 
   //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
   //  VectorValues delta = GFG->optimize();
@@ -383,11 +420,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
     params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_(SmartProjectionCameraFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  gttoc_(SmartProjectionCameraFactor);
-  tictoc_finishedIteration_();
 
   //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
   //  VectorValues delta = GFG->optimize();
@@ -465,11 +499,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
     params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_(SmartProjectionCameraFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  gttoc_(SmartProjectionCameraFactor);
-  tictoc_finishedIteration_();
 
   if (isDebugTest)
     result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
@@ -544,11 +575,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
     params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_(SmartProjectionCameraFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  gttoc_(SmartProjectionCameraFactor);
-  tictoc_finishedIteration_();
 
   if (isDebugTest)
     result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -15,6 +15,7 @@
  *  @author Chris Beall
  *  @author Luca Carlone
  *  @author Zsolt Kira
+ *  @author Frank Dellaert
  *  @date   Sept 2013
  */
 
@@ -38,7 +39,7 @@ static const bool manageDegeneracy = true;
 
 // Create a noise model for the pixel error
 static const double sigma = 0.1;
-static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, sigma));
+static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
 
 // Convenience for named keys
 using symbol_shorthand::X;
@@ -289,7 +290,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
   cameras.push_back(cam2);
 
   // Make sure triangulation works
-  LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras));
+  CHECK(smartFactor1->triangulateSafe(cameras));
   CHECK(!smartFactor1->isDegenerate());
   CHECK(!smartFactor1->isPointBehindCamera());
   boost::optional<Point3> p = smartFactor1->point();
@@ -298,8 +299,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
 
   // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
   Matrix16 A1, A2;
-  A1 << -1000, 0, 0, 0, 100, 0;
+  A1 << -10, 0, 0, 0, 1, 0;
-  A2 << 1000, 0, 100, 0, -100, 0;
+  A2 << 10, 0, 1, 0, -1, 0;
+  A1 *= 10. / sigma;
+  A2 *= 10. / sigma;
+  Matrix expectedInformation; // filled below
   {
     // createHessianFactor
     Matrix66 G11 = 0.5 * A1.transpose() * A1;
@@ -314,10 +318,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     double f = 0;
 
     RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
+    expectedInformation = expected.information();
 
     boost::shared_ptr<RegularHessianFactor<6> > actual =
         smartFactor1->createHessianFactor(cameras, 0.0);
-    EXPECT(assert_equal(expected.information(), actual->information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
     EXPECT(assert_equal(expected, *actual, 1e-8));
   }
 
@@ -336,36 +341,45 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     F2(1, 0) = 100;
     F2(1, 2) = 10;
     F2(1, 4) = -10;
-    Matrix43 E;
+    Matrix E(4, 3);
     E.setZero();
-    E(0, 0) = 100;
+    E(0, 0) = 10;
-    E(1, 1) = 100;
+    E(1, 1) = 10;
-    E(2, 0) = 100;
+    E(2, 0) = 10;
-    E(2, 2) = 10;
+    E(2, 2) = 1;
-    E(3, 1) = 100;
+    E(3, 1) = 10;
-    const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
+    vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
         (make_pair(x1, F1))(make_pair(x2, F2));
-    Matrix3 P = (E.transpose() * E).inverse();
+    Vector b(4);
-    Vector4 b;
     b.setZero();
 
+    // createJacobianQFactor
+    SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
+    Matrix3 P = (E.transpose() * E).inverse();
+    JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
+    EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
+
+    boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
+        smartFactor1->createJacobianQFactor(cameras, 0.0);
+    CHECK(actualQ);
+    EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8));
+    EXPECT(assert_equal(expectedQ, *actualQ));
+
+    // Whiten for RegularImplicitSchurFactor (does not have noise model)
+    model->WhitenSystem(E, b);
+    Matrix3 whiteP = (E.transpose() * E).inverse();
+    BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks)
+      Fblock.second = model->Whiten(Fblock.second);
+
     // createRegularImplicitSchurFactor
-    RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+    RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
 
     boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
         smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
     CHECK(actual);
+    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
     EXPECT(assert_equal(expected, *actual));
-
-    // createJacobianQFactor
-    SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
-    JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
-
-    boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
-        smartFactor1->createJacobianQFactor(cameras, 0.0);
-    CHECK(actual);
-    EXPECT(assert_equal(expectedQ.information(), actualQ->information(), 1e-8));
-    EXPECT(assert_equal(expectedQ, *actualQ));
   }
 
   {
@@ -374,11 +388,12 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     b.setZero();
     double s = sin(M_PI_4);
     JacobianFactor expected(x1, s * A1, x2, s * A2, b);
+    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
 
     boost::shared_ptr<JacobianFactor> actual =
         smartFactor1->createJacobianSVDFactor(cameras, 0.0);
     CHECK(actual);
-    EXPECT(assert_equal(expected.information(), actual->information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
     EXPECT(assert_equal(expected, *actual));
   }
 }
@@ -976,7 +991,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   values.insert(x1, cam2);
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
-  values.insert(x3, Camera(pose_above * noise_pose,sharedK));
+  values.insert(x3, Camera(pose_above * noise_pose, sharedK));
   if (isDebugTest)
     values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
 
@@ -1070,9 +1085,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
   Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
-  rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK));
+  rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK));
-  rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK));
+  rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK));
-  rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK));
+  rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK));
 
   boost::shared_ptr<GaussianFactor> hessianFactorRot =
       smartFactorInstance->linearize(rotValues);
@@ -1086,9 +1101,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
       Point3(10, -4, 5));
 
   Values tranValues;
-  tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK));
+  tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK));
-  tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK));
+  tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK));
-  tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK));
+  tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK));
 
   boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
       smartFactorInstance->linearize(tranValues);
@@ -1130,9 +1145,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
   Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
-  rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK2));
+  rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK2));
-  rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK2));
+  rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK2));
-  rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK2));
+  rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK2));
 
   boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
       rotValues);
@@ -1148,9 +1163,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
       Point3(10, -4, 5));
 
   Values tranValues;
-  tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK2));
+  tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK2));
-  tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK2));
+  tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK2));
-  tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK2));
+  tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK2));
 
   boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
       smartFactor->linearize(tranValues);
@@ -1230,7 +1245,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   values.insert(x1, cam2);
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
-  values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK));
+  values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
   if (isDebugTest)
     values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
 
@@ -1336,7 +1351,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   values.insert(x1, cam2);
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
-  values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK));
+  values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
   if (isDebugTest)
     values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");