From 4e8968e3f5bd88048f20ee5475459ba484c41697 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 4 Aug 2016 18:46:26 -0400 Subject: [PATCH 01/24] starting to add remove functionality in fixedLagSmoothers, test does not pass.. --- .../nonlinear/BatchFixedLagSmoother.cpp | 2 +- .../nonlinear/BatchFixedLagSmoother.h | 2 +- gtsam_unstable/nonlinear/FixedLagSmoother.h | 2 +- .../nonlinear/IncrementalFixedLagSmoother.cpp | 4 +- .../nonlinear/IncrementalFixedLagSmoother.h | 3 +- .../tests/testIncrementalFixedLagSmoother.cpp | 60 ++++++++++++++++++- 6 files changed, 65 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index a0ede5f74..24595552a 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -52,7 +52,7 @@ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { /* ************************************************************************* */ FixedLagSmoother::Result BatchFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps) { + const KeyTimestampMap& timestamps, const FastVector& factorToRemove) { // Update all of the internal variables with the new information gttic(augment_system); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 27fe94451..1d882d7aa 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -48,7 +48,7 @@ public: /** Add new factors, updating the solution and relinearizing as needed. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap()); + const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector& factorToRemove = FastVector()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 4868a7cf1..2d080ad4d 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -91,7 +91,7 @@ public: /** Add new factors, updating the solution and relinearizing as needed. */ virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap()) = 0; + const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector& factorToRemove = FastVector()) = 0; /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index d3c34ff5e..98a3110aa 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -65,7 +65,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, /* ************************************************************************* */ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps) { + const KeyTimestampMap& timestamps, const FastVector& factorToRemove) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 ISAM2Result isamResult = isam_.update(newFactors, newTheta, - FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys); + factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 77d052a21..00ad5d13b 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -62,7 +62,8 @@ public: */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), // - const KeyTimestampMap& timestamps = KeyTimestampMap()); + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorToRemove = FactorIndices()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 0b86a60e9..4dd3b62a0 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -71,8 +71,6 @@ TEST( IncrementalFixedLagSmoother, Example ) Values fullinit; NonlinearFactorGraph fullgraph; - - // i keeps track of the time step size_t i = 0; @@ -177,6 +175,64 @@ TEST( IncrementalFixedLagSmoother, Example ) ++i; } + // add/remove an extra factor + { + Key key1 = MakeKey(i-1); + Key key2 = MakeKey(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // add 2 odometry factors + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + // now remove one of the two and try again + size_t factorIndex = fullgraph.size()-2; // any index that does not break connectivity of the graph + FastVector factorToRemove; + factorToRemove.push_back(factorIndex); + + NonlinearFactorGraph emptyNewFactors; + Values emptyNewValues; + Timestamps emptyNewTimestamps; + + //smoother.getFactors().print(); + + const NonlinearFactorGraph& allFactors = smoother.getFactors(); + size_t nrFactorsBeforeRemoval = allFactors.size(); + NonlinearFactorGraph expected; + for(size_t i=0; i< allFactors.size(); i++){ + // if(i != factorIndex) + // expected.push_back( allFactors[i] ); + // else + std::cout << "ind: " << i << std::endl; + allFactors[i]->print(); + } + + // remove factor + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps, factorToRemove); + size_t nrFactorsAfterRemoval = smoother.getFactors().size(); + + // check that the number of factors is right + DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); + + // check that the factors in the smoother are right + // NonlinearFactorGraph actual = smoother.getFactors(); + //CHECK(assert_equal(expected,actual)); + //smoother.getFactors().print(); + } } /* ************************************************************************* */ From b59a58278b3e914bd9d0b96bb0c9d260b6bf57b8 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 4 Aug 2016 23:38:39 -0400 Subject: [PATCH 02/24] fixed unit test! --- .../tests/testIncrementalFixedLagSmoother.cpp | 43 +++++++++---------- 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 4dd3b62a0..aafaecd2c 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -200,38 +200,37 @@ TEST( IncrementalFixedLagSmoother, Example ) CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); // now remove one of the two and try again - size_t factorIndex = fullgraph.size()-2; // any index that does not break connectivity of the graph - FastVector factorToRemove; - factorToRemove.push_back(factorIndex); - + // empty values and new factors for fake update in which we only remove factors NonlinearFactorGraph emptyNewFactors; Values emptyNewValues; Timestamps emptyNewTimestamps; - //smoother.getFactors().print(); + size_t factorIndex = 25; // any index that does not break connectivity of the graph + FastVector factorToRemove; + factorToRemove.push_back(factorIndex); - const NonlinearFactorGraph& allFactors = smoother.getFactors(); - size_t nrFactorsBeforeRemoval = allFactors.size(); - NonlinearFactorGraph expected; - for(size_t i=0; i< allFactors.size(); i++){ - // if(i != factorIndex) - // expected.push_back( allFactors[i] ); - // else - std::cout << "ind: " << i << std::endl; - allFactors[i]->print(); - } + const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); // remove factor - smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps, factorToRemove); - size_t nrFactorsAfterRemoval = smoother.getFactors().size(); + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); - // check that the number of factors is right - DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); + // Note: the following test (checking that the number of factor is reduced by 1) + // fails since we are not reusing slots, hence also when removing a factor we do not change + // the size of the factor graph + // size_t nrFactorsAfterRemoval = smoother.getFactors().size(); + // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); // check that the factors in the smoother are right - // NonlinearFactorGraph actual = smoother.getFactors(); - //CHECK(assert_equal(expected,actual)); - //smoother.getFactors().print(); + NonlinearFactorGraph actual = smoother.getFactors(); + for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){ + // check that the factors that were not removed are there + if(smootherFactorsBeforeRemove[i] && i != factorIndex){ + EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); + } + else{ // while the factors that were not there or were removed are no longer there + EXPECT(!actual[i]); + } + } } } From fd9bd43cb0b5d034d507b3e5c1bf5f5d6db267ad Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 4 Aug 2016 23:54:34 -0400 Subject: [PATCH 03/24] added remove functionality in BatchIncrementalSmoother with test --- .../nonlinear/BatchFixedLagSmoother.cpp | 6 ++ .../tests/testBatchFixedLagSmoother.cpp | 69 +++++++++++++++++-- 2 files changed, 70 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 24595552a..6bf7c970a 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -69,6 +69,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( insertFactors(newFactors); gttoc(augment_system); + // remove factors in factorToRemove + for(const size_t i : factorToRemove){ + if(factors_[i]) + factors_[i].reset(); + } + // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 25dd5fe98..b673588cf 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -53,11 +53,11 @@ TEST( BatchFixedLagSmoother, Example ) // the BatchFixedLagSmoother should be identical (even with the linearized approximations at // the end of the smoothing lag) -// SETDEBUG("BatchFixedLagSmoother update", true); -// SETDEBUG("BatchFixedLagSmoother reorder", true); -// SETDEBUG("BatchFixedLagSmoother optimize", true); -// SETDEBUG("BatchFixedLagSmoother marginalize", true); -// SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); + // SETDEBUG("BatchFixedLagSmoother update", true); + // SETDEBUG("BatchFixedLagSmoother reorder", true); + // SETDEBUG("BatchFixedLagSmoother optimize", true); + // SETDEBUG("BatchFixedLagSmoother marginalize", true); + // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); @@ -177,6 +177,65 @@ TEST( BatchFixedLagSmoother, Example ) ++i; } + // add/remove an extra factor + { + Key key1 = Key(i-1); + Key key2 = Key(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // add 2 odometry factors + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + +// NonlinearFactorGraph smootherGraph = smoother.getFactors(); +// for(size_t i=0; iprint(); +// } +// } + + // now remove one of the two and try again + // empty values and new factors for fake update in which we only remove factors + NonlinearFactorGraph emptyNewFactors; + Values emptyNewValues; + Timestamps emptyNewTimestamps; + + size_t factorIndex = 6; // any index that does not break connectivity of the graph + FastVector factorToRemove; + factorToRemove.push_back(factorIndex); + + const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + + // remove factor + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); + + // check that the factors in the smoother are right + NonlinearFactorGraph actual = smoother.getFactors(); + for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){ + // check that the factors that were not removed are there + if(smootherFactorsBeforeRemove[i] && i != factorIndex){ + EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); + } + else{ // while the factors that were not there or were removed are no longer there + EXPECT(!actual[i]); + } + } + } } /* ************************************************************************* */ From 82014c4311b00d8f9bbb1335ffb783b94afba97a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 5 Aug 2016 00:03:24 -0400 Subject: [PATCH 04/24] targets for unit tests --- .cproject | 122 +++++++++++++++++++++++++++--------------------------- 1 file changed, 61 insertions(+), 61 deletions(-) diff --git a/.cproject b/.cproject index 10b16f91c..137f96e4d 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -590,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -598,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -646,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -654,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -662,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -678,7 +675,6 @@ make - tests/testBayesTree true false @@ -1110,7 +1106,6 @@ make - testErrors.run true false @@ -1340,6 +1335,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1422,6 +1457,7 @@ make + testSimulated2DOriented.run true false @@ -1461,6 +1497,7 @@ make + testSimulated2D.run true false @@ -1468,6 +1505,7 @@ make + testSimulated3D.run true false @@ -1481,46 +1519,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1786,7 +1784,6 @@ cpack - -G DEB true false @@ -1794,7 +1791,6 @@ cpack - -G RPM true false @@ -1802,7 +1798,6 @@ cpack - -G TGZ true false @@ -1810,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2753,7 +2747,6 @@ make - testGraph.run true false @@ -2761,7 +2754,6 @@ make - testJunctionTree.run true false @@ -2769,7 +2761,6 @@ make - testSymbolicBayesNetB.run true false @@ -2863,6 +2854,14 @@ true true + + make + -j4 + testIncrementalFixedLagSmoother.run + true + true + true + make -j2 @@ -3329,6 +3328,7 @@ make + tests/testGaussianISAM2 true false From 359a61a547d6ed68fb995db6613db5241dfe2bf6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 5 Aug 2016 16:04:54 -0400 Subject: [PATCH 05/24] added print function to FixedLagSmoother::Result struct --- gtsam_unstable/nonlinear/FixedLagSmoother.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 2d080ad4d..e57510be4 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -59,6 +59,13 @@ public: size_t getNonlinearVariables() const { return nonlinearVariables; } size_t getLinearVariables() const { return linearVariables; } double getError() const { return error; } + void print() const{ + std::cout << "Nr iterations: " << iterations << std::endl; + std::cout << "Nr intermediateSteps: " << intermediateSteps << std::endl; + std::cout << "Nr nonlinear variables: " << nonlinearVariables << std::endl; + std::cout << "Nr linear variables: " << linearVariables << std::endl; + std::cout << "error: " << error << std::endl; + } }; From 1337ac8ab02daa7aab8d39259503681a173757b3 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 24 Aug 2016 14:19:02 -0400 Subject: [PATCH 06/24] added method to access isam2 results in incremental fixed lag smoother --- gtsam_unstable/nonlinear/FixedLagSmoother.h | 1 - gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 6 ++++++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index e57510be4..37fdce5a2 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -68,7 +68,6 @@ public: } }; - /** default constructor */ FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 98a3110aa..b4f09fc62 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -125,7 +125,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, + isamResult_ = isam_.update(newFactors, newTheta, factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 00ad5d13b..39af8f27e 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -109,11 +109,17 @@ public: return isam_.marginalCovariance(key); } + /// Get results of latest isam2 update + const ISAM2Result getISAM2Result() const{ return isamResult_; } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ ISAM2 isam_; + /** Store results of latest isam2 update */ + ISAM2Result isamResult_; + /** Erase any keys associated with timestamps before the provided time */ void eraseKeysBefore(double timestamp); From 2d3f4dba88b436dd30721985972683becf8937be Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 1 Sep 2016 12:23:30 -0400 Subject: [PATCH 07/24] added unit test for determinant in Bayes tree --- gtsam/linear/tests/testGaussianBayesTree.cpp | 23 ++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index e5634c357..042189d33 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -297,6 +297,29 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { EXPECT(newError < origError); } +/* ************************************************************************* */ +TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { + + // create small factor graph + GaussianFactorGraph fg; + Key x1 = 2, x2 = 0, l1 = 1; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + + // create corresponding Bayes tree: + boost::shared_ptr bt = fg.eliminateMultifrontal(); + Matrix H = fg.hessian().first; + + // test determinant + // NOTE: the hessian of the factor graph is H = R'R where R is the matrix encoded by the bayes tree, + // for this reason we have to take the sqrt + double expectedDeterminant = sqrt(H.determinant()); // determinant computed from full matrix + double actualDeterminant = bt->determinant(); + EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} From b5acfc5a0f3a7920cdf8ecccb03d28fa3b3eb9be Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 1 Apr 2017 13:05:34 -0400 Subject: [PATCH 08/24] added get to access gravity vector --- gtsam/navigation/PreintegrationParams.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f2b2c0fef..b929a753c 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -56,6 +56,7 @@ struct PreintegrationParams: PreintegratedRotationParams { const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + const Vector3& getGravity() const { return n_gravity; } bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } protected: From dccc742490d1717b4699ab0496c24a4a32078eae Mon Sep 17 00:00:00 2001 From: Toni Rosinol Date: Wed, 28 Feb 2018 13:28:14 -0500 Subject: [PATCH 09/24] Add jacobians to retract functions of Unit3 and OrientedPlane3 --- gtsam/geometry/OrientedPlane3.cpp | 9 +++++++-- gtsam/geometry/OrientedPlane3.h | 2 +- gtsam/geometry/Unit3.cpp | 20 ++++++++++++++++++-- gtsam/geometry/Unit3.h | 2 +- 4 files changed, 27 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 4af0257ec..82207d4ce 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -83,8 +83,13 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { - return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); +OrientedPlane3 OrientedPlane3::retract(const Vector3& v, OptionalJacobian<4,3> H) const { + Matrix32 H_n; + Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); + if (H) { + *H << H_n, Vector3::Zero(), 0, 0, 1; + } + return OrientedPlane3(n_retract, d_ + v(2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index e425a4b81..7306942f2 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -134,7 +134,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector3& v) const; + OrientedPlane3 retract(const Vector3& v, OptionalJacobian<4,3> H = boost::none) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index dacb5c3fd..f31b8641a 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -240,18 +240,34 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector2& v) const { +Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<3,2> H) const { // Compute the 3D xi_hat vector const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { - return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); + // Jacobian + // TODO what happens if theta = 0 ? sin(theta)/theta -> 1 when theta -> 0. + if (H) { + *H = -p_ * xi_hat.transpose() * basis() + + basis(); + } + + return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); } const Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); + + // Jacobian + if (H) { + *H = p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + + std::sin(theta) / theta * basis() + + xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2)) + * xi_hat.transpose() / theta * basis(); + } + return Unit3(exp_p_xi_hat); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index cd05af519..85c429fcd 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -188,7 +188,7 @@ public: }; /// The retract function - Unit3 retract(const Vector2& v) const; + Unit3 retract(const Vector2& v, OptionalJacobian<3,2> H = boost::none) const; /// The local coordinates function Vector2 localCoordinates(const Unit3& s) const; From 2af48952b2760c103d21459a28d5e8a5cfc25301 Mon Sep 17 00:00:00 2001 From: Toni Rosinol Date: Thu, 24 May 2018 18:05:57 -0400 Subject: [PATCH 10/24] Specify key involved when throwing cheirality exception --- gtsam/geometry/CalibratedCamera.h | 14 +++++++++++--- gtsam/geometry/StereoCamera.h | 14 ++++++++++++-- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/StereoFactor.h | 2 +- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f1fa509c1..9bd2f3f2e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -32,9 +32,17 @@ namespace gtsam { class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: - CheiralityException() : - ThreadsafeException("Cheirality Exception") { - } + CheiralityException() + : CheiralityException(std::numeric_limits::max()) {} + + CheiralityException(Key j) + : ThreadsafeException("CheiralityException"), + j_(j) {} + + Key nearbyVariable() const {return j_;} + +private: + Key j_; }; /** diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index ac32be7ae..0723aaec5 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -25,9 +25,19 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : - std::runtime_error("Stereo Cheirality Exception") { + StereoCheiralityException() + : StereoCheiralityException(std::numeric_limits::max()) {} + + StereoCheiralityException(Key j) + : std::runtime_error("Stereo Cheirality Exception"), + j_(j) {} + + Key nearbyVariable() const { + return j_; } + +private: + Key j_; }; /** diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index d13c28e11..aa04bf7d7 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -152,7 +152,7 @@ namespace gtsam { std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw e; + throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 59fc372cb..8828f5de7 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -144,7 +144,7 @@ public: std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw e; + throw StereoCheiralityException(this->key2()); } return Vector3::Constant(2.0 * K_->fx()); } From 65be6e77a9a9d12ae35631e492e0f45f82fbbe0f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 20 Dec 2018 17:21:47 +0100 Subject: [PATCH 11/24] went back to cproject from develop branch --- .cproject | 310 +++++++++++------------------------------------------- 1 file changed, 61 insertions(+), 249 deletions(-) diff --git a/.cproject b/.cproject index abc2dfb38..10b16f91c 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -57,78 +55,18 @@ - - - - - make - -j4 - testSmartStereoProjectionPoseFactor.run - true - true - true - - - make - -j4 - testTriangulation.run - true - true - true - - - make - -j4 - testStereoCamera.run - true - true - true - - - make - -j4 - testSmartProjectionCameraFactor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j8 check - check - true - false - true - - - make - -j4 - true - true - true - - - - - + + @@ -173,78 +111,18 @@ - - - - - make - -j4 - testSmartStereoProjectionPoseFactor.run - true - true - true - - - make - -j4 - testTriangulation.run - true - true - true - - - make - -j4 - testStereoCamera.run - true - true - true - - - make - -j4 - testSmartProjectionCameraFactor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j8 check - check - true - false - true - - - make - -j4 - true - true - true - - - - - + + @@ -293,66 +171,6 @@ - - - - - make - -j4 - testSmartStereoProjectionPoseFactor.run - true - true - true - - - make - -j4 - testTriangulation.run - true - true - true - - - make - -j4 - testStereoCamera.run - true - true - true - - - make - -j4 - testSmartProjectionCameraFactor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j8 check - check - true - false - true - - - make - -j4 - true - true - true - - - @@ -772,6 +590,7 @@ make + tests/testBayesTree.run true false @@ -779,6 +598,7 @@ make + testBinaryBayesNet.run true false @@ -826,6 +646,7 @@ make + testSymbolicBayesNet.run true false @@ -833,6 +654,7 @@ make + tests/testSymbolicFactor.run true false @@ -840,6 +662,7 @@ make + testSymbolicFactorGraph.run true false @@ -855,6 +678,7 @@ make + tests/testBayesTree true false @@ -1286,6 +1110,7 @@ make + testErrors.run true false @@ -1515,46 +1340,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1637,7 +1422,6 @@ make - testSimulated2DOriented.run true false @@ -1677,7 +1461,6 @@ make - testSimulated2D.run true false @@ -1685,7 +1468,6 @@ make - testSimulated3D.run true false @@ -1699,6 +1481,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1964,6 +1786,7 @@ cpack + -G DEB true false @@ -1971,6 +1794,7 @@ cpack + -G RPM true false @@ -1978,6 +1802,7 @@ cpack + -G TGZ true false @@ -1985,6 +1810,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2927,6 +2753,7 @@ make + testGraph.run true false @@ -2934,6 +2761,7 @@ make + testJunctionTree.run true false @@ -2941,6 +2769,7 @@ make + testSymbolicBayesNetB.run true false @@ -3034,22 +2863,6 @@ true true - - make - -j4 - testIncrementalFixedLagSmoother.run - true - true - true - - - make - -j4 - testBatchFixedLagSmoother.run - true - true - true - make -j2 @@ -3516,7 +3329,6 @@ make - tests/testGaussianISAM2 true false From 5d87e6809714d88d6ddfa4404767bf6c3864598c Mon Sep 17 00:00:00 2001 From: Toni Date: Wed, 2 Jan 2019 21:12:49 +0100 Subject: [PATCH 12/24] Add jacobian tests for retract of Unit3 & OrientedPlane3 --- gtsam/geometry/tests/testOrientedPlane3.cpp | 36 +++++++++++++++++++++ gtsam/geometry/tests/testUnit3.cpp | 35 ++++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index a1531e07c..1ad2b0405 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -161,6 +161,42 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } +//******************************************************************************* +// Wrapper to make retract return a Vector3 so we can test numerical derivatives. +Vector4 RetractTest(const OrientedPlane3& plane, const Vector3& v, + OptionalJacobian<4, 3> H) { + OrientedPlane3 plane_retract = plane.retract(v, H); + return Vector4(plane_retract.normal().point3().x(), + plane_retract.normal().point3().y(), + plane_retract.normal().point3().z(), + plane_retract.distance()); +} + +//******************************************************************************* +TEST (OrientedPlane3, jacobian_retract) { + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + Matrix43 H; + { + Vector3 v (-0.1, 0.2, 0.3); + plane.retract(v, H); + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } + { + Matrix43 H; + Vector3 v (0, 0, 0); + plane.retract(v, H); + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 542aca038..f33037b26 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -371,6 +371,41 @@ TEST(Unit3, retract) { } } +//******************************************************************************* +// Wrapper to make retract return a Vector3 so we can test numerical derivatives. +Vector3 RetractTest(const Unit3&p, const Vector2& v, OptionalJacobian<3, 2> H) { + Unit3 p_retract = p.retract(v, H); + return p_retract.point3(); +} + +//******************************************************************************* +TEST (OrientedPlane3, jacobian_retract) { + Unit3 p; + { + Vector2 v (-0.2, 0.1); + Matrix32 H; + p.retract(v, H); + + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } + { + Vector2 v (0, 0); + Matrix32 H; + p.retract(v, H); + + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + + } +} + //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; From 50a9695156797c89c422eac202e252ecfdb9656b Mon Sep 17 00:00:00 2001 From: Toni Date: Wed, 2 Jan 2019 21:13:26 +0100 Subject: [PATCH 13/24] Remove outdated comments --- gtsam/geometry/Unit3.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 891c81e55..1ff536b9e 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -257,8 +257,6 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<3,2> H) const { // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { - // Jacobian - // TODO what happens if theta = 0 ? sin(theta)/theta -> 1 when theta -> 0. if (H) { *H = -p_ * xi_hat.transpose() * basis() + basis(); From 8c8bdc4e674a39c830f928dca92b6d28182ffa2e Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 15 Jan 2019 17:42:54 -0500 Subject: [PATCH 14/24] Fix retract jacobian for Unit3, and tests --- gtsam/geometry/Unit3.cpp | 31 ++++++++++++++++-------------- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/tests/testUnit3.cpp | 23 ++++++---------------- 3 files changed, 24 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 1ff536b9e..193aab9b5 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -250,33 +250,36 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<3,2> H) const { +Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const { // Compute the 3D xi_hat vector const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); // Treat case of very small v differently + Matrix23 H_from_point; if (theta < std::numeric_limits::epsilon()) { - if (H) { - *H = -p_ * xi_hat.transpose() * basis() + - basis(); - } - - return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); + const Unit3 exp_p_xi_hat = + Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, + H? &H_from_point : nullptr); + if (H) { + *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + + basis()); + } + return exp_p_xi_hat; } - const Vector3 exp_p_xi_hat = - std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); - + const Unit3 exp_p_xi_hat = + Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), + H? &H_from_point : nullptr); // Jacobian if (H) { - *H = p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + + *H = H_from_point * + (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + std::sin(theta) / theta * basis() + xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2)) - * xi_hat.transpose() / theta * basis(); + * xi_hat.transpose() / theta * basis()); } - - return Unit3(exp_p_xi_hat); + return exp_p_xi_hat; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f50f5ef44..f182df285 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -188,7 +188,7 @@ public: }; /// The retract function - Unit3 retract(const Vector2& v, OptionalJacobian<3,2> H = boost::none) const; + Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; /// The local coordinates function Vector2 localCoordinates(const Unit3& s) const; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index f33037b26..815231d38 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -372,37 +372,26 @@ TEST(Unit3, retract) { } //******************************************************************************* -// Wrapper to make retract return a Vector3 so we can test numerical derivatives. -Vector3 RetractTest(const Unit3&p, const Vector2& v, OptionalJacobian<3, 2> H) { - Unit3 p_retract = p.retract(v, H); - return p_retract.point3(); -} - -//******************************************************************************* -TEST (OrientedPlane3, jacobian_retract) { +TEST (Unit3, jacobian_retract) { Unit3 p; + boost::function f = + boost::bind(&Unit3::retract, p, _1, boost::none); + Matrix22 H; { Vector2 v (-0.2, 0.1); - Matrix32 H; p.retract(v, H); // Test that jacobian is numerically as expected. - boost::function f = - boost::bind(RetractTest, _1, _2, boost::none); - Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); } { Vector2 v (0, 0); - Matrix32 H; p.retract(v, H); // Test that jacobian is numerically as expected. - boost::function f = - boost::bind(RetractTest, _1, _2, boost::none); - Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); - } } From 5c4e5a4b23cbc3743c507e3e8fd195264cc95c13 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 15 Jan 2019 17:51:38 -0500 Subject: [PATCH 15/24] Reformat Unit3 --- gtsam/geometry/Unit3.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 193aab9b5..8ddef1d7e 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -255,24 +255,21 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const { const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); - // Treat case of very small v differently + // Treat case of very small v differently. Matrix23 H_from_point; if (theta < std::numeric_limits::epsilon()) { - const Unit3 exp_p_xi_hat = - Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, - H? &H_from_point : nullptr); - if (H) { - *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + - basis()); + const Unit3 exp_p_xi_hat = Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, + H? &H_from_point : nullptr); + if (H) { // Jacobian + *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + basis()); } return exp_p_xi_hat; } - const Unit3 exp_p_xi_hat = - Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), + const Unit3 exp_p_xi_hat = Unit3::FromPoint3( + std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), H? &H_from_point : nullptr); - // Jacobian - if (H) { + if (H) { // Jacobian *H = H_from_point * (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + std::sin(theta) / theta * basis() + From 21163e9bdc5869f74c10146e8bd7a794dc208081 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 15 Jan 2019 17:51:58 -0500 Subject: [PATCH 16/24] Reformat Unit3 test --- gtsam/geometry/tests/testUnit3.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 815231d38..9d53a30a6 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -373,23 +373,19 @@ TEST(Unit3, retract) { //******************************************************************************* TEST (Unit3, jacobian_retract) { + Matrix22 H; Unit3 p; boost::function f = boost::bind(&Unit3::retract, p, _1, boost::none); - Matrix22 H; { Vector2 v (-0.2, 0.1); p.retract(v, H); - - // Test that jacobian is numerically as expected. Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); } { Vector2 v (0, 0); p.retract(v, H); - - // Test that jacobian is numerically as expected. Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); } From dccff83e383fa7f886986a4f3fd0d70522d323fa Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 15 Jan 2019 17:52:45 -0500 Subject: [PATCH 17/24] Fix jacobian of retract for OrientedPlane3 and tests --- gtsam/geometry/OrientedPlane3.cpp | 7 +++-- gtsam/geometry/OrientedPlane3.h | 2 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 34 ++++++--------------- 3 files changed, 14 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 82207d4ce..5cfa80779 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -83,11 +83,12 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector3& v, OptionalJacobian<4,3> H) const { - Matrix32 H_n; +OrientedPlane3 OrientedPlane3::retract(const Vector3& v, + OptionalJacobian<3,3> H) const { + Matrix22 H_n; Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); if (H) { - *H << H_n, Vector3::Zero(), 0, 0, 1; + *H << H_n, Vector2::Zero(), 0, 0, 1; } return OrientedPlane3(n_retract, d_ + v(2)); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 7306942f2..596118385 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -134,7 +134,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector3& v, OptionalJacobian<4,3> H = boost::none) const; + OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 1ad2b0405..54cf84ea8 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -161,39 +161,23 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } -//******************************************************************************* -// Wrapper to make retract return a Vector3 so we can test numerical derivatives. -Vector4 RetractTest(const OrientedPlane3& plane, const Vector3& v, - OptionalJacobian<4, 3> H) { - OrientedPlane3 plane_retract = plane.retract(v, H); - return Vector4(plane_retract.normal().point3().x(), - plane_retract.normal().point3().y(), - plane_retract.normal().point3().z(), - plane_retract.distance()); -} - //******************************************************************************* TEST (OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); - Matrix43 H; + Matrix33 H_actual; + boost::function f = + boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); { Vector3 v (-0.1, 0.2, 0.3); - plane.retract(v, H); - // Test that jacobian is numerically as expected. - boost::function f = - boost::bind(RetractTest, _1, _2, boost::none); - Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); - EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); } { - Matrix43 H; Vector3 v (0, 0, 0); - plane.retract(v, H); - // Test that jacobian is numerically as expected. - boost::function f = - boost::bind(RetractTest, _1, _2, boost::none); - Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); - EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); } } From e63ceee9389cb8e9ec46dfb83cabcd6437d559fc Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 16:13:07 -0500 Subject: [PATCH 18/24] Optimize retract function for Unit3 --- gtsam/geometry/Unit3.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 8ddef1d7e..f661f819d 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -254,27 +254,27 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const { // Compute the 3D xi_hat vector const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); + const double c = std::cos(theta); // Treat case of very small v differently. Matrix23 H_from_point; if (theta < std::numeric_limits::epsilon()) { - const Unit3 exp_p_xi_hat = Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, + const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat, H? &H_from_point : nullptr); if (H) { // Jacobian - *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + basis()); + *H = H_from_point * + (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis(); } return exp_p_xi_hat; } - const Unit3 exp_p_xi_hat = Unit3::FromPoint3( - std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), - H? &H_from_point : nullptr); + const double st = std::sin(theta) / theta; + const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st, + H? &H_from_point : nullptr); if (H) { // Jacobian *H = H_from_point * - (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + - std::sin(theta) / theta * basis() + - xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2)) - * xi_hat.transpose() / theta * basis()); + (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() + + xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis(); } return exp_p_xi_hat; } From 80a827457ec018f093e11d0ad549d8a9a557273f Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 16:13:32 -0500 Subject: [PATCH 19/24] Move implementation of print function for Result to .cpp --- gtsam_unstable/nonlinear/FixedLagSmoother.cpp | 9 +++++++++ gtsam_unstable/nonlinear/FixedLagSmoother.h | 8 +------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 62ee07a16..9c04f0eec 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -21,6 +21,15 @@ namespace gtsam { +/* ************************************************************************* */ +void FixedLagSmoother::Result::print() const { + std::cout << "Nr iterations: " << iterations << '\n' + << "Nr intermediateSteps: " << intermediateSteps << '\n' + << "Nr nonlinear variables: " << nonlinearVariables << '\n' + << "Nr linear variables: " << linearVariables << '\n' + << "error: " << error << std::endl; +} + /* ************************************************************************* */ void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 37fdce5a2..7b26c7021 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -59,13 +59,7 @@ public: size_t getNonlinearVariables() const { return nonlinearVariables; } size_t getLinearVariables() const { return linearVariables; } double getError() const { return error; } - void print() const{ - std::cout << "Nr iterations: " << iterations << std::endl; - std::cout << "Nr intermediateSteps: " << intermediateSteps << std::endl; - std::cout << "Nr nonlinear variables: " << nonlinearVariables << std::endl; - std::cout << "Nr linear variables: " << linearVariables << std::endl; - std::cout << "error: " << error << std::endl; - } + void print() const; }; /** default constructor */ From 642f839b7dadd9a13d542430fb7a0281609d6792 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 16:15:56 -0500 Subject: [PATCH 20/24] Refactor getISAM2Result to ISAM2Result, and return const & --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 39af8f27e..28895046a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -110,7 +110,7 @@ public: } /// Get results of latest isam2 update - const ISAM2Result getISAM2Result() const{ return isamResult_; } + const ISAM2Result& ISAM2Result() const {return isamResult_;} protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled From 8e7e5aaf5cd44fe276e303102837319ad9efd637 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 16:56:38 -0500 Subject: [PATCH 21/24] Specify that member ISAM2Result refers to struct --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 28895046a..27782e705 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -118,7 +118,7 @@ protected: ISAM2 isam_; /** Store results of latest isam2 update */ - ISAM2Result isamResult_; + struct ISAM2Result isamResult_; /** Erase any keys associated with timestamps before the provided time */ void eraseKeysBefore(double timestamp); From 4cfe84e523458344458650b94be7d5c3416817c9 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 18:03:38 -0500 Subject: [PATCH 22/24] Rename factorToRemove to factorsToRemove --- gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp | 4 ++-- gtsam_unstable/nonlinear/BatchFixedLagSmoother.h | 6 ++++-- gtsam_unstable/nonlinear/FixedLagSmoother.h | 6 ++++-- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp | 4 ++-- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 6 +++--- 5 files changed, 15 insertions(+), 11 deletions(-) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 6bf7c970a..ac521d9c3 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -52,7 +52,7 @@ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { /* ************************************************************************* */ FixedLagSmoother::Result BatchFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps, const FastVector& factorToRemove) { + const KeyTimestampMap& timestamps, const FastVector& factorsToRemove) { // Update all of the internal variables with the new information gttic(augment_system); @@ -70,7 +70,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( gttoc(augment_system); // remove factors in factorToRemove - for(const size_t i : factorToRemove){ + for(const size_t i : factorsToRemove){ if(factors_[i]) factors_[i].reset(); } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 1d882d7aa..2bb99b7e6 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -47,8 +47,10 @@ public: virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector& factorToRemove = FastVector()); + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FastVector()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 7b26c7021..1d8321eb3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -90,8 +90,10 @@ public: } /** Add new factors, updating the solution and relinearizing as needed. */ - virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector& factorToRemove = FastVector()) = 0; + virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FastVector()) = 0; /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index b4f09fc62..689a3697c 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -65,7 +65,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, /* ************************************************************************* */ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps, const FastVector& factorToRemove) { + const KeyTimestampMap& timestamps, const FastVector& factorsToRemove) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 isamResult_ = isam_.update(newFactors, newTheta, - factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys); + factorsToRemove, constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 27782e705..c1c244a51 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -61,9 +61,9 @@ public: * @param timestamps an (optional) map from keys to real time stamps */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), - const Values& newTheta = Values(), // - const KeyTimestampMap& timestamps = KeyTimestampMap(), - const FastVector& factorToRemove = FactorIndices()); + const Values& newTheta = Values(), // + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FactorIndices()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only From 23df961b5e4d91dd844fdd06578394e6e8c16ab3 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 21 Jan 2019 18:10:40 -0500 Subject: [PATCH 23/24] Udpate comment for factorsToRemove --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index c1c244a51..40e96b206 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -59,6 +59,7 @@ public: * @param newFactors new factors on old and/or new variables * @param newTheta new values for new variables only * @param timestamps an (optional) map from keys to real time stamps + * @param factorsToRemove an (optional) list of factors to remove. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), // From de420692a598e065daa849411eefe4793c10f0e2 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 29 Jan 2019 16:11:51 -0500 Subject: [PATCH 24/24] Change again to getISAM2Result, wo get it is ambiguous --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 40e96b206..0014a0747 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -111,7 +111,7 @@ public: } /// Get results of latest isam2 update - const ISAM2Result& ISAM2Result() const {return isamResult_;} + const ISAM2Result& getISAM2Result() const{ return isamResult_; } protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled @@ -119,7 +119,7 @@ protected: ISAM2 isam_; /** Store results of latest isam2 update */ - struct ISAM2Result isamResult_; + ISAM2Result isamResult_; /** Erase any keys associated with timestamps before the provided time */ void eraseKeysBefore(double timestamp);