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

4
gtsam_unstable/CMakeLists.txt
View File

@ -19,8 +19,8 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail
# Sources to remove from builds
set (excluded_sources # "")
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
)

50
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -93,15 +93,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
std::set<Key> markedKeys;
BOOST_FOREACH(gtsam::Key key, *keysToMove) {
if(isam2_.getLinearizationPoint().exists(key)) {
Index index = isam2_.getOrdering().at(key);
ISAM2Clique::shared_ptr clique = isam2_[index];
GaussianConditional::const_iterator index_iter = clique->conditional()->begin();
while(*index_iter != index) {
markedKeys.insert(isam2_.getOrdering().key(*index_iter));
++index_iter;
ISAM2Clique::shared_ptr clique = isam2_[key];
GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
while(*key_iter != key) {
markedKeys.insert(*key_iter);
++key_iter;
}
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) {
RecursiveMarkAffectedKeys(index, child, isam2_.getOrdering(), markedKeys);
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(key, child, markedKeys);
}
}
}
@ -261,20 +260,21 @@ void ConcurrentIncrementalFilter::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
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
BOOST_FOREACH(Index idx, clique->conditional()->frontals()) {
additionalKeys.insert(ordering.key(idx));
BOOST_FOREACH(Key idx, clique->conditional()->frontals()) {
additionalKeys.insert(idx);
}
// Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
RecursiveMarkAffectedKeys(index, child, ordering, additionalKeys);
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
// 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;
const VariableIndex& variableIndex = isam2.getVariableIndex();
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());
}
@ -345,27 +345,23 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques;
BOOST_FOREACH(Key key, separatorKeys) {
Index index = isam2_.getOrdering().at(key);
ISAM2Clique::shared_ptr clique = isam2_[index];
ISAM2Clique::shared_ptr clique = isam2_[key];
separatorCliques.insert( clique );
}
// Create the set of clique keys
std::vector<Index> cliqueIndices;
// Create the set of clique keys LC:
std::vector<Key> cliqueKeys;
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
BOOST_FOREACH(Index index, clique->conditional()->frontals()) {
cliqueIndices.push_back(index);
cliqueKeys.push_back(isam2_.getOrdering().key(index));
BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
cliqueKeys.push_back(key);
}
}
std::sort(cliqueIndices.begin(), cliqueIndices.end());
std::sort(cliqueKeys.begin(), cliqueKeys.end());
// Gather all factors that involve only clique keys
std::set<size_t> cliqueFactorSlots;
BOOST_FOREACH(Index index, cliqueIndices) {
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) {
BOOST_FOREACH(Key key, cliqueKeys) {
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) {
std::set<Key> factorKeys(factor->begin(), factor->end());
@ -391,7 +387,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
std::set<ISAM2Clique::shared_ptr> childCliques;
// Add all of the children
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
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
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 );
}

7
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -73,11 +73,6 @@ public:
return isam2_.getLinearizationPoint();
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return isam2_.getOrdering();
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return isam2_.getDelta();
@ -171,7 +166,7 @@ protected:
private:
/** 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' */
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 {
/* ************************************************************************* */
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
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
BOOST_FOREACH(Index i, clique->conditional()->frontals()) {
additionalKeys.insert(ordering.key(i));
BOOST_FOREACH(Key i, clique->conditional()->frontals()) {
additionalKeys.insert(i);
}
// Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
recursiveMarkAffectedKeys(index, child, ordering, additionalKeys);
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
// 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
std::set<gtsam::Key> additionalKeys;
BOOST_FOREACH(gtsam::Key key, marginalizableKeys) {
gtsam::Index index = isam_.getOrdering().at(key);
gtsam::ISAM2Clique::shared_ptr clique = isam_[index];
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) {
recursiveMarkAffectedKeys(index, child, isam_.getOrdering(), additionalKeys);
gtsam::ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
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(";
BOOST_FOREACH(Index index, factor->keys()) {
std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]";
BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
}
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;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor, ordering);
PrintSymbolicFactor(factor);
}
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) {
std::cout << label << std::endl;
if(isam.root())
PrintSymbolicTreeHelper(isam.root(), isam.getOrdering());
if(!isam.roots().empty())
{
BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){
PrintSymbolicTreeHelper(root);
}
}
else
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
std::cout << indent << "P( ";
BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) {
std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
}
if(clique->conditional()->nrParents() > 0) std::cout << "| ";
BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) {
std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
// Recursively print all of the children
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) {
PrintSymbolicTreeHelper(child, ordering, indent+" ");
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
PrintSymbolicTreeHelper(child, indent+" ");
}
}

11
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -89,11 +89,6 @@ public:
return isam_.getLinearizationPoint();
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return isam_.getOrdering();
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return isam_.getDelta();
@ -113,10 +108,10 @@ protected:
private:
/** Private methods for printing debug information */
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 PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
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 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

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

@ -83,7 +83,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
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));
newTimestamps[key0] = 0.0;
@ -108,7 +108,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
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));
newTimestamps[key2] = double(i);
@ -134,8 +134,8 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
Timestamps newTimestamps;
newFactors.add(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>(key1, key2, Point2(1.0, 0.0), odometerNoise));
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));
newTimestamps[key2] = double(i);
@ -160,7 +160,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
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));
newTimestamps[key2] = double(i);