merged with develop
Luca
commit
975ee1caa5
20
gtsam.h
20
gtsam.h
|
|
@ -156,12 +156,6 @@ virtual class Value {
|
|||
size_t dim() const;
|
||||
};
|
||||
|
||||
class Vector3 {
|
||||
Vector3(Vector v);
|
||||
};
|
||||
class Vector6 {
|
||||
Vector6(Vector v);
|
||||
};
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
class LieScalar {
|
||||
// Standard constructors
|
||||
|
|
@ -1723,6 +1717,7 @@ class Values {
|
|||
// void insert(size_t j, const gtsam::Value& value);
|
||||
// void update(size_t j, const gtsam::Value& val);
|
||||
// gtsam::Value at(size_t j) const;
|
||||
|
||||
void insert(size_t j, const gtsam::Point2& t);
|
||||
void insert(size_t j, const gtsam::Point3& t);
|
||||
void insert(size_t j, const gtsam::Rot2& t);
|
||||
|
|
@ -1737,6 +1732,9 @@ class Values {
|
|||
void insert(size_t j, Vector t);
|
||||
void insert(size_t j, Matrix t);
|
||||
|
||||
// Fixed size version
|
||||
void insertFixed(size_t j, Vector t, size_t n);
|
||||
|
||||
void update(size_t j, const gtsam::Point2& t);
|
||||
void update(size_t j, const gtsam::Point3& t);
|
||||
void update(size_t j, const gtsam::Rot2& t);
|
||||
|
|
@ -2394,7 +2392,7 @@ class ConstantBias {
|
|||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
class PoseVelocityBias{
|
||||
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
|
||||
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
|
||||
};
|
||||
class ImuFactorPreintegratedMeasurements {
|
||||
// Standard Constructor
|
||||
|
|
@ -2421,8 +2419,8 @@ class ImuFactorPreintegratedMeasurements {
|
|||
// Standard Interface
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
|
||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
|
||||
const gtsam::Vector3& gravity, const gtsam::Vector3& omegaCoriolis) const;
|
||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
|
||||
Vector gravity, Vector omegaCoriolis) const;
|
||||
};
|
||||
|
||||
virtual class ImuFactor : gtsam::NonlinearFactor {
|
||||
|
|
@ -2476,8 +2474,8 @@ class CombinedImuFactorPreintegratedMeasurements {
|
|||
// Standard Interface
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
|
||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
|
||||
const gtsam::Vector3& gravity, const gtsam::Vector3& omegaCoriolis) const;
|
||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
|
||||
Vector gravity, Vector omegaCoriolis) const;
|
||||
};
|
||||
|
||||
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
* ConjugateGradientSolver.cpp
|
||||
*
|
||||
* Created on: Jun 4, 2014
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/ConjugateGradientSolver.h>
|
||||
|
|
|
|||
|
|
@ -9,6 +9,14 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConjugateGradientSolver.h
|
||||
* @brief Implementation of Conjugate Gradient solver for a linear system
|
||||
* @author Yong-Dian Jian
|
||||
* @author Sungtae An
|
||||
* @date Nov 6, 2014
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
|
|
@ -82,9 +90,13 @@ public:
|
|||
/*********************************************************************************************/
|
||||
/*
|
||||
* A template of linear preconditioned conjugate gradient method.
|
||||
* System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v,
|
||||
* rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
|
||||
* System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
|
||||
* leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
|
||||
* Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
|
||||
*
|
||||
** REFERENCES:
|
||||
* [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
|
||||
* 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
|
||||
*/
|
||||
template <class S, class V>
|
||||
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) {
|
||||
|
|
@ -93,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
|
|||
estimate = residual = direction = q1 = q2 = initial;
|
||||
|
||||
system.residual(estimate, q1); /* q1 = b-Ax */
|
||||
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
|
||||
system.rightPrecondition(residual, direction);/* d = S^{-1} r */
|
||||
system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
|
||||
system.rightPrecondition(residual, direction);/* p = L^{-T} r */
|
||||
|
||||
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
|
||||
|
||||
|
|
@ -116,21 +128,21 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
|
|||
|
||||
if ( k % iReset == 0 ) {
|
||||
system.residual(estimate, q1); /* q1 = b-Ax */
|
||||
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
|
||||
system.rightPrecondition(residual, direction); /* d = S^{-1} r */
|
||||
system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
|
||||
system.rightPrecondition(residual, direction); /* p = L^{-T} r */
|
||||
currentGamma = system.dot(residual, residual);
|
||||
}
|
||||
system.multiply(direction, q1); /* q1 = A d */
|
||||
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */
|
||||
system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */
|
||||
system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */
|
||||
system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */
|
||||
system.multiply(direction, q1); /* q1 = A p */
|
||||
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
|
||||
system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
|
||||
system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
|
||||
system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
|
||||
prevGamma = currentGamma;
|
||||
currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */
|
||||
currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
|
||||
beta = currentGamma / prevGamma;
|
||||
system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */
|
||||
system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
|
||||
system.scal(beta, direction);
|
||||
system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */
|
||||
system.axpy(1.0, q1, direction); /* p = q1 + beta * p */
|
||||
|
||||
if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
|
||||
std::cout << "[PCG] k = " << k
|
||||
|
|
|
|||
|
|
@ -2,20 +2,14 @@
|
|||
* PCGSolver.cpp
|
||||
*
|
||||
* Created on: Feb 14, 2012
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
* Author: Sungtae An
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
//#include <gtsam/inference/FactorGraph-inst.h>
|
||||
//#include <gtsam/linear/FactorGraphUtil-inl.h>
|
||||
//#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
//#include <gtsam/linear/LSPCGSolver.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
//#include <gtsam/linear/SuiteSparseUtil.h>
|
||||
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
|
||||
//#include <gsp2/gtsam-interface-sbm.h>
|
||||
//#include <ydjian/tool/ThreadSafeTimer.h>
|
||||
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
|
|
@ -33,6 +27,7 @@ void PCGSolverParameters::print(ostream &os) const {
|
|||
|
||||
/*****************************************************************************/
|
||||
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
|
||||
parameters_ = p;
|
||||
preconditioner_ = createPreconditioner(p.preconditioner_);
|
||||
}
|
||||
|
||||
|
|
@ -76,97 +71,47 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
|
|||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
|
||||
/* implement Ax, assume x and Ax are pre-allocated */
|
||||
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
|
||||
/* implement A^T*(A*x), assume x and AtAx are pre-allocated */
|
||||
|
||||
/* reset y */
|
||||
Ax.setZero();
|
||||
// Build a VectorValues for Vector x
|
||||
VectorValues vvX = buildVectorValues(x,keyInfo_);
|
||||
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
/* accumulate At A x*/
|
||||
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
|
||||
const Matrix Ai = jf->getA(it);
|
||||
/* this map lookup should be replaced */
|
||||
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
|
||||
Ax.segment(entry.colstart(), entry.dim())
|
||||
+= Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
|
||||
}
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
/* accumulate H x */
|
||||
// VectorValues form of A'Ax for multiplyHessianAdd
|
||||
VectorValues vvAtAx;
|
||||
|
||||
/* use buffer to avoid excessive table lookups */
|
||||
const size_t sz = hf->size();
|
||||
vector<Vector> y;
|
||||
y.reserve(sz);
|
||||
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
|
||||
/* initialize y to zeros */
|
||||
y.push_back(zero(hf->getDim(it)));
|
||||
}
|
||||
// vvAtAx += 1.0 * A'Ax for each factor
|
||||
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
|
||||
|
||||
for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
|
||||
/* retrieve the key mapping */
|
||||
const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
|
||||
// xj is the input vector
|
||||
const Vector xj = x.segment(entry.colstart(), entry.dim());
|
||||
size_t idx = 0;
|
||||
for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
|
||||
if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
|
||||
else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
|
||||
}
|
||||
}
|
||||
// Make the result as Vector form
|
||||
AtAx = vvAtAx.vector();
|
||||
|
||||
/* accumulate to r */
|
||||
for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
|
||||
/* retrieve the key mapping */
|
||||
const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
|
||||
Ax.segment(entry.colstart(), entry.dim()) += y[i];
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void GaussianFactorGraphSystem::getb(Vector &b) const {
|
||||
/* compute rhs, assume b pre-allocated */
|
||||
|
||||
/* reset */
|
||||
b.setZero();
|
||||
// Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues
|
||||
VectorValues vvb = gfg_.gradientAtZero();
|
||||
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
const Vector rhs = jf->getb();
|
||||
/* accumulate At rhs */
|
||||
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
|
||||
/* this map lookup should be replaced */
|
||||
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
|
||||
b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
|
||||
}
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
/* accumulate g */
|
||||
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
|
||||
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
|
||||
b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
|
||||
}
|
||||
}
|
||||
// Make the result as Vector form
|
||||
b = -vvb.vector();
|
||||
}
|
||||
|
||||
/**********************************************************************************/
|
||||
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
|
||||
{ preconditioner_.transposeSolve(x, y); }
|
||||
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const {
|
||||
// For a preconditioner M = L*L^T
|
||||
// Calculate y = L^{-1} x
|
||||
preconditioner_.solve(x, y);
|
||||
}
|
||||
|
||||
/**********************************************************************************/
|
||||
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
|
||||
{ preconditioner_.solve(x, y); }
|
||||
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const {
|
||||
// For a preconditioner M = L*L^T
|
||||
// Calculate y = L^{-T} x
|
||||
preconditioner_.transposeSolve(x, y);
|
||||
}
|
||||
|
||||
/**********************************************************************************/
|
||||
VectorValues buildVectorValues(const Vector &v,
|
||||
|
|
|
|||
|
|
@ -2,7 +2,8 @@
|
|||
* Preconditioner.cpp
|
||||
*
|
||||
* Created on: Jun 2, 2014
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
* Author: Sungtae An
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
|
@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
|
|||
|
||||
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
|
||||
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
|
||||
R.triangularView<Eigen::Upper>().solveInPlace(b);
|
||||
R.triangularView<Eigen::Lower>().solveInPlace(b);
|
||||
|
||||
dst += d;
|
||||
ptr += sz;
|
||||
|
|
@ -141,11 +142,9 @@ void BlockJacobiPreconditioner::build(
|
|||
}
|
||||
|
||||
/* getting the block diagonals over the factors */
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
|
||||
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
|
||||
blocks.push_back(hessian);
|
||||
}
|
||||
std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
|
||||
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
|
||||
blocks.push_back(hessian);
|
||||
|
||||
/* if necessary, allocating the memory for cacheing the factorization results */
|
||||
if ( nnz > bufferSize_ ) {
|
||||
|
|
@ -159,11 +158,12 @@ void BlockJacobiPreconditioner::build(
|
|||
double *ptr = buffer_;
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
/* use eigen to decompose Di */
|
||||
const Matrix R = blocks[i].llt().matrixL().transpose();
|
||||
/* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */
|
||||
const Matrix L = blocks[i].llt().matrixL();
|
||||
|
||||
/* store the data in the buffer */
|
||||
size_t sz = dims_[i]*dims_[i] ;
|
||||
std::copy(R.data(), R.data() + sz, ptr);
|
||||
std::copy(L.data(), L.data() + sz, ptr);
|
||||
|
||||
/* advance the pointer */
|
||||
ptr += sz;
|
||||
|
|
|
|||
|
|
@ -2,7 +2,8 @@
|
|||
* Preconditioner.h
|
||||
*
|
||||
* Created on: Jun 2, 2014
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
* Author: Sungtae An
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters {
|
|||
};
|
||||
|
||||
/* PCG aims to solve the problem: A x = b by reparametrizing it as
|
||||
* S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M
|
||||
* L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b,
|
||||
* where A \approx L L^{T}, or A \approx M
|
||||
* The goal of this class is to provide a general interface to all preconditioners */
|
||||
class GTSAM_EXPORT Preconditioner {
|
||||
public:
|
||||
|
|
@ -70,15 +72,15 @@ public:
|
|||
|
||||
/* Computation Interfaces */
|
||||
|
||||
/* implement x = S^{-1} y */
|
||||
/* implement x = L^{-1} y */
|
||||
virtual void solve(const Vector& y, Vector &x) const = 0;
|
||||
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
/* implement x = S^{-T} y */
|
||||
/* implement x = L^{-T} y */
|
||||
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
|
||||
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
// /* implement x = S^{-1} S^{-T} y */
|
||||
// /* implement x = L^{-1} L^{-T} y */
|
||||
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
|
||||
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -130,6 +130,24 @@ namespace gtsam {
|
|||
throw ValuesKeyAlreadyExists(j);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Values::insertFixed(Key j, const Vector& v, size_t n) {
|
||||
switch (n) {
|
||||
case 1: insert<Vector1>(j,v); break;
|
||||
case 2: insert<Vector2>(j,v); break;
|
||||
case 3: insert<Vector3>(j,v); break;
|
||||
case 4: insert<Vector4>(j,v); break;
|
||||
case 5: insert<Vector5>(j,v); break;
|
||||
case 6: insert<Vector6>(j,v); break;
|
||||
case 7: insert<Vector7>(j,v); break;
|
||||
case 8: insert<Vector8>(j,v); break;
|
||||
case 9: insert<Vector9>(j,v); break;
|
||||
default:
|
||||
throw runtime_error(
|
||||
"Values::insert fixed size can only handle n in 1..9");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Values::insert(const Values& values) {
|
||||
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
|
||||
|
|
|
|||
|
|
@ -258,6 +258,10 @@ namespace gtsam {
|
|||
template <typename ValueType>
|
||||
void insert(Key j, const ValueType& val);
|
||||
|
||||
/// Special version for small fixed size vectors, for matlab/python
|
||||
/// throws truntime error if n<1 || n>9
|
||||
void insertFixed(Key j, const Vector& v, size_t n);
|
||||
|
||||
/// overloaded insert version that also specifies a chart
|
||||
template <typename ValueType, typename Chart>
|
||||
void insert(Key j, const ValueType& val);
|
||||
|
|
|
|||
|
|
@ -470,6 +470,15 @@ TEST(Values, Destructors) {
|
|||
LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Values, FixedSize) {
|
||||
Values values;
|
||||
Vector v(3); v << 5.0, 6.0, 7.0;
|
||||
values.insertFixed(key1, v, 3);
|
||||
Vector3 expected(5.0, 6.0, 7.0);
|
||||
CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
|
||||
CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@ namespace gtsam {
|
|||
template<class T>
|
||||
class ExpressionFactor: public NoiseModelFactor {
|
||||
|
||||
protected:
|
||||
|
||||
T measurement_; ///< the measurement to be compared with the expression
|
||||
Expression<T> expression_; ///< the expression that is AD enabled
|
||||
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
|
||||
|
|
|
|||
|
|
@ -28,86 +28,98 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
|
||||
GaussianFactorGraph fg;
|
||||
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
||||
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
|
||||
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
|
||||
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
|
||||
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2);
|
||||
// measurement between x1 and l1: l1-x1=[0.0;0.2]
|
||||
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2);
|
||||
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
|
||||
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2);
|
||||
return fg;
|
||||
TEST( PCGsolver, verySimpleLinearSystem) {
|
||||
|
||||
// Ax = [4 1][u] = [1] x0 = [2]
|
||||
// [1 3][v] [2] [1]
|
||||
//
|
||||
// exact solution x = [1/11, 7/11]';
|
||||
//
|
||||
|
||||
// Create a Gaussian Factor Graph
|
||||
GaussianFactorGraph simpleGFG;
|
||||
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2));
|
||||
|
||||
// Exact solution already known
|
||||
VectorValues exactSolution;
|
||||
exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished());
|
||||
//exactSolution.print("Exact");
|
||||
|
||||
// Solve the system using direct method
|
||||
VectorValues deltaDirect = simpleGFG.optimize();
|
||||
EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7));
|
||||
//deltaDirect.print("Direct");
|
||||
|
||||
// Solve the system using Preconditioned Conjugate Gradient solver
|
||||
// Common PCG parameters
|
||||
gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared<gtsam::PCGSolverParameters>();
|
||||
pcg->setMaxIterations(500);
|
||||
pcg->setEpsilon_abs(0.0);
|
||||
pcg->setEpsilon_rel(0.0);
|
||||
//pcg->setVerbosity("ERROR");
|
||||
|
||||
// With Dummy preconditioner
|
||||
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
|
||||
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7));
|
||||
//deltaPCGDummy.print("PCG Dummy");
|
||||
|
||||
// With Block-Jacobi preconditioner
|
||||
pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
|
||||
// It takes more than 1000 iterations for this test
|
||||
pcg->setMaxIterations(1500);
|
||||
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
|
||||
EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5));
|
||||
//deltaPCGJacobi.print("PCG Jacobi");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Copy of BlockJacobiPreconditioner::build
|
||||
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
|
||||
{
|
||||
const size_t n = keyInfo.size();
|
||||
std::vector<size_t> dims_ = keyInfo.colSpec();
|
||||
TEST(PCGSolver, simpleLinearSystem) {
|
||||
// Create a Gaussian Factor Graph
|
||||
GaussianFactorGraph simpleGFG;
|
||||
//SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
||||
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
|
||||
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||
|
||||
/* prepare the buffer of block diagonals */
|
||||
std::vector<Matrix> blocks; blocks.reserve(n);
|
||||
// Expected solution
|
||||
VectorValues expectedSolution;
|
||||
expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished());
|
||||
expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished());
|
||||
expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished());
|
||||
//expectedSolution.print("Expected");
|
||||
|
||||
/* allocate memory for the factorization of block diagonals */
|
||||
size_t nnz = 0;
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
const size_t dim = dims_[i];
|
||||
blocks.push_back(Matrix::Zero(dim, dim));
|
||||
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
|
||||
nnz += dim*dim;
|
||||
}
|
||||
// Solve the system using direct method
|
||||
VectorValues deltaDirect = simpleGFG.optimize();
|
||||
EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
|
||||
//deltaDirect.print("Direct");
|
||||
|
||||
/* compute the block diagonal by scanning over the factors */
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
|
||||
const Matrix &Ai = jf->getA(it);
|
||||
blocks[entry.index()] += (Ai.transpose() * Ai);
|
||||
}
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
|
||||
const Matrix &Hii = hf->info(it, it).selfadjointView();
|
||||
blocks[entry.index()] += Hii;
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
|
||||
}
|
||||
}
|
||||
// Solve the system using Preconditioned Conjugate Gradient solver
|
||||
// Common PCG parameters
|
||||
gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared<gtsam::PCGSolverParameters>();
|
||||
pcg->setMaxIterations(500);
|
||||
pcg->setEpsilon_abs(0.0);
|
||||
pcg->setEpsilon_rel(0.0);
|
||||
//pcg->setVerbosity("ERROR");
|
||||
|
||||
return blocks;
|
||||
}
|
||||
// With Dummy preconditioner
|
||||
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
|
||||
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
|
||||
//deltaPCGDummy.print("PCG Dummy");
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Preconditioner, buildBlocks ) {
|
||||
// Create simple Gaussian factor graph and initial values
|
||||
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
|
||||
Values initial;
|
||||
initial.insert(0,Point2(4, 5));
|
||||
initial.insert(1,Point2(0, 1));
|
||||
initial.insert(2,Point2(-5, 7));
|
||||
// With Block-Jacobi preconditioner
|
||||
pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
|
||||
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
|
||||
//deltaPCGJacobi.print("PCG Jacobi");
|
||||
|
||||
// Expected Hessian block diagonal matrices
|
||||
std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
|
||||
|
||||
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
|
||||
std::vector<Matrix> actualHessian = buildBlocks(gfg, KeyInfo(gfg));
|
||||
|
||||
// Compare the number of block diagonal matrices
|
||||
EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size());
|
||||
|
||||
// Compare the values of matrices
|
||||
std::map<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
|
||||
std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
|
||||
for(; it1!=expectedHessian.end(); it1++, it2++)
|
||||
EXPECT(assert_equal(it1->second, *it2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue