rename of computeJacobians overloads to better reflect functionality

gtsam/slam/JacobianFactorSVD.h

@@ -9,7 +9,7 @@
 
 namespace gtsam {
 /**
- * JacobianFactor for Schur complement that uses Q noise model
+ * JacobianFactor for Schur complement
  */
 template<size_t D, size_t ZDim>
 class JacobianFactorSVD: public RegularJacobianFactor<D> {
@@ -38,7 +38,14 @@ public:
     JacobianFactor::fillTerms(QF, zeroVector, model);
   }
 
-  /// Constructor
+  /**
+   * @brief Constructor
+   * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
+   * and a reduced point derivative, Enull
+   * and creates a reduced-rank Jacobian factor on the CameraSet
+   *
+   * @Fblocks:
+   */
   JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
       const Matrix& Enull, const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :

gtsam/slam/SmartFactorBase.h

@@ -81,6 +81,7 @@ protected:
   boost::optional<Pose3> body_P_sensor_;
 
   // check that noise model is isotropic and the same
+  // TODO, this is hacky, we should just do this via constructor, not add
   void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
     if (!sharedNoiseModel)
       return;
@@ -213,6 +214,36 @@ public:
                 && body_P_sensor_->equals(*e->body_P_sensor_)));
   }
 
+  /// Compute reprojection errors
+  Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
+    return cameras.reprojectionError(point, measured_);
+  }
+
+  /**
+   *  Compute reprojection errors and derivatives
+   *  TODO: the treatment of body_P_sensor_ is weird: the transformation
+   *  is applied in the caller, but the derivatives are computed here.
+   */
+  Vector reprojectionError(const Cameras& cameras, const Point3& point,
+      typename Cameras::FBlocks& F, Matrix& E) const {
+
+    Vector b = cameras.reprojectionError(point, measured_, F, E);
+
+    // Apply sensor chain rule if needed TODO: no simpler way ??
+    if (body_P_sensor_) {
+      size_t m = keys_.size();
+      for (size_t i = 0; i < m; i++) {
+        const Pose3& pose_i = cameras[i].pose();
+        Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
+        Matrix66 J;
+        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+        F[i].leftCols(6) *= J;
+      }
+    }
+
+    return b;
+  }
+
   /// Calculate vector of re-projection errors, noise model applied
   Vector whitenedErrors(const Cameras& cameras, const Point3& point) const {
     Vector b = cameras.reprojectionError(point, measured_);
@@ -251,36 +282,6 @@ public:
     return 0.5 * b.dot(b);
   }
 
-  /// Compute reprojection errors
-  Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
-    return cameras.reprojectionError(point, measured_);
-  }
-
-  /**
-   *  Compute reprojection errors and derivatives
-   *  TODO: the treatment of body_P_sensor_ is weird: the transformation
-   *  is applied in the caller, but the derivatives are computed here.
-   */
-  Vector reprojectionError(const Cameras& cameras, const Point3& point,
-      typename Cameras::FBlocks& F, Matrix& E) const {
-
-    Vector b = cameras.reprojectionError(point, measured_, F, E);
-
-    // Apply sensor chain rule if needed TODO: no simpler way ??
-    if (body_P_sensor_) {
-      size_t m = keys_.size();
-      for (size_t i = 0; i < m; i++) {
-        const Pose3& pose_i = cameras[i].pose();
-        Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
-        Matrix66 J;
-        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
-        F[i].leftCols(6) *= J;
-      }
-    }
-
-    return b;
-  }
-
   /// Computes Point Covariance P from E
   static Matrix3 PointCov(Matrix& E) {
     return (E.transpose() * E).inverse();
@@ -371,7 +372,6 @@ public:
     Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
     Vector s = svd.singularValues();
     size_t m = this->keys_.size();
-    // Enull = zeros(ZDim * m, ZDim * m - 3);
     Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
 
     return f;
@@ -661,7 +661,7 @@ public:
     Matrix E;
     Vector b;
     computeJacobians(F, E, b, cameras, point);
-    noiseModel_->WhitenSystem(E,b);
+    noiseModel_->WhitenSystem(E, b);
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
     // TODO make WhitenInPlace work with any dense matrix type
     BOOST_FOREACH(KeyMatrix2D& Fblock,F)
@@ -675,13 +675,15 @@ 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> Fblocks;
+    std::vector<KeyMatrix2D> F;
     Matrix E;
     Vector b;
-    computeJacobians(Fblocks, E, b, cameras, point);
+    computeJacobians(F, E, b, cameras, point);
+    const size_t M = b.size();
+    std::cout << M << std::endl;
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
-    return boost::make_shared<JacobianFactorQ<Dim, ZDim> > //
+    SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
-    (Fblocks, E, P, b, noiseModel_);
+    return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(F, E, P, b, n);
   }
 
   /**
@@ -690,13 +692,15 @@ public:
    */
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
-    size_t numKeys = this->keys_.size();
+    size_t m = this->keys_.size();
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector<KeyMatrix2D> F;
     Vector b;
