Merge remote-tracking branch 'origin/fix/matlab_wrapper' into feature/smartCache
commit
d24b70aee1
|
@ -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})
|
||||||
|
|
|
@ -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)
|
||||||
{
|
{
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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 + " ");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue