Merge remote-tracking branch 'origin/fix/matlab_wrapper' into feature/smartCache

release/4.3a0
dellaert 2016-02-25 22:21:23 -08:00
commit d24b70aee1
7 changed files with 38 additions and 40 deletions

View File

@ -58,12 +58,10 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
# Make CMAKE_BUILD_TYPE=None flags default to the CMAKE_BUILD_TYPE=RelWithDebInfo ones:
set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS})
set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
# Apply the gtsam specific build flags as normal variables. This makes it so that they only # Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject # apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS})
set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})

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 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<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)
{ {

View File

@ -530,7 +530,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 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<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,

View File

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

View File

@ -45,14 +45,14 @@ 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< std::vector<size_t> >& removeFactorIndices) { const boost::optional<KeyVector>& removeFactorIndices) {
gttic(update); gttic(update);
// Create the return result meta-data // Create the return result meta-data
Result result; Result result;
gtsam::FastVector<size_t> removedFactors; FastVector<size_t> removedFactors;
if(removeFactorIndices){ if(removeFactorIndices){
// Be very careful to this line // Be very careful to this line
@ -72,7 +72,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
} }
// Use iSAM2 to perform an update // Use iSAM2 to perform an update
gtsam::ISAM2Result isam2Result; ISAM2Result isam2Result;
if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) { if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) {
if(synchronizationUpdatesAvailable_) { if(synchronizationUpdatesAvailable_) {
// Augment any new factors/values with the cached data from the last synchronization // Augment any new factors/values with the cached data from the last synchronization
@ -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, 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 // Get the set of separator keys
gtsam::KeySet separatorKeys; KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key); separatorKeys.insert(key_value.key);
} }

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< std::vector<size_t> >& removeFactorIndices = boost::none); const boost::optional<KeyVector>& 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.
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 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

View File

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