From fad9462661d1f1ad651e91068f532398b7cad434 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 23:51:01 -0800 Subject: [PATCH] A better solution through typedef of FactorIndices -> FastVector --- gtsam.h | 6 +++-- gtsam/nonlinear/ISAM2-impl.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.h | 2 +- gtsam/nonlinear/ISAM2.cpp | 6 ++--- gtsam/nonlinear/ISAM2.h | 9 ++++--- .../nonlinear/ConcurrentIncrementalFilter.cpp | 4 +-- .../ConcurrentIncrementalSmoother.cpp | 4 +-- .../nonlinear/ConcurrentIncrementalSmoother.h | 4 +-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- tests/testGaussianISAM2.cpp | 26 +++++++++---------- 10 files changed, 34 insertions(+), 31 deletions(-) diff --git a/gtsam.h b/gtsam.h index 3d225529e..c5424ef80 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2161,6 +2161,8 @@ class ISAM2Result { size_t getCliques() const; }; +class FactorIndices {}; + class ISAM2 { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); @@ -2173,8 +2175,8 @@ class ISAM2 { gtsam::ISAM2Result update(); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); // TODO: wrap the full version of update //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 4e8c0e303..0211df735 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -46,7 +46,7 @@ void ISAM2::Impl::AddVariables( /* ************************************************************************* */ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices) + NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) { newFactorIndices.resize(newFactors.size()); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index d34480144..161932344 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -52,7 +52,7 @@ struct GTSAM_EXPORT ISAM2::Impl { /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices); + NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); /** * Remove variables from the ISAM2 system. diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 754623e7e..d6f1a636a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -510,7 +510,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyVector& removeFactorIndices, + const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { @@ -753,8 +753,8 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional&> marginalFactorsIndices, - boost::optional&> deletedFactorsIndices) + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a921f6e76..27f21be12 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -257,6 +257,7 @@ struct GTSAM_EXPORT ISAM2Params { /// @} }; +typedef FastVector FactorIndices; /** * @addtogroup ISAM2 @@ -318,7 +319,7 @@ struct GTSAM_EXPORT ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - FastVector newFactorsIndices; + FactorIndices newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. @@ -530,7 +531,7 @@ public: */ virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyVector& removeFactorIndices = KeyVector(), + const FactorIndices& removeFactorIndices = FactorIndices(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, @@ -551,8 +552,8 @@ public: * indices of any factor that was removed during the 'marginalizeLeaves' call */ void marginalizeLeaves(const FastList& leafKeys, - boost::optional&> marginalFactorsIndices = boost::none, - boost::optional&> deletedFactorsIndices = boost::none); + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point const Values& getLinearizationPoint() const { diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 6435bd2df..f0c7212c8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -56,7 +56,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No Result result; // We do not need to remove any factors at this time - KeyVector removedFactors; + FactorIndices removedFactors; if(removeFactorIndices){ removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); @@ -210,7 +210,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth isam2_.params().getEliminationFunction()); // Remove the old factors on the separator and insert the new ones - KeyVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); + FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false); currentSmootherSummarizationSlots_ = result.newFactorsIndices; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 21b6b3cd0..268160451 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -45,7 +45,7 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double /* ************************************************************************* */ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional& removeFactorIndices) { + const boost::optional& removeFactorIndices) { gttic(update); @@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons synchronizationUpdatesAvailable_ = false; } else { // Update the system using iSAM2 - isam2Result = isam2_.update(newFactors, newTheta, KeyVector(), constrainedKeys, noRelinKeys); + isam2Result = isam2_.update(newFactors, newTheta, FactorIndices(), constrainedKeys, noRelinKeys); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 564ed3e52..2d519bf25 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -109,7 +109,7 @@ public: * and additionally, variables that were already in the system must not be included here. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional& removeFactorIndices = boost::none); + const boost::optional& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -152,7 +152,7 @@ protected: Values smootherValues_; ///< New variables to be added to the smoother during the next update NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization. - KeyVector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors + FactorIndices filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet // Storage for information to be sent to the filter diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 2265cab5e..8623136cd 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 ISAM2Result isamResult = isam_.update(newFactors, newTheta, - KeyVector(), constrainedKeys, boost::none, additionalMarkedKeys); + FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 58c718726..d358caa35 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -301,9 +301,9 @@ TEST(ISAM2, AddFactorsStep1) expectedNonlinearFactors += PriorFactor(11, Zero, model); expectedNonlinearFactors += PriorFactor(2, Zero, model); - const FastVector expectedNewFactorIndices = list_of(1)(3); + const FactorIndices expectedNewFactorIndices = list_of(1)(3); - FastVector actualNewFactorIndices; + FactorIndices actualNewFactorIndices; ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); @@ -419,7 +419,7 @@ TEST(ISAM2, removeFactors) ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the 2nd measurement on landmark 0 (Key 100) - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(12); isam.update(NonlinearFactorGraph(), Values(), toRemove); @@ -439,7 +439,7 @@ TEST(ISAM2, removeVariables) ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the measurement on landmark 0 (Key 100) - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(7); toRemove.push_back(14); isam.update(NonlinearFactorGraph(), Values(), toRemove); @@ -466,7 +466,7 @@ TEST(ISAM2, swapFactors) // Remove the measurement on landmark 0 and replace with a different one { size_t swap_idx = isam.getFactorsUnsafe().size()-2; - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(swap_idx); fullgraph.remove(swap_idx); @@ -549,7 +549,7 @@ TEST(ISAM2, constrained_ordering) fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); if(i >= 3) - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); else isam.update(newfactors, init); } @@ -570,7 +570,7 @@ TEST(ISAM2, constrained_ordering) fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); ++ i; } @@ -584,7 +584,7 @@ TEST(ISAM2, constrained_ordering) init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); } // Add odometry from time 10 to 11 and landmark measurement at time 10 @@ -599,7 +599,7 @@ TEST(ISAM2, constrained_ordering) init.insert((i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); ++ i; } @@ -713,7 +713,7 @@ TEST(ISAM2, marginalizeLeaves1) constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(2,2)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -744,7 +744,7 @@ TEST(ISAM2, marginalizeLeaves2) constrainedKeys.insert(make_pair(2,2)); constrainedKeys.insert(make_pair(3,3)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -784,7 +784,7 @@ TEST(ISAM2, marginalizeLeaves3) constrainedKeys.insert(make_pair(4,4)); constrainedKeys.insert(make_pair(5,5)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -810,7 +810,7 @@ TEST(ISAM2, marginalizeLeaves4) constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(2,2)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(1); EXPECT(checkMarginalizeLeaves(isam, leafKeys));