Add check for invalid plane coefficients in pcl::MovingLeastSquares

If the input cloud is non-dense or if the points in the neighborhood are linearly dependent, the algorithm creates invalid output points.
Keep the input point in this case instead.

surface/include/pcl/surface/impl/mls.hpp

@@ -738,9 +738,20 @@ pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
   model_coefficients.head<3> ().matrix () = eigen_vector;
   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
 
+  query_point = cloud.points[index].getVector3fMap ().template cast<double> ();
+
+  if (!pcl_isfinite (eigen_vector[0]) || !pcl_isfinite (eigen_vector[1]) || !pcl_isfinite (eigen_vector[2]))
+  {
+    // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points)
+    // or if the points in the neighborhood are linearly dependent (e.g. on a line).
+    // Keep the input point and stop here.
+    valid = false;
+    mean = query_point;
+    return;
+  }
+
   // Projected query point
   valid = true;
-  query_point = cloud.points[index].getVector3fMap ().template cast<double> ();
   double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
   mean = query_point - distance * model_coefficients.head<3> ();