Finished HessianFactor and Choesky in unordered

gtsam/linear/HessianFactor-inl.h

@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HessianFactor-inl.h
+ * @brief   Contains the HessianFactor class, a general quadratic factor
+ * @author  Richard Roberts
+ * @date    Dec 8, 2010
+ */
+
+#pragma once
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  template<typename KEYS>
+  HessianFactor::HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) :
+    GaussianFactor(keys), info_(augmentedInformation)
+  {
+    // Check number of variables
+    if(Base::keys_.size() != augmentedInformation.nBlocks() - 1)
+      throw std::invalid_argument(
+      "Error in HessianFactor constructor input.  Number of provided keys plus\n"
+      "one for the information vector must equal the number of provided matrix blocks.");
+
+    // Check RHS dimension
+    if(augmentedInformation(0, augmentedInformation.nBlocks() - 1).cols() != 1)
+      throw std::invalid_argument(
+      "Error in HessianFactor constructor input.  The last provided matrix block\n"
+      "must be the information vector, but the last provided block had more than one column.");
+  }
+
+}

gtsam/linear/HessianFactor.cpp

@@ -150,12 +150,12 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
 }
 
 /* ************************************************************************* */
-namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } }
+namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } }
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
         const std::vector<Vector>& gs, double f) :
-GaussianFactor(js), info_(br::join(Gs | br::transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1)))
+GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1)))
 {
   // Get the number of variables
   size_t variable_count = js.size();
@@ -205,10 +205,10 @@ namespace {
     {
       if(jf.get_model()->isConstrained())
         throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
-      info.full() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() *
+      info.full().noalias() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() *
         jfModel->invsigmas().asDiagonal() * jf.matrixObject().full();
     } else {
-      info.full() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
+      info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
     }
   }
 }
@@ -241,26 +241,31 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) :
 }
 
 /* ************************************************************************* */
-HessianFactor::HessianFactor(const GaussianFactorGraph& factors)
+namespace {
-{
+  DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) {
-  Scatter scatter(factors);
+    return key_slotentry.second.dimension; } }
 
-  // Pull out keys and dimensions
+/* ************************************************************************* */
-  gttic(keys);
+HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
-  vector<size_t> dimensions(scatter.size() + 1);
+                             boost::optional<const Ordering&> ordering,
-  BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
+                             boost::optional<const Scatter&> scatter)
-    dimensions[var_slot.second.slot] = var_slot.second.dimension;
+{
+  boost::optional<Scatter> computedScatter;
+  if(!scatter) {
+    computedScatter = Scatter(factors);
+    scatter = computedScatter;
   }
-  // This is for the r.h.s. vector
-  dimensions.back() = 1;
-  gttoc(keys);
 
   // Allocate and copy keys
   gttic(allocate);
-  info_ = SymmetricBlockMatrix(dimensions);
+  // Allocate with dimensions for each variable plus 1 at the end for the information vector
+  vector<DenseIndex> dims(scatter->size() + 1);
+  br::copy(*scatter | br::transformed(&_dimFromScatterEntry), dims.begin());
+  dims.back() = 1;
+  info_ = SymmetricBlockMatrix(dims);
   info_.full().setZero();
-  keys_.resize(scatter.size());
+  keys_.resize(scatter->size());
-  br::copy(scatter | br::map_keys, keys_.begin());
+  br::copy(*scatter | br::map_keys, keys_.begin());
   gttoc(allocate);
 
   // Form A' * A
@@ -269,9 +274,9 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors)
   {
     if(factor) {
       if(const HessianFactor* hessian = dynamic_cast<const HessianFactor*>(factor.get()))
-        updateATA(*hessian, scatter);
+        updateATA(*hessian, *scatter);
       else if(const JacobianFactor* jacobian = dynamic_cast<const JacobianFactor*>(factor.get()))
-        updateATA(*jacobian, scatter);
+        updateATA(*jacobian, *scatter);
       else
         throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
     }
@@ -286,7 +291,7 @@ void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) c
   for(const_iterator key=this->begin(); key!=this->end(); ++key)
     cout << formatter(*key) << "(" << this->getDim(key) << ") ";
   cout << "\n";
-  gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
+  gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Augmented information matrix: ");
 }
 
 /* ************************************************************************* */
