From 9ff18b4e4e274c9a801d2b7104cd6be4d691f8b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Oct 2011 04:28:47 +0000 Subject: [PATCH] Wrapped GaussianFactorGraph.sparse and provided example usage in PlanarSLAMExample_easy --- examples/matlab/PlanarSLAMExample_easy.m | 8 ++++++-- gtsam-broken.h | 1 - gtsam.h | 1 + gtsam/linear/tests/testGaussianFactorGraph.cpp | 5 +++-- 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m index 994aee04a..85a517ede 100644 --- a/examples/matlab/PlanarSLAMExample_easy.m +++ b/examples/matlab/PlanarSLAMExample_easy.m @@ -77,5 +77,9 @@ result.print('final result'); %% Print out the corresponding dense matrix ord = graph.orderingCOLAMD(result); gfg = graph.linearize(result,ord); -A_b = gfg.denseJacobian; -AtA_Atb = gfg.denseHessian; +denseAb = gfg.denseJacobian + +%% Get sparse matrix +IJS = gfg.sparse([12 7 4 10 1 14]'); +Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); +spy(Ab); diff --git a/gtsam-broken.h b/gtsam-broken.h index 7c8bfca6d..09113a695 100644 --- a/gtsam-broken.h +++ b/gtsam-broken.h @@ -108,7 +108,6 @@ class GaussianFactorGraph { GaussianBayesNet* eliminate_(const Ordering& ordering); VectorValues* optimize_(const Ordering& ordering); pair matrix(const Ordering& ordering) const; - Matrix sparse(const Ordering& ordering) const; VectorValues* steepestDescent_(const VectorValues& x0) const; VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; }; diff --git a/gtsam.h b/gtsam.h index 36af38f63..fd6bd962e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -110,6 +110,7 @@ class GaussianFactorGraph { void combine(const GaussianFactorGraph& lfg); Matrix denseJacobian() const; Matrix denseHessian() const; + Matrix sparse(Vector columnIndices) const; }; class Landmark2 { diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index f33160508..da02bb106 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -56,12 +56,13 @@ TEST(GaussianFactorGraph, initialization) { JacobianFactor factor = *graph[0]; // Test sparse, which takes a vector and returns a matrix, used in MATLAB + // Note that this the augmented vector and the RHS is in column 7 Matrix expectedIJS = Matrix_(3,22, 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., - 1., 2., 5., 5., 1., 2., 3., 4., 5., 5., 1., 2., 5., 6., 5., 5., 3., 4., 5., 6., 5., 5., + 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 ); - Vector columnIndices = Vector_(3,1.0,3.0,5.0); + Vector columnIndices = Vector_(4,1.0,3.0,5.0,7.0); Matrix actualIJS = fg.sparse(columnIndices); EQUALITY(expectedIJS, actualIJS); }