Added joint marginals and unit tests
Richard Roberts
parent
bd76692794
commit
516e1610a1
142
.cproject
142
.cproject
|
|
@ -844,14 +844,30 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<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>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1028,6 +1044,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1635,14 +1659,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1803,14 +1819,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2122,110 +2130,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam</buildTarget>
|
||||
<buildArguments>-j8</buildArguments>
|
||||
<buildTarget>testMarginals.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="verbose wrap_gtsam" path="" 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">
|
||||
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.discrete</buildTarget>
|
||||
<buildTarget>wrap.testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam_unstable</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>
|
||||
<buildTarget>wrap.testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -17,11 +17,13 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) {
|
||||
|
||||
// Compute COLAMD ordering
|
||||
|
|
@ -30,6 +32,9 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
|
|||
// Linearize graph
|
||||
graph_ = *graph.linearize(solution, ordering_);
|
||||
|
||||
// Store values
|
||||
values_ = solution;
|
||||
|
||||
// Compute BayesTree
|
||||
factorization_ = factorization;
|
||||
if(factorization_ == CHOLESKY)
|
||||
|
|
@ -38,10 +43,12 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
|
|||
bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Marginals::marginalCovariance(Key variable) const {
|
||||
return marginalInformation(variable).inverse();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Marginals::marginalInformation(Key variable) const {
|
||||
// Get linear key
|
||||
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 */
|
||||
|
|
|
|||
|
|
@ -18,12 +18,15 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/base/blockMatrices.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class JointMarginal;
|
||||
|
||||
/**
|
||||
* A class for computing Gaussian marginals of variables in a NonlinearFactorGraph
|
||||
*/
|
||||
|
|
@ -52,13 +55,62 @@ public:
|
|||
* matrix. */
|
||||
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:
|
||||
|
||||
GaussianFactorGraph graph_;
|
||||
Values values_;
|
||||
Ordering ordering_;
|
||||
Factorization factorization_;
|
||||
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 */
|
||||
|
|
|
|||
|
|
@ -112,21 +112,72 @@ TEST(Marginals, planarSLAMmarginals) {
|
|||
0.293870968, -0.104516129,
|
||||
-0.104516129, 0.391935484;
|
||||
|
||||
// Check marginals covariances for all variables (QR mode)
|
||||
Marginals marginals(graph, soln, Marginals::QR);
|
||||
// Check marginals covariances for all variables (Cholesky mode)
|
||||
Marginals marginals(graph, soln, Marginals::CHOLESKY);
|
||||
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
|
||||
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
|
||||
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
|
||||
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
|
||||
EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
|
||||
|
||||
// Check marginals covariances for all variables (Cholesky mode)
|
||||
marginals = Marginals(graph, soln, Marginals::CHOLESKY);
|
||||
// Check marginals covariances for all variables (QR mode)
|
||||
marginals = Marginals(graph, soln, Marginals::QR);
|
||||
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
|
||||
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
|
||||
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
|
||||
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 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));
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue