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

Merge branch 'prep_0.9.3'

release/4.3a0
Alex Cunningham 2011-06-13 16:55:31 +00:00
parent 2e058f2272
commit fa4faa274a
141 changed files with 2129 additions and 1476 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
.cproject
View File

@ -692,14 +692,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2SLAMwSPCG.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -876,6 +868,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAMwSPCG.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNet.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBoundingConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testBoundingConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<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>
@ -2487,14 +2519,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2SLAMwSPCG.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2671,6 +2695,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAMwSPCG.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNet.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBoundingConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testBoundingConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<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>

4
CppUnitLite/TestRegistry.cpp
View File

@ -19,9 +19,6 @@
#include "TestRegistry.h"
#include "SimpleString.h"
//#include <iostream>
//using namespace std;
void TestRegistry::addTest (Test *test)
{
instance ().add (test);
@ -63,7 +60,6 @@ int TestRegistry::run (TestResult& result)
for (Test *test = tests; test != 0; test = test->getNext ()) {
if (test->safe()) {
try {
// cout << test->getName().asCharString() << ", " << test->getFilename().asCharString() << ":" << test->getLineNumber() << endl;
test->run (result);
} catch (std::exception& e) {
// catch standard exceptions and derivatives

10
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -136,11 +136,11 @@ int main(int argc, char** argv) {
resultMF.values()->print("final result (solved with a multifrontal solver)");
// Print marginals covariances for all variables
print(resultMF.marginalCovariance(x1).second, "x1 covariance");
print(resultMF.marginalCovariance(x2).second, "x2 covariance");
print(resultMF.marginalCovariance(x3).second, "x3 covariance");
print(resultMF.marginalCovariance(l1).second, "l1 covariance");
print(resultMF.marginalCovariance(l2).second, "l2 covariance");
print(resultMF.marginalCovariance(x1), "x1 covariance");
print(resultMF.marginalCovariance(x2), "x2 covariance");
print(resultMF.marginalCovariance(x3), "x3 covariance");
print(resultMF.marginalCovariance(l1), "l1 covariance");
print(resultMF.marginalCovariance(l2), "l2 covariance");
return 0;
}

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -76,8 +76,8 @@ int main(int argc, char** argv) {
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalCovariance(x1).second;
Matrix covariance2 = optimizer_result.marginalCovariance(x2).second;
Matrix covariance1 = optimizer_result.marginalCovariance(x1);
Matrix covariance2 = optimizer_result.marginalCovariance(x2);
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

1
gtsam/base/FastMap.h
View File

@ -57,7 +57,6 @@ public:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::base_object<Base>(*this);
}
};

3
gtsam/base/FastVector.h
View File

@ -40,6 +40,9 @@ public:
/** Default constructor */
FastVector() {}
/** Constructor from size */
FastVector(size_t size) : Base(size) {}
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}

1
gtsam/base/Lie.h
View File

@ -56,7 +56,6 @@
#pragma once
#include <string>
#include <gtsam/base/Matrix.h>
namespace gtsam {

71
gtsam/base/Matrix.cpp
View File

@ -259,32 +259,13 @@ Vector columnNormSquare(const Matrix &A) {
}
/* ************************************************************************* */
void solve(Matrix& A, Matrix& B)
{
void solve(Matrix& A, Matrix& B) {
// Eigen version - untested
Eigen::FullPivLU<Matrix> lu;
lu.compute(A);
B = lu.solve(B);
A = lu.matrixLU();
// typedef ublas::permutation_matrix<std::size_t> pmatrix;
// create a working copy of the input
// Matrix A_(A);
// create a permutation matrix for the LU-factorization
// pmatrix pm(A_.rows());
// perform LU-factorization
// FIXME: add back with Eigen functionality
// size_t res = lu_factorize(A_,pm);
// if( res != 0 ) throw runtime_error ("Matrix::solve: lu_factorize failed!");
// backsubstitute to get the inverse
// lu_substitute(A_, pm, B);
B = A.fullPivLu().solve(B);
}
/* ************************************************************************* */
Matrix inverse(const Matrix& A)
{
Matrix inverse(const Matrix& A) {
return A.inverse();
}
@ -445,31 +426,11 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
/* ************************************************************************* */
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
// @return the solution x of x'*U=b'
size_t n = U.cols();
#ifndef NDEBUG
size_t m = U.rows();
if (m!=n)
throw invalid_argument("backSubstituteUpper: U must be square");
#endif
Vector result(n);
for (size_t i = 1; i <= n; i++) {
double zi = b(i-1);
for (size_t j = 1; j < i; j++)
zi -= U(j-1,i-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: U is singular,\n";
print(U, "U: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
}
return result;
assert(U.rows() == U.cols());
if (unit)
return U.triangularView<Eigen::UnitUpper>().transpose().solve<Eigen::OnTheLeft>(b);
else
return U.triangularView<Eigen::Upper>().transpose().solve<Eigen::OnTheLeft>(b);
}
/* ************************************************************************* */
@ -578,13 +539,14 @@ Matrix skewSymmetric(double wx, double wy, double wz)
* pointers are subtracted by one to provide base 1 access
*/
/* ************************************************************************* */
double** createNRC(Matrix& A) {
const size_t m=A.rows();
double** a = new double* [m];
for(size_t i = 0; i < m; i++)
a[i] = &A(i,0)-1;
return a;
}
// FIXME: assumes row major, rather than column major
//double** createNRC(Matrix& A) {
// const size_t m=A.rows();
// double** a = new double* [m];
// for(size_t i = 0; i < m; i++)
// a[i] = &A(i,0)-1;
// return a;
//}
/* ******************************************
*
@ -653,7 +615,6 @@ Matrix inverse_square_root(const Matrix& A) {
Matrix inverse_square_root(const Matrix& A) {
Matrix R = RtR(A);
Matrix inv = eye(A.rows());
// inplace_solve(R, inv, BNU::upper_tag ());
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv);
return inv;
}

1
gtsam/base/Matrix.h
View File

@ -22,7 +22,6 @@
#pragma once
#include <list>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/3rdparty/Eigen/QR>

2
gtsam/base/blockMatrices.h
View File

@ -17,9 +17,7 @@
*/
#pragma once
#include <vector>
#include <gtsam/3rdparty/Eigen/Dense>
#include <gtsam/3rdparty/Eigen/Core>
#include <gtsam/base/Matrix.h>
namespace gtsam {

47
gtsam/base/cholesky.cpp
View File

@ -148,4 +148,51 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
toc(3, "compute L");
}
/* ************************************************************************* */
Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) {
const bool debug = ISDEBUG("ldlPartial");
assert(ABC.rows() == ABC.cols());
assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));
const size_t n = ABC.rows();
// Compute Cholesky factorization of A, overwrites A = sqrt(D)*R
// tic(1, "ldl");
Eigen::LDLT<Matrix> ldlt;
ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>());
Vector sqrtD = ldlt.vectorD().cwiseSqrt();
if (debug) cout << "Dsqrt: " << sqrtD << endl;
// U = sqrtD * L^
Matrix U = ldlt.matrixU();
// we store the permuted upper triangular matrix
ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U;
if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl;
// toc(1, "ldl");
// Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B
// tic(2, "compute S");
if(n - nFrontal > 0) {
ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal);
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal));
}
if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
// toc(2, "compute S");
// Compute L = C - S' * S
// tic(3, "compute L");
if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
if(n - nFrontal > 0)
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
// toc(3, "compute L");
return ldlt.transpositionsP();
}
}

20
gtsam/base/cholesky.h
View File

@ -50,5 +50,25 @@ std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
*/
void choleskyPartial(Matrix& ABC, size_t nFrontal);
/**
* Partial LDL computes a factor [R S such that [P'R' 0 [RP S = [A B
* 0 F] S' I] 0 F] B' C].
* The input to this function is the matrix ABC = [A B], and the parameter
* [B' C]
* nFrontal determines the split between A, B, and C, with A being of size
* nFrontal x nFrontal.
* P is a permutation matrix obtained from the pivoting process while doing LDL'.
*
* Specifically, if A = P'U'DUP is the LDL' factorization of A,
* then R = sqrt(D)*U is the permuted upper-triangular matrix.
* The permutation is returned so that it can be used in the backsubstitution
* process to return the correct order of variables, and in creating the
* Jacobian factor by permuting R correctly.
*
* Note that S and F are not permuted (in correct original ordering).
*/
Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal);
}

1
gtsam/base/numericalDerivative.h
View File

@ -22,7 +22,6 @@
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/Lie.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h>