-    Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
+    const size_t M = ZDim * m;
-    computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
+    Matrix E0(M, M - 3);
-    return boost::make_shared<JacobianFactorSVD<Dim, ZDim> > //
+    computeJacobiansSVD(F, E0, b, cameras, point);
-    (Fblocks, Enull, b, noiseModel_);
+    std::cout << M << std::endl;
+    SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
+    return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
   }
 
 private:

gtsam/slam/SmartProjectionFactor.h

@@ -368,7 +368,12 @@ public:
     // ==================================================================
     Matrix F, E;
     Vector b;
-    double f = computeJacobians(F, E, b, cameras);
+    double f;
+    {
+      std::vector<typename Base::KeyMatrix2D> Fblocks;
+      f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+      Base::FillDiagonalF(Fblocks,F);  // expensive !
+    }
 
     // Schur complement trick
     // Frank says: should be possible to do this more efficiently?
@@ -486,21 +491,12 @@ public:
     return nonDegenerate;
   }
 
-  /// Version that takes values, and creates the point
-  bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-      Matrix& E, Vector& b, const Values& values) const {
-    Cameras cameras;
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-    if (nonDegenerate)
-      computeJacobians(Fblocks, E, b, cameras);
-    return nonDegenerate;
-  }
-
   /// Compute F, E only (called below in both vanilla and SVD versions)
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
-  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  double computeJacobiansWithTriangulatedPoint(
-      Matrix& E, Vector& b, const Cameras& cameras) const {
+      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
+      const Cameras& cameras) const {
 
     if (this->degenerate_) {
       std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
@@ -515,9 +511,9 @@ public:
       }
 
       // TODO replace all this by Call to CameraSet
-      int numKeys = this->keys_.size();
+      int m = this->keys_.size();
-      E = zeros(2 * numKeys, 2);
+      E = zeros(2 * m, 2);
-      b = zero(2 * numKeys);
+      b = zero(2 * m);
       double f = 0;
       for (size_t i = 0; i < this->measured_.size(); i++) {
         if (i == 0) { // first pose
@@ -541,35 +537,27 @@ public:
     } // 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;
+    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    if (nonDegenerate)
+      computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+    return nonDegenerate;
+  }
+
   /// takes values
-  bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  bool triangulateAndComputeJacobiansSVD(
-      Matrix& Enull, Vector& b, const Values& values) const {
+      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
+      Vector& b, const Values& values) const {
     typename Base::Cameras cameras;
     double good = computeCamerasAndTriangulate(values, cameras);
     if (good)
-      computeJacobiansSVD(Fblocks, Enull, b, cameras);
+      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
     return true;
   }
 
-  /// SVD version
-  double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-      Matrix& Enull, Vector& b, const Cameras& cameras) const {
-    return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
-  }
-
-  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-  // TODO should there be a lambda?
-  double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
-      const Cameras& cameras) const {
-    return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
-  }
-
-  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, b, cameras, point_);
-  }
-
   /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionErrorAfterTriangulation(const Values& values) const {
     Cameras cameras;
@@ -652,6 +640,7 @@ public:
   inline bool isPointBehindCamera() const {
     return cheiralityException_;
   }
+
   /** return cheirality verbosity */
   inline bool verboseCheirality() const {
     return verboseCheirality_;

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -754,17 +754,17 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   cameras.push_back(level_camera_right);
 
   factor1->error(values); // this is important to have a triangulation of the point
-  Point3 expectedPoint;
+  Point3 point;
   if (factor1->point())
-    expectedPoint = *(factor1->point());
+    point = *(factor1->point());
-  factor1->computeJacobians(expectedF, expectedE, expectedb, cameras);
+  factor1->computeJacobians(expectedF, expectedE, expectedb, cameras, point);
 
   NonlinearFactorGraph generalGraph;
   SFMFactor sfm1(level_uv, unit2, c1, l1);
   SFMFactor sfm2(level_uv_right, unit2, c2, l1);
   generalGraph.push_back(sfm1);
   generalGraph.push_back(sfm2);
-  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+  values.insert(l1, point); // note: we get rid of possible errors in the triangulation
   Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
   Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -153,7 +153,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
   // Calculate using computeJacobians
   Vector b;
   vector<SmartFactor::KeyMatrix2D> Fblocks;
-  double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras);
+  double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point);
   EXPECT(assert_equal(expectedE, E, 1e-7));
   EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
 }
