From c9b57ccd74f170007a1d7de11701751ce5920dfb Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Tue, 2 Nov 2010 16:04:23 +0000
Subject: [PATCH] new functions for cg solver

---
 gtsam/linear/GaussianFactorGraph.cpp |  6 ++++++
 gtsam/linear/GaussianFactorGraph.h   |  7 +++++++
 gtsam/linear/VectorValues.h          | 14 ++++++++++++++
 3 files changed, 27 insertions(+)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 0531bbf55..547ee1155 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -264,4 +264,10 @@ void GaussianFactorGraph::getb(VectorValues &b) const {
 	}
 }
 
+VectorValues GaussianFactorGraph::getb() const {
+  VectorValues b = allocateVectorValuesb() ;
+  getb(b) ;
+  return b ;
+}
+
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index be0c30c1a..1a02167c7 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -55,6 +55,12 @@ namespace gtsam {
       push_back(fg);
     }
 
+	/* dummy constructor, to be compatible with conjugate gradient solver */
+    template<class DERIVEDFACTOR>
+    GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg, const VectorValues &x0) {
+      push_back(fg);
+    }
+
   	/** Add a null factor */
     void add(const Vector& b) {
     	push_back(sharedFactor(new GaussianFactor(b)));
@@ -168,6 +174,7 @@ namespace gtsam {
 	void multiply(const VectorValues &x, VectorValues &r) const ;
 	void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
 	void getb(VectorValues &b) const ;
+	VectorValues getb() const ;
   };
 
 } // namespace gtsam
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 2c82fd5ca..90aeef935 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -96,6 +96,20 @@ public:
   iterator end() { return _impl_iterator<VectorValues>(*this, varStarts_.size()-1); }
   const_iterator end() const { return _impl_iterator<const VectorValues>(*this, varStarts_.size()-1); }
 
+  double *ptr(Index idx = 0) { return values_.data().begin() + varStarts_[idx] ; }
+  const double *ptr(Index idx = 0) const { return values_.data().begin() + varStarts_[idx] ; }
+
+  /* return the dimension spec of this vector*/
+  template <typename T>
+  std::vector<T> getDimSpec() const {
+    const Index n = this->size() ;
+    std::vector<T> spec(n) ;
+    for ( Index i = 0 ; i < n ; ++i ) {
+      spec[i] = varStarts_[i+1] - varStarts_[i] ;
+    }
+    return spec;
+  }
+
   /** Reserve space for a total number of variables and dimensionality */
   void reserve(Index nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); }