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

New flag to govern diagonal damping, off by default

release/4.3a0
dellaert 2014-02-16 11:57:51 -05:00
parent 19a06ca68f
commit c31d48dba1
2 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -113,13 +113,16 @@ void LevenbergMarquardtOptimizer::iterate() {
double sigma = 1.0 / std::sqrt(state_.lambda); double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + state_.values.size()); dampedSystem.reserve(dampedSystem.size() + state_.values.size());
// for each of the variables, add a prior // for each of the variables, add a prior
VectorValues diagHessian = linear->hessianDiagonal(); VectorValues hessianDiagonal;
if (params_.diagonalDamping)
hessianDiagonal = linear->hessianDiagonal();
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim(); size_t dim = key_value.value.dim();
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
//Replace the identity matrix with diagonal of Hessian //Replace the identity matrix with diagonal of Hessian
A.diagonal()=diagHessian.at(key_value.key); if (params_.diagonalDamping)
A.diagonal() = hessianDiagonal.at(key_value.key);
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, model); dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);

12
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -52,10 +52,12 @@ public:
bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed
double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
bool diagonalDamping; ///< if true, use diagonal of Hessian
LevenbergMarquardtParams() : LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(0.0), lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(1e-3) { 0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(
1e-3), diagonalDamping(false) {
} }
virtual ~LevenbergMarquardtParams() { virtual ~LevenbergMarquardtParams() {
} }
@ -80,6 +82,9 @@ public:
inline std::string getLogFile() const { inline std::string getLogFile() const {
return logFile; return logFile;
} }
inline bool getDiagonalDamping() const {
return diagonalDamping;
}
inline void setlambdaInitial(double value) { inline void setlambdaInitial(double value) {
lambdaInitial = value; lambdaInitial = value;
@ -99,6 +104,9 @@ public:
inline void setLogFile(const std::string &s) { inline void setLogFile(const std::string &s) {
logFile = s; logFile = s;
} }
inline void setDiagonalDamping(bool flag) {
diagonalDamping = flag;
}
}; };
/** /**