Added Marginals unit test and class
Richard Roberts
parent
6bd94352ca
commit
51d38f4b5d
|
|
@ -0,0 +1,67 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Marginals.cpp
|
||||||
|
* @brief
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @date May 14, 2012
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
|
||||||
|
#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
|
||||||
|
ordering_ = *graph.orderingCOLAMD(solution);
|
||||||
|
|
||||||
|
// Linearize graph
|
||||||
|
graph_ = *graph.linearize(solution, ordering_);
|
||||||
|
|
||||||
|
// Compute BayesTree
|
||||||
|
factorization_ = factorization;
|
||||||
|
if(factorization_ == CHOLESKY)
|
||||||
|
bayesTree_ = *GaussianMultifrontalSolver(graph_, false).eliminate();
|
||||||
|
else if(factorization_ == QR)
|
||||||
|
bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
|
||||||
|
}
|
||||||
|
|
||||||
|
Matrix Marginals::marginalCovariance(Key variable) const {
|
||||||
|
// Get linear key
|
||||||
|
Index index = ordering_[variable];
|
||||||
|
|
||||||
|
// Compute marginal
|
||||||
|
GaussianFactor::shared_ptr marginalFactor;
|
||||||
|
if(factorization_ == CHOLESKY)
|
||||||
|
marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholesky);
|
||||||
|
else if(factorization_ == QR)
|
||||||
|
marginalFactor = bayesTree_.marginalFactor(index, EliminateQR);
|
||||||
|
|
||||||
|
// Get information matrix (only store upper-right triangle)
|
||||||
|
Matrix info;
|
||||||
|
if(typeid(*marginalFactor) == typeid(JacobianFactor)) {
|
||||||
|
JacobianFactor::constABlock A = static_cast<const JacobianFactor&>(*marginalFactor).getA();
|
||||||
|
info = A.transpose() * A; // Compute A'A
|
||||||
|
} else if(typeid(*marginalFactor) == typeid(HessianFactor)) {
|
||||||
|
const HessianFactor& hessian = static_cast<const HessianFactor&>(*marginalFactor);
|
||||||
|
const size_t dim = hessian.getDim(hessian.begin());
|
||||||
|
info = hessian.info().topLeftCorner(dim,dim).selfadjointView<Eigen::Upper>(); // Take the non-augmented part of the information matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute covariance
|
||||||
|
return info.inverse();
|
||||||
|
}
|
||||||
|
|
||||||
|
} /* namespace gtsam */
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Marginals.h
|
||||||
|
* @brief A class for computing marginals in a NonlinearFactorGraph
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @date May 14, 2012
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
#include <gtsam/linear/GaussianBayesTree.h>
|
||||||
|
#include <gtsam/nonlinear/Values.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A class for computing marginals in a NonlinearFactorGraph
|
||||||
|
*/
|
||||||
|
class Marginals {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
enum Factorization {
|
||||||
|
CHOLESKY,
|
||||||
|
QR
|
||||||
|
};
|
||||||
|
|
||||||
|
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
|
||||||
|
|
||||||
|
Matrix marginalCovariance(Key variable) const;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
GaussianFactorGraph graph_;
|
||||||
|
Ordering ordering_;
|
||||||
|
Factorization factorization_;
|
||||||
|
GaussianBayesTree bayesTree_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
} /* namespace gtsam */
|
||||||
|
|
@ -0,0 +1,136 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testMarginals.cpp
|
||||||
|
* @brief
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @date May 14, 2012
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
// for all nonlinear keys
|
||||||
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
|
// for points and poses
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
|
||||||
|
// for modeling measurement uncertainty - all models included here
|
||||||
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
|
|
||||||
|
// add in headers for specific factors
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/BearingRangeFactor.h>
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/Marginals.h>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
TEST(Marginals, planarSLAMmarginals) {
|
||||||
|
|
||||||
|
// Taken from PlanarSLAMSelfContained_advanced
|
||||||
|
|
||||||
|
// create keys for variables
|
||||||
|
Symbol x1('x',1), x2('x',2), x3('x',3);
|
||||||
|
Symbol l1('l',1), l2('l',2);
|
||||||
|
|
||||||
|
// create graph container and add factors to it
|
||||||
|
NonlinearFactorGraph graph;
|
||||||
|
|
||||||
|
/* add prior */
|
||||||
|
// gaussian for prior
|
||||||
|
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
|
||||||
|
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
|
||||||
|
graph.add(PriorFactor<Pose2>(x1, prior_measurement, prior_model)); // add the factor to the graph
|
||||||
|
|
||||||
|
/* add odometry */
|
||||||
|
// general noisemodel for odometry
|
||||||
|
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||||
|
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
|
||||||
|
// create between factors to represent odometry
|
||||||
|
graph.add(BetweenFactor<Pose2>(x1, x2, odom_measurement, odom_model));
|
||||||
|
graph.add(BetweenFactor<Pose2>(x2, x3, odom_measurement, odom_model));
|
||||||
|
|
||||||
|
/* add measurements */
|
||||||
|
// general noisemodel for measurements
|
||||||
|
SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
|
||||||
|
|
||||||
|
// create the measurement values - indices are (pose id, landmark id)
|
||||||
|
Rot2 bearing11 = Rot2::fromDegrees(45),
|
||||||
|
bearing21 = Rot2::fromDegrees(90),
|
||||||
|
bearing32 = Rot2::fromDegrees(90);
|
||||||
|
double range11 = sqrt(4+4),
|
||||||
|
range21 = 2.0,
|
||||||
|
range32 = 2.0;
|
||||||
|
|
||||||
|
// create bearing/range factors
|
||||||
|
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, meas_model));
|
||||||
|
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, meas_model));
|
||||||
|
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, meas_model));
|
||||||
|
|
||||||
|
// linearization point for marginals
|
||||||
|
Values soln;
|
||||||
|
soln.insert(x1, Pose2(0.0, 0.0, 0.0));
|
||||||
|
soln.insert(x2, Pose2(2.0, 0.0, 0.0));
|
||||||
|
soln.insert(x3, Pose2(4.0, 0.0, 0.0));
|
||||||
|
soln.insert(l1, Point2(2.0, 2.0));
|
||||||
|
soln.insert(l2, Point2(4.0, 2.0));
|
||||||
|
|
||||||
|
Matrix expectedx1(3,3);
|
||||||
|
expectedx1 <<
|
||||||
|
0.09, -7.1942452e-18, -1.27897692e-17,
|
||||||
|
-7.1942452e-18, 0.09, 1.27897692e-17,
|
||||||
|
-1.27897692e-17, 1.27897692e-17, 0.01;
|
||||||
|
Matrix expectedx2(3,3);
|
||||||
|
expectedx2 <<
|
||||||
|
0.120967742, -0.00129032258, 0.00451612903,
|
||||||
|
-0.00129032258, 0.158387097, 0.0206451613,
|
||||||
|
0.00451612903, 0.0206451613, 0.0177419355;
|
||||||
|
Matrix expectedx3(3,3);
|
||||||
|
expectedx3 <<
|
||||||
|
0.160967742, 0.00774193548, 0.00451612903,
|
||||||
|
0.00774193548, 0.351935484, 0.0561290323,
|
||||||
|
0.00451612903, 0.0561290323, 0.0277419355;
|
||||||
|
Matrix expectedl1(2,2);
|
||||||
|
expectedl1 <<
|
||||||
|
0.168709677, -0.0477419355,
|
||||||
|
-0.0477419355, 0.163548387;
|
||||||
|
Matrix expectedl2(2,2);
|
||||||
|
expectedl2 <<
|
||||||
|
0.293870968, -0.104516129,
|
||||||
|
-0.104516129, 0.391935484;
|
||||||
|
|
||||||
|
// 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 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));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue