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

Merge remote-tracking branch 'origin/develop' into feature/BAD

release/4.3a0
dellaert 2014-11-14 01:31:49 +01:00
parent 2d654f7fa7 c6c611ae11
commit e2aef1b325
12 changed files with 280 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/JacobianFactor.h
View File

@ -187,7 +187,7 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const;
/* ************************************************************************* */
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor

25
gtsam/linear/Preconditioner.cpp
View File

@ -13,6 +13,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <iostream>
#include <vector>
@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build(
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));
// blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
/* compute the block diagonal by scanning over the factors */
/* getting the block diagonals 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.");
}
std::map<Key, Matrix> hessianMap = gf->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 */

16
gtsam/navigation/ImuFactor.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
class PreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
@ -216,11 +216,21 @@ namespace gtsam {
H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_angles;
// first order uncertainty propagation
// first order uncertainty propagation:
// the deltaT allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
//
// Matrix G(9,9);
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
//
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){

36
gtsam/slam/ImplicitSchurFactor.h → gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -1,5 +1,5 @@
/**
* @file ImplicitSchurFactor.h
* @file RegularImplicitSchurFactor.h
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
* @author Frank Dellaert
* @author Luca Carlone
@ -17,13 +17,13 @@
namespace gtsam {
/**
* ImplicitSchurFactor
* RegularImplicitSchurFactor
*/
template<size_t D> //
class ImplicitSchurFactor: public GaussianFactor {
class RegularImplicitSchurFactor: public GaussianFactor {
public:
typedef ImplicitSchurFactor This; ///< Typedef to this class
typedef RegularImplicitSchurFactor This; ///< Typedef to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
@ -40,11 +40,11 @@ protected:
public:
/// Constructor
ImplicitSchurFactor() {
RegularImplicitSchurFactor() {
}
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
ImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys();
@ -58,7 +58,7 @@ public:
}
/// Destructor
virtual ~ImplicitSchurFactor() {
virtual ~RegularImplicitSchurFactor() {
}
// Write access, only use for construction!
@ -87,7 +87,7 @@ public:
/// print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl;
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
std::cout << " E_ \n" << E_ << std::endl;
@ -96,7 +96,7 @@ public:
/// equals
bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const ImplicitSchurFactor*>(&lf))
if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
return false;
else {
return false;
@ -110,21 +110,21 @@ public:
virtual Matrix augmentedJacobian() const {
throw std::runtime_error(
"ImplicitSchurFactor::augmentedJacobian non implemented");
"RegularImplicitSchurFactor::augmentedJacobian non implemented");
return Matrix();
}
virtual std::pair<Matrix, Vector> jacobian() const {
throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector());
}
virtual Matrix augmentedInformation() const {
throw std::runtime_error(
"ImplicitSchurFactor::augmentedInformation non implemented");
"RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix();
}
virtual Matrix information() const {
throw std::runtime_error(
"ImplicitSchurFactor::information non implemented");
"RegularImplicitSchurFactor::information non implemented");
return Matrix();
}
@ -210,18 +210,18 @@ public:
}
virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::clone non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
}
virtual bool empty() const {
return false;
}
virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::negate non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
}
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@ -454,7 +454,7 @@ public:
}
};
// ImplicitSchurFactor
// RegularImplicitSchurFactor
}

19
gtsam/slam/SmartFactorBase.h
View File

