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

Add min_diagonal and max_diagonal as Ceres.

release/4.3a0
hchiu 2014-02-16 16:32:18 -05:00
parent 9279d4402f
commit a1ff716daf
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -100,6 +100,9 @@ void LevenbergMarquardtOptimizer::iterate() {
GaussianFactorGraph::shared_ptr linear = linearize(); GaussianFactorGraph::shared_ptr linear = linearize();
double modelFidelity = 0.0; double modelFidelity = 0.0;
//Set two parameters as Ceres, will move out later
double min_diagonal_ = 1e-6;
double max_diagonal_ = 1e32;
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while (true) { while (true) {
@ -124,9 +127,9 @@ void LevenbergMarquardtOptimizer::iterate() {
//Replace the identity matrix with diagonal of Hessian //Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping) { if (params_.diagonalDamping) {
A.diagonal() = hessianDiagonal.at(key_value.key); A.diagonal() = hessianDiagonal.at(key_value.key);
for (int aa=0; aa<dim; aa++) for (size_t aa=0; aa<dim; aa++)
{ {
A(aa,aa)=sqrt(A(aa,aa)); A(aa,aa)=sqrt(std::min(std::max(A(aa,aa), min_diagonal_), max_diagonal_));
} }
} }
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);