Merge pull request #5568 from mvieth/tools_icp_remove_nans

[tools] Remove NaNs from clouds after loading in icp tool

Author: mvieth

tools/icp.cpp

@@ -40,6 +40,7 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
+#include <pcl/filters/filter.h> // for removeNaNFromPointCloud
 #include <pcl/registration/icp.h>
 #include <pcl/registration/icp_nl.h>
 #include <pcl/registration/incremental_registration.h>
@@ -97,6 +98,8 @@ main (int argc, char **argv)
       std::cout << "Could not read file" << std::endl;
       return -1;
     }
+    pcl::Indices dummy_indices;
+    pcl::removeNaNFromPointCloud(*data, *data, dummy_indices);
 
     if (!iicp.registerCloud (data))
     {