Migrated to non-keyed Fblocks

gtsam/slam/JacobianFactorSVD.h

@@ -16,7 +16,6 @@ class JacobianFactorSVD: public RegularJacobianFactor<D> {
 
   typedef RegularJacobianFactor<D> Base;
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
-  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
   typedef std::pair<Key, Matrix> KeyMatrix;
 
 public:
@@ -46,12 +45,13 @@ public:
    *
    * @Fblocks:
    */
-  JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
+  JacobianFactorSVD(const FastVector<Key>& keys,
-      const Matrix& Enull, const Vector& b, //
+      const std::vector<MatrixZD>& Fblocks, const Matrix& Enull,
+      const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     size_t numKeys = Enull.rows() / ZDim;
-    size_t j = 0, m2 = ZDim * numKeys - 3;
+    size_t m2 = ZDim * numKeys - 3;
     // PLAIN NULL SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
     // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
@@ -59,10 +59,12 @@ public:
     // JacobianFactor factor(QF, Q * b);
     std::vector<KeyMatrix> QF;
     QF.reserve(numKeys);
-    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
+    for (size_t k = 0; k < Fblocks.size(); ++k) {
+      Key key = keys[k];
       QF.push_back(
-          KeyMatrix(it.first,
+          KeyMatrix(key,
-              (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
+              (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
+    }
     JacobianFactor::fillTerms(QF, -Enull.transpose() * b, model);
   }
 };

gtsam/slam/SmartFactorBase.h

@@ -281,12 +281,12 @@ public:
    *  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
    *  with respect to the point. The value of cameras/point are passed as parameters.
+   *  TODO: Kill this obsolete method
    */
   double computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
       const Cameras& cameras, const Point3& point) const {
     // Project into Camera set and calculate derivatives
-    typename Cameras::FBlocks F;
+    b = reprojectionError(cameras, point, Fblocks, E);
-    b = reprojectionError(cameras, point, F, E);
     return b.squaredNorm();
   }
 
@@ -357,8 +357,8 @@ public:
   void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
     noiseModel_->WhitenSystem(E, b);
     // TODO make WhitenInPlace work with any dense matrix type
-    BOOST_FOREACH(MatrixZD& Fblock,F)
+    for (size_t i = 0; i < F.size(); i++)
-      Fblock.second = noiseModel_->Whiten(Fblock.second);
+      F[i] = noiseModel_->Whiten(F[i]);
   }
 
   /**
@@ -390,7 +390,7 @@ public:
     const size_t M = b.size();
     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);
+    return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
   }
 
   /**
@@ -407,7 +407,7 @@ public:
     computeJacobiansSVD(F, E0, b, cameras, point);
     SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
         noiseModel_->sigma());
-    return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
+    return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
   }
 
   /// Create BIG block-diagonal matrix F from Fblocks
@@ -416,7 +416,7 @@ public:
     F.resize(ZDim * m, Dim * m);
     F.setZero();
     for (size_t i = 0; i < m; ++i)
-      F.block<This::ZDim, Dim>(This::ZDim * i, Dim * i) = Fblocks.at(i).second;
+      F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i);
   }
 
 private:

gtsam/slam/SmartProjectionCameraFactor.h

@@ -112,7 +112,7 @@ public:
   }
 
   /// linearize returns a Hessianfactor that is an approximation of error(p)
-  virtual boost::shared_ptr<RegularImplicitSchurFactor<Dim> > linearizeToImplicit(
+  virtual boost::shared_ptr<RegularImplicitSchurFactor<Camera> > linearizeToImplicit(
       const Values& values, double lambda=0.0) const {
     return Base::createRegularImplicitSchurFactor(Base::cameras(values),lambda);
   }

gtsam/slam/SmartProjectionFactor.h

@@ -278,7 +278,7 @@ public:
     Vector b;
     double f;
     {
-      std::vector<typename Base::KeyMatrix2D> Fblocks;
+      std::vector<typename Base::MatrixZD> Fblocks;
       f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
       Base::whitenJacobians(Fblocks, E, b);
       Base::FillDiagonalF(Fblocks, F); // expensive !
@@ -326,12 +326,12 @@ public:
   }
 
   // create factor
-  boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> > createRegularImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
       return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
     else
-      return boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> >();
+      return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
   }
 
   /// create factor
@@ -374,7 +374,7 @@ public:
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
   double computeJacobiansWithTriangulatedPoint(
-      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
       const Cameras& cameras) const {
 
     if (!result_) {
@@ -385,18 +385,15 @@ public:
       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++) {
         Matrix Fi, Ei;
         Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei)
             - this->measured_.at(i)).vector();
-
+        Fblocks.push_back(Fi);
-        f += bi.squaredNorm();
-        Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
         E.block<2, 2>(2 * i, 0) = Ei;
         subInsert(b, bi, 2 * i);
       }
-      return f;
+      return b.squaredNorm();
     } else {
       // valid result: just return Base version
       return Base::computeJacobians(Fblocks, E, b, cameras, *result_);
@@ -405,7 +402,7 @@ public:
 
   /// Version that takes values, and creates the point
   bool triangulateAndComputeJacobians(
-      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
       const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
@@ -416,8 +413,8 @@ public:
 
   /// takes values
   bool triangulateAndComputeJacobiansSVD(
-      std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
-      Vector& b, const Values& values) const {
+      const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -772,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   Point3 point;
   if (factor1->point())
     point = *(factor1->point());
-  vector<SmartFactorBundler::KeyMatrix2D> Fblocks;
+  vector<Matrix29> Fblocks;
   factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
 
   NonlinearFactorGraph generalGraph;
@@ -823,7 +823,7 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
   implicitFactor->add(level_uv_right, c2, unit2);
   GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
       implicitFactor->linearize(values);
-  typedef RegularImplicitSchurFactor<9> Implicit9;
+  typedef RegularImplicitSchurFactor<Camera> Implicit9;
   Implicit9& implicitSchurFactor =
       dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -142,7 +142,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
 
   // Calculate using computeJacobians
   Vector b;
-  vector<SmartFactor::KeyMatrix2D> Fblocks;
+  vector<SmartFactor::MatrixZD> Fblocks;
   double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point);
   EXPECT(assert_equal(expectedE, E, 1e-7));
   EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
@@ -264,10 +264,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, Factors ) {
 
+  typedef PinholePose<Cal3_S2> Camera;
+
   // Default cameras for simple derivatives
   Rot3 R;
   static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
-  PinholePose<Cal3_S2> cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
+  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
       Pose3(R, Point3(1, 0, 0)), sharedK);
 
   // one landmarks 1m in front of camera
@@ -350,15 +352,19 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     E(2, 0) = 10;
     E(2, 2) = 1;
     E(3, 1) = 10;
-    vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
+    vector<Matrix26> Fblocks = list_of<Matrix>(F1)(F2);
-        (make_pair(x1, F1))(make_pair(x2, F2));
     Vector b(4);
     b.setZero();
 
+    // Create smart factors
+    FastVector<Key> keys;
+    keys.push_back(x1);
+    keys.push_back(x2);
+    
     // createJacobianQFactor
     SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
     Matrix3 P = (E.transpose() * E).inverse();
-    JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
+    JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n);
     EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
 
     boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
@@ -370,13 +376,13 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     // Whiten for RegularImplicitSchurFactor (does not have noise model)
     model->WhitenSystem(E, b);
     Matrix3 whiteP = (E.transpose() * E).inverse();
-    BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks)
+    Fblocks[0] = model->Whiten(Fblocks[0]);
-      Fblock.second = model->Whiten(Fblock.second);
+    Fblocks[1] = model->Whiten(Fblocks[1]);
 
     // createRegularImplicitSchurFactor
-    RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
+    RegularImplicitSchurFactor<Camera> expected(keys, Fblocks, E, whiteP, b);
 
-    boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
+    boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
         smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
     CHECK(actual);
     EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
@@ -764,8 +770,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
   values.insert(L(1), landmark1);
   values.insert(L(2), landmark2);
   values.insert(L(3), landmark3);
-  if (isDebugTest)
-    values.at<Pose3>(x3).print("Pose3 before optimization: ");
 
   DOUBLES_EQUAL(48406055, graph.error(values), 1);
 
@@ -779,8 +783,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 
   DOUBLES_EQUAL(0, graph.error(result), 1e-9);
 
-  if (isDebugTest)
-    result.at<Pose3>(x3).print("Pose3 after optimization: ");
   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
 }