@@ -294,6 +299,8 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
   if(!dynamic_cast<const HessianFactor*>(&lf))
     return false;
   else {
+    if(!Factor::equals(lf, tol))
+      return false;
     Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
     thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
     Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
@@ -348,9 +355,9 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
   // First build an array of slots
   gttic(slots);
   //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
-  vector<size_t> slots(update.size());
+  vector<DenseIndex> slots(update.size());
-  size_t slot = 0;
+  DenseIndex slot = 0;
-  BOOST_FOREACH(Index j, update) {
+  BOOST_FOREACH(Key j, update) {
     slots[slot] = scatter.find(j)->second.slot;
     ++ slot;
   }
@@ -358,10 +365,10 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
 
   // Apply updates to the upper triangle
   gttic(update);
-  for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
+  for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) {
-    size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
+    DenseIndex slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
-    for(size_t j1=0; j1<=j2; ++j1) {
+    for(DenseIndex j1=0; j1<=j2; ++j1) {
-      size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
+      DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
       if(slot2 > slot1)
         info_(slot1, slot2).noalias() += update.info_(j1, j2);
       else if(slot1 > slot2)
@@ -379,24 +386,16 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
   updateATA(HessianFactor(update), scatter);
 }
 
-/* ************************************************************************* */
-void HessianFactor::partialCholesky(size_t nrFrontals)
-{
-  throw std::runtime_error("Not implemented");
-
-  //if(!choleskyPartial(matrix_, info_.offset(nrFrontals)))
-  //  throw IndeterminantLinearSystemException(this->keys().front());
-}
-
 /* ************************************************************************* */
 GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals)
 {
-  static const bool debug = false;
+  gttic(HessianFactor_splitEliminatedFactor);
 
   // Create one big conditionals with many frontal variables.
   gttic(Construct_conditional);
-  size_t varDim = info_.offset(nrFrontals);
+  const size_t varDim = info_.offset(nrFrontals);
   VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim);
+  Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks());
   GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
     keys_, nrFrontals, Ab);
   gttoc(Construct_conditional);
@@ -414,24 +413,51 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr
 /* ************************************************************************* */
 GaussianFactor::shared_ptr HessianFactor::negate() const
 {
-  throw std::runtime_error("Not implemented");
+  shared_ptr result = boost::make_shared<This>(*this);
+  result->info_.full() = -result->info_.full(); // Negate the information matrix of the result
+  return result;
+}
 
-  //// Copy Hessian Blocks from Hessian factor and invert
+/* ************************************************************************* */
-  //std::vector<Index> js;
+std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
-  //std::vector<Matrix> Gs;
+  EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
-  //std::vector<Vector> gs;
+{
-  //double f;
+  gttic(EliminateCholesky);
-  //js.insert(js.end(), begin(), end());
-  //for(size_t i = 0; i < js.size(); ++i){
-  //  for(size_t j = i; j < js.size(); ++j){
-  //    Gs.push_back( -info(begin()+i, begin()+j) );
-  //  }
-  //  gs.push_back( -linearTerm(begin()+i) );
-  //}
-  //f = -constantTerm();
 
-  //// Create the Anti-Hessian Factor from the negated blocks
+  // Build joint factor
-  //return HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f));
+  HessianFactor::shared_ptr jointFactor;
+  try {
+    jointFactor = boost::make_shared<HessianFactor>(factors, keys);
+  } catch(std::invalid_argument&) {
+    throw InvalidDenseElimination(
+      "EliminateCholesky was called with a request to eliminate variables that are not\n"
+      "involved in the provided factors.");
+  }
+
+  // Do dense elimination
+  if(!choleskyPartial(jointFactor->info_.matrix(), jointFactor->info_.offset(keys.size())))
+    throw IndeterminantLinearSystemException(keys.front());
+
+  // Split conditional
+  GaussianConditional::shared_ptr conditional = jointFactor->splitEliminatedFactor(keys.size());
+
+  return make_pair(conditional, jointFactor);
+}
+
+/* ************************************************************************* */
+std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
+  EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
+{
+  gttic(EliminatePreferCholesky);
+
+  // If any JacobianFactors have constrained noise models, we have to convert
+  // all factors to JacobianFactors.  Otherwise, we can convert all factors
+  // to HessianFactors.  This is because QR can handle constrained noise
+  // models but Cholesky cannot.
+  if (hasConstraints(factors))
+    return EliminateQR(factors, keys);
+  else
+    return EliminateCholesky(factors, keys);
 }
 
 } // gtsam

