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

computation of modelMismatch in LM

release/4.3a0
Luca 2014-02-07 18:27:37 -05:00
parent 7bf0d8ebf1
commit c9ead8bbd3
1 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -163,14 +163,20 @@ void LevenbergMarquardtOptimizer::iterate() {
// linearizedCostChange = oldCost - newLinearizedCost = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step] // linearizedCostChange = oldCost - newLinearizedCost = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step]
// = -f'J * step - step' * J' * J * step / 2 = -(f' + modelResidual') * (modelResidual) // = -f'J * step - step' * J' * J * step / 2 = -(f' + modelResidual') * (modelResidual)
// (with modelResidual = J * step) // (with modelResidual = J * step)
/* Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta // Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta
Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list? // modelResidualList.print("");
Vector residuals = - linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign? // Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list?
double linearizedCostChange = dot(- modelResidual, (residuals + modelResidual / 2.0) ); //cout << "modelResidual: " << modelResidual << endl;
// cout << "linear->jacobian().second: " << linear->jacobian().second << endl;
// cout << "linear->augmentedJacobian().second: " << linear->augmentedJacobian() << endl;
// cout << "linear->augmentedHessian().second: " << linear->augmentedHessian() << endl;
// Vector residuals = linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign?
// double linearizedCostChange = dot(- modelResidual, (- residuals + modelResidual / 2.0) );
//
// // Measure of mismatch between original (usually nonlinear) system and its linearized version
// modelMismatch = costChange / linearizedCostChange;
// Measure of mismatch between original (usually nonlinear) system and its linearized version
modelMismatch = costChange / linearizedCostChange;
*/
if (error <= state_.error) { if (error <= state_.error) {
state_.values.swap(newValues); state_.values.swap(newValues);