new functions for cg solver

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

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

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); }