gtsam/linear/HessianFactor.h

@@ -22,27 +22,21 @@
 #include <gtsam/linear/GaussianFactor.h>
 
 #include <vector>
-
+#include <boost/make_shared.hpp>
-// Forward declarations for friend unit tests
-class HessianFactorConversionConstructorTest;
-class HessianFactorConstructor1Test;
-class HessianFactorConstructor1bTest;
-class HessianFactorcombineTest;
-
 
 namespace gtsam {
 
   // Forward declarations
+  class Ordering;
   class JacobianFactor;
   class GaussianConditional;
   class GaussianBayesNet;
+  class GaussianFactorGraph;
 
-  // Definition of Scatter, which is an intermediate data structure used when
+  // Definition of Scatter, which is an intermediate data structure used when building a
-  // building a HessianFactor incrementally, to get the keys in the right
+  // HessianFactor incrementally, to get the keys in the right order. The "scatter" is a map from
-  // order.
+  // global variable indices to slot indices in the union of involved variables.  We also include
-  // The "scatter" is a map from global variable indices to slot indices in the
+  // the dimensionality of the variable.
-  // union of involved variables.  We also include the dimensionality of the
-  // variable.
   struct GTSAM_EXPORT SlotEntry {
     size_t slot;
     size_t dimension;
@@ -50,7 +44,7 @@ namespace gtsam {
     : slot(_slot), dimension(_dimension) {}
     std::string toString() const;
   };
-  class Scatter : public FastMap<Index, SlotEntry> {
+  class Scatter : public FastMap<Key, SlotEntry> {
   public:
     Scatter() {}
     Scatter(const GaussianFactorGraph& gfg);
@@ -182,6 +176,11 @@ namespace gtsam {
     HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
         const std::vector<Vector>& gs, double f);
 
+    /** Constructor with an arbitrary number of keys and with the augmented information matrix
+    *   specified as a block matrix. */
+    template<typename KEYS>
+    HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation);
+
     /** Construct from a JacobianFactor (or from a GaussianConditional since it derives from it) */
     explicit HessianFactor(const JacobianFactor& cg);
 
@@ -190,7 +189,9 @@ namespace gtsam {
     explicit HessianFactor(const GaussianFactor& factor);
 
     /** Combine a set of factors into a single dense HessianFactor */
-    HessianFactor(const GaussianFactorGraph& factors);
+    explicit HessianFactor(const GaussianFactorGraph& factors,
+      boost::optional<const Ordering&> ordering = boost::none,
+      boost::optional<const Scatter&> scatter = boost::none);
 
     /** Destructor */
     virtual ~HessianFactor() {}
@@ -331,20 +332,8 @@ namespace gtsam {
      */
     virtual Matrix augmentedJacobian() const;
 
-    // Friend unit test classes
+    /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */
-    friend class ::HessianFactorConversionConstructorTest;
+    const SymmetricBlockMatrix& matrixObject() const { return info_; }
-    friend class ::HessianFactorConstructor1Test;
-    friend class ::HessianFactorConstructor1bTest;
-    friend class ::HessianFactorcombineTest;
-
-    // used in eliminateCholesky:
-
-    /**
-     * Do Cholesky. Note that after this, the lower triangle still contains
-     * some untouched non-zeros that should be zero.  We zero them while
-     * extracting submatrices in splitEliminatedFactor. Frank says :-(
-     */
-    void partialCholesky(size_t nrFrontals);
 
     /** Update the factor by adding the information from the JacobianFactor
      * (used internally during elimination).
@@ -360,29 +349,42 @@ namespace gtsam {
      */
     void updateATA(const HessianFactor& update, const Scatter& scatter);
 
-    /** assert invariants */
+    /**
-    void assertInvariants() const;
+    *   Densely partially eliminate with Cholesky factorization.  JacobianFactors are
+    *   left-multiplied with their transpose to form the Hessian using the conversion constructor
+    *   HessianFactor(const JacobianFactor&).
+    *   
+    *   If any factors contain constrained noise models, this function will fail because our current
+    *   implementation cannot handle constrained noise models in Cholesky factorization.  The
+    *   function EliminatePreferCholesky() automatically does QR instead when this is the case.
+    *   
+    *   Variables are eliminated in the order specified in \c keys.
+    *   
+    *   @param factors Factors to combine and eliminate
+    *   @param keys The variables to eliminate and their elimination ordering
+    *   @return The conditional and remaining factor
+    *   
+    *   \addtogroup LinearSolving */
+    friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
+      EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
 
     /**
-     * Densely partially eliminate with Cholesky factorization.  JacobianFactors
+    *   Densely partially eliminate with Cholesky factorization.  JacobianFactors are
-     * are left-multiplied with their transpose to form the Hessian using the
+    *   left-multiplied with their transpose to form the Hessian using the conversion constructor
-     * conversion constructor HessianFactor(const JacobianFactor&).
+    *   HessianFactor(const JacobianFactor&).
-     *
+    *   
-     * If any factors contain constrained noise models, this function will fail
+    *   This function will fall back on QR factorization for any cliques containing JacobianFactor's
-     * because our current implementation cannot handle constrained noise models
+    *   with constrained noise models.
-     * in Cholesky factorization.  The function EliminatePreferCholesky()
+    *   
-     * automatically does QR instead when this is the case.
+    *   Variables are eliminated in the order specified in \c keys.
-     *
+    *   
-     * Variables are eliminated in the natural order of the variable indices of in
+    *   @param factors Factors to combine and eliminate
-     * the factors.
+    *   @param keys The variables to eliminate and their elimination ordering
-     * @param factors Factors to combine and eliminate
+    *   @return The conditional and remaining factor
-     * @param nrFrontals Number of frontal variables to eliminate.
+    *   
-     * @return The conditional and remaining factor
+    *   \addtogroup LinearSolving */
-
-     * \addtogroup LinearSolving
-     */
     friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
-      EliminateCholeskyOrdered(const GaussianFactorGraph& factors, const Ordering& keys);
+      EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
 
   private:
 
@@ -400,3 +402,4 @@ namespace gtsam {
 
 }
 
+#include <gtsam/linear/HessianFactor-inl.h>

gtsam/linear/tests/testHessianFactorUnordered.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <gtsam/base/TestableAssertions.h>
@@ -37,7 +38,8 @@ using namespace gtsam;
 const double tol = 1e-5;
 
 /* ************************************************************************* */
-TEST(HessianFactor, emptyConstructor) {
+TEST(HessianFactor, emptyConstructor)
+{
   HessianFactor f;
   DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9);        // Constant term should be zero
   EXPECT(assert_equal(Vector(), f.linearTerm()));    // Linear term should be empty
@@ -46,68 +48,39 @@ TEST(HessianFactor, emptyConstructor) {
 }
 
 /* ************************************************************************* */
-TEST(HessianFactor, ConversionConstructor) {
+TEST(HessianFactor, ConversionConstructor)
+{
+  HessianFactor expected(list_of(0)(1),
+    SymmetricBlockMatrix(list_of(2)(4)(1), Matrix_(7,7,
+      125.0000,       0.0,  -25.0000,       0.0, -100.0000,       0.0,   25.0000,
+           0.0,  125.0000,       0.0,  -25.0000,       0.0, -100.0000,  -17.5000,
+      -25.0000,       0.0,   25.0000,       0.0,       0.0,       0.0,   -5.0000,
+           0.0,  -25.0000,       0.0,   25.0000,       0.0,       0.0,    7.5000,
+     -100.0000,       0.0,       0.0,       0.0,  100.0000,       0.0,  -20.0000,
+           0.0, -100.0000,       0.0,       0.0,       0.0,  100.0000,   10.0000,
+       25.0000,  -17.5000,   -5.0000,    7.5000,  -20.0000,   10.0000,    8.2500)));
 
-  HessianFactor expected;
+  JacobianFactor jacobian(
-  expected.keys_.push_back(0);
+    0, Matrix_(4,2, -1., 0.,
-  expected.keys_.push_back(1);
+                    +0.,-1.,
-  size_t dims[] = { 2, 4, 1 };
+                     1., 0.,
-  expected.info_.resize(dims, dims+3, false);
+                    +0.,1.),
-  expected.matrix_ = Matrix_(7,7,
+    1, Matrix_(4,4, 1., 0., 0.00,  0., // f4
-         125.0000,       0.0,  -25.0000,       0.0, -100.0000,       0.0,   25.0000,
+                    0., 1., 0.00,  0., // f4
-              0.0,  125.0000,       0.0,  -25.0000,       0.0, -100.0000,  -17.5000,
+                    0., 0.,  -1.,  0., // f2
-         -25.0000,       0.0,   25.0000,       0.0,       0.0,       0.0,   -5.0000,
+                    0., 0., 0.00, -1.), // f2
-              0.0,  -25.0000,       0.0,   25.0000,       0.0,       0.0,    7.5000,
+    Vector_(4, -0.2, 0.3, 0.2, -0.1),
-        -100.0000,       0.0,       0.0,       0.0,  100.0000,       0.0,  -20.0000,
+    noiseModel::Diagonal::Sigmas(Vector_(4, 0.2, 0.2, 0.1, 0.1)));
-              0.0, -100.0000,       0.0,       0.0,       0.0,  100.0000,   10.0000,
-          25.0000,  -17.5000,   -5.0000,    7.5000,  -20.0000,   10.0000,    8.2500);
 
-  // sigmas
+  HessianFactor actual(jacobian);
-  double sigma1 = 0.2;
-  double sigma2 = 0.1;
-  Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
 
-  // the combined linear factor
+  VectorValues values = pair_list_of
-  Matrix Ax2 = Matrix_(4,2,
+    (0, Vector_(2, 1.0, 2.0))
-      // x2
+    (1, Vector_(4, 3.0, 4.0, 5.0, 6.0));
-      -1., 0.,
-      +0.,-1.,
-      1., 0.,
-      +0.,1.
-  );
-
-  Matrix Al1x1 = Matrix_(4,4,
-      // l1   x1
-      1., 0., 0.00,  0., // f4
-      0., 1., 0.00,  0., // f4
-      0., 0., -1.,  0., // f2
-      0., 0., 0.00,-1.  // f2
-  );
-
-  // the RHS
-  Vector b2(4);
-  b2(0) = -0.2;
-  b2(1) =  0.3;
-  b2(2) =  0.2;
-  b2(3) = -0.1;
-
-  vector<pair<Index, Matrix> > meas;
-  meas.push_back(make_pair(0, Ax2));
-  meas.push_back(make_pair(1, Al1x1));
-  JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
-
-  HessianFactor actual(combined);
-
-  VectorValues values(std::vector<size_t>(dims, dims+2));
-  values[0] = Vector_(2, 1.0, 2.0);
-  values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0);
-
-  EXPECT_LONGS_EQUAL(2, actual.size());
 
+  EXPECT_LONGS_EQUAL(2, (long)actual.size());
   EXPECT(assert_equal(expected, actual, 1e-9));
-
+  EXPECT_DOUBLES_EQUAL(jacobian.error(values), actual.error(values), 1e-9);
-  // error terms
-  EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9);
 }
 
 /* ************************************************************************* */
@@ -117,29 +90,23 @@ TEST(HessianFactor, Constructor1)
   Vector g = Vector_(2, -8.0, -9.0);
   double f = 10.0;
 
-  Vector dxv = Vector_(2, 1.5, 2.5);
+  VectorValues dx = pair_list_of
-
+    (0, Vector_(2, 1.5, 2.5));
-  vector<size_t> dims;
-  dims.push_back(2);
-  VectorValues dx(dims);
-
-  dx[0] = dxv;
 
   HessianFactor factor(0, G, g, f);
 
   // extract underlying parts
-  Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
+  EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin()))));
-  EXPECT(assert_equal(Matrix(G), info_matrix));
   EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
-  EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10));
+  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
-  EXPECT_LONGS_EQUAL(1, factor.size());
+  EXPECT_LONGS_EQUAL(1, (long)factor.size());
 
   // error 0.5*(f - 2*x'*g + x'*G*x)
   double expected = 80.375;
   double actual = factor.error(dx);
