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

Updating ConcurrentIncrementalSmoother/Filter to work with unordered (work in progress)

release/4.3a0
Luca Carlone 2013-08-14 19:12:24 +00:00
parent 0514ad39c6
commit e4f65eacf8
6 changed files with 61 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -93,15 +93,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
std::set<Key> markedKeys; std::set<Key> markedKeys;
BOOST_FOREACH(gtsam::Key key, *keysToMove) { BOOST_FOREACH(gtsam::Key key, *keysToMove) {
if(isam2_.getLinearizationPoint().exists(key)) { if(isam2_.getLinearizationPoint().exists(key)) {
Index index = isam2_.getOrdering().at(key); ISAM2Clique::shared_ptr clique = isam2_[key];
ISAM2Clique::shared_ptr clique = isam2_[index]; GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
GaussianConditional::const_iterator index_iter = clique->conditional()->begin(); while(*key_iter != key) {
while(*index_iter != index) { markedKeys.insert(*key_iter);
markedKeys.insert(isam2_.getOrdering().key(*index_iter)); ++key_iter;
++index_iter;
} }
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(index, child, isam2_.getOrdering(), markedKeys); RecursiveMarkAffectedKeys(key, child, markedKeys);
} }
} }
} }
@ -261,20 +260,21 @@ void ConcurrentIncrementalFilter::postsync() {
gttoc(postsync); gttoc(postsync);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys) { void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
// Check if the separator keys of the current clique contain the specified key // Check if the separator keys of the current clique contain the specified key
if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique // Mark the frontal keys of the current clique
BOOST_FOREACH(Index idx, clique->conditional()->frontals()) { BOOST_FOREACH(Key idx, clique->conditional()->frontals()) {
additionalKeys.insert(ordering.key(idx)); additionalKeys.insert(idx);
} }
// Recursively mark all of the children // Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(index, child, ordering, additionalKeys); RecursiveMarkAffectedKeys(key, child, additionalKeys);
} }
} }
// If the key was not found in the separator/parents, then none of its children can have it either // If the key was not found in the separator/parents, then none of its children can have it either
@ -288,7 +288,7 @@ std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2
std::vector<size_t> removedFactorSlots; std::vector<size_t> removedFactorSlots;
const VariableIndex& variableIndex = isam2.getVariableIndex(); const VariableIndex& variableIndex = isam2.getVariableIndex();
BOOST_FOREACH(Key key, keys) { BOOST_FOREACH(Key key, keys) {
const FastList<size_t>& slots = variableIndex[isam2.getOrdering().at(key)]; const FastList<size_t>& slots = variableIndex[key];
removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
} }
@ -345,27 +345,23 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// Find all cliques that contain any separator variables // Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques; std::set<ISAM2Clique::shared_ptr> separatorCliques;
BOOST_FOREACH(Key key, separatorKeys) { BOOST_FOREACH(Key key, separatorKeys) {
Index index = isam2_.getOrdering().at(key); ISAM2Clique::shared_ptr clique = isam2_[key];
ISAM2Clique::shared_ptr clique = isam2_[index];
separatorCliques.insert( clique ); separatorCliques.insert( clique );
} }
// Create the set of clique keys // Create the set of clique keys LC:
std::vector<Index> cliqueIndices;
std::vector<Key> cliqueKeys; std::vector<Key> cliqueKeys;
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
BOOST_FOREACH(Index index, clique->conditional()->frontals()) { BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
cliqueIndices.push_back(index); cliqueKeys.push_back(key);
cliqueKeys.push_back(isam2_.getOrdering().key(index));
} }
} }
std::sort(cliqueIndices.begin(), cliqueIndices.end());
std::sort(cliqueKeys.begin(), cliqueKeys.end()); std::sort(cliqueKeys.begin(), cliqueKeys.end());
// Gather all factors that involve only clique keys // Gather all factors that involve only clique keys
std::set<size_t> cliqueFactorSlots; std::set<size_t> cliqueFactorSlots;
BOOST_FOREACH(Index index, cliqueIndices) { BOOST_FOREACH(Key key, cliqueKeys) {
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) { BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
std::set<Key> factorKeys(factor->begin(), factor->end()); std::set<Key> factorKeys(factor->begin(), factor->end());
@ -391,7 +387,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
std::set<ISAM2Clique::shared_ptr> childCliques; std::set<ISAM2Clique::shared_ptr> childCliques;
// Add all of the children // Add all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
childCliques.insert(clique->children().begin(), clique->children().end()); childCliques.insert(clique->children.begin(), clique->children.end());
} }
// Remove any separator cliques that were added because they were children of other separator cliques // Remove any separator cliques that were added because they were children of other separator cliques
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
@ -400,7 +396,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// Augment the factor graph with cached factors from the children // Augment the factor graph with cached factors from the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) {
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint())); LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
graph.push_back( factor ); graph.push_back( factor );
} }

