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;
};
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<Key,int>& constrainedKeys);
//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,
NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices)
NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices)
{
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
/// 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.

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(
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<std::vector<size_t>&> deletedFactorsIndices)
boost::optional<FactorIndices&> marginalFactorsIndices,
boost::optional<FactorIndices&> deletedFactorsIndices)
{
// Convert to ordered set
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
@ -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<std::vector<size_t>&> deletedFactorsIndices = boost::none);
boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
/// Access the current linearization point
const Values& getLinearizationPoint() const {

4
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -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;

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,
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);
}
}

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.
*/
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

2
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

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

26
tests/testGaussianISAM2.cpp
View File

@ -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));