12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Wrapped GaussianFactorGraph.sparse and provided example usage in PlanarSLAMExample_easy

release/4.3a0
Frank Dellaert 2011-10-29 04:28:47 +00:00
parent 65616dbde5
commit 9ff18b4e4e
4 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -77,5 +77,9 @@ result.print('final result');
%% Print out the corresponding dense matrix %% Print out the corresponding dense matrix
ord = graph.orderingCOLAMD(result); ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord); gfg = graph.linearize(result,ord);
A_b = gfg.denseJacobian; denseAb = gfg.denseJacobian
AtA_Atb = gfg.denseHessian;
%% Get sparse matrix
IJS = gfg.sparse([12 7 4 10 1 14]');
Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:));
spy(Ab);

1
gtsam-broken.h
View File

@ -108,7 +108,6 @@ class GaussianFactorGraph {
GaussianBayesNet* eliminate_(const Ordering& ordering); GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorValues* optimize_(const Ordering& ordering); VectorValues* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const; pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const;
VectorValues* steepestDescent_(const VectorValues& x0) const; VectorValues* steepestDescent_(const VectorValues& x0) const;
VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
}; };

1
gtsam.h
View File

@ -110,6 +110,7 @@ class GaussianFactorGraph {
void combine(const GaussianFactorGraph& lfg); void combine(const GaussianFactorGraph& lfg);
Matrix denseJacobian() const; Matrix denseJacobian() const;
Matrix denseHessian() const; Matrix denseHessian() const;
Matrix sparse(Vector columnIndices) const;
}; };
class Landmark2 { class Landmark2 {

5
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -56,12 +56,13 @@ TEST(GaussianFactorGraph, initialization) {
JacobianFactor factor = *graph[0]; JacobianFactor factor = *graph[0];
// Test sparse, which takes a vector and returns a matrix, used in MATLAB // 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, 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., 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 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); Matrix actualIJS = fg.sparse(columnIndices);
EQUALITY(expectedIJS, actualIJS); EQUALITY(expectedIJS, actualIJS);
} }