From fad9462661d1f1ad651e91068f532398b7cad434 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 23:51:01 -0800 Subject: [PATCH 1/3] 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)); From 2ca649a11f3a0e03ab0c58c207f74f5bb61ce827 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 00:01:59 -0800 Subject: [PATCH 2/3] Made some type changes to FactorIndices --- .../nonlinear/ConcurrentIncrementalFilter.cpp | 12 ++++++------ .../nonlinear/ConcurrentIncrementalFilter.h | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index f0c7212c8..1b7f86d5c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol /* ************************************************************************* */ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional >& keysToMove, const boost::optional< std::vector >& removeFactorIndices) { + const boost::optional >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) { gttic(update); @@ -117,7 +117,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No gttic(cache_smoother_factors); // Find the set of factors that will be removed - std::vector removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); + FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); // Cache these factors for later transmission to the smoother NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { @@ -134,8 +134,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No gttoc(cache_smoother_factors); gttic(marginalize); - std::vector marginalFactorsIndices; - std::vector deletedFactorsIndices; + FactorIndices marginalFactorsIndices; + FactorIndices deletedFactorsIndices; isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices); currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end()); BOOST_FOREACH(size_t index, deletedFactorsIndices) { @@ -285,10 +285,10 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons } /* ************************************************************************* */ -std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore) { +FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const FactorIndices& factorsToIgnore) { // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) - std::vector removedFactorSlots; + FactorIndices removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { const FastVector& slots = variableIndex[key]; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 11012674e..c28b3bcd1 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -46,7 +46,7 @@ public: * factors passed as \c newFactors update(). These indices may be * used later to refer to the factors in order to remove them. */ - std::vector newFactorsIndices; + FactorIndices newFactorsIndices; double error; ///< The final factor graph error @@ -124,7 +124,7 @@ public: */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none, - const boost::optional< std::vector >& removeFactorIndices = boost::none); + const boost::optional< FactorIndices >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -170,7 +170,7 @@ protected: // ??? NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization - std::vector currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator + FactorIndices currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) // Storage for information to be sent to the smoother @@ -183,7 +183,7 @@ private: static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys); /** Find the set of iSAM2 factors adjacent to 'keys' */ - static std::vector FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore); + static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const FactorIndices& factorsToIgnore); /** Update the shortcut marginal between the current separator keys and the previous separator keys */ // TODO: Make this a static function From 853b5192a54d522b33200f378c72ca6fb5cd1c97 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 07:32:32 -0800 Subject: [PATCH 3/3] Changed to more types to FactorIndices --- .../tests/testConcurrentIncrementalFilter.cpp | 30 +++++-------------- .../testConcurrentIncrementalSmootherGN.cpp | 9 +----- 2 files changed, 8 insertions(+), 31 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 08dbc311c..a283ece29 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -559,7 +559,6 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_0 ) { - std::cout << "*********************** synchronize_0 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; @@ -593,7 +592,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_0 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_1 ) { - std::cout << "*********************** synchronize_1 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -641,7 +639,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_2 ) { - std::cout << "*********************** synchronize_2 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -712,7 +709,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_2 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_3 ) { - std::cout << "*********************** synchronize_3 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -800,7 +796,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_4 ) { - std::cout << "*********************** synchronize_4 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -896,7 +891,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_5 ) { - std::cout << "*********************** synchronize_5 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -1092,7 +1086,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) { - std::cout << "*********************** CalculateMarginals_1 ************************" << std::endl; // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals NonlinearFactor::shared_ptr factor1(new PriorFactor(1, poseInitial, noisePrior)); NonlinearFactor::shared_ptr factor2(new BetweenFactor(1, 2, poseOdometry, noiseOdometery)); @@ -1141,8 +1134,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) { - std::cout << "*********************** CalculateMarginals_2 ************************" << std::endl; - // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals NonlinearFactor::shared_ptr factor1(new PriorFactor(1, poseInitial, noisePrior)); NonlinearFactor::shared_ptr factor2(new BetweenFactor(1, 2, poseOdometry, noiseOdometery)); @@ -1165,7 +1156,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) newValues.insert(3, value3); - // Create the set of marginalizable variables std::vector linearIndices; linearIndices.push_back(1); @@ -1190,8 +1180,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) // Check CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6)); -// actualMarginals.print("actualMarginals \n"); -// expectedMarginals.print("expectedMarginals \n"); } @@ -1199,8 +1187,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) { - std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; - // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -1233,8 +1219,9 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected - // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,1); + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery) + FactorIndices removeFactorIndices; + removeFactorIndices.push_back(1); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1245,7 +1232,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph expectedGraph; expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); @@ -1258,7 +1245,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) { - std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; // we try removing the last factor ISAM2Params parameters; @@ -1293,7 +1279,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,4); + FactorIndices removeFactorIndices(1,4); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1318,7 +1304,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) { - std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; // we try removing the first factor ISAM2Params parameters; @@ -1353,7 +1338,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,0); + FactorIndices removeFactorIndices(1,0); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1376,7 +1361,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_values ) { - std::cout << "*********************** removeFactors_values ************************" << std::endl; // we try removing the last factor ISAM2Params parameters; @@ -1411,7 +1395,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,4); + FactorIndices removeFactorIndices(1,4); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index c372577ca..858fbb75c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -600,8 +600,6 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) { - std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; - // Create a set of optimizer parameters ISAM2Params parameters; parameters.optimizationParams = ISAM2GaussNewtonParams(); @@ -629,9 +627,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(2,1); - - + FactorIndices removeFactorIndices(2,1); // Add no factors to the smoother (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -657,7 +653,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 ) //{ -// std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; // // we try removing the last factor // // // Create a set of optimizer parameters @@ -711,7 +706,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) //{ -// std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; // // we try removing the first factor // // // Create a set of optimizer parameters @@ -761,7 +755,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentBatchSmoother, removeFactors_values ) //{ -// std::cout << "*********************** removeFactors_values ************************" << std::endl; // // we try removing the last factor // // // Create a set of optimizer parameters