2
gtsam/base/tests/testBTree.cpp
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */
/*
/**
* testBTree.cpp
*
* Created on: Feb 3, 2010

11
gtsam/base/tests/testBlockMatrices.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* 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 testBlockMatrices
* @author Alex Cunningham

36
gtsam/base/tests/testCholesky.cpp
View File

@ -21,6 +21,7 @@
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST(cholesky, choleskyPartial) {
@ -53,6 +54,41 @@ TEST(cholesky, choleskyPartial) {
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************* */
TEST(cholesky, ldlPartial) {
// choleskyPartial should only use the upper triangle, so this represents a
// symmetric matrix.
Matrix ABC = Matrix_(7,7,
4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063,
0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914,
0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992,
0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347,
0., 0., 0., 0., 3.0788, 2.6283, 2.3791,
0., 0., 0., 0., 0., 2.9227, 2.4056,
0., 0., 0., 0., 0., 0., 2.5776);
// Do partial Cholesky on 3 frontal scalar variables
Matrix RSL(ABC);
Eigen::LDLT<Matrix>::TranspositionType permutation = ldlPartial(RSL, 3);
// See the function comment for choleskyPartial, this decomposition should hold.
Matrix R1 = RSL.transpose();
Matrix R2 = RSL;
R1.block(3, 3, 4, 4).setIdentity();
R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>();
// un-permute the permuted upper triangular part
R1.topLeftCorner(3,3) = permutation.transpose() * R1.topLeftCorner(3,3);
R2.topLeftCorner(3,3) = R2.topLeftCorner(3,3)*permutation;
Matrix actual = R1 * R2;
Matrix expected = ABC.selfadjointView<Eigen::Upper>();
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

4
gtsam/base/tests/timeMatrixOps.cpp
View File

@ -16,10 +16,6 @@
* @created Sep 18, 2010
*/
//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/matrix_proxy.hpp>
//#include <boost/numeric/ublas/triangular.hpp>
//#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/format.hpp>

2
gtsam/geometry/Cal3Bundler.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

2
gtsam/geometry/Cal3DS2.h
View File

@ -19,8 +19,6 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

2
gtsam/geometry/Cal3_S2.cpp
View File

@ -28,7 +28,7 @@ namespace gtsam {
Cal3_S2::Cal3_S2(double fov, int w, int h) :
s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians
fx_ = (double) w * tan(a);
fx_ = (double)w / (2.0*tan(a)); // old formula: fx_ = (double) w * tan(a);
fy_ = fx_;
}

1
gtsam/geometry/Cal3_S2.h
View File

@ -17,7 +17,6 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

2
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -17,8 +17,6 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3_S2.h>
namespace gtsam {

1
gtsam/geometry/CalibratedCameraT.h
View File

@ -8,7 +8,6 @@
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>

8
gtsam/geometry/GeneralCameraT.h
View File

@ -18,16 +18,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>

1
gtsam/geometry/Point2.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>

2
gtsam/geometry/Pose2.h
View File

@ -22,8 +22,6 @@
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>

2
gtsam/geometry/Pose3.h
View File

@ -20,8 +20,6 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
namespace gtsam {

8
gtsam/geometry/Rot2.h
View File

@ -16,14 +16,10 @@
* Author: Frank Dellaert
*/
#ifndef ROT2_H_
#define ROT2_H_
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -175,5 +171,3 @@ namespace gtsam {
}; // Rot2
} // gtsam
#endif /* ROT2_H_ */

11
gtsam/geometry/Rot3.cpp
View File

@ -161,8 +161,15 @@ namespace gtsam {
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr > 3.0 - 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
double theta = acos((tr-1.0)/2.0);
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
return (0.5 + theta*theta/12)*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(fabs(R.r3().z() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *

2
gtsam/geometry/Rot3.h
View File

@ -22,8 +22,6 @@
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
namespace gtsam {

3
gtsam/geometry/StereoCamera.h
View File

@ -17,8 +17,7 @@
#pragma once
#include <gtsam/base/Lie.h>
#include "boost/tuple/tuple.hpp"
#include <boost/tuple/tuple.hpp>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>

10
gtsam/geometry/StereoPoint2.h
View File

@ -9,20 +9,16 @@
* -------------------------------------------------------------------------- */
/*
/**
* StereoPoint2.h
*
* Created on: Jan 26, 2010
* Author: dellaert
*/
#ifndef STEREOPOINT2_H_
#define STEREOPOINT2_H_
#pragma once
#include <iostream>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -133,5 +129,3 @@ namespace gtsam {
};
}
#endif /* STEREOPOINT2_H_ */

2
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -26,7 +26,7 @@ Point2 p(1, -2);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)
{
Cal3_S2 expected(369.504, 369.504, 0, 640 / 2, 480 / 2);
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
double fov = 60; // degrees
size_t w=640,h=480;

29
gtsam/geometry/tests/testRot3.cpp
View File

@ -402,6 +402,35 @@ TEST( Rot3, RQ)
CHECK(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = Vector_(3, 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
Vector w = Vector_(3, 1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
// std::cout << "theta: " << w.norm() << std::endl;
// std::cout << "trace: " << tr << std::endl;
// R.print("R = ");
Vector actualw = Rot3::Logmap(R);
CHECK(assert_equal(w, actualw, 1e-15));
}
/* ************************************************************************* */
int main() {
TestResult tr;

60
gtsam/inference/BayesNet-inl.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
void BayesNet<CONDITIONAL>::print(const string& s) const {
cout << s << ":\n";
BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
conditional->print((boost::format("Node[%1%]") % conditional->key()).str());
conditional->print();
}
/* ************************************************************************* */
@ -82,39 +82,43 @@ namespace gtsam {
push_front(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.push_back(conditional->key());
return ord;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s) const {
ofstream of(s.c_str());
of<< "digraph G{\n";
BOOST_FOREACH(const_sharedConditional conditional,conditionals_) {
Index child = conditional->key();
BOOST_FOREACH(Index parent, conditional->parents()) {
of << parent << "->" << child << endl;
}
}
of<<"}";
of.close();
}
// /* ************************************************************************* */
// template<class CONDITIONAL>
// FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
// FastList<Index> ord;
// BOOST_FOREACH(sharedConditional conditional,conditionals_)
// ord.push_back(conditional->key());
// return ord;
// }
//
// /* ************************************************************************* */
// template<class CONDITIONAL>
// void BayesNet<CONDITIONAL>::saveGraph(const std::string &s) const {
// ofstream of(s.c_str());
// of<< "digraph G{\n";
// BOOST_FOREACH(const_sharedConditional conditional,conditionals_) {
// Index child = conditional->key();
// BOOST_FOREACH(Index parent, conditional->parents()) {
// of << parent << "->" << child << endl;
// }
// }
// of<<"}";
// of.close();
// }
//
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::sharedConditional
BayesNet<CONDITIONAL>::operator[](Index key) const {
const_iterator it = find_if(conditionals_.begin(), conditionals_.end(), boost::lambda::bind(&CONDITIONAL::key, *boost::lambda::_1) == key);
if (it == conditionals_.end()) throw(invalid_argument((boost::format(
BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
typename CONDITIONAL::const_iterator it = std::find(conditional->beginFrontals(), conditional->endFrontals(), key);
if (it!=conditional->endFrontals()) {
return conditional;
}
}
throw(invalid_argument((boost::format(
"BayesNet::operator['%1%']: not found") % key).str()));
return *it;
}
/* ************************************************************************* */

24
gtsam/inference/BayesNet.h
View File

@ -19,15 +19,15 @@
#pragma once
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Permutation.h>
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
@ -67,6 +67,12 @@ namespace gtsam {
public:
/** Default constructor as an empty BayesNet */
BayesNet() {};
/** BayesNet with 1 conditional */
BayesNet(const sharedConditional& conditional) { push_back(conditional); }
/** print */
void print(const std::string& s = "") const;
@ -110,10 +116,10 @@ namespace gtsam {
return conditionals_.size();
}
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
// /** return keys in reverse topological sort order, i.e., elimination order */
// FastList<Index> ordering() const;
//
// /** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
@ -135,7 +141,7 @@ namespace gtsam {
inline const_reverse_iterator const rend() const {return conditionals_.rend();}
/** saves the bayes to a text file in GraphViz format */
void saveGraph(const std::string& s) const;
// void saveGraph(const std::string& s) const;
private:
/** Serialization function */

229
gtsam/inference/BayesTree-inl.h
View File

@ -15,11 +15,13 @@
* @author Frank Dellaert
* @author Michael Kaess
* @author Viorela Ila
* @author Richard Roberts
*/
#pragma once
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
@ -45,87 +47,24 @@ namespace gtsam {
void BayesTree<CONDITIONAL>::Clique::assertInvariants() const {
#ifndef NDEBUG
// We rely on the keys being sorted
typename BayesNet<CONDITIONAL>::const_iterator cond = this->begin();
if(cond != this->end()) {
Index lastKey = (*cond)->key();
++cond;
for(; cond != this->end(); ++cond)
assert(lastKey < (*cond)->key());
}
FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
assert(sortedUniqueKeys.size() == conditional_->size() &&
std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin()));
#endif
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique() { assertInvariants(); }
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) {
separator_.assign(conditional->parents().begin(), conditional->parents().end());
this->push_back(conditional);
BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) : conditional_(conditional) {
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique(const BayesNet<CONDITIONAL>& bayesNet)
: BayesNet<CONDITIONAL>(bayesNet) {
if(bayesNet.size() > 0) {
#ifndef NDEBUG
// Debug check that each parent variable is either a frontal variable in
// a later conditional in the Bayes net fragment, or that the parent is
// also a parent of the last conditional. This checks that the fragment
// is "enough like a clique". todo: this should really check that the
// fragment *is* a clique.
if(bayesNet.size() > 1) {
typename BayesNet<CONDITIONAL>::const_reverse_iterator cond = bayesNet.rbegin();
++ cond;
typename CONDITIONAL::shared_ptr lastConditional = *cond;
for( ; cond != bayesNet.rend(); ++cond)
for(typename CONDITIONAL::const_iterator parent=(*cond)->beginParents(); parent!=(*cond)->endParents(); ++parent) {
bool infragment = false;
typename BayesNet<CONDITIONAL>::const_reverse_iterator parentCond = cond;
do {
if(*parent == (*parentCond)->key())
infragment = true;
--parentCond;
} while(parentCond != bayesNet.rbegin());
assert(infragment || find(lastConditional->beginParents(), lastConditional->endParents(), *parent) != lastConditional->endParents());
}
}
#endif
// separator_.assign((*bayesNet.rbegin())->parents().begin(), (*bayesNet.rbegin())->parents().end());
separator_.assign((*bayesNet.rbegin())->beginParents(), (*bayesNet.rbegin())->endParents());
}
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
vector<Index> BayesTree<CONDITIONAL>::Clique::keys() const {
vector<Index> keys;
keys.reserve(this->size() + separator_.size());
BOOST_FOREACH(const sharedConditional conditional, *this) {
keys.push_back(conditional->key());
}
keys.insert(keys.end(), separator_.begin(), separator_.end());
return keys;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::print(const string& s) const {
cout << s << "Clique ";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
cout << conditional->key() << " ";
cout << "| ";
BOOST_FOREACH(const Index sep, separator_)
cout << sep << " ";
cout << "\n";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
conditional->print(" " + s + "conditional");
conditional_->print(s);
}
/* ************************************************************************* */
@ -148,8 +87,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::permuteWithInverse(const Permutation& inversePermutation) {
BayesNet<CONDITIONAL>::permuteWithInverse(inversePermutation);
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
conditional_->permuteWithInverse(inversePermutation);
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
@ -160,17 +98,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesTree<CONDITIONAL>::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(inversePermutation);
bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, separator_) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(const shared_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
#endif
if(changed) {
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation);
@ -192,8 +129,8 @@ namespace gtsam {
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::getCliqueData(CliqueData& data,
BayesTree<CONDITIONAL>::sharedClique clique) const {
data.conditionalSizes.push_back(clique->conditionals_.size());
data.separatorSizes.push_back(clique->separator_.size());
data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) {
getCliqueData(data, c);
}
@ -277,7 +214,6 @@ namespace gtsam {
// The shortcut density is a conditional P(S|R) of the separator of this
// clique on the root. We can compute it recursively from the parent shortcut
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
// TODO, why do we actually return a shared pointer, why does eliminate?
/* ************************************************************************* */
template<class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R,
@ -296,11 +232,11 @@ namespace gtsam {
}
// The root conditional
FactorGraph<FactorType> p_R(*R);
FactorGraph<FactorType> p_R(BayesNet<CONDITIONAL>(R->conditional()));
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FactorGraph<FactorType> p_Fp_Sp(*parent);
FactorGraph<FactorType> p_Fp_Sp(BayesNet<CONDITIONAL>(parent->conditional()));
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
@ -330,20 +266,20 @@ namespace gtsam {
FastSet<Index> variablesAtBack;
FastSet<Index> separator;
size_t uniqueRootVariables = 0;
BOOST_FOREACH(const Index separatorIndex, this->separator_) {
BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
variablesAtBack.insert(separatorIndex);
separator.insert(separatorIndex);
if(debug) cout << "At back (this): " << separatorIndex << endl;
}
BOOST_FOREACH(const sharedConditional& conditional, *R) {
if(variablesAtBack.insert(conditional->key()).second)
BOOST_FOREACH(const Index key, R->conditional()->keys()) {
if(variablesAtBack.insert(key).second)
++ uniqueRootVariables;
if(debug) cout << "At back (root): " << conditional->key() << endl;
if(debug) cout << "At back (root): " << key << endl;
}
Permutation toBack = Permutation::PushToBack(
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
R->back()->key() + 1);
R->conditional()->lastFrontalKey() + 1);
Permutation::shared_ptr toBackInverse(toBack.inverse());
BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
factor->permuteWithInverse(*toBackInverse); }
@ -356,7 +292,8 @@ namespace gtsam {
// want to include those variables in the conditional.
BayesNet<CONDITIONAL> p_S_R;
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) {
if(separator.find(toBack[conditional->key()]) != separator.end()) {
// TODO: CHECK!!! is it firstFrontalKey() or lastFrontalKey() or something else???
if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
if(debug)
conditional->print("Taking C|R conditional: ");
p_S_R.push_front(conditional);
@ -385,16 +322,17 @@ namespace gtsam {
shared_ptr R, Eliminate function) {
// If we are the root, just return this root
// NOTE: immediately cast to a factor graph
if (R.get()==this) return *R;
BayesNet<CONDITIONAL> bn(R->conditional());
if (R.get()==this) return bn;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R, function);
p_FSR.push_front(*this);
p_FSR.push_back(*R);
p_FSR.push_front(this->conditional());
p_FSR.push_back(R->conditional());
assertInvariants();
GenericSequentialSolver<FactorType> solver(p_FSR);
return *solver.jointFactorGraph(keys(), function);
return *solver.jointFactorGraph(conditional_->keys(), function);
}
/* ************************************************************************* */
@ -407,15 +345,15 @@ namespace gtsam {
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
FactorGraph<FactorType> joint;
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R)
joint.push_back(*R); // P(R)
joint.push_back(R->conditional()->toFactor()); // P(R)
// Find the keys of both C1 and C2
vector<Index> keys1(keys());
vector<Index> keys2(C2->keys());
vector<Index> keys1(conditional_->keys());
vector<Index> keys2(C2->conditional_->keys());
FastSet<Index> keys12;
keys12.insert(keys1.begin(), keys1.end());
keys12.insert(keys2.begin(), keys2.end());
@ -447,9 +385,9 @@ namespace gtsam {
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique(
const sharedConditional& conditional, sharedClique parent_clique) {
sharedClique new_clique(new Clique(conditional));
Index key = conditional->key();
nodes_.resize(std::max(key+1, nodes_.size()));
nodes_[key] = new_clique;
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, conditional->frontals())
nodes_[key] = new_clique;
if (parent_clique != NULL) {
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
@ -463,9 +401,9 @@ namespace gtsam {
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique(
const sharedConditional& conditional, list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional));
Index key = conditional->key();
nodes_.resize(max(key+1, nodes_.size()));
nodes_[key] = new_clique;
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, conditional->frontals())
nodes_[key] = new_clique;
new_clique->children_ = child_cliques;
BOOST_FOREACH(sharedClique& child, child_cliques)
child->parent_ = new_clique;
@ -475,34 +413,28 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
inline void BayesTree<CONDITIONAL>::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
inline void BayesTree<CONDITIONAL>::addToCliqueFront(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false;
#ifndef NDEBUG
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(Index parent, conditional->parents()) {
assert(parent > conditional->key());
bool hasParent = false;
assert(parent > conditional->lastFrontalKey());
const Clique& cliquer(*clique);
BOOST_FOREACH(const sharedConditional& child, cliquer) {
if(child->key() == parent) {
hasParent = true;
break;
}
}
if(!hasParent)
if(find(clique->separator_.begin(), clique->separator_.end(), parent) != clique->separator_.end())
hasParent = true;
assert(hasParent);
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
}
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
Index key = conditional->key();
nodes_.resize(std::max(key+1, nodes_.size()));
nodes_[key] = clique;
clique->push_front(conditional);
Index key = conditional->lastFrontalKey();
bayesTree.nodes_.resize(std::max(key+1, bayesTree.nodes_.size()));
bayesTree.nodes_[key] = clique;
FastVector<Index> newKeys((*clique)->size() + 1);
newKeys[0] = key;
std::copy((*clique)->begin(), (*clique)->end(), newKeys.begin()+1);
clique->conditional_ = CONDITIONAL::FromKeys(newKeys, (*clique)->nrFrontals() + 1);
// clique->conditional_->addFrontal(conditional);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
@ -520,13 +452,9 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename BayesTree<CONDITIONAL>::Clique::weak_ptr();
BOOST_FOREACH(Index key, clique->separator_) {
BOOST_FOREACH(Index key, (*clique->conditional())) {
nodes_[key].reset();
}
const Clique& cliquer(*clique);
BOOST_FOREACH(const sharedConditional& conditional, cliquer) {
nodes_[conditional->key()].reset();
}
}
/* ************************************************************************* */
@ -539,7 +467,7 @@ namespace gtsam {
BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*rit);
insert(*this, *rit);
}
/* ************************************************************************* */
@ -562,7 +490,7 @@ namespace gtsam {
if (!new_clique.get())
new_clique = addClique(conditional,childRoots);
else
addToCliqueFront(conditional, new_clique);
addToCliqueFront(*this, conditional, new_clique);
}
root_ = new_clique;
@ -622,31 +550,31 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::insert(const sharedConditional& conditional)
void BayesTree<CONDITIONAL>::insert(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional)
{
static const bool debug = false;
// get key and parents
typename CONDITIONAL::Parents parents = conditional->parents(); // todo: const reference?
const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional ");
// if no parents, start a new root clique
if (parents.empty()) {
if(debug) cout << "No parents so making root" << endl;
root_ = addClique(conditional);
bayesTree.root_ = bayesTree.addClique(conditional);
return;
}
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = findParentClique(parents);
if(debug) cout << "First-eliminated parent is " << parentRepresentative << ", have " << nodes_.size() << " nodes." << endl;
sharedClique parent_clique = (*this)[parentRepresentative];
Index parentRepresentative = bayesTree.findParentClique(parents);
if(debug) cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << endl;
sharedClique parent_clique = bayesTree[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
// if the parents and parent clique have the same size, add to parent clique
if (parent_clique->size() == size_t(parents.size())) {
if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) cout << "Adding to parent clique" << endl;
#ifndef NDEBUG
// Debug check that the parent keys of the new conditional match the keys
@ -660,11 +588,11 @@ namespace gtsam {
// ++ cond;
// }
#endif
addToCliqueFront(conditional, parent_clique);
addToCliqueFront(bayesTree, conditional, parent_clique);
} else {
if(debug) cout << "Starting new clique" << endl;
// otherwise, start a new clique and add it to the tree
addClique(conditional,parent_clique);
bayesTree.addClique(conditional,parent_clique);
}
}
@ -672,24 +600,14 @@ namespace gtsam {
//TODO: remove this function after removing TSAM.cpp
template<class CONDITIONAL>
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::insert(
const BayesNet<CONDITIONAL>& bayesNet, list<sharedClique>& children, bool isRootClique)
{
const sharedConditional& clique, list<sharedClique>& children, bool isRootClique) {
static const bool debug = false;
if (bayesNet.size() == 0)
throw invalid_argument("BayesTree::insert: empty bayes net!");
if (clique->nrFrontals() == 0)
throw invalid_argument("BayesTree::insert: empty clique!");
// create a new clique and add all the conditionals to the clique
sharedClique new_clique;
typename BayesNet<CONDITIONAL>::sharedConditional conditional;
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
if(debug) conditional->print("Inserting conditional: ");
if (!new_clique.get())
new_clique = addClique(conditional,children);
else
addToCliqueFront(conditional, new_clique);
}
sharedClique new_clique = addClique(clique, children);
if (isRootClique) root_ = new_clique;
return new_clique;
@ -699,7 +617,7 @@ namespace gtsam {
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, *subtree) { nodes_[cond->key()] = subtree; }
BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; }
// Fill index for each child
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, subtree->children_) {
fillNodesIndex(child); }
@ -713,19 +631,20 @@ namespace gtsam {
// property, those separator variables in the subtree that are ordered
// lower than the highest frontal variable of the subtree root will all
// appear in the separator of the subtree root.
if(subtree->separator_.empty()) {
if(subtree->conditional()->parents().empty()) {
assert(!root_);
root_ = subtree;
} else {
Index parentRepresentative = findParentClique(subtree->separator_);
Index parentRepresentative = findParentClique(subtree->conditional()->parents());
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += subtree;
subtree->parent_ = parent; // set new parent!
}
// Now fill in the nodes index
if(nodes_.size() == 0 || subtree->back()->key() > (nodes_.size() - 1))
nodes_.resize(subtree->back()->key() + 1);
if(nodes_.size() == 0 || (subtree->conditional()->lastFrontalKey()) > (nodes_.size() - 1)) {
nodes_.resize(subtree->conditional()->lastFrontalKey() + 1);
}
fillNodesIndex(subtree);
}
}
@ -743,7 +662,7 @@ namespace gtsam {
// calculate or retrieve its marginal
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
return GenericSequentialSolver<FactorType> (cliqueMarginal).marginalFactor(
return GenericSequentialSolver<FactorType>(cliqueMarginal).marginalFactor(
key, function);
}
@ -817,7 +736,7 @@ namespace gtsam {
// add children to list of orphans (splice also removed them from clique->children_)
orphans.splice (orphans.begin(), clique->children_);
bn.push_back(*clique);
bn.push_back(clique->conditional());
}
}

57
gtsam/inference/BayesTree.h
View File

@ -19,13 +19,11 @@
#pragma once
#include <list>
#include <vector>
#include <stdexcept>
#include <deque>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
@ -51,43 +49,45 @@ namespace gtsam {
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*/
struct Clique: public BayesNet<CONDITIONAL> {
struct Clique {
protected:
void assertInvariants() const;
public:
typedef BayesNet<CONDITIONAL> Base;
typedef CONDITIONAL Base;
typedef typename boost::shared_ptr<Clique> shared_ptr;
typedef typename boost::weak_ptr<Clique> weak_ptr;
sharedConditional conditional_;
weak_ptr parent_;
std::list<shared_ptr> children_;
std::list<Index> separator_; /** separator keys */
typename FactorType::shared_ptr cachedFactor_;
friend class BayesTree<CONDITIONAL>;
//* Constructor */
Clique();
Clique() {}
Clique(const sharedConditional& conditional);
Clique(const BayesNet<CONDITIONAL>& bayesNet);
/** return keys in frontal:separator order */
std::vector<Index> keys() const;
/** print this node */
void print(const std::string& s = "") const;
/** The size *includes* the separator */
size_t size() const {
return this->conditionals_.size() + separator_.size();
}
/** The arrow operator accesses the conditional */
const CONDITIONAL* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
CONDITIONAL* operator->() { return conditional_.get(); }
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired();}
inline bool isRoot() const { return parent_.expired(); }
/** return the const reference of children */
std::list<shared_ptr>& children() { return children_; }
@ -122,15 +122,19 @@ namespace gtsam {
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
bool equals(const Clique& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*(other.conditional()), tol);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
ar & BOOST_SERIALIZATION_NVP(separator_);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
}
@ -194,7 +198,8 @@ namespace gtsam {
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
static void addToCliqueFront(BayesTree<CONDITIONAL>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
@ -220,15 +225,18 @@ namespace gtsam {
* Constructing Bayes trees
*/
/** Insert a new conditional */
void insert(const sharedConditional& conditional);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const BayesNet<CONDITIONAL>& bayesNet,
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
@ -262,6 +270,9 @@ namespace gtsam {
return 0;
}
/** return nodes */
Nodes nodes() const { return nodes_; }
/** return root clique */
const sharedClique& root() const { return root_; }
sharedClique& root() { return root_; }
@ -275,7 +286,7 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
typename CONDITIONAL::FactorType::shared_ptr marginalFactor(Index key,
typename FactorType::shared_ptr marginalFactor(Index key,
Eliminate function) const;
/**

10
gtsam/inference/ClusterTree.h
View File

@ -19,15 +19,15 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <list>
#include <vector>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <list>
#include <vector>
#include <iostream>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
namespace gtsam {

10
gtsam/inference/Conditional.h
View File

@ -24,7 +24,6 @@
#include <boost/foreach.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Factor.h>
namespace gtsam {
@ -47,9 +46,6 @@ class Conditional: public gtsam::Factor<KEY>, boost::noncopyable, public Testabl
private:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
@ -61,6 +57,9 @@ private:
protected:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
// Calls the base class assertInvariants, which checks for unique keys
void assertInvariants() const { Factor<KEY>::assertInvariants(); }
@ -131,7 +130,8 @@ public:
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
/** Special accessor when there is only one frontal variable. */
Key key() const { assert(nrFrontals_==1); return FactorType::front(); }
Key firstFrontalKey() const { return FactorType::front(); }
Key lastFrontalKey() const { return *(endFrontals()-1); }
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const { return FactorType::begin(); }

6
gtsam/inference/EliminationTree-inl.h
View File

@ -47,12 +47,12 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
// TODO: wait for completion of all threads
// Combine all factors (from this node and from subtrees) into a joint factor
pair<typename BayesNet::shared_ptr, typename FACTOR::shared_ptr>
typename FactorGraph<FACTOR>::EliminationResult
eliminated(function(factors, 1));
conditionals[this->key_] = eliminated.first->front();
conditionals[this->key_] = eliminated.first;
if(debug) cout << "Eliminated " << this->key_ << " to get:\n";
if(debug) eliminated.first->front()->print("Conditional: ");
if(debug) eliminated.first->print("Conditional: ");
if(debug) eliminated.second->print("Factor: ");
return eliminated.second;

3
gtsam/inference/EliminationTree.h
View File

@ -6,11 +6,8 @@
*/
#pragma once
#include <list>
#include <string>
#include <utility>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesNet.h>

1
gtsam/inference/Factor-inl.h
View File

@ -36,6 +36,7 @@ namespace gtsam {
template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) :
keys_(c.keys()) {
// assert(c.nrFrontals() == 1);
assertInvariants();
}

2
gtsam/inference/Factor.h
View File

@ -21,10 +21,8 @@
#pragma once
#include <map>
#include <set>
#include <vector>
#include <boost/utility.hpp> // for noncopyable
#include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <gtsam/base/Testable.h>

17
gtsam/inference/FactorGraph.h
View File

@ -21,14 +21,11 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/graph.h>
@ -51,7 +48,7 @@ namespace gtsam {
/** typedef for elimination result */
typedef std::pair<
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr,
boost::shared_ptr<typename FACTOR::ConditionalType>,
typename FACTOR::shared_ptr> EliminationResult;
/** typedef for an eliminate subroutine */
@ -88,6 +85,10 @@ namespace gtsam {
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor);
/** push back many factors stored in a vector*/
template<typename DERIVEDFACTOR>
void push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors);
/** ------------------ Querying Factor Graphs ---------------------------- */
/** print out graph */
@ -236,5 +237,13 @@ namespace gtsam {
this->push_back(*(factor++));
}
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
void FactorGraph<FACTOR>::push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors) {
BOOST_FOREACH(const boost::shared_ptr<DERIVEDFACTOR>& factor, factors)
this->push_back(factor);
}
} // namespace gtsam

7
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -18,12 +18,11 @@
#pragma once
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
#include <utility>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/FactorGraph.h>
namespace gtsam {
/**

6
gtsam/inference/GenericSequentialSolver.h
View File

@ -18,12 +18,10 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/BayesNet.h>
#include <utility>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam {
template<class FACTOR>

27
gtsam/inference/ISAM-inl.h
View File

@ -24,7 +24,7 @@ using namespace boost::assign;
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
#include <gtsam/inference/GenericMultifrontalSolver-inl.h>
namespace gtsam {
@ -35,9 +35,9 @@ namespace gtsam {
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
/** Create a Bayes Tree from a Bayes Net */
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
BayesTree<CONDITIONAL>(bayesNet) {}
// template<class CONDITIONAL>
// ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
// BayesTree<CONDITIONAL>(bayesNet) {}
/* ************************************************************************* */
template<class CONDITIONAL>
@ -53,13 +53,18 @@ namespace gtsam {
factors.push_back(newFactors);
// eliminate into a Bayes net
GenericSequentialSolver<typename CONDITIONAL::FactorType> solver(factors);
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = solver.eliminate(function);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
this->insert(*rit);
// GenericSequentialSolver<typename CONDITIONAL::FactorType> solver(factors);
// typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = solver.eliminate(function);
//
// // insert conditionals back in, straight into the topless bayesTree
// typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
// for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
// BayesTree<CONDITIONAL>::insert(*this,*rit);
GenericMultifrontalSolver<typename CONDITIONAL::FactorType, JunctionTree<FG> > solver(factors);
boost::shared_ptr<BayesTree<CONDITIONAL> > bayesTree;
bayesTree = solver.eliminate(function);
this->root_ = bayesTree->root();
this->nodes_ = bayesTree->nodes();
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans)

16
gtsam/inference/ISAM.h
View File

@ -19,23 +19,14 @@
#pragma once
#include <map>
#include <list>
#include <vector>
#include <deque>
//#include <boost/serialization/map.hpp>
//#include <boost/serialization/list.hpp>
#include <stdexcept>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTree.h>
namespace gtsam {
template<class CONDITIONAL>
class ISAM: public BayesTree<CONDITIONAL> {
private:
typedef BayesTree<CONDITIONAL> Base;
public:
@ -45,6 +36,9 @@ namespace gtsam {
/** Create a Bayes Tree from a Bayes Net */
ISAM(const BayesNet<CONDITIONAL>& bayesNet);
/** Create a Bayes Tree from a Bayes Tree */
ISAM(const Base& bayesTree) : Base(bayesTree) {}
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique;
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques;

13
gtsam/inference/IndexConditional.h
View File

@ -73,8 +73,19 @@ namespace gtsam {
assertInvariants();
}
/** Named constructor directly returning a shared pointer */
template<class KEYS>
static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) {
shared_ptr conditional(new IndexConditional());
conditional->keys_.assign(keys.begin(), keys.end());
conditional->nrFrontals_ = nrFrontals;
return conditional;
}
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); }
IndexFactor::shared_ptr toFactor() const {
return IndexFactor::shared_ptr(new IndexFactor(*this));
}
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and

8
gtsam/inference/JunctionTree-inl.h
View File

@ -116,8 +116,8 @@ namespace gtsam {
if(bayesClique) {
// create a new clique in the junction tree
FastList<Index> frontals = bayesClique->ordering();
sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(),
(*bayesClique)->beginParents(), (*bayesClique)->endParents()));
// count the factors for this cluster to pre-allocate space
{
@ -173,7 +173,7 @@ namespace gtsam {
// come from and go to, create and eliminate the new joint factor.
tic(2, "CombineAndEliminate");
pair<
typename BayesNet<typename FG::FactorType::ConditionalType>::shared_ptr,
typename FG::FactorType::ConditionalType::shared_ptr,
typename FG::sharedFactor> eliminated(function(fg,
current->frontal.size()));
toc(2, "CombineAndEliminate");
@ -182,7 +182,7 @@ namespace gtsam {
tic(3, "Update tree");
// create a new clique corresponding the combined factors
typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(*eliminated.first));
typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(eliminated.first));
new_clique->children_ = children;
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children) {

2
gtsam/inference/JunctionTree.h
View File

@ -21,8 +21,6 @@
#include <set>
#include <vector>
#include <list>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/ClusterTree.h>

4
gtsam/inference/Permutation.h
View File

@ -18,12 +18,12 @@
#pragma once
#include <gtsam/base/types.h>
#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h>
namespace gtsam {
class Inference;

14
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -80,7 +80,7 @@ namespace gtsam {
}
/* ************************************************************************* */
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> //
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> //
EliminateSymbolic(const FactorGraph<IndexFactor>& factors, size_t nrFrontals) {
FastSet<Index> keys;
@ -91,14 +91,10 @@ namespace gtsam {
if (keys.size() < 1) throw invalid_argument(
"IndexFactor::CombineAndEliminate called on factors with no variables.");
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> result;
result.first.reset(new BayesNet<IndexConditional> ());
FastSet<Index>::const_iterator it;
for (it = keys.begin(); result.first->size() < nrFrontals; ++it) {
std::vector<Index> newKeys(it,keys.end());
result.first->push_back(boost::make_shared<IndexConditional>(newKeys, 1));
}
result.second.reset(new IndexFactor(it, keys.end()));
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
std::vector<Index> newKeys(keys.begin(),keys.end());
result.first.reset(new IndexConditional(newKeys, nrFrontals));
result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
return result;
}

3
gtsam/inference/SymbolicFactorGraph.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/EliminationTree.h>
@ -74,7 +73,7 @@ namespace gtsam {
* Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work.
*/
std::pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr>
std::pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr>
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
/* Template function implementation */

1
gtsam/inference/SymbolicMultifrontalSolver.h
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {

1
gtsam/inference/VariableIndex.cpp
View File

@ -16,6 +16,7 @@
* @created Oct 22, 2010
*/
#include <iostream>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {

3
gtsam/inference/VariableIndex.h
View File

@ -18,13 +18,10 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/Permutation.h>
#include <vector>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
namespace gtsam {

3
gtsam/inference/VariableSlots.h
View File

@ -27,9 +27,8 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <map>
#include <vector>
#include <string>
#include <vector>
namespace gtsam {

3
gtsam/inference/inference.h
View File

@ -18,15 +18,12 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
#include <boost/foreach.hpp>
#include <vector>
#include <deque>
namespace gtsam {

64
gtsam/inference/tests/testBayesTree.cpp
View File

@ -18,6 +18,8 @@
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
@ -66,16 +68,20 @@ IndexConditional::shared_ptr
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Cliques
IndexConditional::shared_ptr
ELB(IndexConditional::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));
// Bayes Tree for Asia example
SymbolicBayesTree createAsiaSymbolicBayesTree() {
SymbolicBayesTree bayesTree;
// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
bayesTree.insert(B);
bayesTree.insert(L);
bayesTree.insert(E);
bayesTree.insert(S);
bayesTree.insert(T);
bayesTree.insert(X);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X);
return bayesTree;
}
@ -89,12 +95,8 @@ TEST( BayesTree, constructor )
LONGS_EQUAL(4,bayesTree.size());
// Check root
BayesNet<IndexConditional> expected_root;
expected_root.push_back(E);
expected_root.push_back(L);
expected_root.push_back(B);
boost::shared_ptr<BayesNet<IndexConditional> > actual_root = bayesTree.root();
CHECK(assert_equal(expected_root,*actual_root));
boost::shared_ptr<IndexConditional> actual_root = bayesTree.root()->conditional();
CHECK(assert_equal(*ELB,*actual_root));
// Create from symbolic Bayes chain in which we want to discover cliques
BayesNet<IndexConditional> ASIA;
@ -154,18 +156,18 @@ TEST( BayesTree, removePath )
F(new IndexConditional(_F_, _E_));
SymbolicBayesTree bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
bayesTree.insert(A);
bayesTree.insert(B);
bayesTree.insert(C);
bayesTree.insert(D);
bayesTree.insert(E);
bayesTree.insert(F);
SymbolicBayesTree::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C);
SymbolicBayesTree::insert(bayesTree, D);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, F);
// remove C, expected outcome: factor graph with ABC,
// Bayes Tree now contains two orphan trees: D|C and E|B,F|E
SymbolicFactorGraph expected;
expected.push_factor(_B_,_A_);
expected.push_factor(_A_);
// expected.push_factor(_A_);
expected.push_factor(_C_,_A_);
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_];
@ -205,8 +207,8 @@ TEST( BayesTree, removePath2 )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor(_E_,_L_,_B_);
expected.push_factor(_L_,_B_);
expected.push_factor(_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
@ -227,8 +229,8 @@ TEST( BayesTree, removePath3 )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor(_E_,_L_,_B_);
expected.push_factor(_L_,_B_);
expected.push_factor(_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
@ -254,8 +256,8 @@ TEST( BayesTree, removeTop )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor(_E_,_L_,_B_);
expected.push_factor(_L_,_B_);
expected.push_factor(_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
@ -295,8 +297,8 @@ TEST( BayesTree, removeTop2 )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor(_E_,_L_,_B_);
expected.push_factor(_L_,_B_);
expected.push_factor(_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
@ -318,10 +320,10 @@ TEST( BayesTree, removeTop3 )
// Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTree bayesTree;
bayesTree.insert(X);
bayesTree.insert(A);
bayesTree.insert(B);
bayesTree.insert(C);
SymbolicBayesTree::insert(bayesTree, X);
SymbolicBayesTree::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C);
// remove all
list<Index> keys;

48
gtsam/inference/tests/testSymbolicFactor.cpp
View File

@ -71,28 +71,38 @@ TEST(SymbolicFactor, EliminateSymbolic) {
factors.push_factor(1,2,5);
factors.push_factor(0,3);
IndexFactor expected_factor(4,5,6);
BayesNet<IndexConditional> expected_bn;
vector<Index> parents;
IndexFactor expectedFactor(4,5,6);
std::vector<Index> keys; keys += 0,1,2,3,4,5,6;
IndexConditional::shared_ptr expectedConditional(new IndexConditional(keys, 4));
parents.clear(); parents += 1,2,3,4,5,6;
expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(0, parents)));
IndexFactor::shared_ptr actualFactor;
IndexConditional::shared_ptr actualConditional;
boost::tie(actualConditional, actualFactor) = EliminateSymbolic(factors, 4);
parents.clear(); parents += 2,3,4,5,6;
expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(1, parents)));
CHECK(assert_equal(*expectedConditional, *actualConditional));
CHECK(assert_equal(expectedFactor, *actualFactor));
parents.clear(); parents += 3,4,5,6;
expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(2, parents)));
parents.clear(); parents += 4,5,6;
expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(3, parents)));
BayesNet<IndexConditional>::shared_ptr actual_bn;
IndexFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4);
CHECK(assert_equal(expected_bn, *actual_bn));
CHECK(assert_equal(expected_factor, *actual_factor));
// BayesNet<IndexConditional> expected_bn;
// vector<Index> parents;
//
// parents.clear(); parents += 1,2,3,4,5,6;
// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(0, parents)));
//
// parents.clear(); parents += 2,3,4,5,6;
// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(1, parents)));
//
// parents.clear(); parents += 3,4,5,6;
// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(2, parents)));
//
// parents.clear(); parents += 4,5,6;
// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(3, parents)));
//
// BayesNet<IndexConditional>::shared_ptr actual_bn;
// IndexFactor::shared_ptr actual_factor;
// boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4);
//
// CHECK(assert_equal(expected_bn, *actual_bn));
// CHECK(assert_equal(expected_factor, *actual_factor));
}
/* ************************************************************************* */

4
gtsam/linear/Errors.h
View File

@ -19,10 +19,6 @@
#pragma once
#include <vector>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {

62
gtsam/linear/GaussianBayesNet.cpp
View File

@ -73,28 +73,27 @@ boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn)
vector<size_t> dimensions(bn.size());
Index var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->get_R().rows();
dimensions[var++] = conditional->dim();
}
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
}
/* ************************************************************************* */
VectorValues optimize(const GaussianBayesNet& bn)
{
VectorValues optimize(const GaussianBayesNet& bn) {
return *optimize_(bn);
}
/* ************************************************************************* */
boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn)
{
boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
// get the RHS as a VectorValues to initialize system
boost::shared_ptr<VectorValues> result(new VectorValues(rhs(bn)));
/** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) {
Vector x = cg->solve(*result); // Solve for that variable
(*result)[cg->key()] = x; // store result in partial solution
}
return result;
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn)
cg->solveInPlace(*result); // solve and store solution in same step
return result;
}
/* ************************************************************************* */
@ -112,13 +111,7 @@ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
// i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
Index i = cg->key();
Vector zi = emul(y[i],cg->get_sigmas());
GaussianConditional::const_iterator it;
for (it = cg->beginParents(); it!= cg->endParents(); it++) {
multiplyAdd(-1.0,cg->get_S(it),x[*it],zi);
}
x[i] = gtsam::backSubstituteUpper(cg->get_R(), zi);
cg->solveInPlace(x);
}
}
@ -135,21 +128,12 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
Index j = cg->key();
gy[j] = gtsam::backSubstituteUpper(gy[j],Matrix(cg->get_R()));
GaussianConditional::const_iterator it;
for (it = cg->beginParents(); it!= cg->endParents(); it++) {
const Index i = *it;
transposeMultiplyAdd(-1.0,cg->get_S(it),gy[j],gy[i]);
}
}
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn)
cg->solveTransposeInPlace(gy);
// Scale gy
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
Index j = cg->key();
gy[j] = emul(gy[j],cg->get_sigmas());
}
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn)
cg->scaleFrontalsBySigma(gy);
return gy;
}
@ -161,8 +145,11 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// and at the same time create a mapping from keys to indices
size_t N=0; map<Index,size_t> mapping;
BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
mapping.insert(make_pair(cg->key(),N));
N += cg->dim();
GaussianConditional::const_iterator it = cg->beginFrontals();
for (; it != cg->endFrontals(); ++it) {
mapping.insert(make_pair(*it,N));
N += cg->dim(it);
}
}
// create matrix and copy in values
@ -172,7 +159,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
boost::shared_ptr<const GaussianConditional> cg = bn[key];
// get sigmas
Vector sigmas = cg->get_sigmas();
@ -207,15 +194,8 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
/* ************************************************************************* */
VectorValues rhs(const GaussianBayesNet& bn) {
boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn) {
Index key = cg->key();
// get sigmas
const Vector& sigmas = cg->get_sigmas();
// get RHS and copy to d
GaussianConditional::const_d_type d = cg->get_d();
(*result)[key] = ediv_(d,sigmas); // TODO ediv_? I think not
}
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn)
cg->rhs(*result);
return *result;
}

9
gtsam/linear/GaussianBayesNet.h
View File

@ -20,10 +20,6 @@
#pragma once
#include <list>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/inference/BayesNet.h>
@ -71,11 +67,12 @@ namespace gtsam {
/*
* Backsubstitute
* (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
* @param y is the RHS of the system
*/
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y);
/*
* Backsubstitute in place, y is replaced with solution
* Backsubstitute in place, y starts as RHS and is replaced with solution
*/
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y);
@ -89,6 +86,8 @@ namespace gtsam {
/**
* Return (dense) upper-triangular matrix representation
* NOTE: if this is the result of elimination with LDL, the matrix will
* not necessarily be upper triangular due to column permutations
*/
std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);

114
gtsam/linear/GaussianConditional.cpp
View File

@ -39,7 +39,7 @@ GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size()));
rsd_(0) = R; //.triangularView<Eigen::Upper>();
rsd_(0) = R.triangularView<Eigen::Upper>();
get_d_() = d;
}
@ -93,9 +93,13 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
/* ************************************************************************* */
void GaussianConditional::print(const string &s) const
{
cout << s << ": density on " << key() << endl;
cout << s << ": density on ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
cout << (boost::format("[%1%]")%(*it)).str() << " ";
}
cout << endl;
gtsam::print(Matrix(get_R()),"R");
for(const_iterator it = beginParents() ; it != endParents() ; it++ ) {
for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(*it)).str());
}
gtsam::print(Vector(get_d()),"d");
@ -135,35 +139,99 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const
return true;
}
/* ************************************************************************* */
GaussianConditional::rsd_type::constBlock GaussianConditional::get_R() const {
return rsd_.range(0, nrFrontals());
}
/* ************************************************************************* */
JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
return JacobianFactor::shared_ptr(new JacobianFactor(*this));
}
/* ************************************************************************* */
Vector GaussianConditional::solve(const VectorValues& x) const {
static const bool debug = false;
if(debug) print("Solving conditional ");
Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
rhs += -get_S(parent) * x[*parent];
}
if(debug) gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
if(debug) gtsam::print(rhs, "rhs: ");
if(debug) {
Vector soln = backSubstituteUpper(get_R(), rhs, false);
gtsam::print(soln, "back-substitution solution: ");
return soln;
} else
return backSubstituteUpper(get_R(), rhs, false);
void GaussianConditional::rhs(VectorValues& x) const {
Vector d = rhs();
x.range(beginFrontals(), endFrontals(), d);
}
/* ************************************************************************* */
Vector GaussianConditional::solve(const Permuted<VectorValues>& x) const {
Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent)
rhs += -get_S(parent) * x[*parent];
return backSubstituteUpper(get_R(), rhs, false);
void GaussianConditional::solveInPlace(VectorValues& x) const {
static const bool debug = false;
if(debug) print("Solving conditional in place");
// Vector rhs(get_d());
Vector rhs = x.range(beginFrontals(), endFrontals());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
rhs += -get_S(parent) * x[*parent];
}
Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false);
if(debug) {
gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
gtsam::print(rhs, "rhs: ");
gtsam::print(soln, "full back-substitution solution: ");
}
x.range(beginFrontals(), endFrontals(), soln);
}
/* ************************************************************************* */
void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
static const bool debug = false;
if(debug) print("Solving conditional in place (permuted)");
// Vector rhs(get_d());
// Extract RHS from values - inlined from VectorValues
size_t s = 0;
for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it)
s += x[*it].size();
Vector rhs(s); size_t start = 0;
for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) {
SubVector v = x[*it];
const size_t d = v.size();
rhs.segment(start, d) = v;
start += d;
}
// apply parents to rhs
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
rhs += -get_S(parent) * x[*parent];
}
// solve system
Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false);
// apply solution: inlined manually due to permutation
size_t solnStart = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
const size_t d = dim(frontal);
x[*frontal] = soln.segment(solnStart, d);
solnStart += d;
}
}
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const {
VectorValues result = x;
solveInPlace(result);
return result;
}
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = gy.range(beginFrontals(), endFrontals());
// TODO: verify permutation
frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
GaussianConditional::const_iterator it;
for (it = beginParents(); it!= endParents(); it++) {
const Index i = *it;
transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
}
gy.range(beginFrontals(), endFrontals(), frontalVec);
}
/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
Vector frontalVec = gy.range(beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas());
gy.range(beginFrontals(), endFrontals(), frontalVec);
}
}

