From 02ff7ae276514bb3633638416fda95e7b3100fae Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 28 Mar 2020 09:56:29 -0400
Subject: [PATCH] output message for exception in debug mode

---
 gtsam/nonlinear/utilities.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 1228cd4db..2044d091f 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -244,6 +244,9 @@ Values localToWorld(const Values& local, const Pose2& base,
         world.insert(key, base.transformFrom(point));
       } catch (const std::exception& e2) {
         // if not Pose2 or Point2, do nothing
+        #ifndef NDEBUG
+          std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
+        #endif
       }
     }
   }