Added denseJacobian() to GaussianFactorGraph

gtsam/linear/GaussianFactorGraph.cpp

@@ -112,6 +112,14 @@ namespace gtsam {
 		return entries;
 	}
 
+	/* ************************************************************************* */
+	Matrix GaussianFactorGraph::denseJacobian() const {
+		// combine all factors
+	  JacobianFactor combined(*CombineJacobians(*convertCastFactors<FactorGraph<
+				JacobianFactor> > (), VariableSlots(*this)));
+	  return combined.matrix_augmented();
+	}
+
 	//  VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
 	//    std::vector<size_t> dimensions(size()) ;
 	//    Index i = 0 ;

gtsam/linear/GaussianFactorGraph.h

@@ -152,13 +152,19 @@ namespace gtsam {
     void combine(const GaussianFactorGraph &lfg);
 
     /**
-     * Return vector of i, j, and s to generate an m-by-n sparse Jacobain matrix
+     * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix
      * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
      * The standard deviations are baked into A and b
      * @param first column index for each variable
      */
     std::vector<boost::tuple<size_t,size_t,double> > sparseJacobian(const std::vector<size_t>& columnIndices) const;
 
+    /**
+     * Return a dense m-by-n Jacobian matrix, augmented with b
+     * with standard deviations are baked into A and b
+     */
+    Matrix denseJacobian() const;
+
     // get b
 //    void getb(VectorValues &b) const ;
 //    VectorValues getb() const ;

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -377,6 +377,10 @@ TEST(GaussianFactor, eliminateFrontals)
   factors.push_back(factor3);
   factors.push_back(factor4);
 
+  // extract the dense matrix for the graph
+  Matrix actualDense = factors.denseJacobian();
+  EXPECT(assert_equal(2.0 * Ab, actualDense));
+
   // Create combined factor
   JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors<FactorGraph<
 			JacobianFactor> > (), VariableSlots(factors)));