111
gtsam/linear/GaussianConditional.h
View File

@ -19,20 +19,22 @@
#pragma once
#include <list>
#include <vector>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
// Forward declaration to friend unit tests
class eliminate2GaussianFactorTest;
class constructorGaussianConditionalTest;
class eliminationGaussianFactorGraphTest;
namespace gtsam {
// Forward declarations
class GaussianFactor;
/**
@ -55,16 +57,28 @@ public:
typedef rsd_type::Column d_type;
typedef rsd_type::constColumn const_d_type;
typedef Eigen::LDLT<Matrix>::TranspositionType TranspositionType;
protected:
/** Store the conditional as one big upper-triangular wide matrix, arranged
* as [ R S1 S2 ... d ]. Access these blocks using a VerticalBlockView. */
* as [ R S1 S2 ... d ]. Access these blocks using a VerticalBlockView.
*
* WARNING!!! When using with LDL, R is the permuted upper triangular matrix.
* Its columns/rows do not correspond to the correct components of the variables.
* Use R*permutation_ to get back the correct non-permuted order,
* for example when converting to the Jacobian
* */
AbMatrix matrix_;
rsd_type rsd_;
/** vector of standard deviations */
Vector sigmas_;
/** Store the permutation matrix, used by LDL' in the pivoting process
* This is used to get back the correct ordering of x after solving by backSubstitition */
TranspositionType permutation_;
public:
/** default constructor needed for serialization */
@ -102,7 +116,9 @@ public:
* for multiple frontal variables.
*/
template<typename ITERATOR, class MATRIX>
GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas);
GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals,
const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas,
const TranspositionType& permutation = TranspositionType());
/** print */
void print(const std::string& = "GaussianConditional") const;
@ -113,14 +129,37 @@ public:
/** dimension of multivariate variable */
size_t dim() const { return rsd_.rows(); }
/** Compute the information matrix */
Matrix computeInformation() const {
Matrix R = get_R();
R = R*permutation_;
return R.transpose()*R;
}
/** return stuff contained in GaussianConditional */
const_d_type get_d() const { return rsd_.column(1+nrParents(), 0); }
rsd_type::constBlock get_R() const { return rsd_(0); }
rsd_type::constBlock get_R() const;
/** access the d vector */
const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); }
/**
* get a RHS as a single vector
* FIXME: shouldn't this be weighted by sigmas?
*/
Vector rhs() const { return get_d(); }
/** get the dimension of a variable */
size_t dim(const_iterator variable) const { return rsd_(variable - this->begin()).cols(); }
rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); }
const Vector& get_sigmas() const {return sigmas_;}
const AbMatrix& matrix() const { return matrix_; }
const rsd_type& rsd() const { return rsd_; } /// DEBUGGING ONLY
protected:
const AbMatrix& matrix() const { return matrix_; }
const rsd_type& rsd() const { return rsd_; }
public:
/**
* Copy to a Factor (this creates a JacobianFactor and returns it as its
* base class GaussianFactor.
@ -128,25 +167,57 @@ public:
boost::shared_ptr<JacobianFactor> toFactor() const;
/**
* solve a conditional Gaussian
* @param x values structure in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...)
* Adds the RHS to a given VectorValues for use in solve() functions.
* @param x is the values to be updated, assumed allocated
*/
Vector solve(const VectorValues& x) const;
void rhs(VectorValues& x) const;
/**
* solve a conditional Gaussian
* @param x values structure in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...)
* solves a conditional Gaussian and stores the result in x
* This function works for multiple frontal variables.
* NOTE: assumes that the RHS for the frontals is stored in x, and
* then replaces the RHS with the partial result for this conditional,
* assuming that parents have been solved already.
*
* @param x values structure with solved parents, and the RHS for this conditional
* @return solution x = R \ (d - Sy - Tz - ...) for each frontal variable
*/
Vector solve(const Permuted<VectorValues>& x) const;
void solveInPlace(VectorValues& x) const;
/**
* solves a conditional Gaussian and stores the result in x
* Identical to solveInPlace() above, with a permuted x
*/
void solveInPlace(Permuted<VectorValues>& x) const;
/**
* Solves a conditional Gaussian and returns a new VectorValues
* This function works for multiple frontal variables, but should
* only be used for testing as it copies the input vector values
*
* Assumes, as in solveInPlace, that the RHS has been stored in x
* for all frontal variables
*/
VectorValues solve(const VectorValues& x) const;
// functions for transpose backsubstitution
/**
* Performs backsubstition in place on values
*/
void solveTransposeInPlace(VectorValues& gy) const;
void scaleFrontalsBySigma(VectorValues& gy) const;
protected:
rsd_type::Column get_d_() { return rsd_.column(1+nrParents(), 0); }
rsd_type::Block get_R_() { return rsd_(0); }
rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); }
// Friends
friend class JacobianFactor;
friend class ::eliminate2GaussianFactorTest;
friend class ::constructorGaussianConditionalTest;
friend class ::eliminationGaussianFactorGraphTest;
private:
/** Serialization function */
@ -164,9 +235,9 @@ private:
template<typename ITERATOR, class MATRIX>
GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
const Vector& sigmas) :
const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) :
IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
matrix_), sigmas_(sigmas) {
matrix_), sigmas_(sigmas), permutation_(permutation) {
rsd_.assignNoalias(matrices);
}

