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

commented functionality - not working yet

release/4.3a0
Luca 2014-01-27 22:56:21 -05:00
parent c9a8ad0ac6
commit 7bf0d8ebf1
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -163,13 +163,14 @@ 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? Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list?
Vector residuals = - linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign? Vector residuals = - linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign?
double linearizedCostChange = dot(- modelResidual, (residuals + modelResidual / 2.0) ); double linearizedCostChange = dot(- modelResidual, (residuals + modelResidual / 2.0) );
// Measure of mismatch between original (usually nonlinear) system and its linearized version // Measure of mismatch between original (usually nonlinear) system and its linearized version
modelMismatch = costChange / linearizedCostChange; modelMismatch = costChange / linearizedCostChange;
*/
if (error <= state_.error) { if (error <= state_.error) {
state_.values.swap(newValues); state_.values.swap(newValues);