-  double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView<Eigen::Upper>() * dxv);
+  double expected_manual = 0.5 * (f - 2.0 * dx[0].dot(g) + dx[0].transpose() * G.selfadjointView<Eigen::Upper>() * dx[0]);
   EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10);
-  DOUBLES_EQUAL(expected, actual, 1e-10);
+  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10);
 }
 
 /* ************************************************************************* */
@@ -155,11 +122,10 @@ TEST(HessianFactor, Constructor1b)
   double f = dot(g,mu);
 
   // Check
-  Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
+  EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin()))));
-  EXPECT(assert_equal(Matrix(G), info_matrix));
   EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
-  EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10));
+  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
-  EXPECT_LONGS_EQUAL(1, factor.size());
+  EXPECT_LONGS_EQUAL(1, (long)factor.size());
 }
 
 /* ************************************************************************* */
@@ -175,13 +141,9 @@ TEST(HessianFactor, Constructor2)
   Vector dx0 = Vector_(1, 0.5);
   Vector dx1 = Vector_(2, 1.5, 2.5);
 
-  vector<size_t> dims;
+  VectorValues dx = pair_list_of
-  dims.push_back(1);
+    (0, dx0)
-  dims.push_back(2);
+    (1, dx1);
-  VectorValues dx(dims);
-
-  dx[0] = dx0;
-  dx[1] = dx1;
 
   HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
 