154
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -277,7 +277,7 @@ namespace gtsam {
CombineJacobians(factors, VariableSlots(factors));
toc(1, "Combine");
tic(2, "eliminate");
GaussianBayesNet::shared_ptr gbn(jointFactor->eliminate(nrFrontals));
GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals);
toc(2, "eliminate");
return make_pair(gbn, jointFactor);
}
@ -287,7 +287,7 @@ namespace gtsam {
FastMap<Index, SlotEntry> findScatterAndDims
(const FactorGraph<GaussianFactor>& factors) {
static const bool debug = false;
const bool debug = ISDEBUG("findScatterAndDims");
// The "scatter" is a map from global variable indices to slot indices in the
// union of involved variables. We also include the dimensionality of the
@ -352,7 +352,7 @@ namespace gtsam {
// Extract conditionals and fill in details of the remaining factor
tic(5, "split");
GaussianBayesNet::shared_ptr conditionals =
GaussianConditional::shared_ptr conditionals =
combinedFactor->splitEliminatedFactor(nrFrontals, keys);
if (debug) {
conditionals->print("Extracted conditionals: ");
@ -413,12 +413,12 @@ namespace gtsam {
toc(1, "convert to Jacobian");
tic(2, "Jacobian EliminateGaussian");
GaussianBayesNet::shared_ptr bn;
GaussianConditional::shared_ptr conditionals;
GaussianFactor::shared_ptr factor;
boost::tie(bn, factor) = EliminateJacobians(jacobians, nrFrontals);
boost::tie(conditionals, factor) = EliminateJacobians(jacobians, nrFrontals);
toc(2, "Jacobian EliminateGaussian");
return make_pair(bn, factor);
return make_pair(conditionals, factor);
} // EliminateQR
/* ************************************************************************* */
@ -501,4 +501,146 @@ namespace gtsam {
} // EliminatePreferCholesky
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateLDL");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<Index> keys(scatter.size());
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
keys[var_slot.second.slot] = var_slot.first;
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
if (debug) {
// print out everything before combine
factors.print("Factors to be combined into hessian");
cout << "Dimensions (" << dimensions.size() << "): ";
BOOST_FOREACH(size_t d, dimensions) cout << d << " ";
cout << "\nScatter:" << endl;
BOOST_FOREACH(const Scatter::value_type& p, scatter)
cout << " Index: " << p.first << ", " << p.second.toString() << endl;
}
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
// Do LDL, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial LDL");
Eigen::LDLT<Matrix>::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals);
toc(4, "partial LDL");
// Extract conditionals and fill in details of the remaining factor
tic(5, "split");
GaussianConditional::shared_ptr conditionals =
combinedFactor->splitEliminatedFactor(nrFrontals, keys, permutation);
if (debug) {
conditionals->print("Extracted conditionals: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditionals, combinedFactor);
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but LDL cannot.
// Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models.
bool useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
useQR = true;
break;
}
}
// Convert all factors to the appropriate type
// and call the type-specific EliminateGaussian.
if (useQR) return EliminateQR(factors, nrFrontals);
GaussianFactorGraph::EliminationResult ret;
try {
tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL");
} catch (const exception& e) {
cout << "Exception in EliminateLDL: " << e.what() << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("ldlPartial", true);
SETDEBUG("findScatterAndDims", true);
factors.print("Combining factors: ");
EliminateLDL(factors, nrFrontals);
throw;
}
const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
if (checkLDL) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "LDL and QR do not agree" << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("LDL and QR do not agree");
}
}
return ret;
} // EliminatePreferLDL
} // namespace gtsam

