Improve robustness of mapper

[m0rsch1]

Currently, the mapper fails HARD (assertion!) if the point coulds contain any invalid points (e.g. NaN, Inf components).
This can happen f.e. when the point clouds come from ROS or are completely unfiltered/processed.
PCL has a function called pcl::removeNaNFromPointCloud() which could do that.

Having such a handling would greatly improve usability of the point cloud mapper.