catching csm exception in icp

src/icp.cpp

@@ -147,7 +147,16 @@ icpOutput ICP::align(const LaserScan &_current_ls,
     csm_input.use_ml_weights = _icp_params.use_ml_weights ? 1 : 0;
     csm_input.use_sigma_weights = _icp_params.use_sigma_weights ? 1 : 0;
 
-    sm_icp(&csm_input, &csm_output);
+    try
+    {
+        sm_icp(&csm_input, &csm_output);
+    }
+    catch(...)
+    {
+        icpOutput result{};
+        result.valid = false;
+        return result;
+    }
 
     icpOutput result{};
     result.nvalid = csm_output.nvalid;