Got mostly rid of computeEP: just a call to cameras.project2

gtsam/slam/SmartFactorBase.h

@@ -306,13 +306,6 @@ public:
     return (EtE).inverse();
   }
 
-  /// Assumes non-degenerate !
-  void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
-      const Point3& point) const {
-    cameras.reprojectionError(point, measured_, boost::none, E);
-    P = PointCov(E);
-  }
-
   /**
    *  Compute F, E, and b (called below in both vanilla and SVD versions), where
    *  F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
@@ -394,8 +387,7 @@ public:
       const Cameras& cameras, const Point3& point, const double lambda = 0.0,
       bool diagonalDamping = false) const {
 
-    int numKeys = this->keys_.size();
-
+    int m = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
     Vector b;
@@ -405,8 +397,8 @@ public:
 //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
 #ifdef HESSIAN_BLOCKS
     // Create structures for Hessian Factors
-    std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
-    std::vector < Vector > gs(numKeys);
+    std::vector < Matrix > Gs(m * (m + 1) / 2);
+    std::vector < Vector > gs(m);
 
     sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
     // schurComplement(Fblocks, E, P, b, Gs, gs);
@@ -416,15 +408,15 @@ public:
 
     return boost::make_shared < RegularHessianFactor<Dim> > (this->keys_, Gs, gs, f);
 #else // we create directly a SymmetricBlockMatrix
-    size_t n1 = Dim * numKeys + 1;
-    std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
+    size_t n1 = 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(n1, n1)); // for 10 cameras, size should be (10*Dim+1 x 10*Dim+1)
     sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ...
-    augmentedHessian(numKeys, numKeys)(0, 0) = f;
-    return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
+    augmentedHessian(m, m)(0, 0) = f;
+    return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
         augmentedHessian);
 #endif
   }

gtsam/slam/SmartProjectionFactor.h

@@ -482,12 +482,15 @@ public:
     return true;
   }
 
-  /// Takes values
-  bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
+  /**
+   * Triangulate and compute derivative of error with respect to point
+   * @return whether triangulation worked
+   */
+  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
     Cameras cameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
     if (nonDegenerate)
-      Base::computeEP(E, P, cameras, point_);
+      cameras.project2(point_, boost::none, E);
     return nonDegenerate;
   }
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -131,18 +131,20 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
   // Calculate expected derivative for point (easiest to check)
   boost::function<Vector(Point3)> f = //
       boost::bind(&SmartFactor::whitenedErrors, factor, cameras, _1);
+
+  // Calculate using computeEP
+  Matrix actualE;
+  factor.triangulateAndComputeE(actualE, values);
+
+  // get point
   boost::optional<Point3> point = factor.point();
   CHECK(point);
 
-  // Note ! After refactor the noiseModel is only in the factors, not these matrices
+  // calculate numerical derivative with triangulated point
   Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
-
-  // Calculate using computeEP
-  Matrix actualE, PointCov;
-  factor.computeEP(actualE, PointCov, values);
   EXPECT(assert_equal(expectedE, actualE, 1e-7));
 
-  // Calculate using reprojectionError, note not yet divided by sigma !
+  // Calculate using reprojectionError
   SmartFactor::Cameras::FBlocks F;
   Matrix E;
   Vector actualErrors = factor.reprojectionError(cameras, *point, F, E);
@@ -361,6 +363,10 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
 
   // Check derivatives
 
+  // Calculate E and P using computeEP, triangulates
+  Matrix actualE;
+  smartFactor1->triangulateAndComputeE(actualE, values);
+
   // Calculate expected derivative for point (easiest to check)
   SmartFactor::Cameras cameras = smartFactor1->cameras(values);
   boost::function<Vector(Point3)> f = //
@@ -370,10 +376,6 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
 
   // TODO, this is really a test of CameraSet
   Matrix expectedE = numericalDerivative11<Vector, Point3>(f, *point);
-
-  // Calculate using computeEP
-  Matrix actualE, PointCov;
-  smartFactor1->computeEP(actualE, PointCov, values);
   EXPECT(assert_equal(expectedE, actualE, 1e-7));
 
   // Calculate using reprojectionError
@@ -383,7 +385,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
   EXPECT(assert_equal(expectedE, E, 1e-7));
 
   // Success ! The derivatives of reprojectionError now agree with f !
-  EXPECT(assert_equal(f(*point) * sigma, actualErrors, 1e-7));
+  EXPECT(assert_equal(f(*point), actualErrors, 1e-7));
 }
 
 /* *************************************************************************/

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -513,17 +513,15 @@ public:
     return true;
   }
 
-  /// Assumes non-degenerate !
-  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
-    Base::computeEP(E, PointCov, cameras, point_);
-  }
-
-  /// Takes values
-  bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
+  /**
+   * Triangulate and compute derivative of error with respect to point
+   * @return whether triangulation worked
+   */
+  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
     Cameras cameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
     if (nonDegenerate)
-      computeEP(E, PointCov, cameras);
+      cameras.project2(point_, boost::none, E);
     return nonDegenerate;
   }