Merge remote-tracking branch 'origin/develop' into feature/BAD

gtsam/linear/JacobianFactor.h

@@ -187,7 +187,7 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
-    /* ************************************************************************* */
+    /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const;
 
     /// Return the block diagonal of the Hessian for this factor

gtsam/linear/Preconditioner.cpp

@@ -13,6 +13,7 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <boost/shared_ptr.hpp>
 #include <boost/algorithm/string.hpp>
+#include <boost/range/adaptor/map.hpp>
 #include <iostream>
 #include <vector>
 
@@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build(
   size_t nnz = 0;
   for ( size_t i = 0 ; i < n ; ++i ) {
     const size_t dim = dims_[i];
-    blocks.push_back(Matrix::Zero(dim, dim));
+    // blocks.push_back(Matrix::Zero(dim, dim));
     // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
     nnz += dim*dim;
   }
 
-  /* compute the block diagonal by scanning over the factors */
+  /* getting the block diagonals over the factors */
   BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
-    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+    std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
-      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+    BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
-        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+      blocks.push_back(hessian);
-        const Matrix &Ai = jf->getA(it);
-        blocks[entry.index()] += (Ai.transpose() * Ai);
-      }
-    }
-    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
-      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
-        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
-        const Matrix &Hii = hf->info(it, it).selfadjointView();
-        blocks[entry.index()] += Hii;
-      }
-    }
-    else {
-      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
-    }
   }
 
   /* if necessary, allocating the memory for cacheing the factorization results */

gtsam/navigation/ImuFactor.h

@@ -57,7 +57,7 @@ namespace gtsam {
     class PreintegratedMeasurements {
     public:
       imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
-      Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
+      Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
       Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
       Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
@@ -216,11 +216,21 @@ namespace gtsam {
             H_vel_pos, H_vel_vel, H_vel_angles,
             H_angles_pos, H_angles_vel, H_angles_angles;
 
-
+        // first order uncertainty propagation:
-        // first order uncertainty propagation
         // the deltaT allows to pass from continuous time noise to discrete time noise
+        // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
+        // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
         PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
 
+        // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
+        //
+        //        Matrix G(9,9);
+        //        G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+        //            Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+        //            Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+        //
+        //        PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+
         // Update preintegrated measurements
         /* ----------------------------------------------------------------------------------------------------------------------- */
         if(!use2ndOrderIntegration_){

gtsam/slam/RegularImplicitSchurFactor.h

@@ -1,5 +1,5 @@
 /**
- * @file    ImplicitSchurFactor.h
+ * @file    RegularImplicitSchurFactor.h
  * @brief   A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
  * @author  Frank Dellaert
  * @author  Luca Carlone
@@ -17,13 +17,13 @@
 namespace gtsam {
 
 /**
- * ImplicitSchurFactor
+ * RegularImplicitSchurFactor
  */
 template<size_t D> //
-class ImplicitSchurFactor: public GaussianFactor {
+class RegularImplicitSchurFactor: public GaussianFactor {
 
 public:
-  typedef ImplicitSchurFactor This; ///< Typedef to this class
+  typedef RegularImplicitSchurFactor This; ///< Typedef to this class
   typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
 protected:
@@ -40,11 +40,11 @@ protected:
 public:
 
   /// Constructor
-  ImplicitSchurFactor() {
+  RegularImplicitSchurFactor() {
   }
 
   /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
-  ImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+  RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
       const Matrix3& P, const Vector& b) :
       Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
     initKeys();
@@ -58,7 +58,7 @@ public:
   }
 
   /// Destructor
-  virtual ~ImplicitSchurFactor() {
+  virtual ~RegularImplicitSchurFactor() {
   }
 
   // Write access, only use for construction!
@@ -87,7 +87,7 @@ public:
   /// print
   void print(const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << " ImplicitSchurFactor " << std::endl;
+    std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
     std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
     std::cout << " E_ \n" << E_ << std::endl;
@@ -96,7 +96,7 @@ public:
 
   /// equals
   bool equals(const GaussianFactor& lf, double tol) const {
-    if (!dynamic_cast<const ImplicitSchurFactor*>(&lf))
+    if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
       return false;
     else {
       return false;
@@ -110,21 +110,21 @@ public:
 
   virtual Matrix augmentedJacobian() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::augmentedJacobian non implemented");
+        "RegularImplicitSchurFactor::augmentedJacobian non implemented");
     return Matrix();
   }
   virtual std::pair<Matrix, Vector> jacobian() const {
-    throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
     return std::make_pair(Matrix(), Vector());
   }
   virtual Matrix augmentedInformation() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::augmentedInformation non implemented");
+        "RegularImplicitSchurFactor::augmentedInformation non implemented");
     return Matrix();
   }
   virtual Matrix information() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::information non implemented");
+        "RegularImplicitSchurFactor::information non implemented");
     return Matrix();
   }
 
@@ -210,18 +210,18 @@ public:
   }
 
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
         PointCovariance_, E_, b_);
-    throw std::runtime_error("ImplicitSchurFactor::clone non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
   }
   virtual bool empty() const {
     return false;
   }
 
   virtual GaussianFactor::shared_ptr negate() const {
-    return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
         PointCovariance_, E_, b_);
-    throw std::runtime_error("ImplicitSchurFactor::negate non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
   }
 
   // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@@ -454,7 +454,7 @@ public:
   }
 
 };
