diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m index a46217deb..668b1196b 100644 --- a/examples/matlab/PlanarSLAMExample_easy.m +++ b/examples/matlab/PlanarSLAMExample_easy.m @@ -74,12 +74,14 @@ initialEstimate.print('initial estimate'); result = graph.optimize_(initialEstimate); result.print('final result'); -%% Print out the corresponding dense matrix +%% Get the corresponding dense matrix ord = graph.orderingCOLAMD(result); gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian +denseAb = gfg.denseJacobian; -%% Get sparse matrix -IJS = gfg.sparse(); +%% Get sparse matrix A and RHS b +IJS = gfg.sparseJacobian_(); Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); -spy(Ab(:,1:end-1)); +A = Ab(:,1:end-1); +b = full(Ab(:,end)); +spy(A); diff --git a/gtsam.h b/gtsam.h index c45fbf48c..034f9ef85 100644 --- a/gtsam.h +++ b/gtsam.h @@ -110,7 +110,7 @@ class GaussianFactorGraph { void combine(const GaussianFactorGraph& lfg); Matrix denseJacobian() const; Matrix denseHessian() const; - Matrix sparse() const; + Matrix sparseJacobian_() const; }; class Landmark2 { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8fc1711db..334c18676 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -141,7 +141,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparse() const { + Matrix GaussianFactorGraph::sparseJacobian_() const { // call sparseJacobian typedef boost::tuple triplet; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 7f86fbf50..631d3872d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -160,7 +160,7 @@ namespace gtsam { * 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 */ - Matrix sparse() const; + Matrix sparseJacobian_() const; /** * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b