7
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -73,11 +73,6 @@ public:
return isam2_.getLinearizationPoint(); return isam2_.getLinearizationPoint();
} }
/** Access the current ordering */
const Ordering& getOrdering() const {
return isam2_.getOrdering();
}
/** Access the current set of deltas to the linearization point */ /** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const { const VectorValues& getDelta() const {
return isam2_.getDelta(); return isam2_.getDelta();
@ -171,7 +166,7 @@ protected:
private: private:
/** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */ /** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */
static void RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys); static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
/** Find the set of iSAM2 factors adjacent to 'keys' */ /** 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 std::vector<size_t> FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore);

51
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -25,19 +25,19 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys) { void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
// Check if the separator keys of the current clique contain the specified key // Check if the separator keys of the current clique contain the specified key
if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique // Mark the frontal keys of the current clique
BOOST_FOREACH(Index i, clique->conditional()->frontals()) { BOOST_FOREACH(Key i, clique->conditional()->frontals()) {
additionalKeys.insert(ordering.key(i)); additionalKeys.insert(i);
} }
// Recursively mark all of the children // Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
recursiveMarkAffectedKeys(index, child, ordering, additionalKeys); recursiveMarkAffectedKeys(key, child, additionalKeys);
} }
} }
// If the key was not found in the separator/parents, then none of its children can have it either // If the key was not found in the separator/parents, then none of its children can have it either
@ -106,10 +106,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
// Mark additional keys between the marginalized keys and the leaves // Mark additional keys between the marginalized keys and the leaves
std::set<gtsam::Key> additionalKeys; std::set<gtsam::Key> additionalKeys;
BOOST_FOREACH(gtsam::Key key, marginalizableKeys) { BOOST_FOREACH(gtsam::Key key, marginalizableKeys) {
gtsam::Index index = isam_.getOrdering().at(key); gtsam::ISAM2Clique::shared_ptr clique = isam_[key];
gtsam::ISAM2Clique::shared_ptr clique = isam_[index]; BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { recursiveMarkAffectedKeys(key, child, additionalKeys);
recursiveMarkAffectedKeys(index, child, isam_.getOrdering(), additionalKeys);
} }
} }
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
@ -184,48 +183,52 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const s
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) { void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
BOOST_FOREACH(Index index, factor->keys()) { BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; std::cout << " " << gtsam::DefaultKeyFormatter(key);
} }
std::cout << " )" << std::endl; std::cout << " )" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) { void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor, ordering); PrintSymbolicFactor(factor);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
if(isam.root()) if(!isam.roots().empty())
PrintSymbolicTreeHelper(isam.root(), isam.getOrdering()); {
BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){
PrintSymbolicTreeHelper(root);
}
}
else else
std::cout << "{Empty Tree}" << std::endl; std::cout << "{Empty Tree}" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) { void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) {
// Print the current clique // Print the current clique
std::cout << indent << "P( "; std::cout << indent << "P( ";
BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) {
std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; std::cout << gtsam::DefaultKeyFormatter(key) << " ";
} }
if(clique->conditional()->nrParents() > 0) std::cout << "| "; if(clique->conditional()->nrParents() > 0) std::cout << "| ";
BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) {
std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; std::cout << gtsam::DefaultKeyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
// Recursively print all of the children // Recursively print all of the children
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
PrintSymbolicTreeHelper(child, ordering, indent+" "); PrintSymbolicTreeHelper(child, indent+" ");
} }
} }

11
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -89,11 +89,6 @@ public:
return isam_.getLinearizationPoint(); return isam_.getLinearizationPoint();
} }
/** Access the current ordering */
const Ordering& getOrdering() const {
return isam_.getOrdering();
}
/** Access the current set of deltas to the linearization point */ /** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const { const VectorValues& getDelta() const {
return isam_.getDelta(); return isam_.getDelta();
@ -113,10 +108,10 @@ protected:
private: private:
/** Private methods for printing debug information */ /** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label = "Keys:"); static void PrintKeySet(const std::set<Key>& keys, const std::string& label = "Keys:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:"); static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = ""); static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = "");
}; // IncrementalFixedLagSmoother }; // IncrementalFixedLagSmoother

10
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -83,7 +83,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues; Values newValues;
Timestamps newTimestamps; Timestamps newTimestamps;
newFactors.add(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise)); newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
newValues.insert(key0, Point2(0.01, 0.01)); newValues.insert(key0, Point2(0.01, 0.01));
newTimestamps[key0] = 0.0; newTimestamps[key0] = 0.0;
@ -108,7 +108,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues; Values newValues;
Timestamps newTimestamps; Timestamps newTimestamps;
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise)); newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i); newTimestamps[key2] = double(i);
@ -134,8 +134,8 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues; Values newValues;
Timestamps newTimestamps; Timestamps newTimestamps;
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise)); newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.add(BetweenFactor<Point2>(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); newFactors.push_back(BetweenFactor<Point2>(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i); newTimestamps[key2] = double(i);
@ -160,7 +160,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues; Values newValues;
Timestamps newTimestamps; Timestamps newTimestamps;
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise)); newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i); newTimestamps[key2] = double(i);