12
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,11 +21,9 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
@ -192,8 +190,8 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
JacobianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph<
HessianFactor>& factors, size_t nrFrontals = 1);
// GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph<
// HessianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
@ -204,4 +202,10 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminatePreferLDL(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateLDL(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
} // namespace gtsam

35
gtsam/linear/GaussianISAM.cpp
View File

@ -37,10 +37,9 @@ BayesNet<GaussianConditional>::shared_ptr GaussianISAM::marginalBayesNet(Index j
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianISAM::marginal(Index j) const {
Matrix GaussianISAM::marginalCovariance(Index j) const {
GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
return conditional->computeInformation().inverse();
}
/* ************************************************************************* */
@ -52,20 +51,30 @@ std::pair<Vector, Matrix> GaussianISAM::marginal(Index j) const {
/* ************************************************************************* */
void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
GaussianISAM::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
Vector x = cg->solve(result); // Solve for that variable
result[cg->key()] = x; // store result in partial solution
}
BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_) {
// RHS for current conditional should already be in place in result
clique->conditional()->solveInPlace(result);
BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_)
optimize(child, result);
}
}
/* ************************************************************************* */
void GaussianISAM::treeRHS(const GaussianISAM::sharedClique& clique, VectorValues& result) {
clique->conditional()->rhs(result);
BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_)
treeRHS(child, result);
}
/* ************************************************************************* */
VectorValues GaussianISAM::rhs(const GaussianISAM& bayesTree) {
VectorValues result(bayesTree.dims_); // allocate
treeRHS(bayesTree.root(), result); // recursively fill
return result;
}
/* ************************************************************************* */
VectorValues optimize(const GaussianISAM& bayesTree) {
VectorValues result(bayesTree.dims_);
VectorValues result = GaussianISAM::rhs(bayesTree);
// starting from the root, call optimize on each conditional
optimize(bayesTree.root(), result);
return result;
@ -77,4 +86,4 @@ BayesNet<GaussianConditional> GaussianISAM::shortcut(sharedClique clique, shared
}
/* ************************************************************************* */
} /// namespace gtsam
} // \namespace gtsam

30
gtsam/linear/GaussianISAM.h
View File

@ -20,8 +20,8 @@
#pragma once
#include <gtsam/inference/ISAM.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianJunctionTree.h>
namespace gtsam {
@ -36,7 +36,10 @@ public:
GaussianISAM() : Super() {}
/** Create a Bayes Tree from a Bayes Net */
GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {}
// GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {}
GaussianISAM(const BayesTree<GaussianConditional>& bayesTree) : Super(bayesTree) {
GaussianJunctionTree::countDims(bayesTree, dims_);
}
/** Override update_internal to also keep track of variable dimensions. */
template<class FACTORGRAPH>
@ -73,19 +76,28 @@ public:
/** return marginal on any variable as a factor, Bayes net, or mean/cov */
GaussianFactor::shared_ptr marginalFactor(Index j) const;
BayesNet<GaussianConditional>::shared_ptr marginalBayesNet(Index key) const;
std::pair<Vector,Matrix> marginal(Index key) const;
Matrix marginalCovariance(Index key) const;
/** return joint between two variables, as a Bayes net */
BayesNet<GaussianConditional>::shared_ptr jointBayesNet(Index key1, Index key2) const;
/** return the conditional P(S|Root) on the separator given the root */
static BayesNet<GaussianConditional> shortcut(sharedClique clique, sharedClique root);
};
// recursively optimize this conditional and all subtrees
void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result);
/** load a VectorValues with the RHS of the system for backsubstitution */
static VectorValues rhs(const GaussianISAM& bayesTree);
// optimize the BayesTree, starting from the root
VectorValues optimize(const GaussianISAM& bayesTree);
protected:
}/// namespace gtsam
/** recursively load RHS for system */
static void treeRHS(const GaussianISAM::sharedClique& clique, VectorValues& result);
}; // \class GaussianISAM
// recursively optimize this conditional and all subtrees
void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result);
// optimize the BayesTree, starting from the root
VectorValues optimize(const GaussianISAM& bayesTree);
} // \namespace gtsam

34
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -36,11 +36,15 @@ namespace gtsam {
/* ************************************************************************* */
void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const {
// solve the bayes net in the current node
GaussianBayesNet::const_reverse_iterator it = current->rbegin();
for (; it!=current->rend(); ++it) {
Vector x = (*it)->solve(config); // Solve for that variable
config[(*it)->key()] = x; // store result in partial solution
}
current->conditional()->solveInPlace(config);
// GaussianBayesNet::const_reverse_iterator it = current->rbegin();
// for (; it!=current->rend(); ++it) {
// (*it)->solveInPlace(config); // solve and store result
//
//// Vector x = (*it)->solve(config); // Solve for that variable
//// config[(*it)->key()] = x; // store result in partial solution
// }
// solve the bayes nets in the child nodes
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) {
@ -48,16 +52,11 @@ namespace gtsam {
}
}
/* ************************************************************************* */
void countDims(const boost::shared_ptr<const BayesTree<GaussianConditional>::Clique>& clique, vector<size_t>& dims) {
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional>& cond, *clique) {
// There should be no two conditionals on the same variable
assert(dims[cond->key()] == 0);
dims[cond->key()] = cond->dim();
}
BOOST_FOREACH(const boost::shared_ptr<const BayesTree<GaussianConditional>::Clique>& child, clique->children()) {
countDims(child, dims);
}
/* ************************************************************************* */
void GaussianJunctionTree::btreeRHS(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const {
current->conditional()->rhs(config);
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children())
btreeRHS(child, config);
}
/* ************************************************************************* */
@ -67,11 +66,12 @@ namespace gtsam {
boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate(function));
toc(1, "GJT eliminate");
// Allocate solution vector
// Allocate solution vector and copy RHS
tic(2, "allocate VectorValues");
vector<size_t> dims(rootClique->back()->key() + 1, 0);
vector<size_t> dims(rootClique->conditional()->back()+1, 0);
countDims(rootClique, dims);
VectorValues result(dims);
btreeRHS(rootClique, result);
toc(2, "allocate VectorValues");
// back-substitution

31
gtsam/linear/GaussianJunctionTree.h
View File

@ -19,12 +19,12 @@
#pragma once
#include <boost/foreach.hpp>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/* ************************************************************************* */
@ -43,6 +43,9 @@ namespace gtsam {
// back-substitute in topological sort order (parents first)
void btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const;
// find the RHS for the system in order to perform backsubstitution
void btreeRHS(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const;
public :
/** Default constructor */
@ -52,11 +55,33 @@ namespace gtsam {
GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {}
/** Construct from a factor graph and a pre-computed variable index. */
GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) : Base(fg, variableIndex) {}
GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex)
: Base(fg, variableIndex) {}
// optimize the linear graph
VectorValues optimize(Eliminate function) const;
// convenient function to return dimensions of all variables in the BayesTree<GaussianConditional>
template<class DIM_CONTAINER>
static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) {
dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0);
countDims(bayesTree.root(), dims);
}
private:
template<class DIM_CONTAINER>
static void countDims(const boost::shared_ptr<const BayesTree::Clique>& clique, DIM_CONTAINER& dims) {
GaussianConditional::const_iterator it = clique->conditional()->beginFrontals();
for (; it != clique->conditional()->endFrontals(); ++it) {
assert(dims.at(*it) == 0);
dims.at(*it) = clique->conditional()->dim(it);
}
BOOST_FOREACH(const boost::shared_ptr<const BayesTree::Clique>& child, clique->children()) {
countDims(child, dims);
}
}
}; // GaussianJunctionTree
} // namespace gtsam

49
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -23,18 +23,19 @@
namespace gtsam {
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) :
Base(factorGraph) {}
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) {}
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR) :
Base(factorGraph, variableIndex), useQR_(useQR) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::shared_ptr
GaussianMultifrontalSolver::Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) {
return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex));
const VariableIndex::shared_ptr& variableIndex, bool useQR) {
return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex, useQR));
}
/* ************************************************************************* */
@ -44,34 +45,52 @@ void GaussianMultifrontalSolver::replaceFactors(const FactorGraph<GaussianFactor
/* ************************************************************************* */
BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate() const {
return Base::eliminate(&EliminateQR);
if (useQR_)
return Base::eliminate(&EliminateQR);
else
return Base::eliminate(&EliminatePreferLDL);
}
/* ************************************************************************* */
VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
tic(2,"optimize");
VectorValues::shared_ptr values(new VectorValues(junctionTree_->optimize(&EliminateQR)));
VectorValues::shared_ptr values;
if (useQR_)
values = VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminateQR)));
else
values= VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminatePreferLDL)));
toc(2,"optimize");
return values;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR)));
if (useQR_)
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR)));
else
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminatePreferLDL)));
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j,&EliminateQR);
if (useQR_)
return Base::marginalFactor(j,&EliminateQR);
else
return Base::marginalFactor(j,&EliminatePreferLDL);
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalCovariance(Index j) const {
Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const {
FactorGraph<GaussianFactor> fg;
fg.push_back(Base::marginalFactor(j,&EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
GaussianConditional::shared_ptr conditional;
if (useQR_) {
fg.push_back(Base::marginalFactor(j,&EliminateQR));
conditional = EliminateQR(fg,1).first;
} else {
fg.push_back(Base::marginalFactor(j,&EliminatePreferLDL));
conditional = EliminatePreferLDL(fg,1).first;
}
return conditional->computeInformation().inverse();
}
}

19
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -20,12 +20,9 @@
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <utility>
#include <vector>
namespace gtsam {
@ -50,26 +47,31 @@ protected:
typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base;
typedef boost::shared_ptr<const GaussianMultifrontalSolver> shared_ptr;
/** flag to determine whether to use LDL or QR */
bool useQR_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph);
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph, bool useQR = false);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR = false);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR = false);
/**
* Return a new solver that solves the given factor graph, which must have
@ -115,7 +117,10 @@ public:
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
std::pair<Vector, Matrix> marginalCovariance(Index j) const;
Matrix marginalCovariance(Index j) const;
/** @return true if using QR */
inline bool usingQR() const { return useQR_; }
};

