From fff86f98b58666dfaa875422376cad0db92017e6 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 7 Feb 2011 06:09:16 +0000
Subject: [PATCH] Optimizations converting JacobianFactor to HessianFactor
 (i.e. forming A^T * A)

---
 gtsam/linear/HessianFactor.cpp | 16 +++++++++++++---
 gtsam/linear/NoiseModel.h      |  6 ++++++
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 242b14a6a..9f23bf4b8 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -150,9 +150,19 @@ namespace gtsam {
     // Copy the matrix data depending on what type of factor we're copying from
     if(dynamic_cast<const JacobianFactor*>(&gf)) {
       const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
-      JacobianFactor whitened(jf.whiten());
-      info_.copyStructureFrom(whitened.Ab_);
-      matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_);
+      if(jf.model_->isConstrained())
+        throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
+      else {
+        typedef Eigen::Map<Eigen::MatrixXd> EigenMap;
+        typedef typeof(EigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock;
+        EigenBlock A(EigenMap(&jf.matrix_(0,0),jf.matrix_.size1(),jf.matrix_.size2()).block(
+            jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().size1(), jf.Ab_.full().size2()));
+        typedef typeof(Eigen::Map<Eigen::VectorXd>(&jf.model_->invsigmas()(0),0).asDiagonal()) EigenDiagonal;
+        EigenDiagonal R(Eigen::Map<Eigen::VectorXd>(&jf.model_->invsigmas()(0),jf.model_->dim()).asDiagonal());
+        info_.copyStructureFrom(jf.Ab_);
+        EigenMap L(EigenMap(&matrix_(0,0), matrix_.size1(), matrix_.size2()));
+        L.noalias() = A.transpose() * R * R * A;
+      }
     } else if(dynamic_cast<const HessianFactor*>(&gf)) {
       const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
       info_.assignNoalias(hf.info_);
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index af738d94f..aebbdddf7 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -239,6 +239,12 @@ namespace gtsam {
 			inline const Vector& sigmas() const { return sigmas_; }
 			inline double sigma(size_t i) const { return sigmas_(i); }
 
+			/**
+			 * Return sqrt precisions
+			 */
+			const Vector& invsigmas() const { return invsigmas_; }
+			double invsigma(size_t i) const { return invsigmas_(i); }
+
 			/**
 			 * generate random variate
 			 */