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

Added joint marginals and unit tests

release/4.3a0
Richard Roberts 2012-05-15 00:01:38 +00:00
parent bd76692794
commit 516e1610a1
4 changed files with 215 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
.cproject
View File

@ -844,14 +844,30 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1028,6 +1044,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>SimpleRotation.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1635,14 +1659,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1803,14 +1819,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="UGM_small.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>UGM_small.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2122,110 +2130,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap_gtsam" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testMarginals.run" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j8</buildArguments>
<buildTarget>wrap_gtsam</buildTarget> <buildTarget>testMarginals.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="verbose wrap_gtsam" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>VERBOSE=1</buildArguments>
<buildTarget>wrap_gtsam</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.discrete" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>check.discrete</buildTarget> <buildTarget>wrap.testSpirit.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap_gtsam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable</buildTarget> <buildTarget>wrap.testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.dynamics_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.dynamics_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.slam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.slam_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.base_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>

74
gtsam/nonlinear/Marginals.cpp
View File

@ -17,11 +17,13 @@
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/Dense> #include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) { Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) {
// Compute COLAMD ordering // Compute COLAMD ordering
@ -30,6 +32,9 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
// Linearize graph // Linearize graph
graph_ = *graph.linearize(solution, ordering_); graph_ = *graph.linearize(solution, ordering_);
// Store values
values_ = solution;
// Compute BayesTree // Compute BayesTree
factorization_ = factorization; factorization_ = factorization;
if(factorization_ == CHOLESKY) if(factorization_ == CHOLESKY)
@ -38,10 +43,12 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate(); bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
} }
/* ************************************************************************* */
Matrix Marginals::marginalCovariance(Key variable) const { Matrix Marginals::marginalCovariance(Key variable) const {
return marginalInformation(variable).inverse(); return marginalInformation(variable).inverse();
} }
/* ************************************************************************* */
Matrix Marginals::marginalInformation(Key variable) const { Matrix Marginals::marginalInformation(Key variable) const {
// Get linear key // Get linear key
Index index = ordering_[variable]; Index index = ordering_[variable];
@ -66,4 +73,71 @@ Matrix Marginals::marginalInformation(Key variable) const {
} }
} }
/* ************************************************************************* */
JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
JointMarginal info = jointMarginalInformation(variables);
info.fullMatrix_ = info.fullMatrix_.inverse();
return info;
}
/* ************************************************************************* */
JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const {
// If 2 variables, we can use the BayesTree::joint function, otherwise we
// have to use sequential elimination.
if(variables.size() == 1) {
Matrix info = marginalInformation(variables.front());
std::vector<size_t> dims;
dims.push_back(info.rows());
Ordering indices;
indices.insert(variables.front(), 0);
return JointMarginal(info, dims, indices);
} else {
// Convert keys to linear indices
vector<Index> indices(variables.size());
for(size_t i=0; i<variables.size(); ++i) { indices[i] = ordering_[variables[i]]; }
// Compute joint factor graph
GaussianFactorGraph jointFG;
if(variables.size() == 2) {
if(factorization_ == CHOLESKY)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminatePreferCholesky);
else if(factorization_ == QR)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminateQR);
} else {
if(factorization_ == CHOLESKY)
jointFG = *GaussianSequentialSolver(graph_, false).jointFactorGraph(indices);
else if(factorization_ == QR)
jointFG = *GaussianSequentialSolver(graph_, true).jointFactorGraph(indices);
}
// Conversion from variable keys to position in factor graph variables,
// which are sorted in index order.
Ordering variableConversion;
{
FastMap<Index,Key> usedIndices;
for(size_t i=0; i<variables.size(); ++i)
usedIndices.insert(make_pair(indices[i], variables[i]));
size_t slot = 0;
typedef pair<Index,Key> Index_Key;
BOOST_FOREACH(const Index_Key& index_key, usedIndices) {
variableConversion.insert(index_key.second, slot);
++ slot;
}
}
// Get dimensions from factor graph
std::vector<size_t> dims(indices.size(), 0);
for(size_t i = 0; i < variables.size(); ++i)
dims[i] = values_.at(variables[i]).dim();
// Get information matrix
Matrix augmentedInfo = jointFG.denseHessian();
Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
return JointMarginal(info, dims, variableConversion);
}
}
} /* namespace gtsam */ } /* namespace gtsam */