@@ -189,7 +151,7 @@ TEST(HessianFactor, Constructor2)
   double actual = factor.error(dx);
 
   DOUBLES_EQUAL(expected, actual, 1e-10);
-  LONGS_EQUAL(4, factor.rows());
+  LONGS_EQUAL(4, (long)factor.rows());
   DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
 
   Vector linearExpected(3);  linearExpected << g1, g2;
@@ -200,11 +162,10 @@ TEST(HessianFactor, Constructor2)
   EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
 
   // Check case when vector values is larger than factor
-  dims.push_back(2);
+  VectorValues dxLarge = pair_list_of
-  VectorValues dxLarge(dims);
+    (0, dx0)
-  dxLarge[0] = dx0;
+    (1, dx1)
-  dxLarge[1] = dx1;
+    (2, Vector_(2, 0.1, 0.2));
-  dxLarge[2] = Vector_(2, 0.1, 0.2);
   EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
 }
 
@@ -230,15 +191,10 @@ TEST(HessianFactor, Constructor3)
   Vector dx1 = Vector_(2, 1.5, 2.5);
   Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
 
-  vector<size_t> dims;
+  VectorValues dx = pair_list_of
-  dims.push_back(1);
+    (0, dx0)
-  dims.push_back(2);
+    (1, dx1)
-  dims.push_back(3);
+    (2, dx2);
-  VectorValues dx(dims);
-
-  dx[0] = dx0;
-  dx[1] = dx1;
-  dx[2] = dx2;
 
   HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
 