-// ImplicitSchurFactor
+// RegularImplicitSchurFactor
 
 }
 

gtsam/slam/SmartFactorBase.h

@@ -21,7 +21,7 @@
 
 #include "JacobianFactorQ.h"
 #include "JacobianFactorSVD.h"
-#include "ImplicitSchurFactor.h"
+#include "RegularImplicitSchurFactor.h"
 #include "RegularHessianFactor.h"
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -73,7 +73,7 @@ public:
 
   /**
    * Constructor
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
       body_P_sensor_(body_P_sensor) {
@@ -271,8 +271,13 @@ public:
 
       Vector bi;
       try {
-        bi =
+        bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
-            -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
+        if(body_P_sensor_){
+          Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
+          Matrix J(6, 6);
+          Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+          Fi = Fi * J;
+        }
       } catch (CheiralityException&) {
         std::cout << "Cheirality exception " << std::endl;
         exit(EXIT_FAILURE);
@@ -624,11 +629,11 @@ public:
   }
 
   // ****************************************************************************************************
-  boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    typename boost::shared_ptr<ImplicitSchurFactor<D> > f(
+    typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
-        new ImplicitSchurFactor<D>());
+        new RegularImplicitSchurFactor<D>());
     computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
         cameras, point, lambda, diagonalDamping);
     f->initKeys();

gtsam/slam/SmartProjectionFactor.h

@@ -120,7 +120,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
@@ -298,7 +298,7 @@ public:
         || (!this->manageDegeneracy_
             && (this->cheiralityException_ || this->degenerate_))) {
       if (isDebug) {
-        std::cout << "createImplicitSchurFactor: degenerate configuration"
+        std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
             << std::endl;
       }
       return false;
@@ -409,12 +409,12 @@ public:
   }
 
   // create factor
-  boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createImplicitSchurFactor(cameras, point_, lambda);
+      return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
     else
-      return boost::shared_ptr<ImplicitSchurFactor<D> >();
+      return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
   }
 
   /// create factor
@@ -685,7 +685,7 @@ public:
   inline bool isPointBehindCamera() const {
     return cheiralityException_;
   }
-  /** return chirality verbosity */
+  /** return cheirality verbosity */
   inline bool verboseCheirality() const {
     return verboseCheirality_;
   }

gtsam/slam/SmartProjectionPoseFactor.h

@@ -63,7 +63,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
@@ -157,6 +157,9 @@ public:
     size_t i=0;
     BOOST_FOREACH(const Key& k, this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
+      if(Base::body_P_sensor_)
+        pose = pose.compose(*(Base::body_P_sensor_));
+
       typename Base::Camera camera(pose, *K_all_[i++]);
       cameras.push_back(camera);
     }

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -6,7 +6,7 @@
  */
 
 //#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
-#include <gtsam/slam/ImplicitSchurFactor.h>
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
 //#include <gtsam_unstable/slam/JacobianFactorQ.h>
 #include <gtsam/slam/JacobianFactorQ.h>
 //#include "gtsam_unstable/slam/JacobianFactorQR.h"
@@ -38,19 +38,19 @@ const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
 
 //*************************************************************************************
