diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index ff5b5f71f..c260e012e 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -112,6 +112,14 @@ namespace gtsam { return entries; } + /* ************************************************************************* */ + Matrix GaussianFactorGraph::denseJacobian() const { + // combine all factors + JacobianFactor combined(*CombineJacobians(*convertCastFactors > (), VariableSlots(*this))); + return combined.matrix_augmented(); + } + // VectorValues GaussianFactorGraph::allocateVectorValuesb() const { // std::vector dimensions(size()) ; // Index i = 0 ; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 49a6cdac8..9ab83def1 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/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 > sparseJacobian(const std::vector& 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 ; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ea90e7249..46f7431ca 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/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 > (), VariableSlots(factors)));