search

koide3 / fast_gicp

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

the calculation of **rho** in step_lm() seems wrong #148

Open QiMingZhenFan opened 9 months ago

QiMingZhenFan commented 9 months ago

Thanks for the wonderful work!!

When I read the source code, I was confused by the calculation of rho in this line. I'll paste it here.

double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b));

However, because we want to evaluate the similarity between the original model and the approximate model, I think the correct formula should be as follows:

double rho = (y0 - yi) / (d.dot((H + lm_lambda_ * Eigen::Matrix<double, 6, 6>::Identity()) * d + 2 * b)) * (-1.0);

I will attach my proof here.

screenshot-20240202-175119

koide3 commented 9 months ago

Thank you for reporting this issue. I simply implemented the algorithm appear in Fig.1 in this paper, and I suppose rho is derived with some approximations. Although the formula you provided may be better, I'm not very sure. Note that this part brings only a very small difference in convergence speed.

QiMingZhenFan commented 9 months ago

Thanks for the reply! Seems I should have a look at that paper :)