@@ -246,7 +202,7 @@ TEST(HessianFactor, Constructor3)
   double actual = factor.error(dx);
 
   DOUBLES_EQUAL(expected, actual, 1e-10);
-  LONGS_EQUAL(7, factor.rows());
+  LONGS_EQUAL(7, (long)factor.rows());
   DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
 
   Vector linearExpected(6);  linearExpected << g1, g2, g3;
@@ -282,16 +238,10 @@ TEST(HessianFactor, ConstructorNWay)
   Vector dx1 = Vector_(2, 1.5, 2.5);
   Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
 
-  vector<size_t> dims;
+  VectorValues dx = pair_list_of
-  dims.push_back(1);
+    (0, dx0)
-  dims.push_back(2);
+    (1, dx1)
-  dims.push_back(3);
+    (2, dx2);
-  VectorValues dx(dims);
-
-  dx[0] = dx0;
-  dx[1] = dx1;
-  dx[2] = dx2;
-
 
   std::vector<Index> js;
   js.push_back(0); js.push_back(1); js.push_back(2);
@@ -305,7 +255,7 @@ TEST(HessianFactor, ConstructorNWay)
   double actual = factor.error(dx);
 
   DOUBLES_EQUAL(expected, actual, 1e-10);
-  LONGS_EQUAL(7, factor.rows());
+  LONGS_EQUAL(7, (long)factor.rows());
   DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
 
   Vector linearExpected(6);  linearExpected << g1, g2, g3;
