[registration] Assert not triggered in release mode, continues with nonsensical computation

[themightyoarfish]

I noticed that the tools/icp program prints NaN matrices for me on a data set. After some digging, I can see that when built in debug mode, this assert here is triggered:

pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp

Line 241 in 58f9cfb

assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");

Is it intended to just continue in release mode? It seems to me this should be a fatal error.

[mvieth]

Generally, it makes sense that this check in nearestKSearch is an assert, i.e. that it is not tested in release mode. In release mode, the assumption is that the user wants speed. Most classes/functions that call nearestKSearch have already made sure that the point is valid (not nan/inf), so checking again inside the function is just waste of time.
I am not sure yet where the check for nan points should be done. Maybe in tools/icp.cpp, maybe in icp.hpp. It should definitely only be done once, and not again in every icp iteration. Can you provide your point clouds (the ones you pass to the icp tool) as well as the parameters you use, that would help me in debugging.