From 55ed99141e620742ffdfc0e0723dae535dc4593c Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 20 Oct 2015 13:36:26 -0400 Subject: [PATCH 1/6] Add C++11 compiler flag to GtsamBuildTypes.cmake --- cmake/GtsamBuildTypes.cmake | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index c2cd7b449..43ae36929 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -34,19 +34,19 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING) From e694d62b3fb3ac5f8abe6d622c3576e0bdeb96f1 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 20 Oct 2015 14:41:28 -0400 Subject: [PATCH 2/6] Allow binding to optional rvalues (We should fix this properly) --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b168077b3..0380b8a2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,7 +131,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES) + add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### From 04bcf26aa6314408f93e95325950597e23e4a428 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 20 Oct 2015 14:44:00 -0400 Subject: [PATCH 3/6] Explicitly cast optional to bool --- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 5146c5a32..13a4dd38f 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -246,7 +246,7 @@ public: /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ - return (result_); + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e7118a36c..6651c005f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -314,7 +314,7 @@ public: /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ - return (result_); + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) @@ -583,7 +583,7 @@ public: /// Is result valid? bool isValid() const { - return result_; + return bool(result_); } /** return the degenerate state */ From 1949fc251183e66f82a70a7c5b3e9d2df67b7b0a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 20 Oct 2015 16:25:41 -0400 Subject: [PATCH 4/6] Cache noise models in LM damping instead of constructing from scratch for each variable. Time savings of 5.6%! --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index cacb0a1ff..960ba59dd 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -189,11 +189,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( } } else { // Straightforward damping: + typedef map NoiseMap; // Cache noise models + NoiseMap noises; BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { size_t dim = key_value.value.dim(); Matrix A = Matrix::Identity(dim, dim); Vector b = Vector::Zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + + // Check if noise model of appropriate size already exists, else create it and cache it! + NoiseMap::iterator it = noises.find(dim); + SharedDiagonal model = it == noises.end() ? noiseModel::Isotropic::Sigma(dim, sigma) : it->second; + if(it == noises.end()) { + noises[dim] = model; + } damped += boost::make_shared(key_value.key, A, b, model); } } From 7e462b997f3ba259bd1b7fa0f9e87d40ccd62674 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 25 Oct 2015 16:55:42 -0400 Subject: [PATCH 5/6] Cache A and b in addition to noise model for damped system --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 17 +++++++++++------ gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 11 +++++++++++ 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 960ba59dd..50f4a0838 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -189,20 +189,25 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( } } else { // Straightforward damping: - typedef map NoiseMap; // Cache noise models + NoiseMap noises; BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { size_t dim = key_value.value.dim(); - Matrix A = Matrix::Identity(dim, dim); - Vector b = Vector::Zero(dim); // Check if noise model of appropriate size already exists, else create it and cache it! NoiseMap::iterator it = noises.find(dim); - SharedDiagonal model = it == noises.end() ? noiseModel::Isotropic::Sigma(dim, sigma) : it->second; if(it == noises.end()) { - noises[dim] = model; + NoiseCacheItem item; + item.A = Matrix::Identity(dim, dim); + item.b = Vector::Zero(dim); + item.model = noiseModel::Isotropic::Sigma(dim, sigma); + noises[dim] = item; + damped += boost::make_shared(key_value.key, item.A, item.b, item.model); + + } else { + const NoiseCacheItem& item = it->second; + damped += boost::make_shared(key_value.key, item.A, item.b, item.model); } - damped += boost::make_shared(key_value.key, A, b, model); } } gttoc(damp); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index a965c6cf0..1a4169e16 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -272,6 +272,17 @@ public: GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + /** Small struct to cache objects needed for damping. + * This is used in buildDampedSystem */ + struct NoiseCacheItem { + Matrix A; + Vector b; + SharedDiagonal model; + }; + + /// Noise model Cache + typedef std::map NoiseMap; + void writeLogFile(double currentError); /// @} From 44aaf9ad956d3bceb95e45128e7d973578d32d28 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 25 Oct 2015 18:31:44 -0400 Subject: [PATCH 6/6] Switch to vector for noise model caching --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 23 +++++++++---------- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 50f4a0838..9e42afa33 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -190,24 +190,23 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( } else { // Straightforward damping: - NoiseMap noises; + // initialize noise model cache to a reasonable default size + NoiseCacheVector noises(6); BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { size_t dim = key_value.value.dim(); - // Check if noise model of appropriate size already exists, else create it and cache it! - NoiseMap::iterator it = noises.find(dim); - if(it == noises.end()) { - NoiseCacheItem item; + if (dim > noises.size()) + noises.resize(dim); + + NoiseCacheItem& item = noises[dim-1]; + + // Initialize noise model, A and b if we haven't done so already + if(!item.model) { item.A = Matrix::Identity(dim, dim); item.b = Vector::Zero(dim); - item.model = noiseModel::Isotropic::Sigma(dim, sigma); - noises[dim] = item; - damped += boost::make_shared(key_value.key, item.A, item.b, item.model); - - } else { - const NoiseCacheItem& item = it->second; - damped += boost::make_shared(key_value.key, item.A, item.b, item.model); + item.model = noiseModel::Isotropic::Sigma(dim, sigma); } + damped += boost::make_shared(key_value.key, item.A, item.b, item.model); } } gttoc(damp); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 1a4169e16..2be4a218e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -281,7 +281,7 @@ public: }; /// Noise model Cache - typedef std::map NoiseMap; + typedef std::vector NoiseCacheVector; void writeLogFile(double currentError);