Merge remote-tracking branch 'origin/fix/matlab_wrapper' into feature/smartCache

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::FactorIndices& 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, 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<Key,int>& constrainedKeys);
   //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);

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<size_t>& newFactorIndices)
+  NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices)
 {
   newFactorIndices.resize(newFactors.size());
 

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<size_t>& newFactorIndices);
+    NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices);
     
   /**
    * Remove variables from the ISAM2 system.

gtsam/nonlinear/ISAM2.cpp

@@ -510,7 +510,7 @@ boost::shared_ptr<KeySet > 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<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys,
     const boost::optional<FastList<Key> >& extraReelimKeys, bool force_relinearize)
 {
@@ -753,8 +753,8 @@ ISAM2Result ISAM2::update(
 
 /* ************************************************************************* */
 void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
-                              boost::optional<std::vector<size_t>&> marginalFactorsIndices,
+                              boost::optional<FactorIndices&> marginalFactorsIndices,
-                              boost::optional<std::vector<size_t>&> deletedFactorsIndices)
+                              boost::optional<FactorIndices&> deletedFactorsIndices)
 {
   // Convert to ordered set
   KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());

gtsam/nonlinear/ISAM2.h

@@ -257,6 +257,7 @@ struct GTSAM_EXPORT ISAM2Params {
   /// @}
 };
 
+typedef FastVector<size_t> 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<size_t> 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<FastMap<Key,int> >& constrainedKeys = boost::none,
       const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
       const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
@@ -551,8 +552,8 @@ public:
    * indices of any factor that was removed during the 'marginalizeLeaves' call
    */
   void marginalizeLeaves(const FastList<Key>& leafKeys,
-    boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
+    boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
-    boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);
+    boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
 
   /// Access the current linearization point
   const Values& getLinearizationPoint() const {

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<FastList<Key> >& keysToMove, const boost::optional< std::vector<size_t> >& removeFactorIndices) {
+    const boost::optional<FastList<Key> >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) {
 
   gttic(update);
 
@@ -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());
@@ -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<size_t> 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<size_t> marginalFactorsIndices;
+    FactorIndices marginalFactorsIndices;
-    std::vector<size_t> deletedFactorsIndices;
+    FactorIndices deletedFactorsIndices;
     isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
     currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
     BOOST_FOREACH(size_t index, deletedFactorsIndices) {
@@ -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;
 
@@ -285,10 +285,10 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons
 }
 
 /* ************************************************************************* */
-std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore) {
+FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore) {
 
   // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
-  std::vector<size_t> removedFactorSlots;
+  FactorIndices removedFactorSlots;
   const VariableIndex& variableIndex = isam2.getVariableIndex();
   BOOST_FOREACH(Key key, keys) {
     const FastVector<size_t>& slots = variableIndex[key];

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<size_t> 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<FastList<Key> >& keysToMove = boost::none,
-      const boost::optional< std::vector<size_t> >& 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<size_t> 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<Key>& additionalKeys);
 
   /** Find the set of iSAM2 factors adjacent to 'keys' */
-  static std::vector<size_t> FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore);
+  static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
 
   /** Update the shortcut marginal between the current separator keys and the previous separator keys */
   // TODO: Make this a static function

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<KeyVector>& removeFactorIndices) {
+    const boost::optional<FactorIndices>& 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);
     }
   }
 

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<KeyVector>& removeFactorIndices = boost::none);
+      const boost::optional<FactorIndices>& 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

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_,

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<Pose3>(1, poseInitial, noisePrior));
   NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
   NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
@@ -1165,7 +1156,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   newValues.insert(3, value3);
 
 
-
     // Create the set of marginalizable variables
   std::vector<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
+  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery)
-  std::vector<size_t> removeFactorIndices(1,1);
+  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<Pose3>(1, poseInitial, noisePrior));
-  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
-  std::vector<size_t> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
-  std::vector<size_t> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
-  std::vector<size_t> removeFactorIndices(1,4);
+  FactorIndices removeFactorIndices(1,4);
 
   // Add no factors to the filter (we only want to test the removal)
   NonlinearFactorGraph noFactors;

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<Pose3>(1, 2, poseOdometry, noiseOdometery);
-  std::vector<size_t> 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

tests/testGaussianISAM2.cpp

@@ -301,9 +301,9 @@ TEST(ISAM2, AddFactorsStep1)
   expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
   expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
 
-  const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
+  const FactorIndices expectedNewFactorIndices = list_of(1)(3);
 
-  FastVector<size_t> 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<size_t> 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<size_t> 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<size_t> 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<size_t>(), 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<size_t>(), 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<size_t>(), 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<size_t>(), 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<size_t>(), constrainedKeys);
+  isam.update(factors, values, FactorIndices(), constrainedKeys);
 
   FastList<Key> 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<size_t>(), constrainedKeys);
+  isam.update(factors, values, FactorIndices(), constrainedKeys);
 
   FastList<Key> 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<size_t>(), constrainedKeys);
+  isam.update(factors, values, FactorIndices(), constrainedKeys);
 
   FastList<Key> 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<size_t>(), constrainedKeys);
+  isam.update(factors, values, FactorIndices(), constrainedKeys);
 
   FastList<Key> leafKeys = list_of(1);
   EXPECT(checkMarginalizeLeaves(isam, leafKeys));