@@ -320,63 +270,7 @@ TEST(HessianFactor, ConstructorNWay)
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment)
+TEST(HessianFactor, CombineAndEliminate)
-{
-  Matrix G11 = Matrix_(1,1, 1.0);
-  Matrix G12 = Matrix_(1,2, 2.0, 4.0);
-  Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
-  Vector g1 = Vector_(1, -7.0);
-  Vector g2 = Vector_(2, -8.0, -9.0);
-  double f = 10.0;
-
-  Vector dx0 = Vector_(1, 0.5);
-  Vector dx1 = Vector_(2, 1.5, 2.5);
-
-  vector<size_t> dims;
-  dims.push_back(1);
-  dims.push_back(2);
-  VectorValues dx(dims);
-
-  dx[0] = dx0;
-  dx[1] = dx1;
-
-  HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f);
-
-  // Make a copy
-  HessianFactor copy1(originalFactor);
-
-  double expected = 90.5;
-  double actual = copy1.error(dx);
-
-  DOUBLES_EQUAL(expected, actual, 1e-10);
-  LONGS_EQUAL(4, copy1.rows());
-  DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10);
-
-  Vector linearExpected(3);  linearExpected << g1, g2;
-  EXPECT(assert_equal(linearExpected, copy1.linearTerm()));
-
-  EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin())));
-  EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1)));
-  EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1)));
-
-  // Make a copy using the assignment operator
-  HessianFactor copy2;
-  copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references
-
-  actual = copy2.error(dx);
-  DOUBLES_EQUAL(expected, actual, 1e-10);
-  LONGS_EQUAL(4, copy2.rows());
-  DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10);
-
-  EXPECT(assert_equal(linearExpected, copy2.linearTerm()));
-
-  EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin())));
-  EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1)));
-  EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1)));
-}
-
-/* ************************************************************************* */
-TEST_UNSAFE(HessianFactor, CombineAndEliminate)
 {
   Matrix A01 = Matrix_(3,3,
       1.0, 0.0, 0.0,
@@ -417,29 +311,18 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate)
   // create a full, uneliminated version of the factor
   JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
 
-  // perform elimination
+  // perform elimination on jacobian
-  GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
+  GaussianConditional::shared_ptr expectedConditional;
-
+  JacobianFactor::shared_ptr expectedRemainingFactor;
-  // create expected Hessian after elimination
+  boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
-  HessianFactor expectedCholeskyFactor(expectedFactor);
-
-  // Convert all factors to hessians
-  FactorGraph<HessianFactor> hessians;
-  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
-    if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
-      hessians.push_back(hf);
-    else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
-      hessians.push_back(boost::make_shared<HessianFactor>(*jf));
-    else
-      CHECK(false);
-  }
 
   // Eliminate
-  GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
+  GaussianConditional::shared_ptr actualConditional;
-  HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
+  HessianFactor::shared_ptr actualCholeskyFactor;
+  boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
 
-  EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
+  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
-  EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
+  EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
 }
 
 /* ************************************************************************* */
@@ -481,13 +364,10 @@ TEST(HessianFactor, eliminate2 )
 
   // eliminate the combined factor
   HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
-  FactorGraph<HessianFactor> combinedLFG_Chol;
+  GaussianFactorGraph combinedLFG_Chol = list_of(combinedLF_Chol);
-  combinedLFG_Chol.push_back(combinedLF_Chol);
 
-  GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
+  std::pair<GaussianConditional::shared_ptr, HessianFactor::shared_ptr> actual_Chol =
-      combinedLFG_Chol, 1);
+    EliminateCholesky(combinedLFG_Chol, Ordering(list_of(0)));
-  HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
-      HessianFactor>(actual_Chol.second);
 
   // create expected Conditional Gaussian
   double oldSigma = 0.0894427; // from when R was made unit
@@ -500,8 +380,8 @@ TEST(HessianFactor, eliminate2 )
       +0.00,-0.20,+0.00,-0.80
   )/oldSigma;
   Vector d = Vector_(2,0.2,-0.14)/oldSigma;
-  GaussianConditional expectedCG(0,d,R11,1,S12,ones(2));
+  GaussianConditional expectedCG(0, d, R11, 1, S12);
-  EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4));
+  EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
 
   // the expected linear factor
   double sigma = 0.2236;
@@ -512,89 +392,7 @@ TEST(HessianFactor, eliminate2 )
   )/sigma;
   Vector b1 = Vector_(2,0.0,0.894427);
   JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
