From a89781a9e6c08765900f5d64d4efd6d91240e09f Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 6 Nov 2014 14:26:46 -0500 Subject: [PATCH 1/8] Add some comments --- tests/testPCGSolver.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 38a40521a..04c4e9d52 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -13,6 +13,7 @@ * @file testPCGSolver.cpp * @brief Unit tests for PCGSolver class * @author Yong-Dian Jian + * @date Aug 06, 2014 */ #include @@ -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; From 85d6456a69ef462edb0e286d6de52ce04e14e871 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 6 Nov 2014 16:10:56 -0500 Subject: [PATCH 2/8] Add comments --- gtsam/linear/JacobianFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b90012822..99ea91cd9 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -185,7 +185,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 From 7bbd0513f4026d0e6ca5f6899d5406d7482fd928 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 6 Nov 2014 23:14:52 -0500 Subject: [PATCH 3/8] Add the first unit test for Block-Jacobi Preconditioner --- tests/testPreconditioner.cpp | 115 +++++++++++++++++++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 tests/testPreconditioner.cpp diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp new file mode 100644 index 000000000..40d49a934 --- /dev/null +++ b/tests/testPreconditioner.cpp @@ -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 + +#include +#include +#include +#include +#include + +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 buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) +{ + const size_t n = keyInfo.size(); + std::vector dims_ = keyInfo.colSpec(); + + /* prepare the buffer of block diagonals */ + std::vector 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(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(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 expectedHessian =gfg.hessianBlockDiagonal(); + + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector 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::const_iterator it1 = expectedHessian.begin(); + std::vector::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); } +/* ************************************************************************* */ From 61d9cdd20f0c919049d98a6de255f607d6d317ca Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 7 Nov 2014 19:27:03 -0500 Subject: [PATCH 4/8] included sensor to body transformation in smartProjectionPoseFactors (with unit test) --- gtsam/slam/SmartFactorBase.h | 11 ++- gtsam/slam/SmartProjectionFactor.h | 4 +- gtsam/slam/SmartProjectionPoseFactor.h | 5 +- .../tests/testSmartProjectionPoseFactor.cpp | 89 +++++++++++++++++++ 4 files changed, 103 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1ecaaf170..e4469eba3 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.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 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); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 043528fea..56af6255b 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -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, @@ -685,7 +685,7 @@ public: inline bool isPointBehindCamera() const { return cheiralityException_; } - /** return chirality verbosity */ + /** return cheirality verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 273102bda..f871ab3aa 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -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(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); } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 6b849ba5a..4e4fde3e4 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -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 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 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(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(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(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} + + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; From 9b3049311307a8863e5e757d449f5e90c24ddc4b Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sat, 8 Nov 2014 20:08:01 -0500 Subject: [PATCH 5/8] Replace the part of computing block diagonal manually for each factor with calling hessianBlockDiagonal --- gtsam/linear/Preconditioner.cpp | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c180f1160..9af362fba 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -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(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(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 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 */ From 3dbc9929a4a143b0fbcdd0510ae9852e6e065fa4 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 10 Nov 2014 23:37:42 -0500 Subject: [PATCH 6/8] Rename ImplicitSchurFactor to RegularImplicitSchurFactor ('Regular' means fixed-size) --- ...rFactor.h => RegularImplicitSchurFactor.h} | 36 +++++++++---------- gtsam/slam/SmartFactorBase.h | 8 ++--- gtsam/slam/SmartProjectionFactor.h | 8 ++--- ...cpp => testRegularImplicitSchurFactor.cpp} | 16 ++++----- 4 files changed, 34 insertions(+), 34 deletions(-) rename gtsam/slam/{ImplicitSchurFactor.h => RegularImplicitSchurFactor.h} (92%) rename gtsam/slam/tests/{testImplicitSchurFactor.cpp => testRegularImplicitSchurFactor.cpp} (94%) diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h similarity index 92% rename from gtsam/slam/ImplicitSchurFactor.h rename to gtsam/slam/RegularImplicitSchurFactor.h index 8c6d8f1ff..f1a11e2cd 100644 --- a/gtsam/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -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 // -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 shared_ptr; ///< shared_ptr to this class protected: @@ -41,11 +41,11 @@ protected: public: /// Constructor - ImplicitSchurFactor() { + RegularImplicitSchurFactor() { } /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b - ImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, + RegularImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b) : Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { initKeys(); @@ -59,7 +59,7 @@ public: } /// Destructor - virtual ~ImplicitSchurFactor() { + virtual ~RegularImplicitSchurFactor() { } // Write access, only use for construction! @@ -88,7 +88,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; @@ -97,7 +97,7 @@ public: /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) + if (!dynamic_cast(&lf)) return false; else { return false; @@ -111,21 +111,21 @@ public: virtual Matrix augmentedJacobian() const { throw std::runtime_error( - "ImplicitSchurFactor::augmentedJacobian non implemented"); + "RegularImplicitSchurFactor::augmentedJacobian non implemented"); return Matrix(); } virtual std::pair 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(); } @@ -211,18 +211,18 @@ public: } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, + return boost::make_shared >(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 >(Fblocks_, + return boost::make_shared >(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 @@ -455,7 +455,7 @@ public: } }; -// ImplicitSchurFactor +// RegularImplicitSchurFactor } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e4469eba3..8ae26e7cd 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -21,7 +21,7 @@ #include "JacobianFactorQ.h" #include "JacobianFactorSVD.h" -#include "ImplicitSchurFactor.h" +#include "RegularImplicitSchurFactor.h" #include "RegularHessianFactor.h" #include @@ -629,11 +629,11 @@ public: } // **************************************************************************************************** - boost::shared_ptr > createImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - typename boost::shared_ptr > f( - new ImplicitSchurFactor()); + typename boost::shared_ptr > f( + new RegularImplicitSchurFactor()); computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), cameras, point, lambda, diagonalDamping); f->initKeys(); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 56af6255b..bfd73d9fb 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -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 > createImplicitSchurFactor( + boost::shared_ptr > 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 >(); + return boost::shared_ptr >(); } /// create factor diff --git a/gtsam/slam/tests/testImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp similarity index 94% rename from gtsam/slam/tests/testImplicitSchurFactor.cpp rename to gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 77faaacc1..0161d4fb6 100644 --- a/gtsam/slam/tests/testImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -6,7 +6,7 @@ */ //#include -#include +#include //#include #include //#include "gtsam_unstable/slam/JacobianFactorQR.h" @@ -38,19 +38,19 @@ const vector > Fblocks = list_of > // 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; From 1c97d1270d7a4c72e9ec71fe64fedfcc222e7c47 Mon Sep 17 00:00:00 2001 From: Abhijit Kundu Date: Tue, 11 Nov 2014 17:14:51 -0500 Subject: [PATCH 7/8] Changing include order so as to OpenMP flags (if used) are defined before metis. This fixes a compilation error with testFindSeparator.cpp --- gtsam_unstable/partition/FindSeparator-inl.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 849cf7a05..ace13dc41 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -17,12 +17,14 @@ #include #include +#include "FindSeparator.h" + extern "C" { #include #include "metislib.h" } -#include "FindSeparator.h" + namespace gtsam { namespace partition { From c6c611ae11226722de1a8714c1fa56124484484f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 12 Nov 2014 19:06:16 -0500 Subject: [PATCH 8/8] added comments to imu preintegration --- gtsam/navigation/ImuFactor.h | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b95f9c346..69c430a78 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -58,7 +58,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) @@ -217,11 +217,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_){