commit
35b6986d32
|
@ -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()
|
||||
|
||||
###############################################################################
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -189,12 +189,24 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
|
|||
}
|
||||
} else {
|
||||
// Straightforward damping:
|
||||
|
||||
// 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();
|
||||
Matrix A = Matrix::Identity(dim, dim);
|
||||
Vector b = Vector::Zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
|
||||
damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
|
||||
|
||||
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);
|
||||
}
|
||||
damped += boost::make_shared<JacobianFactor>(key_value.key, item.A, item.b, item.model);
|
||||
}
|
||||
}
|
||||
gttoc(damp);
|
||||
|
|
|
@ -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::vector<NoiseCacheItem> NoiseCacheVector;
|
||||
|
||||
void writeLogFile(double currentError);
|
||||
|
||||
/// @}
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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 */
|
||||
|
|
Loading…
Reference in New Issue