-TEST( implicitSchurFactor, creation ) {
+TEST( regularImplicitSchurFactor, creation ) {
   // Matrix E = Matrix::Ones(6,3);
   Matrix E = zeros(6, 3);
   E.block<2,2>(0, 0) = eye(2);
   E.block<2,3>(2, 0) = 2 * ones(2, 3);
   Matrix3 P = (E.transpose() * E).inverse();
-  ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
   Matrix expectedP = expected.getPointCovariance();
   EXPECT(assert_equal(expectedP, P));
 }
 
 /* ************************************************************************* */
-TEST( implicitSchurFactor, addHessianMultiply ) {
+TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
   Matrix E = zeros(6, 3);
   E.block<2,2>(0, 0) = eye(2);
@@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
   keys += 0,1,2,3;
   Vector x = xvalues.vector(keys);
   Vector expected = zero(24);
-  ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
+  RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
   EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
 
   // Create ImplicitSchurFactor
-  ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
 
   VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
   { // First Version
@@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
 }
 
 /* ************************************************************************* */
-TEST(implicitSchurFactor, hessianDiagonal)
+TEST(regularImplicitSchurFactor, hessianDiagonal)
 {
   /* TESTED AGAINST MATLAB
    *  F = [ones(2,6) zeros(2,6) zeros(2,6)
@@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal)
   E.block<2,3>(2, 0) << 1,2,3,4,5,6;
   E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
   Matrix3 P = (E.transpose() * E).inverse();
-  ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
 
   // hessianDiagonal
   VectorValues expected;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
   if(isDebugTest) tictoc_print_();
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
+  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
+
+  SimpleCamera cam1(cameraPose1, *K); // with camera poses
+  SimpleCamera cam2(cameraPose2, *K);
+  SimpleCamera cam3(cameraPose3, *K);
+
+  // create arbitrary body_Pose_sensor (transforms from sensor to body)
+  Pose3 sensor_to_body =  Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
+
+  // These are the poses we want to estimate, from camera measurements
+  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
+  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
+  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(5, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  // Create smart factors
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  double rankTol = 1;
+  double linThreshold = -1;
+  bool manageDegeneracy = false;
+  bool enableEPI = false;
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor2->add(measurements_cam2, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor3->add(measurements_cam3, views, model, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  // Put all factors in factor graph, adding priors
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
+
+  // Check errors at ground truth poses
+  Values gtValues;
+  gtValues.insert(x1, bodyPose1);
+  gtValues.insert(x2, bodyPose2);
+  gtValues.insert(x3, bodyPose3);
+  double actualError = graph.error(gtValues);
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
+  Values values;
+  values.insert(x1, bodyPose1);
+  values.insert(x2, bodyPose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(x3, bodyPose3*noise_pose);
+
+  LevenbergMarquardtParams params;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
+}
+
+
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
   // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

gtsam_unstable/partition/FindSeparator-inl.h

@@ -17,12 +17,14 @@
 #include <boost/shared_array.hpp>
 #include <boost/timer.hpp>
 
+#include "FindSeparator.h"
+
 extern "C" {
 #include <metis.h>
 #include "metislib.h"
 }
 
-#include "FindSeparator.h"
+
 
 namespace gtsam { namespace partition {
 

tests/testPCGSolver.cpp

@@ -13,6 +13,7 @@
  * @file    testPCGSolver.cpp
  * @brief   Unit tests for PCGSolver class
  * @author  Yong-Dian Jian
+ * @date    Aug 06, 2014
  */
 
 #include <tests/smallExample.h>
@@ -51,6 +52,7 @@ using symbol_shorthand::X;
 using symbol_shorthand::L;
 
 /* ************************************************************************* */
+// Test cholesky decomposition
 TEST( PCGSolver, llt ) {
   Matrix R = (Matrix(3,3) <<
                 1., -1., -1.,
@@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) {
 }
 
 /* ************************************************************************* */
+// Test Dummy Preconditioner
 TEST( PCGSolver, dummy )
 {
   LevenbergMarquardtParams paramsPCG;
@@ -110,6 +113,7 @@ TEST( PCGSolver, dummy )
 }
 
 /* ************************************************************************* */
+// Test Block-Jacobi Precondioner
 TEST( PCGSolver, blockjacobi )
 {
   LevenbergMarquardtParams paramsPCG;
@@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi )
 }
 
 /* ************************************************************************* */
+// Test Incremental Subgraph PCG Solver
 TEST( PCGSolver, subgraph )
 {
   LevenbergMarquardtParams paramsPCG;

tests/testPreconditioner.cpp

@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   testPreconditioner.cpp
+ *  @brief  Unit tests for Preconditioners
+ *  @author Sungtae An
+ *  @date   Nov 6, 2014
+ **/
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/linear/PCGSolver.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+static GaussianFactorGraph createSimpleGaussianFactorGraph() {
+  GaussianFactorGraph fg;
+  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
+  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
+  fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
+  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
+  fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
+  // measurement between x1 and l1: l1-x1=[0.0;0.2]
+  fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
+  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
+  fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
+  return fg;
+}
+
+/* ************************************************************************* */
+// Copy of BlockJacobiPreconditioner::build
+std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
+{
+  const size_t n = keyInfo.size();
+  std::vector<size_t> dims_ = keyInfo.colSpec();
+
+  /* prepare the buffer of block diagonals */
+  std::vector<Matrix> blocks; blocks.reserve(n);
+
+  /* allocate memory for the factorization of block diagonals */
+  size_t nnz = 0;
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t dim = dims_[i];
+    blocks.push_back(Matrix::Zero(dim, dim));
+    // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
+    nnz += dim*dim;
+  }
+
+  /* compute the block diagonal by scanning over the factors */
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Ai = jf->getA(it);
+        blocks[entry.index()] += (Ai.transpose() * Ai);
+      }
+    }
+    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Hii = hf->info(it, it).selfadjointView();
+        blocks[entry.index()] += Hii;
+      }
+    }
+    else {
+      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+  }
+
+  return blocks;
+}
+
+/* ************************************************************************* */
+TEST( Preconditioner, buildBlocks ) {
+  // Create simple Gaussian factor graph and initial values
+  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
+  Values initial;
+  initial.insert(0,Point2(4, 5));
+  initial.insert(1,Point2(0,  1));
+  initial.insert(2,Point2(-5, 7));
+
+  // Expected Hessian block diagonal matrices
+  std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
+
+  // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
+  std::vector<Matrix> actualHessian = buildBlocks(gfg, KeyInfo(gfg));
+
+  // Compare the number of block diagonal matrices
+  EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size());
+
+  // Compare the values of matrices
+  std::map<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
+  std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
+  for(; it1!=expectedHessian.end(); it1++, it2++)
+    EXPECT(assert_equal(it1->second, *it2));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */