12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

A better solution through typedef of FactorIndices -> FastVector<size_t>

release/4.3a0
dellaert 2016-02-25 23:51:01 -08:00
parent 64aae16b3c
commit fad9462661
10 changed files with 34 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam.h
View File

@ -2161,6 +2161,8 @@ class ISAM2Result {
size_t getCliques() const; size_t getCliques() const;
}; };
class FactorIndices {};
class ISAM2 { class ISAM2 {
ISAM2(); ISAM2();
ISAM2(const gtsam::ISAM2Params& params); ISAM2(const gtsam::ISAM2Params& params);
@ -2173,8 +2175,8 @@ class ISAM2 {
gtsam::ISAM2Result update(); 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);
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 // 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);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize); //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);

2
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -46,7 +46,7 @@ void ISAM2::Impl::AddVariables(
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices) NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices)
{ {
newFactorIndices.resize(newFactors.size()); newFactorIndices.resize(newFactors.size());

2
gtsam/nonlinear/ISAM2-impl.h
View File

@ -52,7 +52,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
/// complete list of nonlinear factors, and populates the list of new factor indices, both /// complete list of nonlinear factors, and populates the list of new factor indices, both
/// optionally finding and reusing empty factor slots. /// optionally finding and reusing empty factor slots.
static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices); NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices);
/** /**
* Remove variables from the ISAM2 system. * Remove variables from the ISAM2 system.

6
gtsam/nonlinear/ISAM2.cpp
View File

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

9
gtsam/nonlinear/ISAM2.h
View File

@ -257,6 +257,7 @@ struct GTSAM_EXPORT ISAM2Params {
/// @} /// @}
}; };
typedef FastVector<size_t> FactorIndices;
/** /**
* @addtogroup ISAM2 * @addtogroup ISAM2
@ -318,7 +319,7 @@ struct GTSAM_EXPORT ISAM2Result {
* factors passed as \c newFactors to ISAM2::update(). These indices may be * factors passed as \c newFactors to ISAM2::update(). These indices may be
* used later to refer to the factors in order to remove them. * 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 /** A struct holding detailed results, which must be enabled with
* ISAM2Params::enableDetailedResults. * ISAM2Params::enableDetailedResults.
@ -530,7 +531,7 @@ public:
*/ */
virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(), const Values& newTheta = Values(),
const KeyVector& removeFactorIndices = KeyVector(), const FactorIndices& removeFactorIndices = FactorIndices(),
const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none, const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
const boost::optional<FastList<Key> >& noRelinKeys = boost::none, const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
const boost::optional<FastList<Key> >& extraReelimKeys = 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 * indices of any factor that was removed during the 'marginalizeLeaves' call
*/ */
void marginalizeLeaves(const FastList<Key>& leafKeys, 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 /// Access the current linearization point
const Values& getLinearizationPoint() const { const Values& getLinearizationPoint() const {

4
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -56,7 +56,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
Result result; Result result;
// We do not need to remove any factors at this time // We do not need to remove any factors at this time
KeyVector removedFactors; FactorIndices removedFactors;
if(removeFactorIndices){ if(removeFactorIndices){
removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
@ -210,7 +210,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
isam2_.params().getEliminationFunction()); isam2_.params().getEliminationFunction());
// Remove the old factors on the separator and insert the new ones // 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); ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false);
currentSmootherSummarizationSlots_ = result.newFactorsIndices; currentSmootherSummarizationSlots_ = result.newFactorsIndices;

4
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -45,7 +45,7 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double
/* ************************************************************************* */ /* ************************************************************************* */
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional<KeyVector>& removeFactorIndices) { const boost::optional<FactorIndices>& removeFactorIndices) {
gttic(update); gttic(update);
@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
synchronizationUpdatesAvailable_ = false; synchronizationUpdatesAvailable_ = false;
} else { } else {
// Update the system using iSAM2 // Update the system using iSAM2
isam2Result = isam2_.update(newFactors, newTheta, KeyVector(), constrainedKeys, noRelinKeys); isam2Result = isam2_.update(newFactors, newTheta, FactorIndices(), constrainedKeys, noRelinKeys);
} }
} }

4
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
View File

@ -109,7 +109,7 @@ public:
* and additionally, variables that were already in the system must not be included here. * and additionally, variables that were already in the system must not be included here.
*/ */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), 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. * 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 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 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. 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 bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet
// Storage for information to be sent to the filter // Storage for information to be sent to the filter

2
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
// Update iSAM2 // Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta, ISAM2Result isamResult = isam_.update(newFactors, newTheta,
KeyVector(), constrainedKeys, boost::none, additionalMarkedKeys); FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) { if (debug) {
PrintSymbolicTree(isam_, PrintSymbolicTree(isam_,

26
tests/testGaussianISAM2.cpp
View File

@ -301,9 +301,9 @@ TEST(ISAM2, AddFactorsStep1)
expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model); expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
expectedNonlinearFactors += PriorFactor<LieScalar>(2, 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); 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)); ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Remove the 2nd measurement on landmark 0 (Key 100) // Remove the 2nd measurement on landmark 0 (Key 100)
FastVector<size_t> toRemove; FactorIndices toRemove;
toRemove.push_back(12); toRemove.push_back(12);
isam.update(NonlinearFactorGraph(), Values(), toRemove); 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)); ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Remove the measurement on landmark 0 (Key 100) // Remove the measurement on landmark 0 (Key 100)
FastVector<size_t> toRemove; FactorIndices toRemove;
toRemove.push_back(7); toRemove.push_back(7);
toRemove.push_back(14); toRemove.push_back(14);
isam.update(NonlinearFactorGraph(), Values(), toRemove); isam.update(NonlinearFactorGraph(), Values(), toRemove);
@ -466,7 +466,7 @@ TEST(ISAM2, swapFactors)
// Remove the measurement on landmark 0 and replace with a different one // Remove the measurement on landmark 0 and replace with a different one
{ {
size_t swap_idx = isam.getFactorsUnsafe().size()-2; size_t swap_idx = isam.getFactorsUnsafe().size()-2;
FastVector<size_t> toRemove; FactorIndices toRemove;
toRemove.push_back(swap_idx); toRemove.push_back(swap_idx);
fullgraph.remove(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)); fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
if(i >= 3) if(i >= 3)
isam.update(newfactors, init, FastVector<size_t>(), constrained); isam.update(newfactors, init, FactorIndices(), constrained);
else else
isam.update(newfactors, init); 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(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))); 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; ++ i;
} }
@ -584,7 +584,7 @@ TEST(ISAM2, constrained_ordering)
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); 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)); 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 // 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)); init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.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; ++ i;
} }
@ -713,7 +713,7 @@ TEST(ISAM2, marginalizeLeaves1)
constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2)); 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); FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -744,7 +744,7 @@ TEST(ISAM2, marginalizeLeaves2)
constrainedKeys.insert(make_pair(2,2)); constrainedKeys.insert(make_pair(2,2));
constrainedKeys.insert(make_pair(3,3)); 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); FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -784,7 +784,7 @@ TEST(ISAM2, marginalizeLeaves3)
constrainedKeys.insert(make_pair(4,4)); constrainedKeys.insert(make_pair(4,4));
constrainedKeys.insert(make_pair(5,5)); 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); FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -810,7 +810,7 @@ TEST(ISAM2, marginalizeLeaves4)
constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2)); 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); FastList<Key> leafKeys = list_of(1);
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));