12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Don't do diagonal damping for variables not in linear system

release/4.3a0
dellaert 2014-03-04 01:06:15 -05:00
parent e321ac60c0
commit 63f8c75fb2
1 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -121,39 +121,39 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
gttic(damp); gttic(damp);
if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
cout << "building damped system with lambda " << state_.lambda << endl; cout << "building damped system with lambda " << state_.lambda << endl;
GaussianFactorGraph dampedSystem = linear;
{ // Only retrieve diagonal vector when reuse_diagonal = false
double sigma = 1.0 / std::sqrt(state_.lambda); if (params_.diagonalDamping && params_.reuse_diagonal_ == false) {
dampedSystem.reserve(dampedSystem.size() + state_.values.size()); state_.hessianDiagonal = linear.hessianDiagonal();
// Only retrieve diagonal vector when reuse_diagonal = false BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) {
if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { for (size_t aa = 0; aa < v.size(); aa++) {
state_.hessianDiagonal = linear.hessianDiagonal(); v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_);
BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) v(aa) = sqrt(v(aa));
{
for( size_t aa = 0 ; aa < v.size() ; aa++)
{
v(aa) = std::min(std::max(v(aa), min_diagonal_),
max_diagonal_);
v(aa) = sqrt(v(aa));
}
} }
} }
// for each of the variables, add a prior } // reuse diagonal
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim(); // for each of the variables, add a prior
Matrix A = Matrix::Identity(dim, dim); double sigma = 1.0 / std::sqrt(state_.lambda);
//Replace the identity matrix with diagonal of Hessian GaussianFactorGraph damped = linear;
if (params_.diagonalDamping) { damped.reserve(damped.size() + state_.values.size());
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim();
Matrix A = Matrix::Identity(dim, dim);
//Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping)
try {
A.diagonal() = state_.hessianDiagonal.at(key_value.key); A.diagonal() = state_.hessianDiagonal.at(key_value.key);
} catch (std::exception e) {
// Don't attempt diagonal damping if no key given
} }
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
model);
}
} }
gttoc(damp); gttoc(damp);
return dampedSystem; return damped;
} }
/* ************************************************************************* */ /* ************************************************************************* */