Why ICP is much faster than NDT in front_end?

[FishInWave]

Hi, @koide3 , I just want to know the time cost of every matching process of different way，especially ICP，GICP and NDT(no OMP for justice).So I add the code into function "matching" in scan_matching_odometry_nodelet.cpp as follow: static double sum_time=0.0; static int count=0; auto time_start = ros::WallTime::now; registration->align(*aligned,prev_trans*msf_delta.matrix()); auto time_end = ros:WallTime::now; sum_time += (time_end-time_start).toSec(); count++; if(count == 200){std::cout << sum_time/count <<std::endl;}

And the result is: ICP->0.039s, NDT-> 0.306846s; GICP->0.201762s. It's a counterintuitive result, so I run NDT_OMP as well and get NDT_OMP->0.0320954s.

Usually, I think ICP is a time-consuming algorithm but more precise, while NDT is more robust and faster. (I tested this on my own bag got with Velodyne 64.)

Can you please help to figure out the reason?

[koide3]

Hi @FishInWave , Because a closed-form pose estimation can be used in the naive ICP (although it requires time-consuming correspondence search), when the number of points in the target cloud is not very large, it can be faster than NDT and GICP that inherently require non-linear optimization.

In my opinion, NDT is often more accurate than ICP if a proper initial guess is available because NDT exploits the non-linear distribution-based distance function while ICP is based on the linear distance function.

[koide3]

One more thing to note: when you choose "NDT" option but not "NDT_OMP", PCL's original NDT is used but that implementation has some issues affecting the processing speed.

[FishInWave]

Thank you for your pacient explanation!