search

koide3 / fast_gicp

A collection of GICP-based fast point cloud registration algorithms
BSD 3-Clause "New" or "Revised" License
1.23k stars 316 forks source link

to match scan and map #76

Open git-chenjiqing opened 2 years ago

git-chenjiqing commented 2 years ago

LM getting stuck between two poses，scan matching has not converged!

optimize

--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.0019006 0.000256035 0.999961 4.26949e-05 0.0275067 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00139957 0.00108299 0.999875 1.42316e-05 0.00943503 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000931759 0.000901194 0.999234 4.74388e-06 0.00374655 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00127828 0.000852582 1 1.58129e-06 0.0129846 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000963567 0.000577186 1.00042 5.27098e-07 0.00827548 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00150657 0.00127261 1.00007 1.75699e-07 0.0058606 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00183795 0.0013874 0.999862 5.85664e-08 0.0113555 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00209659 0.000964341 0.999997 1.95221e-08 0.0224745 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.00119574 0.000643282 1.00014 6.50738e-09 0.00870167 x --- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643287 -59.8768 2.16913e-09 4.12943e-06
1 0.000643282 0.000643287 -59.8768 4.33825e-09 4.12943e-06
2 0.000643282 0.000643287 -59.8768 1.7353e-08 4.12943e-06
3 0.000643282 0.000643287 -59.8768 1.38824e-07 4.12943e-06
4 0.000643282 0.000643287 -59.8768 2.22119e-06 4.12943e-06
5 0.000643282 0.000643287 -59.8768 7.10779e-05 4.12931e-06
6 0.000643282 0.000643286 -42.4444 0.00454899 4.12183e-06
7 0.000643282 0.000643285 -36.3345 0.582271 3.35351e-06
8 0.000643282 0.000643285 -137.503 149.061 2.42515e-07
9 0.000643282 0.000643282 0 76319.4 7.18212e-10
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 152639 3.59293e-10
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 305277 1.79694e-10
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 610555 8.98585e-11
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.22111e+06 4.49322e-11
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 2.44222e+06 2.24668e-11
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 4.88444e+06 1.12336e-11
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 9.76888e+06 5.61684e-12
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.95378e+07 2.80843e-12
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 3.90755e+07 1.40422e-12
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 7.8151e+07 7.0211e-13
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.56302e+08 3.51055e-13
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 3.12604e+08 1.75528e-13
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 6.25208e+08 8.77639e-14
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.25042e+09 4.38819e-14
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 2.50083e+09 2.1941e-14
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 5.00167e+09 1.09705e-14
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.00033e+10 5.48524e-15
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 2.00067e+10 2.74262e-15
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 4.00133e+10 1.37131e-15
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 8.00267e+10 6.85655e-16
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.60053e+11 3.42828e-16
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 3.20107e+11 1.71414e-16
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 6.40213e+11 8.57069e-17
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.28043e+12 4.28535e-17
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 2.56085e+12 2.14267e-17
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 5.12171e+12 1.07134e-17
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.02434e+13 5.35668e-18
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 2.04868e+13 2.67834e-18
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 4.09736e+13 1.33917e-18
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 8.19473e+13 6.69585e-19
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 1.63895e+14 3.34793e-19
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 3.27789e+14 1.67396e-19
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000643282 0 6.55578e+14 8.36981e-20
--- LM optimization --- i y0 yi rho lambda |delta| dec 0 0.000643282 0.000