45
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -24,18 +24,18 @@
namespace gtsam {
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph) :
Base(factorGraph) {}
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {}
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) {}
const VariableIndex::shared_ptr& variableIndex, bool useQR) :
Base(factorGraph, variableIndex), useQR_(useQR) {}
/* ************************************************************************* */
GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(
const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) {
return shared_ptr(new GaussianSequentialSolver(factorGraph, variableIndex));
const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex, bool useQR) {
return shared_ptr(new GaussianSequentialSolver(factorGraph, variableIndex, useQR));
}
/* ************************************************************************* */
@ -45,7 +45,10 @@ void GaussianSequentialSolver::replaceFactors(const FactorGraph<GaussianFactor>:
/* ************************************************************************* */
GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const {
return Base::eliminate(&EliminateQR);
if (useQR_)
return Base::eliminate(&EliminateQR);
else
return Base::eliminate(&EliminatePreferLDL);
}
/* ************************************************************************* */
@ -78,23 +81,35 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j,&EliminateQR);
if (useQR_)
return Base::marginalFactor(j,&EliminateQR);
else
return Base::marginalFactor(j,&EliminatePreferLDL);
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j) const {
Matrix GaussianSequentialSolver::marginalCovariance(Index j) const {
FactorGraph<GaussianFactor> fg;
fg.push_back(Base::marginalFactor(j, &EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
GaussianConditional::shared_ptr conditional;
if (useQR_) {
fg.push_back(Base::marginalFactor(j, &EliminateQR));
conditional = EliminateQR(fg, 1).first;
} else {
fg.push_back(Base::marginalFactor(j, &EliminatePreferLDL));
conditional = EliminatePreferLDL(fg, 1).first;
}
return conditional->computeInformation().inverse();
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr
GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(
*Base::jointFactorGraph(js, &EliminateQR)));
if (useQR_)
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(
*Base::jointFactorGraph(js, &EliminateQR)));
else
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(
*Base::jointFactorGraph(js, &EliminatePreferLDL)));
}
}

16
gtsam/linear/GaussianSequentialSolver.h
View File

@ -19,13 +19,10 @@
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianConditional.h>
#include <utility>
#include <vector>
namespace gtsam {
@ -57,26 +54,31 @@ protected:
typedef GenericSequentialSolver<GaussianFactor> Base;
typedef boost::shared_ptr<const GaussianSequentialSolver> shared_ptr;
/** flag to determine whether to use LDL or QR */
bool useQR_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph);
GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph, bool useQR = false);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR = false);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR = false);
/**
* Return a new solver that solves the given factor graph, which must have
@ -115,7 +117,7 @@ public:
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
std::pair<Vector, Matrix> marginalCovariance(Index j) const;
Matrix marginalCovariance(Index j) const;
/**
* Compute the marginal joint over a set of variables, by integrating out

180
gtsam/linear/HessianFactor.cpp
View File

@ -11,11 +11,19 @@
/**
* @file HessianFactor.cpp
* @brief
* @author Richard Roberts
* @created Dec 8, 2010
*/
#include <sstream>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
@ -27,21 +35,19 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/tuple/tuple.hpp>
#include <sstream>
using namespace std;
using namespace boost::lambda;
namespace gtsam {
/* ************************************************************************* */
string SlotEntry::toString() const {
ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
void HessianFactor::assertInvariants() const {
#ifndef NDEBUG
@ -137,7 +143,7 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky");
const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
// Form Ab' * Ab
tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false);
@ -146,6 +152,7 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
toc(2, "zero");
tic(3, "update");
if (debug) cout << "Combining " << factors.size() << " factors" << endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
@ -230,7 +237,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
if(debug) {
this->print("Updating this: ");
update.print("with: ");
update.print("with (Hessian): ");
}
// Apply updates to the upper triangle
@ -288,25 +295,26 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
if(debug) {
this->print("Updating this: ");
update.print("with: ");
update.print("with (Jacobian): ");
}
Eigen::Block<typeof(update.matrix_)> updateA(update.matrix_.block(
update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
if (debug) cout << "updateA: \n" << updateA << endl;
Eigen::MatrixXd updateInform;
Matrix updateInform;
if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
updateInform.noalias() = updateA.transpose() * updateA;
} else {
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
if(diagonal) {
typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
updateInform.noalias() = updateA.transpose() * R * R * updateA;
Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas());
updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA;
} else
throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
}
toc(2, "form A^T*A");
if (debug) cout << "updateInform: \n" << updateInform << endl;
toc(2, "form A^T*A");
// Apply updates to the upper triangle
tic(3, "update");
@ -337,74 +345,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
}
}
toc(3, "update");
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateA(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
//
// // Apply updates to the upper triangle
// tic(2, "update");
// assert(this->info_.nBlocks() - 1 == scatter.size());
// for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
// for(size_t j1=0; j1<=j2; ++j1) {
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
// typedef typeof(updateA.block(0,0,0,0)) ABlock;
// ABlock A1(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j1), update.Ab_(j1).rows(),update.Ab_(j1).cols()));
// ABlock A2(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j2), update.Ab_(j2).rows(),update.Ab_(j2).cols()));
// if(slot2 > slot1) {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()) +=
// A1.transpose() * A2;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
// A1.transpose() * R * R * A2;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else if(slot1 > slot2) {
// if(debug)
// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
// A2.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
// A2.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).triangularView<Eigen::Upper>() +=
// A1.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).triangularView<Eigen::Upper>() +=
// A1.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// }
// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
// if(debug) this->print();
// }
// }
// toc(2, "update");
}
/* ************************************************************************* */
@ -413,49 +353,43 @@ void HessianFactor::partialCholesky(size_t nrFrontals) {
}
/* ************************************************************************* */
GaussianBayesNet::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys) {
Eigen::LDLT<Matrix>::TranspositionType HessianFactor::partialLDL(size_t nrFrontals) {
return ldlPartial(matrix_, info_.offset(nrFrontals));
}
static const bool debug = false;
/* ************************************************************************* */
GaussianConditional::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys, const Eigen::LDLT<Matrix>::TranspositionType& permutation) {
// Extract conditionals
tic(1, "extract conditionals");
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
typedef VerticalBlockView<Matrix> BlockAb;
BlockAb Ab(matrix_, info_);
for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the
// eliminated Ab_ matrix to create the GaussianConditional from it.
size_t varDim = Ab(0).cols();
Ab.rowEnd() = Ab.rowStart() + varDim;
static const bool debug = false;
// Zero the entries below the diagonal
{
tic(1, "zero");
BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks()));
zeroBelowDiagonal(remainingMatrix);
toc(1, "zero");
}
// Extract conditionals
tic(1, "extract conditionals");
GaussianConditional::shared_ptr conditionals(new GaussianConditional());
typedef VerticalBlockView<Matrix> BlockAb;
BlockAb Ab(matrix_, info_);
tic(2, "construct cond");
Vector sigmas = Vector::Ones(varDim);
conditionals->push_back(boost::make_shared<ConditionalType>(keys.begin()+j, keys.end(), 1, Ab, sigmas));
toc(2, "construct cond");
if(debug) conditionals->back()->print("Extracted conditional: ");
Ab.rowStart() += varDim;
Ab.firstBlock() += 1;
if(debug) cout << "rowStart = " << Ab.rowStart() << ", rowEnd = " << Ab.rowEnd() << endl;
}
toc(1, "extract conditionals");
size_t varDim = info_.offset(nrFrontals);
Ab.rowEnd() = Ab.rowStart() + varDim;
// Take lower-right block of Ab_ to get the new factor
tic(2, "remaining factor");
info_.blockStart() = nrFrontals;
// Assign the keys
keys_.assign(keys.begin() + nrFrontals, keys.end());
toc(2, "remaining factor");
// Create one big conditionals with many frontal variables.
// Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense.
tic(2, "construct cond");
Vector sigmas = Vector::Ones(varDim);
conditionals = boost::make_shared<ConditionalType>(keys.begin(), keys.end(), nrFrontals, Ab, sigmas, permutation);
toc(2, "construct cond");
if(debug) conditionals->print("Extracted conditional: ");
return conditionals;
toc(1, "extract conditionals");
// Take lower-right block of Ab_ to get the new factor
tic(2, "remaining factor");
info_.blockStart() = nrFrontals;
// Assign the keys
keys_.assign(keys.begin() + nrFrontals, keys.end());
toc(2, "remaining factor");
return conditionals;
}
} // gtsam

16
gtsam/linear/HessianFactor.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
@ -26,6 +25,7 @@
// Forward declarations for friend unit tests
class ConversionConstructorHessianFactorTest;
class Constructor1HessianFactorTest;
class combineHessianFactorTest;
namespace gtsam {
@ -40,7 +40,9 @@ namespace gtsam {
struct SlotEntry {
size_t slot;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension) : slot(_slot), dimension(_dimension) {}
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
typedef FastMap<Index, SlotEntry> Scatter;
@ -131,6 +133,7 @@ namespace gtsam {
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;
friend class ::combineHessianFactorTest;
// Friend JacobianFactor for conversion
friend class JacobianFactor;
@ -144,9 +147,14 @@ namespace gtsam {
*/
void partialCholesky(size_t nrFrontals);
/**
* Do LDL.
*/
Eigen::LDLT<Matrix>::TranspositionType partialLDL(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<BayesNet<GaussianConditional> > splitEliminatedFactor(
size_t nrFrontals, const std::vector<Index>& keys);
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(
size_t nrFrontals, const std::vector<Index>& keys, const Eigen::LDLT<Matrix>::TranspositionType& permutation = Eigen::LDLT<Matrix>::TranspositionType());
/** assert invariants */
void assertInvariants() const;

5
gtsam/linear/IterativeOptimizationParameters.h
View File

@ -21,7 +21,8 @@ public:
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
typedef enum {
SILENT, ERROR,
SILENT = 0,
ERROR,
} verbosityLevel;
public:
@ -37,7 +38,7 @@ public:
public:
IterativeOptimizationParameters() :
maxIterations_(100), reset_(101), epsilon_(1e-5), epsilon_abs_(1e-5),
verbosity_(ERROR), nReduce_(0), skeleton_spec_(), est_cond_(false) {
verbosity_(SILENT), nReduce_(0), skeleton_spec_(), est_cond_(false) {
}
IterativeOptimizationParameters(

26
gtsam/linear/IterativeSolver.h
View File

@ -1,17 +1,23 @@
/*
* IterativeSolver.h
*
* Created on: Oct 24, 2010
* Author: Yong-Dian Jian
*
* Base Class for all iterative solvers of linear systems
/* ----------------------------------------------------------------------------
* 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 IterativeSolver.h
* @date Oct 24, 2010
* @author Yong-Dian Jian
* @brief Base Class for all iterative solvers of linear systems
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeOptimizationParameters.h>
namespace gtsam {

28
gtsam/linear/JacobianFactor.cpp
View File

@ -153,6 +153,8 @@ namespace gtsam {
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_);
// TODO: permutation for all frontal blocks
Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_;
// todo SL: make firstNonzeroCols triangular?
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
assertInvariants();
@ -197,7 +199,6 @@ namespace gtsam {
} else {
for(const_iterator key=begin(); key!=end(); ++key)
cout << boost::format("A[%1%]=\n")%*key << getA(key) << endl;
// gtsam::print(getA(key), (boost::format("A[%1%]=\n")%*key).str());
cout << "b=" << getb() << endl;
model_->print("model");
}
@ -360,11 +361,11 @@ namespace gtsam {
/* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() {
return this->eliminate(1)->front();
return this->eliminate(1);
}
/* ************************************************************************* */
GaussianBayesNet::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) {
GaussianConditional::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals);
@ -433,18 +434,15 @@ namespace gtsam {
// Extract conditionals
tic(3, "cond Rd");
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the
// eliminated Ab matrix to create the GaussianConditional from it.
size_t varDim = Ab_(0).cols();
Ab_.rowEnd() = Ab_.rowStart() + varDim;
const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
conditionals->push_back(boost::make_shared<ConditionalType>(begin()+j, end(), 1, Ab_, sigmas));
if(debug) conditionals->back()->print("Extracted conditional: ");
Ab_.rowStart() += varDim;
Ab_.firstBlock() += 1;
}
GaussianConditional::shared_ptr conditionals(new GaussianConditional());
// Restrict the matrix to be in the first nrFrontals variables
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
conditionals = boost::make_shared<ConditionalType>(begin(), end(), nrFrontals, Ab_, sigmas);
if(debug) conditionals->print("Extracted conditional: ");
Ab_.rowStart() += frontalDim;
Ab_.firstBlock() += nrFrontals;
toc(3, "cond Rd");
if(debug) conditionals->print("Extracted conditionals: ");

8
gtsam/linear/JacobianFactor.h
View File

@ -18,15 +18,12 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/Errors.h>
#include <map>
#include <boost/tuple/tuple.hpp>
// Forward declarations of friend unit tests
@ -72,8 +69,6 @@ namespace gtsam {
/** Construct Null factor */
JacobianFactor(const Vector& b_in);
// FIXME: make these constructors use other matrix types - column major and blocks
/** Construct unary factor */
JacobianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model);
@ -200,7 +195,8 @@ namespace gtsam {
boost::shared_ptr<GaussianConditional> eliminateFirst();
boost::shared_ptr<BayesNet<GaussianConditional> > eliminate(size_t nrFrontals = 1);
/** return a multi-frontal conditional. It's actually a chordal Bayesnet */
boost::shared_ptr<GaussianConditional> eliminate(size_t nrFrontals = 1);
// Friend HessianFactor to facilitate convertion constructors
friend class HessianFactor;

2
gtsam/linear/Makefile.am
View File

@ -16,7 +16,7 @@ sources += NoiseModel.cpp Errors.cpp Sampler.cpp
check_PROGRAMS += tests/testNoiseModel tests/testErrors tests/testSampler
# Vector Configurations
headers += VectorValues.h
sources += VectorValues.cpp
check_PROGRAMS += tests/testVectorValues
# Solvers

7
gtsam/linear/NoiseModel.h
View File

@ -19,10 +19,8 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
@ -103,7 +101,6 @@ namespace gtsam {
protected:
// TODO: store as boost upper-triangular or whatever is passed from above
/* Matrix square root of information matrix (R) */
boost::optional<Matrix> sqrt_information_;
@ -182,9 +179,6 @@ namespace gtsam {
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/
virtual SharedDiagonal QR(Matrix& Ab) const;
// FIXME: these previously had firstZeroRows - what did this do?
// virtual SharedDiagonal QRColumnWise(Matrix& Ab, std::vector<int>& firstZeroRows) const;
// virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
/**
* Cholesky factorization
@ -212,7 +206,6 @@ namespace gtsam {
}; // Gaussian
/**
* A diagonal noise model implements a diagonal covariance matrix, with the
* elements of the diagonal specified in a Vector. This class has no public

2
gtsam/linear/SharedGaussian.h
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */
/*
/**
* SharedGaussian.h
* @brief Class that wraps a shared noise model with diagonal covariance
* @Author: Frank Dellaert

2
gtsam/linear/SubgraphPreconditioner.h
View File

@ -19,7 +19,7 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Ordering.h> // FIXME shouldn't have nonlinear things in linear
namespace gtsam {

20
gtsam/linear/SubgraphSolver.h
View File

@ -12,12 +12,8 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -52,29 +48,33 @@ namespace gtsam {
/* preconditioner */
shared_preconditioner pc_;
/* flag for direct solver - either QR or LDL */
bool useQR_;
public:
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters()):
IterativeSolver(parameters){ initialize(G,theta0); }
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
SubgraphSolver(const LINEAR& GFG) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure) {
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
}
SubgraphSolver(const SubgraphSolver& solver) :
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){}
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
SubgraphSolver(shared_ordering ordering,
mapPairIndex pairs,
shared_preconditioner pc,
sharedParameters parameters = boost::make_shared<Parameters>()) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc) {}
sharedParameters parameters = boost::make_shared<Parameters>(),
bool useQR = true) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() const ;

132
gtsam/linear/VectorValues.cpp Normal file
View File

@ -0,0 +1,132 @@
/* ----------------------------------------------------------------------------
* 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 VectorValues.cpp
* @brief Implementations for VectorValues
* @author Richard Roberts
* @author Alex Cunningham
*/
#include <gtsam/linear/VectorValues.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars+1) {
varStarts_[0] = 0;
size_t varStart = 0;
for(Index j=1; j<=nVars; ++j)
varStarts_[j] = (varStart += varDim);
values_.resize(varStarts_.back());
}
/* ************************************************************************* */
VectorValues::VectorValues(const std::vector<size_t>& dimensions, const Vector& values) :
values_(values), varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
assert(varStarts_.back() == (size_t) values.size());
}
/* ************************************************************************* */
VectorValues::VectorValues(const std::vector<size_t>& dimensions, const double* values) :
varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
values_ = Vector_(varStart, values);
}
/* ************************************************************************* */
VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
VectorValues ret;
ret.varStarts_ = otherValues.varStarts_;
ret.values_.resize(ret.varStarts_.back(), false);
return ret;
}
/* ************************************************************************* */
VectorValues::mapped_type VectorValues::operator[](Index variable) {
checkVariable(variable);
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
}
/* ************************************************************************* */
VectorValues::const_mapped_type VectorValues::operator[](Index variable) const {
checkVariable(variable);
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
}
/* ************************************************************************* */
Index VectorValues::push_back_preallocated(const Vector& vector) {
Index var = varStarts_.size()-1;
varStarts_.push_back(varStarts_.back()+vector.size());
this->operator[](var) = vector; // This will assert that values_ has enough allocated space.
return var;
}
/* ************************************************************************* */
void VectorValues::print(const std::string& str) const {
std::cout << str << ": " << varStarts_.size()-1 << " elements\n";
for(Index var=0; var<size(); ++var) {
std::cout << " " << var << ": \n" << operator[](var) << "\n";
}
std::cout.flush();
}
/* ************************************************************************* */
bool VectorValues::equals(const VectorValues& x, double tol) const {
return varStarts_ == x.varStarts_ && equal_with_abs_tol(values_, x.values_, tol);
}
/* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const {
assert(varStarts_ == c.varStarts_);
VectorValues result;
result.varStarts_ = varStarts_;
result.values_ = values_.head(varStarts_.back()) + c.values_.head(varStarts_.back());
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
assert(varStarts_ == c.varStarts_);
this->values_ += c.values_.head(varStarts_.back());
}
/* ************************************************************************* */
VectorValues VectorValues::zero(const VectorValues& x) {
VectorValues cloned(x);
cloned.makeZero();
return cloned;
}
/* ************************************************************************* */
size_t VectorValues::dim(Index variable) const {
checkVariable(variable);
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return n;
}
/* ************************************************************************* */

