diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 99fa5d598..96a0f11f3 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -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_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 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_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index af37ca954..754623e7e 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -510,7 +510,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const vector& removeFactorIndices, + const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyVector& removeFactorIndices, const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index e8165aad9..a921f6e76 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -530,7 +530,7 @@ public: */ virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const std::vector& removeFactorIndices = std::vector(), + const KeyVector& removeFactorIndices = KeyVector(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 4c4442141..6435bd2df 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -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 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 > orderingConstraints = boost::none; + boost::optional > orderingConstraints = boost::none; if(keysToMove && keysToMove->size() > 0) { - orderingConstraints = gtsam::FastMap(); + orderingConstraints = FastMap(); 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 > noRelinKeys = boost::none; + boost::optional > noRelinKeys = boost::none; // Mark additional keys between the 'keys to move' and the leaves boost::optional > additionalKeys = boost::none; if(keysToMove && keysToMove->size() > 0) { std::set 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 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; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index b87409aae..21b6b3cd0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -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 >& removeFactorIndices) { + const boost::optional& removeFactorIndices) { gttic(update); // Create the return result meta-data Result result; - gtsam::FastVector removedFactors; + FastVector 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(), 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); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 78bab5079..564ed3e52 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -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 >& removeFactorIndices = boost::none); + const boost::optional& 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 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 diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 23cd42a0a..2265cab5e 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -115,10 +115,10 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } // Mark additional keys between the marginalized keys and the leaves - std::set 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 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(), 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& 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& 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 + " "); } }