diff --git a/sm/apps/sm2.c b/sm/apps/sm2.c
index 28f69bad5246558b82743b64dc1759ddf50269b3..23d7c2de09964ceaa03260316ca42f3e6d8ad814 100644
--- a/sm/apps/sm2.c
+++ b/sm/apps/sm2.c
@@ -123,7 +123,8 @@ int main(int argc, const char*argv[]) {
 		
 		if(!result.valid){
 			sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n");
-			break;
+			ld_free(laser_ref);
+			return 2;
 		}
 		
 		/* Add the result to the previous estimate */