54
gtsam/nonlinear/Marginals.h
View File

@ -18,12 +18,15 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
namespace gtsam { namespace gtsam {
class JointMarginal;
/** /**
* A class for computing Gaussian marginals of variables in a NonlinearFactorGraph * A class for computing Gaussian marginals of variables in a NonlinearFactorGraph
*/ */
@ -52,13 +55,62 @@ public:
* matrix. */ * matrix. */
Matrix marginalInformation(Key variable) const; Matrix marginalInformation(Key variable) const;
/** Compute the joint marginal covariance of several variables */
JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const;
/** Compute the joint marginal information of several variables */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
protected: protected:
GaussianFactorGraph graph_; GaussianFactorGraph graph_;
Values values_;
Ordering ordering_; Ordering ordering_;
Factorization factorization_; Factorization factorization_;
GaussianBayesTree bayesTree_; GaussianBayesTree bayesTree_;
};
/**
* A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation
*/
class JointMarginal {
protected:
typedef SymmetricBlockView<Matrix> BlockView;
public:
/** A block view of the joint marginal - this stores a reference to the
* JointMarginal object, so the JointMarginal object must be kept in scope
* while this block view is needed, otherwise assign this block object to a
* Matrix to store it.
*/
typedef BlockView::constBlock Block;
/** Access a block, corresponding to a pair of variables, of the joint
* marginal. Each block is accessed by its "vertical position",
* corresponding to the variable with nonlinear Key \c iVariable and
* "horizontal position", corresponding to the variable with nonlinear Key
* \c jVariable.
*
* For example, if we have the joint marginal on a 2D pose "x3" and a 2D
* landmark "l2", then jointMarginal(Symbol('x',3), Symbol('l',2)) will
* return the 3x2 block of the joint covariance matrix corresponding to x3
* and l2.
* @param iVariable The nonlinear Key specifying the "vertical position" of the requested block
* @param jVariable The nonlinear Key specifying the "horizontal position" of the requested block
*/
Block operator()(Key iVariable, Key jVariable) const {
return blockView_(indices_[iVariable], indices_[jVariable]); }
protected:
Matrix fullMatrix_;
BlockView blockView_;
Ordering indices_;
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
friend class Marginals;
}; };
} /* namespace gtsam */ } /* namespace gtsam */

59
tests/testMarginals.cpp
View File

@ -112,21 +112,72 @@ TEST(Marginals, planarSLAMmarginals) {
0.293870968, -0.104516129, 0.293870968, -0.104516129,
-0.104516129, 0.391935484; -0.104516129, 0.391935484;
// Check marginals covariances for all variables (QR mode) // Check marginals covariances for all variables (Cholesky mode)
Marginals marginals(graph, soln, Marginals::QR); Marginals marginals(graph, soln, Marginals::CHOLESKY);
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
// Check marginals covariances for all variables (Cholesky mode) // Check marginals covariances for all variables (QR mode)
marginals = Marginals(graph, soln, Marginals::CHOLESKY); marginals = Marginals(graph, soln, Marginals::QR);
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
// Check joint marginals for 3 variables
Matrix expected_l2x1x3(8,8);
expected_l2x1x3 <<
0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, 0.151935669757191, -0.104516127560770, -0.050967744878460,
-0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.090000180000270, 0.000000000000000, 0.000000000000000,
-0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, -0.000000000000000, 0.090000180000270, 0.000000000000000,
-0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.000000000000000, 0.040000000000000, 0.010000000000000,
0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770,
-0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
-0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615;
vector<Key> variables(3);
variables[0] = l2;
variables[1] = x1;
variables[2] = x3;
JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6));
// Check joint marginals for 2 variables (different code path than >2 variable case above)
Matrix expected_l2x1(5,5);
expected_l2x1 <<
0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000,
-0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000,
0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000,
-0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000,
-0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000;
variables.resize(2);
variables[0] = l2;
variables[1] = x1;
JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables);
EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6));
EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6));
// Check joint marginal for 1 variable (different code path than >1 variable cases above)
variables.resize(1);
variables[0] = x1;
JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables);
EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6));
} }