From 5556f9e92279f69b59f72d923263c8cab0f43860 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 17 Dec 2021 14:39:11 +0100
Subject: [PATCH] catching csm exception in icp

---
 src/icp.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index 4f45427..e2f9245 100644
--- a/src/icp.cpp
+++ b/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;
-- 
GitLab