added raw memory access version of hessianDiagonal

gtsam/linear/JacobianFactor.cpp

@@ -59,8 +59,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor() :
-    Ab_(cref_list_of<1>(1), 0)
+    Ab_(cref_list_of<1>(1), 0) {
-  {
   getb().setZero();
 }
 
@@ -72,48 +71,42 @@ namespace gtsam {
   else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
     *this = JacobianFactor(*rhs);
   else
-      throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
+    throw std::invalid_argument(
+        "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
 }
 
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor(const Vector& b_in) :
-    Ab_(cref_list_of<1>(1), b_in.size())
+    Ab_(cref_list_of<1>(1), b_in.size()) {
-  {
   getb() = b_in;
 }
 
 /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(Key i1, const Matrix& A1,
+JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
-    const Vector& b, const SharedDiagonal& model)
+    const SharedDiagonal& model) {
-  {
   fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
 }
 
 /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(
+JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
-    const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
+    const Matrix& A2, const Vector& b, const SharedDiagonal& model) {
-    const Vector& b, const SharedDiagonal& model)
+  fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model);
-  {
-    fillTerms(cref_list_of<2>
-      (make_pair(i1,A1))
-      (make_pair(i2,A2)), b, model);
 }
 
 /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(
+JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
-    const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
+    const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
-      Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model)
+    const SharedDiagonal& model) {
-  {
+  fillTerms(
-    fillTerms(cref_list_of<3>
+      cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)),
-      (make_pair(i1,A1))
+      b, model);
-      (make_pair(i2,A2))
-      (make_pair(i3,A3)), b, model);
 }
 
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor(const HessianFactor& factor) :
-    Base(factor), Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), factor.rows()))
+    Base(factor), Ab_(
-  {
+        VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(),
+            factor.rows())) {
   // Copy Hessian into our matrix and then do in-place Cholesky
   Ab_.full() = factor.matrixObject().full();
 
@@ -138,31 +131,33 @@ namespace gtsam {
 // Helper functions for combine constructor
 namespace {
 boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
-      const FastVector<JacobianFactor::shared_ptr>& factors, const FastVector<VariableSlots::const_iterator>& variableSlots)
+    const FastVector<JacobianFactor::shared_ptr>& factors,
-    {
+    const FastVector<VariableSlots::const_iterator>& variableSlots) {
   gttic(countDims);
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
   FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
 #else
-      FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
+  FastVector<DenseIndex> varDims(variableSlots.size(),
+      numeric_limits<DenseIndex>::max());
 #endif
   DenseIndex m = 0;
   DenseIndex n = 0;
-      for(size_t jointVarpos = 0; jointVarpos < variableSlots.size(); ++jointVarpos)
+  for (size_t jointVarpos = 0; jointVarpos < variableSlots.size();
-      {
+      ++jointVarpos) {
     const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
 
     assert(slots->second.size() == factors.size());
 
     bool foundVariable = false;
-        for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI)
+    for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
-        {
+        ++sourceFactorI) {
       const size_t sourceVarpos = slots->second[sourceFactorI];
       if (sourceVarpos != VariableSlots::Empty) {
         const JacobianFactor& sourceFactor = *factors[sourceFactorI];
         if (sourceFactor.cols() > 1) {
           foundVariable = true;
-              DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
+          DenseIndex vardim = sourceFactor.getDim(
+              sourceFactor.begin() + sourceVarpos);
 
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
           if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
@@ -186,7 +181,8 @@ namespace gtsam {
     }
 
     if (!foundVariable)
-          throw std::invalid_argument("Unable to determine dimensionality for all variables");
+      throw std::invalid_argument(
+          "Unable to determine dimensionality for all variables");
   }
 
   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
@@ -203,15 +199,15 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
-    FastVector<JacobianFactor::shared_ptr>
+FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
-      _convertOrCastToJacobians(const GaussianFactorGraph& factors)
+    const GaussianFactorGraph& factors) {
-    {
   gttic(Convert_to_Jacobians);
   FastVector<JacobianFactor::shared_ptr> jacobians;
   jacobians.reserve(factors.size());
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
     if (factor) {
-          if(JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
+      if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
+          JacobianFactor>(factor))
         jacobians.push_back(jf);
       else
         jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
@@ -222,11 +218,9 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
-    const GaussianFactorGraph& graph,
     boost::optional<const Ordering&> ordering,
-    boost::optional<const VariableSlots&> variableSlots)
+    boost::optional<const VariableSlots&> variableSlots) {
-  {
   gttic(JacobianFactor_combine_constructor);
 
   // Compute VariableSlots if one was not provided
@@ -237,7 +231,8 @@ namespace gtsam {
   }
 
   // Cast or convert to Jacobians
-    FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
+  FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
+      graph);
 
   gttic(Order_slots);
   // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
@@ -251,8 +246,10 @@ namespace gtsam {
     size_t nOrderingSlotsUsed = 0;
     orderedSlots.resize(ordering->size());
     FastMap<Key, size_t> inverseOrdering = ordering->invert();
-      for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item) {
+    for (VariableSlots::const_iterator item = variableSlots->begin();
-        FastMap<Key, size_t>::const_iterator orderingPosition = inverseOrdering.find(item->first);
+        item != variableSlots->end(); ++item) {
+      FastMap<Key, size_t>::const_iterator orderingPosition =
+          inverseOrdering.find(item->first);
       if (orderingPosition == inverseOrdering.end()) {
         unorderedSlots.push_back(item);
       } else {
@@ -271,7 +268,8 @@ namespace gtsam {
   } else {
     // If no ordering is provided, arrange the slots as they were, which will be sorted
     // numerically since VariableSlots uses a map sorting on Key.
-      for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item)
+    for (VariableSlots::const_iterator item = variableSlots->begin();
+        item != variableSlots->end(); ++item)
       orderedSlots.push_back(item);
   }
   gttoc(Order_slots);
@@ -285,8 +283,10 @@ namespace gtsam {
   gttic(allocate);
   Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
   Base::keys_.resize(orderedSlots.size());
-    boost::range::copy(    // Get variable keys
+  boost::range::copy(
-      orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());
+      // Get variable keys
+      orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys,
+      Base::keys_.begin());
   gttoc(allocate);
 
   // Loop over slots in combined factor and copy blocks from source factors
@@ -301,10 +301,12 @@ namespace gtsam {
       const size_t sourceSlot = varslot->second[factorI];
       const DenseIndex sourceRows = jacobians[factorI]->rows();
       if (sourceRows > 0) {
-          JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
+        JacobianFactor::ABlock::RowsBlockXpr destBlock(
+            destSlot.middleRows(nextRow, sourceRows));
         // Copy if exists in source factor, otherwise set zero
         if (sourceSlot != VariableSlots::Empty)
-            destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
+          destBlock = jacobians[factorI]->getA(
+              jacobians[factorI]->begin() + sourceSlot);
         else
           destBlock.setZero();
         nextRow += sourceRows;
@@ -328,7 +330,8 @@ namespace gtsam {
         // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
         if (!sigmas)
           sigmas = Vector::Constant(m, 1.0);
-          sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas();
+        sigmas->segment(nextRow, sourceRows) =
+            jacobians[factorI]->get_model()->sigmas();
         if (jacobians[factorI]->isConstrained())
           anyConstrained = true;
       }
@@ -342,13 +345,14 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
-  void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const
+void JacobianFactor::print(const string& s,
-  {
+    const KeyFormatter& formatter) const {
   if (!s.empty())
     cout << s << "\n";
   for (const_iterator key = begin(); key != end(); ++key) {
-      cout <<
+    cout
-        formatMatrixIndented((boost::format("  A[%1%] = ") % formatter(*key)).str(), getA(key))
+        << formatMatrixIndented(
+            (boost::format("  A[%1%] = ") % formatter(*key)).str(), getA(key))
         << endl;
   }
   cout << formatMatrixIndented("  b = ", getb(), true) << "\n";
@@ -360,8 +364,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 // Check if two linear factors are equal
-  bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const
+bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
-  {
   if (!dynamic_cast<const JacobianFactor*>(&f_))
     return false;
   else {
@@ -385,8 +388,8 @@ namespace gtsam {
     constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
     constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
     for (size_t row = 0; row < (size_t) Ab1.rows(); ++row)
-        if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) &&
+      if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol)
-            !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
+          && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
         return false;
 
     return true;
@@ -411,7 +414,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 double JacobianFactor::error(const VectorValues& c) const {
-    if (empty()) return 0;
+  if (empty())
+    return 0;
   Vector weighted = error_vector(c);
   return 0.5 * weighted.dot(weighted);
 }
@@ -441,14 +445,14 @@ namespace gtsam {
 /* ************************************************************************* */
 VectorValues JacobianFactor::hessianDiagonal() const {
   VectorValues d;
-    for(size_t pos=0; pos<size(); ++pos)
+  for (size_t pos = 0; pos < size(); ++pos) {
-    {
     Key j = keys_[pos];
     size_t nj = Ab_(pos).cols();
     Vector dj(nj);
     for (size_t k = 0; k < nj; ++k) {
       Vector column_k = Ab_(pos).col(k);
-        if (model_) column_k = model_->whiten(column_k);
+      if (model_)
+        column_k = model_->whiten(column_k);
       dj(k) = dot(column_k, column_k);
     }
     d.insert(j, dj);
@@ -457,18 +461,32 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
+// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
 void JacobianFactor::hessianDiagonal(double* d) const {
-    throw std::runtime_error("JacobianFactor::hessianDiagonal non implemented (use VectorValues version)");
+
+  // Use eigen magic to access raw memory
+  typedef Eigen::Matrix<double, 9, 1> DVector;
+  typedef Eigen::Map<DVector> DMap;
+
+  // Loop over all variables in the factor
+  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
+    // Get the diagonal block, and insert its diagonal
+    DVector dj;
+    for (size_t k = 0; k < 9; ++k)
+      dj(k) = Ab_(j).col(k).squaredNorm();
+
+    DMap(d + 9 * j) += dj;
+  }
 }
 
 /* ************************************************************************* */
 map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
   map<Key, Matrix> blocks;
-    for(size_t pos=0; pos<size(); ++pos)
+  for (size_t pos = 0; pos < size(); ++pos) {
-    {
     Key j = keys_[pos];
     Matrix Aj = Ab_(pos);
-      if (model_) Aj = model_->Whiten(Aj);
+    if (model_)
+      Aj = model_->Whiten(Aj);
     blocks.insert(make_pair(j, Aj.transpose() * Aj));
   }
   return blocks;
@@ -477,7 +495,8 @@ namespace gtsam {
 /* ************************************************************************* */
 Vector JacobianFactor::operator*(const VectorValues& x) const {
   Vector Ax = zero(Ab_.rows());
-    if (empty()) return Ax;
+  if (empty())
+    return Ax;
 
   // Just iterate over all A matrices and multiply in correct config part
   for (size_t pos = 0; pos < size(); ++pos)
@@ -488,12 +507,10 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
-      VectorValues& x) const
+    VectorValues& x) const {
-  {
   Vector E = alpha * (model_ ? model_->whiten(e) : e);
   // Just iterate over all A matrices and insert Ai^e into VectorValues
-    for(size_t pos=0; pos<size(); ++pos)
+  for (size_t pos = 0; pos < size(); ++pos) {
-    {
     Key j = keys_[pos];
     // Create the value as a zero vector if it does not exist.
     pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
@@ -511,29 +528,37 @@ namespace gtsam {
   transposeMultiplyAdd(alpha, Ax, y);
 }
 
-  void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const {
+void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
+    double* y, std::vector<size_t> keys) const {
 
   // Use eigen magic to access raw memory
   typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
   typedef Eigen::Map<DVector> DMap;
   typedef Eigen::Map<const DVector> ConstDMap;
 
-	  if (empty()) return;
+  if (empty())
+    return;
   Vector Ax = zero(Ab_.rows());
 
   // Just iterate over all A matrices and multiply in correct config part
   for (size_t pos = 0; pos < size(); ++pos)
-	       Ax += Ab_(pos) * ConstDMap(x + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]);
+    Ax += Ab_(pos)
+        * ConstDMap(x + keys[keys_[pos]],
+            keys[keys_[pos] + 1] - keys[keys_[pos]]);
 
   // Deal with noise properly, need to Double* whiten as we are dividing by variance
-	     if  (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
+  if (model_) {
+    model_->whitenInPlace(Ax);
+    model_->whitenInPlace(Ax);
+  }
 
   // multiply with alpha
   Ax *= alpha;
 
   // Again iterate over all A matrices and insert Ai^e into y
   for (size_t pos = 0; pos < size(); ++pos)
-	       DMap(y + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]) += Ab_(pos).transpose() * Ax;
+    DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
+        pos).transpose() * Ax;
 
 }
 
@@ -594,9 +619,9 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
-  std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
+std::pair<boost::shared_ptr<GaussianConditional>,
-    JacobianFactor::eliminate(const Ordering& keys)
+    boost::shared_ptr<JacobianFactor> > JacobianFactor::eliminate(
-  {
+    const Ordering& keys) {
   GaussianFactorGraph graph;
   graph.add(*this);
   return EliminateQR(graph, keys);
@@ -613,9 +638,9 @@ namespace gtsam {
 }
 
 /* ************************************************************************* */
-  std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
+std::pair<boost::shared_ptr<GaussianConditional>,
-    EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys)
+    boost::shared_ptr<JacobianFactor> > EliminateQR(
-  {
+    const GaussianFactorGraph& factors, const Ordering& keys) {
   gttic(EliminateQR);
   // Combine and sort variable blocks in elimination order
   JacobianFactor::shared_ptr jointFactor;
@@ -638,18 +663,20 @@ namespace gtsam {
   jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
 
   // Split elimination result into conditional and remaining factor
-    GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(keys.size());
+  GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
+      keys.size());
 
   return make_pair(conditional, jointFactor);
 }
 
 /* ************************************************************************* */
-  GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals)
+GaussianConditional::shared_ptr JacobianFactor::splitConditional(
-  {
+    size_t nrFrontals) {
   gttic(JacobianFactor_splitConditional);
 
   if (nrFrontals > size())
-      throw std::invalid_argument("Requesting to split more variables than exist using JacobianFactor::splitConditional");
+    throw std::invalid_argument(
+        "Requesting to split more variables than exist using JacobianFactor::splitConditional");
 
   DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();
 
@@ -661,15 +688,17 @@ namespace gtsam {
   if (model_) {
     if ((DenseIndex) model_->dim() < frontalDim)
       throw IndeterminantLinearSystemException(this->keys().front());
-      conditionalNoiseModel =
+    conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
-        noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
+        model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
   }
-    GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
+  GaussianConditional::shared_ptr conditional = boost::make_shared<
-      Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
+      GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
-    const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) - Ab_.rowStart() - frontalDim;
+  const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
+      - Ab_.rowStart() - frontalDim;
   const DenseIndex remainingRows =
-      model_ ? std::min(model_->sigmas().size() - frontalDim, maxRemainingRows)
+      model_ ?
-      : maxRemainingRows;
+          std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
+          maxRemainingRows;
   Ab_.rowStart() += frontalDim;
   Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
   Ab_.firstBlock() += nrFrontals;
@@ -681,9 +710,11 @@ namespace gtsam {
   // Set sigmas with the right model
   if (model_) {
     if (model_->isConstrained())
-        model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
+      model_ = noiseModel::Constrained::MixedSigmas(
+          model_->sigmas().tail(remainingRows));
     else
-        model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
+      model_ = noiseModel::Diagonal::Sigmas(
+          model_->sigmas().tail(remainingRows));
     assert(model_->dim() == (size_t)Ab_.rows());
   }
   gttoc(remaining_factor);