Open stachnis opened 3 years ago
Thanks @stachnis!
I only now got to look at this and understand the change and you are totally right! The order of normals was, of course, totally messed up!
I merged the first change from a different PR (#7) as that generated a better diff. This PR adds 47 thousand lines! The diff might be so huge either because you did not re-run the notebook with a kernel restart before submitting or because it was done from a mac :man_shrugging:
Could you tell me in some way what was the second change you suggested in this MR?
Changes:
Fixed bug in correspondences of LS point to plane ICP (in function: prepare_system_normals) Changes to: for (i, j) in correspondences: p_point = P[:, [i]] q_point = Q[:, [j]] nn = normals[j] e = nn.dot(error(x, p_point, q_point)) J = nn.dot(jacobian(x, p_point))
Compute and plot norm(P-Q) error consistently for all ICP variants for better comparison