@@ -364,19 +364,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
   // Calculate expected derivative for point (easiest to check)
   SmartFactor::Cameras cameras = smartFactor1->cameras(values);
   boost::function<Vector(Point3)> f = //
-      boost::bind(&SmartFactor::whitenedErrors, *smartFactor1, cameras, _1);
+      boost::bind(&SmartFactor::reprojectionError, *smartFactor1, cameras, _1);
   boost::optional<Point3> point = smartFactor1->point();
   CHECK(point);
 
-  // Note ! After refactor the noiseModel is only in the factors, not these matrices
+  // TODO, this is really a test of CameraSet
-  Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
+  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 whitenedError
+  // Calculate using reprojectionError
   Matrix E;
   SmartFactor::Cameras::FBlocks F;
   Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E);
@@ -472,7 +472,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     E(2, 2) = 10;
     E(3, 1) = 100;
     const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
-        (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2));
+        (make_pair(x1, F1))(make_pair(x2, F2));
     Matrix3 P = (E.transpose() * E).inverse();
     Vector4 b;
     b.setZero();
@@ -483,10 +483,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
         smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
     CHECK(actual);
-    CHECK(assert_equal(expected, *actual));
+    EXPECT(assert_equal(expected, *actual));
 
     // createJacobianQFactor
-    JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b);
+    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);
@@ -890,8 +891,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   using namespace vanillaPose;
 
   // Two slightly different cameras
-  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose2 = level_pose
-  Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose3 = level_pose
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Camera cam2(pose2, sharedK);
   Camera cam3(pose3, sharedK);
 
@@ -970,8 +973,10 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   views.push_back(x3);
 
   // Two different cameras
-  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose2 = level_pose
-  Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose3 = level_pose
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Camera cam2(pose2, sharedK2);
   Camera cam3(pose3, sharedK2);
 
@@ -1050,8 +1055,10 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   views.push_back(x3);
 
   // Two different cameras
-  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose2 = level_pose
-  Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose3 = level_pose
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Camera cam2(pose2, sharedK);
   Camera cam3(pose3, sharedK);
 
@@ -1390,8 +1397,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   views.push_back(x3);
 
   // Two different cameras
-  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose2 = level_pose
-  Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+  Pose3 pose3 = level_pose
+      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Camera cam2(pose2, sharedBundlerK);
   Camera cam3(pose3, sharedBundlerK);
 

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -582,41 +582,16 @@ public:
     } // end else
   }
 
-//  /// Version that computes PointCov, with optional lambda parameter
+  /// takes values
-//  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  bool triangulateAndComputeJacobiansSVD(
-//      Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
+      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
-//      const double lambda = 0.0) const {
+      Vector& b, const Values& values) const {
-//
+    typename Base::Cameras cameras;
-//    double f = computeJacobians(Fblocks, E, b, cameras);
+    double good = computeCamerasAndTriangulate(values, cameras);
-//
+    if (good)
-//    // Point covariance inv(E'*E)
+      return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
-//    PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
+    return true;
-//
+  }
-//    return f;
-//  }
-//
-//  /// takes values
-//  bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-//      Matrix& Enull, Vector& b, const Values& values) const {
-//    typename Base::Cameras cameras;
-//    double good = computeCamerasAndTriangulate(values, cameras);
-//    if (good)
-//      computeJacobiansSVD(Fblocks, Enull, b, cameras);
-//    return true;
-//  }
-//
-//  /// SVD version
-//  double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-//      Matrix& Enull, Vector& b, const Cameras& cameras) const {
-//    return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
-//  }
-//
-//  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-//  // TODO should there be a lambda?
-//  double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
-//      const Cameras& cameras) const {
-//    return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
-//  }
 
   /// Returns Matrix, TODO: maybe should not exist -> not sparse !
   double computeJacobians(Matrix& F, Matrix& E, Vector& b,