@ -21,7 +21,7 @@
#include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h"
#include "RegularImplicitSchurFactor.h"
#include "RegularHessianFactor.h"
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -73,7 +73,7 @@ public:
/**
* Constructor
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor) {
@ -271,8 +271,13 @@ public:
Vector bi;
try {
bi =
-(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
if(body_P_sensor_){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6);
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
Fi = Fi * J;
}
} catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE);
@ -624,11 +629,11 @@ public:
}
// ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
typename boost::shared_ptr<ImplicitSchurFactor<D> > f(
new ImplicitSchurFactor<D>());
typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
cameras, point, lambda, diagonalDamping);
f->initKeys();

12
gtsam/slam/SmartProjectionFactor.h
View File

@ -120,7 +120,7 @@ public:
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
@ -298,7 +298,7 @@ public:
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration"
std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
@ -409,12 +409,12 @@ public:
}
// create factor
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createImplicitSchurFactor(cameras, point_, lambda);
return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
else
return boost::shared_ptr<ImplicitSchurFactor<D> >();
return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
}
/// create factor
@ -685,7 +685,7 @@ public:
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return chirality verbosity */
/** return cheirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}

5
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -63,7 +63,7 @@ public:
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
@ -157,6 +157,9 @@ public:
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if(Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
typename Base::Camera camera(pose, *K_all_[i++]);
cameras.push_back(camera);
}

16
gtsam/slam/tests/testImplicitSchurFactor.cpp → gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -6,7 +6,7 @@
*/
//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h"
@ -38,19 +38,19 @@ const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
//*************************************************************************************
TEST( implicitSchurFactor, creation ) {
TEST( regularImplicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P));
}
/* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) {
TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
keys += 0,1,2,3;
Vector x = xvalues.vector(keys);
Vector expected = zero(24);
ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
// Create ImplicitSchurFactor
ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version
@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
}
/* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal)
TEST(regularImplicitSchurFactor, hessianDiagonal)
{
/* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6)
@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal)
E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
// hessianDiagonal
VectorValues expected;

89
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
if(isDebugTest) tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
// These are the poses we want to estimate, from camera measurements
Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
double rankTol = 1;
double linThreshold = -1;
bool manageDegeneracy = false;
bool enableEPI = false;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// Put all factors in factor graph, adding priors
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
// Check errors at ground truth poses
Values gtValues;
gtValues.insert(x1, bodyPose1);
gtValues.insert(x2, bodyPose2);
gtValues.insert(x3, bodyPose3);
double actualError = graph.error(gtValues);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Values values;
values.insert(x1, bodyPose1);
values.insert(x2, bodyPose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, bodyPose3*noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
// cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

4
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -17,12 +17,14 @@
#include <boost/shared_array.hpp>
#include <boost/timer.hpp>
#include "FindSeparator.h"
extern "C" {
#include <metis.h>
#include "metislib.h"
}
#include "FindSeparator.h"
namespace gtsam { namespace partition {

5
tests/testPCGSolver.cpp
View File

@ -13,6 +13,7 @@
* @file testPCGSolver.cpp
* @brief Unit tests for PCGSolver class
* @author Yong-Dian Jian
* @date Aug 06, 2014
*/
#include <tests/smallExample.h>
@ -51,6 +52,7 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
// Test cholesky decomposition
TEST( PCGSolver, llt ) {
Matrix R = (Matrix(3,3) <<
1., -1., -1.,
@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) {
}
/* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy )
{
LevenbergMarquardtParams paramsPCG;
@ -110,6 +113,7 @@ TEST( PCGSolver, dummy )
}
/* ************************************************************************* */
// Test Block-Jacobi Precondioner
TEST( PCGSolver, blockjacobi )
{
LevenbergMarquardtParams paramsPCG;
@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi )
}
/* ************************************************************************* */
// Test Incremental Subgraph PCG Solver
TEST( PCGSolver, subgraph )
{
LevenbergMarquardtParams paramsPCG;

115
tests/testPreconditioner.cpp Normal file
View File

@ -0,0 +1,115 @@
/* ----------------------------------------------------------------------------
* 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 testPreconditioner.cpp
* @brief Unit tests for Preconditioners
* @author Sungtae An
* @date Nov 6, 2014
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/linear/PCGSolver.h>
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), (Vector(2) << 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), (Vector(2) << 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), (Vector(2) << -1.0, 1.5), unit2);
return fg;
}
/* ************************************************************************* */
// 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();
/* prepare the buffer of block diagonals */
std::vector<Matrix> blocks; blocks.reserve(n);
/* 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;
}
/* 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.");
}
}
return blocks;
}
/* ************************************************************************* */
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));
// 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));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */