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

Fixed problem with wrapping iSAM by a hack: made vector<size_t> into KeyVector. This is not a long-term solution: we should enable wrapping of vector<T>.

release/4.3a0
dellaert 2016-02-25 22:07:04 -08:00
parent 066402ec35
commit 64aae16b3c
6 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
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 vector<size_t>& removeFactorIndices,
const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyVector& removeFactorIndices,
const boost::optional<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys,
const boost::optional<FastList<Key> >& extraReelimKeys, bool force_relinearize)
{

2
gtsam/nonlinear/ISAM2.h
View File

@ -530,7 +530,7 @@ public:
*/
virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const std::vector<size_t>& removeFactorIndices = std::vector<size_t>(),
const KeyVector& removeFactorIndices = KeyVector(),
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,

22
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -56,43 +56,43 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
Result result;
// We do not need to remove any factors at this time
gtsam::FastVector<size_t> removedFactors;
KeyVector removedFactors;
if(removeFactorIndices){
removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
}
// Generate ordering constraints that force the 'keys to move' to the end
boost::optional<gtsam::FastMap<gtsam::Key,int> > orderingConstraints = boost::none;
boost::optional<FastMap<Key,int> > orderingConstraints = boost::none;
if(keysToMove && keysToMove->size() > 0) {
orderingConstraints = gtsam::FastMap<gtsam::Key,int>();
orderingConstraints = FastMap<Key,int>();
int group = 1;
// Set all existing variables to Group1
if(isam2_.getLinearizationPoint().size() > 0) {
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
orderingConstraints->operator[](key_value.key) = group;
}
++group;
}
// Assign new variables to the root
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, newTheta) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
orderingConstraints->operator[](key_value.key) = group;
}
// Set marginalizable variables to Group0
BOOST_FOREACH(gtsam::Key key, *keysToMove){
BOOST_FOREACH(Key key, *keysToMove){
orderingConstraints->operator[](key) = 0;
}
}
// Create the set of linear keys that iSAM2 should hold constant
// iSAM2 takes care of this for us; no need to specify additional noRelin keys
boost::optional<gtsam::FastList<gtsam::Key> > noRelinKeys = boost::none;
boost::optional<FastList<Key> > noRelinKeys = boost::none;
// Mark additional keys between the 'keys to move' and the leaves
boost::optional<FastList<Key> > additionalKeys = boost::none;
if(keysToMove && keysToMove->size() > 0) {
std::set<Key> markedKeys;
BOOST_FOREACH(gtsam::Key key, *keysToMove) {
BOOST_FOREACH(Key key, *keysToMove) {
if(isam2_.getLinearizationPoint().exists(key)) {
ISAM2Clique::shared_ptr clique = isam2_[key];
GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
markedKeys.insert(*key_iter);
++key_iter;
}
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(key, child, markedKeys);
}
}
@ -110,7 +110,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
// Update the system using iSAM2
gttic(isam2);
gtsam::ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
gttoc(isam2);
if(keysToMove && keysToMove->size() > 0) {
@ -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
FastVector<size_t> removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
KeyVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false);
currentSmootherSummarizationSlots_ = result.newFactorsIndices;

10
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -45,14 +45,14 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double
/* ************************************************************************* */
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional< std::vector<size_t> >& removeFactorIndices) {
const boost::optional<KeyVector>& removeFactorIndices) {
gttic(update);
// Create the return result meta-data
Result result;
gtsam::FastVector<size_t> removedFactors;
FastVector<size_t> removedFactors;
if(removeFactorIndices){
// Be very careful to this line
@ -72,7 +72,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
}
// Use iSAM2 to perform an update
gtsam::ISAM2Result isam2Result;
ISAM2Result isam2Result;
if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) {
if(synchronizationUpdatesAvailable_) {
// Augment any new factors/values with the cached data from the last synchronization
@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
synchronizationUpdatesAvailable_ = false;
} else {
// Update the system using iSAM2
isam2Result = isam2_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys, noRelinKeys);
isam2Result = isam2_.update(newFactors, newTheta, KeyVector(), constrainedKeys, noRelinKeys);
}
}
@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
}
// Get the set of separator keys
gtsam::KeySet separatorKeys;
KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key);
}

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< std::vector<size_t> >& removeFactorIndices = boost::none);
const boost::optional<KeyVector>& 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.
FastVector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
KeyVector 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

32
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -115,10 +115,10 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
}
// Mark additional keys between the marginalized keys and the leaves
std::set<gtsam::Key> additionalKeys;
BOOST_FOREACH(gtsam::Key key, marginalizableKeys) {
gtsam::ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
std::set<Key> additionalKeys;
BOOST_FOREACH(Key key, marginalizableKeys) {
ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
// Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta,
FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);
KeyVector(), constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) {
PrintSymbolicTree(isam_,
@ -194,8 +194,8 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) {
std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
BOOST_FOREACH(Key key, keys) {
std::cout << " " << DefaultKeyFormatter(key);
}
std::cout << std::endl;
}
@ -204,8 +204,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
void IncrementalFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) {
std::cout << "f(";
BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
BOOST_FOREACH(Key key, factor->keys()) {
std::cout << " " << DefaultKeyFormatter(key);
}
std::cout << " )" << std::endl;
}
@ -220,7 +220,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam,
const std::string& label) {
std::cout << label << std::endl;
if (!isam.roots().empty()) {
@ -233,22 +233,22 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) {
const ISAM2Clique::shared_ptr& clique, const std::string indent) {
// Print the current clique
std::cout << indent << "P( ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
if (clique->conditional()->nrParents() > 0)
std::cout << "| ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
BOOST_FOREACH(Key key, clique->conditional()->parents()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
// Recursively print all of the children
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
PrintSymbolicTreeHelper(child, indent + " ");
}
}