-  EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3));
+  EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
-}
-
-/* ************************************************************************* */
-TEST(HessianFactor, eliminateUnsorted) {
-
-  JacobianFactor::shared_ptr factor1(
-      new JacobianFactor(0,
-                         Matrix_(3,3,
-                                 44.7214,     0.0,       0.0,
-                                 0.0,     44.7214,       0.0,
-                                 0.0,         0.0,   44.7214),
-                         1,
-                         Matrix_(3,3,
-                                 -0.179168,    -44.721,  0.717294,
-                                 44.721, -0.179168,  -44.9138,
-                                 0.0,         0.0,  -44.7214),
-                         Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
-                         noiseModel::Unit::Create(3)));
-  HessianFactor::shared_ptr unsorted_factor2(
-      new HessianFactor(JacobianFactor(0,
-                        Matrix_(6,3,
-                                25.8367,    0.1211,    0.0593,
-                                    0.0,   23.4099,   30.8733,
-                                    0.0,       0.0,   25.8729,
-                                    0.0,       0.0,       0.0,
-                                    0.0,       0.0,       0.0,
-                                    0.0,       0.0,       0.0),
-                        1,
-                        Matrix_(6,3,
-                                25.7429,   -1.6897,    0.4587,
-                                 1.6400,   23.3095,   -8.4816,
-                                 0.0034,    0.0509,  -25.7855,
-                                 0.9997,   -0.0002,    0.0824,
-                                    0.0,    0.9973,    0.9517,
-                                    0.0,       0.0,    0.9973),
-                        Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-                        noiseModel::Unit::Create(6))));
-  Permutation permutation(2);
-  permutation[0] = 1;
-  permutation[1] = 0;
-  unsorted_factor2->permuteWithInverse(permutation);
-
-  HessianFactor::shared_ptr sorted_factor2(
-      new HessianFactor(JacobianFactor(0,
-                        Matrix_(6,3,
-                                25.7429,   -1.6897,    0.4587,
-                                 1.6400,   23.3095,   -8.4816,
-                                 0.0034,    0.0509,  -25.7855,
-                                 0.9997,   -0.0002,    0.0824,
-                                    0.0,    0.9973,    0.9517,
-                                    0.0,       0.0,    0.9973),
-                        1,
-                        Matrix_(6,3,
-                                25.8367,    0.1211,    0.0593,
-                                    0.0,   23.4099,   30.8733,
-                                    0.0,       0.0,   25.8729,
-                                    0.0,       0.0,       0.0,
-                                    0.0,       0.0,       0.0,
-                                    0.0,       0.0,       0.0),
-                        Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-                        noiseModel::Unit::Create(6))));
-
-  GaussianFactorGraph sortedGraph;
-//  sortedGraph.push_back(factor1);
-  sortedGraph.push_back(sorted_factor2);
-
-  GaussianFactorGraph unsortedGraph;
-//  unsortedGraph.push_back(factor1);
-  unsortedGraph.push_back(unsorted_factor2);
-
-  GaussianConditional::shared_ptr expected_bn;
-  GaussianFactor::shared_ptr expected_factor;
-  boost::tie(expected_bn, expected_factor) =
-      EliminatePreferCholesky(sortedGraph, 1);
-
-  GaussianConditional::shared_ptr actual_bn;
-  GaussianFactor::shared_ptr actual_factor;
-  boost::tie(actual_bn, actual_factor) =
-      EliminatePreferCholesky(unsortedGraph, 1);
-
-  EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
-  EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
 }
 
 /* ************************************************************************* */
@@ -613,8 +411,7 @@ TEST(HessianFactor, combine) {
   Vector b = Vector_(2, 2.23606798,-1.56524758);
   SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
   GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
-  FactorGraph<GaussianFactor> factors;
+  GaussianFactorGraph factors = list_of(f);
-  factors.push_back(f);
 
   // Form Ab' * Ab
   HessianFactor actual(factors);
@@ -628,7 +425,7 @@ TEST(HessianFactor, combine) {
        0.0, -100.0000,       0.0,   20.0000,       0.0,   80.0000,   14.0000,
    25.0000,  -17.5000,   -5.0000,    3.5000,  -20.0000,   14.0000,    7.4500);
   EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
-      Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
+      Matrix(actual.matrixObject().full().triangularView<Eigen::Upper>()), tol));
 
 }