fix: update error value in LevenbergMarquardtOptimizer

[fateshelled]

LevenbergMarquardtOptimizer error values did not reflect last calculation results.

Original Version Result with verbose (error:1376.79 is not equal to last new_e=1376.11)

$ ./01_basic_registration 
iter=0 inner=0 e=39223.2 new_e=2512.16 lambda=0.001 dt=0.490688 dr=0.0143822
iter=1 inner=0 e=1497.14 new_e=1365.72 lambda=0.0001 dt=0.0215351 dr=0.00432626
iter=2 inner=0 e=1376.79 new_e=1376.11 lambda=1e-05 dt=0.000719879 dr=0.000520019
--- T_target_source ---
    0.99989   0.0147726 -0.00103016    0.493199
 -0.0147798    0.999863   -0.007402    0.126538
 0.00092067  0.00741642    0.999972  -0.0257501
          0           0           0           1
converged:1
error:1376.79
iterations:2
num_inliers:5866
--- H ---
 5.97785e+06 -2.31786e+06   1.8801e+06      74387.2      39319.4      -640739
-2.31786e+06  1.60442e+07  2.93907e+06       189152     -43242.7      -111143
  1.8801e+06  2.93907e+06  3.20679e+07  1.61834e+06       150786     -31144.6
     74387.2       189152  1.61834e+06       439092      -7376.5      3040.04
     39319.4     -43242.7       150786      -7376.5       334859      24429.1
     -640739      -111143     -31144.6      3040.04      24429.1       506785
--- b ---
-2229.92  165.315  2198.71 -52.9216 -36.2867  49.3337
iter=0 inner=0 e=39223.2 new_e=2512.16 lambda=0.001 dt=0.490688 dr=0.0143822
iter=1 inner=0 e=1497.14 new_e=1365.72 lambda=0.0001 dt=0.0215351 dr=0.00432626
iter=2 inner=0 e=1376.79 new_e=1376.11 lambda=1e-05 dt=0.000719879 dr=0.000520019
--- T_target_source ---
    0.99989   0.0147726 -0.00103016    0.493199
 -0.0147798    0.999863   -0.007402    0.126538
 0.00092067  0.00741642    0.999972  -0.0257501
          0           0           0           1
converged:1
error:1376.79
iterations:2
num_inliers:5866
--- H ---
 5.97785e+06 -2.31786e+06   1.8801e+06      74387.2      39319.4      -640739
-2.31786e+06  1.60442e+07  2.93907e+06       189152     -43242.7      -111143
  1.8801e+06  2.93907e+06  3.20679e+07  1.61834e+06       150786     -31144.6
     74387.2       189152  1.61834e+06       439092      -7376.5      3040.04
     39319.4     -43242.7       150786      -7376.5       334859      24429.1
     -640739      -111143     -31144.6      3040.04      24429.1       506785
--- b ---
-2229.92  165.315  2198.71 -52.9216 -36.2867  49.3337
iter=0 inner=0 e=41934.9 new_e=2428.75 lambda=0.001 dt=0.493634 dr=0.0135497
iter=1 inner=0 e=1579.3 new_e=1495.66 lambda=0.0001 dt=0.0184596 dr=0.00300417
iter=2 inner=0 e=1502.34 new_e=1502.16 lambda=1e-05 dt=0.000276122 dr=0.000165256

Fixed version with verbose (error is equal to last new_e)

./build/01_basic_registration 
iter=0 inner=0 e=39223.2 new_e=2512.16 lambda=0.001 dt=0.490688 dr=0.0143822
iter=1 inner=0 e=1497.14 new_e=1365.72 lambda=0.0001 dt=0.0215351 dr=0.00432626
iter=2 inner=0 e=1376.79 new_e=1376.11 lambda=1e-05 dt=0.000719879 dr=0.000520019
--- T_target_source ---
    0.99989   0.0147726 -0.00103016    0.493199
 -0.0147798    0.999863   -0.007402    0.126538
 0.00092067  0.00741642    0.999972  -0.0257501
          0           0           0           1
converged:1
error:1376.11
iterations:2
num_inliers:5866
--- H ---
 5.97785e+06 -2.31786e+06   1.8801e+06      74387.2      39319.4      -640739
-2.31786e+06  1.60442e+07  2.93907e+06       189152     -43242.7      -111143
  1.8801e+06  2.93907e+06  3.20679e+07  1.61834e+06       150786     -31144.6
     74387.2       189152  1.61834e+06       439092      -7376.5      3040.04
     39319.4     -43242.7       150786      -7376.5       334859      24429.1
     -640739      -111143     -31144.6      3040.04      24429.1       506785
--- b ---
-2229.92  165.315  2198.71 -52.9216 -36.2867  49.3337
iter=0 inner=0 e=39223.2 new_e=2512.16 lambda=0.001 dt=0.490688 dr=0.0143822
iter=1 inner=0 e=1497.14 new_e=1365.72 lambda=0.0001 dt=0.0215351 dr=0.00432626
iter=2 inner=0 e=1376.79 new_e=1376.11 lambda=1e-05 dt=0.000719879 dr=0.000520019
--- T_target_source ---
    0.99989   0.0147726 -0.00103016    0.493199
 -0.0147798    0.999863   -0.007402    0.126538
 0.00092067  0.00741642    0.999972  -0.0257501
          0           0           0           1
converged:1
error:1376.11
iterations:2
num_inliers:5866
--- H ---
 5.97785e+06 -2.31786e+06   1.8801e+06      74387.2      39319.4      -640739
-2.31786e+06  1.60442e+07  2.93907e+06       189152     -43242.7      -111143
  1.8801e+06  2.93907e+06  3.20679e+07  1.61834e+06       150786     -31144.6
     74387.2       189152  1.61834e+06       439092      -7376.5      3040.04
     39319.4     -43242.7       150786      -7376.5       334859      24429.1
     -640739      -111143     -31144.6      3040.04      24429.1       506785
--- b ---
-2229.92  165.315  2198.71 -52.9216 -36.2867  49.3337
iter=0 inner=0 e=41934.9 new_e=2428.75 lambda=0.001 dt=0.493634 dr=0.0135497
iter=1 inner=0 e=1579.3 new_e=1495.66 lambda=0.0001 dt=0.0184596 dr=0.00300417
iter=2 inner=0 e=1502.34 new_e=1502.16 lambda=1e-05 dt=0.000276122 dr=0.000165256

[koide3]

Thanks for correcting it!