129
gtsam/linear/VectorValues.h
View File

@ -24,8 +24,6 @@
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
namespace gtsam {
class VectorValues : public Testable<VectorValues> {
@ -75,6 +73,9 @@ public:
mapped_type operator[](Index variable);
const_mapped_type operator[](Index variable) const;
/** dimension of a particular element */
size_t dim(Index variable) const;
/** Number of elements */
Index size() const { return varStarts_.size()-1; }
@ -104,50 +105,36 @@ public:
/** Reserve space for a total number of variables and dimensionality */
void reserve(Index nVars, size_t totalDims) { values_.resize(totalDims); varStarts_.reserve(nVars+1); }
/** access a range of indices (of no particular order) as a single vector */
template<class ITERATOR>
Vector range(const ITERATOR& idx_begin, const ITERATOR& idx_end) const;
/** set a range of indices as a single vector split across the range */
template<class ITERATOR>
void range(const ITERATOR& idx_begin, const ITERATOR& idx_end, const Vector& v);
/**
* Append a variable using the next variable ID, and return that ID. Space
* must have been allocated ahead of time using reserve(...).
*/
Index push_back_preallocated(const Vector& vector) {
Index var = varStarts_.size()-1;
varStarts_.push_back(varStarts_.back()+vector.size());
this->operator[](var) = vector; // This will assert that values_ has enough allocated space.
return var;
}
Index push_back_preallocated(const Vector& vector);
/** Set all elements to zero */
void makeZero() { values_.setZero(); }
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const {
std::cout << str << ": " << varStarts_.size()-1 << " elements\n";
for(Index var=0; var<size(); ++var) {
std::cout << " " << var << " " << operator[](var) << "\n";
}
std::cout.flush();
}
void print(const std::string& str = "VectorValues: ") const;
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol=1e-9) const {
return varStarts_ == x.varStarts_ && equal_with_abs_tol(values_, x.values_, tol);
}
bool equals(const VectorValues& x, double tol=1e-9) const;
/** + operator simply adds Vectors. This checks for structural equivalence
/**
* + operator simply adds Vectors. This checks for structural equivalence
* when NDEBUG is not defined.
*/
VectorValues operator+(const VectorValues& c) const {
assert(varStarts_ == c.varStarts_);
VectorValues result;
result.varStarts_ = varStarts_;
result.values_ = values_.head(varStarts_.back()) + c.values_.head(varStarts_.back());
return result;
}
void operator+=(const VectorValues& c) {
assert(varStarts_ == c.varStarts_);
this->values_ += c.values_.head(varStarts_.back());
}
VectorValues operator+(const VectorValues& c) const;
void operator+=(const VectorValues& c);
/**
* Iterator (handles both iterator and const_iterator depending on whether
@ -177,11 +164,8 @@ public:
value_type operator*() { return config_[curVariable_]; }
};
static VectorValues zero(const VectorValues& x) {
VectorValues cloned(x);
cloned.makeZero();
return cloned;
}
/** Copy structure of x, but set all values to zero */
static VectorValues zero(const VectorValues& x);
protected:
void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); }
@ -243,53 +227,34 @@ inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dime
values_.resize(varStarts_.back());
}
inline VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars+1) {
varStarts_[0] = 0;
size_t varStart = 0;
for(Index j=1; j<=nVars; ++j)
varStarts_[j] = (varStart += varDim);
values_.resize(varStarts_.back());
}
template<class ITERATOR>
inline Vector VectorValues::range(const ITERATOR& idx_begin, const ITERATOR& idx_end) const {
// find the size of the vector to build
size_t s = 0;
for (ITERATOR it=idx_begin; it!=idx_end; ++it)
s += dim(*it);
inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const Vector& values) :
values_(values), varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
assert(varStarts_.back() == (size_t) values.size());
}
inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const double* values) :
varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
// assign vector
Vector result(s);
size_t start = 0;
for (ITERATOR it=idx_begin; it!=idx_end; ++it) {
ConstSubVector v = (*this)[*it];
const size_t d = v.size();
result.segment(start, d) = v;
start += d;
}
values_ = Vector_(varStart, values);
return result;
}
inline VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
VectorValues ret;
ret.varStarts_ = otherValues.varStarts_;
ret.values_.resize(ret.varStarts_.back(), false);
return ret;
}
inline VectorValues::mapped_type VectorValues::operator[](Index variable) {
checkVariable(variable);
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
}
inline VectorValues::const_mapped_type VectorValues::operator[](Index variable) const {
checkVariable(variable);
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
template<class ITERATOR>
inline void VectorValues::range(const ITERATOR& idx_begin, const ITERATOR& idx_end, const Vector& v) {
size_t start = 0;
for (ITERATOR it=idx_begin; it!=idx_end; ++it) {
checkVariable(*it);
const size_t d = dim(*it);
(*this)[*it] = v.segment(start, d);
start += d;
}
}
struct DimSpec: public std::vector<size_t> {
@ -306,8 +271,6 @@ struct DimSpec: public std::vector<size_t> {
(*this)[i] = V[i].size() ;
}
}
} ;
};
}
} // \namespace gtsam

6
gtsam/linear/iterative-inl.h
View File

@ -94,7 +94,7 @@ namespace gtsam {
// check for convergence
double new_gamma = dot(g, g);
if (parameters_.verbosity())
if (parameters_.verbosity() != IterativeOptimizationParameters::SILENT)
cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << new_gamma << endl;
if (new_gamma < threshold) return true;
@ -128,14 +128,14 @@ namespace gtsam {
bool steepest = false) {
CGState<S, V, E> state(Ab, x, parameters, steepest);
if (parameters.verbosity())
if (parameters.verbosity() != IterativeOptimizationParameters::SILENT)
cout << "CG: epsilon = " << parameters.epsilon()
<< ", maxIterations = " << parameters.maxIterations()
<< ", ||g0||^2 = " << state.gamma
<< ", threshold = " << state.threshold << endl;
if ( state.gamma < state.threshold ) {
if (parameters.verbosity())
if (parameters.verbosity() != IterativeOptimizationParameters::SILENT)
cout << "||g0||^2 < threshold, exiting immediately !" << endl;
return x;

151
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -20,16 +20,19 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
using namespace gtsam;
using namespace std;
using namespace boost::assign;
static const double tol = 1e-5;
static const Index _x_=0, _x1_=1, _l1_=2;
/* ************************************************************************* */
@ -112,17 +115,16 @@ TEST( GaussianConditional, equals )
actual(_x_,d, R, _x1_, A1, _l1_, A2, tau);
EXPECT( expected.equals(actual) );
}
/* ************************************************************************* */
TEST( GaussianConditional, solve )
{
//expected solution
Vector expected(2);
expected(0) = 20-3-11 ; expected(1) = 40-7-15;
Vector expectedX(2);
expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
// create a conditional gaussion node
// create a conditional Gaussian node
Matrix R = Matrix_(2,2, 1., 0.,
0., 1.);
@ -137,7 +139,8 @@ TEST( GaussianConditional, solve )
Vector tau = ones(2);
GaussianConditional cg(_x_,d, R, _x1_, A1, _l1_, A2, tau);
// RHS is different than the one in the solution vector
GaussianConditional cg(_x_,ones(2), R, _x1_, A1, _l1_, A2, tau);
Vector sx1(2);
sx1(0) = 1.0; sx1(1) = 1.0;
@ -146,13 +149,145 @@ TEST( GaussianConditional, solve )
sl1(0) = 1.0; sl1(1) = 1.0;
VectorValues solution(vector<size_t>(3, 2));
solution[_x1_] = sx1;
solution[_x_] = d; // RHS
solution[_x1_] = sx1; // parents
solution[_l1_] = sl1;
Vector result = cg.solve(solution);
// NOTE: the solve functions assume the RHS is passed as the initialization of
// the solution.
VectorValues expected(vector<size_t>(3, 2));
expected[_x_] = expectedX;
expected[_x1_] = sx1;
expected[_l1_] = sl1;
EXPECT(assert_equal(expected , result, 0.0001));
VectorValues copy_result = cg.solve(solution);
cg.solveInPlace(solution);
EXPECT(assert_equal(expected, copy_result, 0.0001));
EXPECT(assert_equal(expected, solution, 0.0001));
}
/* ************************************************************************* */
TEST( GaussianConditional, solve_simple )
{
// no pivoting from LDL, so R matrix is not permuted
// RHS is deliberately not the same as below
Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.0,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0);
// solve system as a non-multifrontal version first
// 2 variables, frontal has dim=4
vector<size_t> dims; dims += 4, 2, 1;
GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
Vector sigmas = ones(4);
vector<size_t> cgdims; cgdims += _x_, _x1_;
GaussianConditional cg(cgdims.begin(), cgdims.end(), 1, matrices, sigmas);
// partial solution
Vector sx1 = Vector_(2, 9.0, 10.0);
// elimination order; _x_, _x1_
vector<size_t> vdim; vdim += 4, 2;
VectorValues actual(vdim);
actual[_x_] = Vector_(4, 0.1, 0.2, 0.3, 0.4); // d
actual[_x1_] = sx1; // parent
VectorValues expected(vdim);
expected[_x1_] = sx1;
expected[_x_] = Vector_(4, -3.1,-3.4,-11.9,-13.2);
// verify indices/size
EXPECT_LONGS_EQUAL(2, cg.size());
EXPECT_LONGS_EQUAL(4, cg.dim());
// solve and verify
cg.solveInPlace(actual);
EXPECT(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */
TEST( GaussianConditional, solve_multifrontal )
{
// create full system, 3 variables, 2 frontals, all 2 dim
// no pivoting from LDL, so R matrix is not permuted
Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
// 3 variables, all dim=2
vector<size_t> dims; dims += 2, 2, 2, 1;
GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
Vector sigmas = ones(4);
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
// partial solution
Vector sl1 = Vector_(2, 9.0, 10.0);
// elimination order; _x_, _x1_, _l1_
VectorValues actual(vector<size_t>(3, 2));
actual[_x_] = Vector_(2, 0.1, 0.2); // rhs
actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
actual[_l1_] = sl1; // parent
VectorValues expected(vector<size_t>(3, 2));
expected[_x_] = Vector_(2, -3.1,-3.4);
expected[_x1_] = Vector_(2, -11.9,-13.2);
expected[_l1_] = sl1;
// verify indices/size
EXPECT_LONGS_EQUAL(3, cg.size());
EXPECT_LONGS_EQUAL(4, cg.dim());
// solve and verify
cg.solveInPlace(actual);
EXPECT(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */
TEST( GaussianConditional, solveTranspose ) {
static const Index _y_=1;
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
GaussianBayesNet cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
// x=R'*y, y=inv(R')*x
// 2 = 1 2
// 5 1 1 3
VectorValues y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
x[_x_] = Vector_(1,2.);
x[_y_] = Vector_(1,5.);
y[_x_] = Vector_(1,2.);
y[_y_] = Vector_(1,3.);
// test functional version
VectorValues actual = backSubstituteTranspose(cbn,x);
CHECK(assert_equal(y,actual));
}
/* ************************************************************************* */

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

@ -180,16 +180,16 @@ TEST_UNSAFE(GaussianFactor, CombineAndEliminate)
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianBayesNet expectedBN(*expectedFactor.eliminate(1));
GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
GaussianBayesNet::shared_ptr actualBN;
GaussianConditional::shared_ptr actualBN;
GaussianFactor::shared_ptr actualFactor;
boost::tie(actualBN, actualFactor) = //
EliminateQR(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> > (), 1);
JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
JacobianFactor>(actualFactor);
EXPECT(assert_equal(expectedBN, *actualBN));
EXPECT(assert_equal(*expectedBN, *actualBN));
EXPECT(assert_equal(expectedFactor, *actualJacobian));
}
@ -434,10 +434,17 @@ TEST(GaussianFactor, eliminateFrontals)
GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2)));
// Create expected Bayes net fragment from three conditionals above
GaussianBayesNet expectedFragment;
expectedFragment.push_back(cond1);
expectedFragment.push_back(cond2);
expectedFragment.push_back(cond3);
// GaussianBayesNet expectedFragment;
// expectedFragment.push_back(cond1);
// expectedFragment.push_back(cond2);
// expectedFragment.push_back(cond3);
Index ikeys[] = {3,5,7,9,11};
std::vector<Index> keys(ikeys, ikeys + sizeof(ikeys)/sizeof(Index));
size_t dims[] = { 2,2,2,2,2,1 };
size_t height = 11;
VerticalBlockView<Matrix> Rblock(R, dims, dims+6, height);
GaussianConditional::shared_ptr expectedFragment( new GaussianConditional(keys.begin(), keys.end(), 3,
Rblock, ones(6)) );
// Get expected matrices for remaining factor
Matrix Ae1 = sub(R, 6,10, 6,8);
@ -445,9 +452,9 @@ TEST(GaussianFactor, eliminateFrontals)
Vector be = R.col(10).segment(6, 4);
// Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3);
GaussianConditional::shared_ptr actualFragment_QR = actualFactor_QR.eliminate(3);
EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001));
EXPECT(assert_equal(*expectedFragment, *actualFragment_QR, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1]));

96
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -35,20 +35,20 @@ static const Index x2=0, x1=1, x3=2, x4=3;
GaussianFactorGraph createChain() {
typedef GaussianFactorGraph::sharedFactor Factor;
SharedDiagonal model(Vector_(1, 0.5));
Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor4(new JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), model));
typedef GaussianFactorGraph::sharedFactor Factor;
SharedDiagonal model(Vector_(1, 0.5));
Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor4(new JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), model));
GaussianFactorGraph fg;
fg.push_back(factor1);
fg.push_back(factor2);
fg.push_back(factor3);
fg.push_back(factor4);
GaussianFactorGraph fg;
fg.push_back(factor1);
fg.push_back(factor2);
fg.push_back(factor3);
fg.push_back(factor4);
return fg;
return fg;
}
/* ************************************************************************* */
@ -65,43 +65,55 @@ GaussianFactorGraph createChain() {
*
* 1 0 0 1
*/
TEST( GaussianJunctionTree, eliminate )
TEST_UNSAFE( GaussianJunctionTree, eliminate )
{
GaussianFactorGraph fg = createChain();
GaussianJunctionTree junctionTree(fg);
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate(&EliminateQR);
GaussianFactorGraph fg = createChain();
GaussianJunctionTree junctionTree(fg);
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate(&EliminateQR);
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
Matrix two = Matrix_(1,1,2.);
Matrix one = Matrix_(1,1,1.);
BayesTree<GaussianConditional> bayesTree_expected;
bayesTree_expected.insert(sharedConditional(new GaussianConditional(x4, Vector_(1,2.), two, Vector_(1,1.))));
bayesTree_expected.insert(sharedConditional(new GaussianConditional(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.))));
bayesTree_expected.insert(sharedConditional(new GaussianConditional(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.))));
bayesTree_expected.insert(sharedConditional(new GaussianConditional(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.))));
CHECK(assert_equal(*bayesTree_expected.root(), *rootClique));
CHECK(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front())));
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
Matrix two = Matrix_(1,1,2.);
Matrix one = Matrix_(1,1,1.);
BayesTree<GaussianConditional> bayesTree_expected;
Index keys_root[] = {x3,x4};
Matrix rsd_root = Matrix_(2,3, 2., 2., 2., 0., 2., 2.);
size_t dim_root[] = {1, 1, 1};
sharedConditional root_expected(new GaussianConditional(keys_root, keys_root+2, 2,
VerticalBlockView<Matrix>(rsd_root, dim_root, dim_root+3, 2), ones(2)));
BayesTree<GaussianConditional>::sharedClique rootClique_expected(new BayesTree<GaussianConditional>::Clique(root_expected));
Index keys_child[] = {x2,x1,x3};
Matrix rsd_child = Matrix_(2,4, 2., 1., 1., 2., 0., -1., 1., 0.);
size_t dim_child[] = {1, 1, 1, 1};
sharedConditional child_expected(new GaussianConditional(keys_child, keys_child+3, 2,
VerticalBlockView<Matrix>(rsd_child, dim_child, dim_child+4, 2), ones(2)));
BayesTree<GaussianConditional>::sharedClique childClique_expected(new BayesTree<GaussianConditional>::Clique(child_expected));
bayesTree_expected.insert(rootClique_expected);
bayesTree_expected.insert(childClique_expected);
// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x4, Vector_(1,2.), two, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.))));
CHECK(assert_equal(*bayesTree_expected.root(), *rootClique));
EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front())));
}
/* ************************************************************************* */
TEST( GaussianJunctionTree, optimizeMultiFrontal )
TEST_UNSAFE( GaussianJunctionTree, optimizeMultiFrontal )
{
GaussianFactorGraph fg = createChain();
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize(&EliminateQR);
VectorValues expected(vector<size_t>(4,1));
expected[x1] = Vector_(1, 0.);
expected[x2] = Vector_(1, 1.);
expected[x3] = Vector_(1, 0.);
expected[x4] = Vector_(1, 1.);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(GaussianJunctionTree, complexExample) {
GaussianFactorGraph fg = createChain();
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize(&EliminateQR);
VectorValues expected(vector<size_t>(4,1));
expected[x1] = Vector_(1, 0.);
expected[x2] = Vector_(1, 1.);
expected[x3] = Vector_(1, 0.);
expected[x4] = Vector_(1, 1.);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */

82
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -16,19 +16,25 @@
* @created Dec 15, 2010
*/
#include <vector>
#include <utility>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <vector>
#include <utility>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace std;
using namespace boost::assign;
using namespace gtsam;
const double tol = 1e-5;
/* ************************************************************************* */
TEST(HessianFactor, ConversionConstructor) {
@ -158,7 +164,7 @@ TEST(HessianFactor, Constructor2)
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate)
TEST_UNSAFE(HessianFactor, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
@ -200,17 +206,17 @@ TEST(HessianFactor, CombineAndEliminate)
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// perform elimination
GaussianBayesNet expectedBN(*expectedFactor.eliminate(1));
GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
// create expected Hessian after elimination
HessianFactor expectedCholeskyFactor(expectedFactor);
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(
GaussianFactorGraph::EliminationResult actualCholesky = EliminateLDL(
*gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actualCholesky.second);
EXPECT(assert_equal(expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
}
@ -256,7 +262,7 @@ TEST(HessianFactor, eliminate2 )
FactorGraph<HessianFactor> combinedLFG_Chol;
combinedLFG_Chol.push_back(combinedLF_Chol);
GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
GaussianFactorGraph::EliminationResult actual_Chol = EliminateLDL(
combinedLFG_Chol, 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actual_Chol.second);
@ -273,7 +279,7 @@ TEST(HessianFactor, eliminate2 )
)/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(0,d,R11,1,S12,ones(2));
EXPECT(assert_equal(expectedCG,*actual_Chol.first->front(),1e-4));
EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4));
// the expected linear factor
double sigma = 0.2236;
@ -355,20 +361,72 @@ TEST(HessianFactor, eliminateUnsorted) {
// unsortedGraph.push_back(factor1);
unsortedGraph.push_back(unsorted_factor2);
GaussianBayesNet::shared_ptr expected_bn;
GaussianConditional::shared_ptr expected_bn;
GaussianFactor::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) =
EliminatePreferCholesky(sortedGraph, 1);
GaussianBayesNet::shared_ptr actual_bn;
GaussianConditional::shared_ptr actual_bn;
GaussianFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) =
EliminatePreferCholesky(unsortedGraph, 1);
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
// Test LDL
boost::tie(expected_bn, expected_factor) =
EliminatePreferLDL(sortedGraph, 1);
boost::tie(actual_bn, actual_factor) =
EliminatePreferLDL(unsortedGraph, 1);
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
}
/* ************************************************************************* */
TEST(HessianFactor, combine) {
// update the information matrix with a single jacobian factor
Matrix A0 = Matrix_(2, 2,
11.1803399, 0.0,
0.0, 11.1803399);
Matrix A1 = Matrix_(2, 2,
-2.23606798, 0.0,
0.0, -2.23606798);
Matrix A2 = Matrix_(2, 2,
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = Vector_(2, 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
FactorGraph<GaussianFactor> factors;
factors.push_back(f);
vector<size_t> dimensions;
dimensions += 2, 2, 2, 1;
Scatter scatter;
scatter += make_pair(0, SlotEntry(0, 2));
scatter += make_pair(1, SlotEntry(1, 2));
scatter += make_pair(2, SlotEntry(2, 2));
// Form Ab' * Ab
HessianFactor actual(factors, dimensions, scatter);
Matrix expected = Matrix_(7, 7,
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
-100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500);
EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

4
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -230,13 +230,13 @@ TEST(NoiseModel, Cholesky)
{
SharedDiagonal expected = noiseModel::Unit::Create(4);
Matrix Ab = exampleQR::Ab; // otherwise overwritten !
SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); // ASSERTION FAILURE: access out of bounds
SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4);
EXPECT(assert_equal(*expected,*actual));
// Ab was modified in place !!!
Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
// ublas::project(ublas::triangular_adaptor<Matrix, ublas::upper>(Ab),
// ublas::range(0,actual->dim()), ublas::range(0, Ab.cols()));
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4));
}
/* ************************************************************************* */

55
gtsam/linear/tests/testVectorValues.cpp
View File

@ -16,15 +16,16 @@
* @created Sep 16, 2010
*/
#include <vector>
#include <boost/assign/std/vector.hpp>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Permutation.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace std;
using namespace boost::assign;
using namespace gtsam;
/* ************************************************************************* */
TEST(VectorValues, constructor) {
@ -52,6 +53,9 @@ TEST(VectorValues, standard) {
EXPECT_LONGS_EQUAL(3, combined.size());
EXPECT_LONGS_EQUAL(9, combined.dim());
EXPECT_LONGS_EQUAL(9, combined.dimCapacity());
EXPECT_LONGS_EQUAL(3, combined.dim(0));
EXPECT_LONGS_EQUAL(2, combined.dim(1));
EXPECT_LONGS_EQUAL(4, combined.dim(2));
combined[0] = v1;
combined[1] = v2;
combined[2] = v3;
@ -179,6 +183,53 @@ TEST(VectorValues, makeZero ) {
EXPECT(assert_equal(zero(14), values.vector()));
}
/* ************************************************************************* */
TEST(VectorValues, range ) {
VectorValues v(7,2);
v.makeZero();
v[1] = Vector_(2, 1.0, 2.0);
v[2] = Vector_(2, 3.0, 4.0);
v[3] = Vector_(2, 5.0, 6.0);
vector<size_t> idx1, idx2;
idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
idx2 += 1, 0, 2; // unordered
// test access
Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
Vector actRange3 = v.range(idx2.begin(), idx2.end());
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
// test setting values
VectorValues act1 = v, act2 = v, act3 = v;
Vector a = Vector_(2, 0.1, 0.2);
VectorValues exp1 = act1; exp1[0] = a;
act1.range(idx1.begin(), idx1.begin()+1, a);
EXPECT(assert_equal(exp1, act1));
Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
VectorValues exp2 = act2;
exp2[2] = Vector_(2, 0.1, 0.2);
exp2[3] = Vector_(2, 0.3, 0.4);
act2.range(idx1.begin()+2, idx1.begin()+4, bc);
EXPECT(assert_equal(exp2, act2));
Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
VectorValues exp3 = act3;
exp3[1] = Vector_(2, 0.1, 0.2);
exp3[0] = Vector_(2, 0.3, 0.4);
exp3[2] = Vector_(2, 0.5, 0.6);
act3.range(idx2.begin(), idx2.end(), def);
EXPECT(assert_equal(exp3, act3));
}
/* ************************************************************************* */
int main() {
TestResult tr; return TestRegistry::runAllTests(tr);

1
gtsam/nonlinear/Key.h
View File

@ -19,7 +19,6 @@
#pragma once
#include <vector>
#include <list>
#include <iostream>
#include <boost/mpl/char.hpp>

12
gtsam/nonlinear/LieValues-inl.h
View File

@ -16,12 +16,13 @@
#pragma once
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <utility>
#include <iostream>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h>
@ -29,12 +30,7 @@
#include <gtsam/nonlinear/LieValues.h>
#define INSTANTIATE_LIE_VALUES(J) \
/*INSTANTIATE_LIE(T);*/ \
/*template LieValues<J> expmap(const LieValues<J>&, const VectorValues&);*/ \
/*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \
/*template VectorValues logmap(const LieValues<J>&, const LieValues<J>&);*/ \
template class LieValues<J>;
#define INSTANTIATE_LIE_VALUES(J) template class LieValues<J>;
using namespace std;

1
gtsam/nonlinear/LieValues.h
View File

@ -24,7 +24,6 @@
#pragma once
#include <map>
#include <set>
#include <gtsam/base/FastMap.h>

Some files were not shown because too many files have changed in this diff Show More