From d8570ce09b380a899fee3a5ed0c74186f4c3c7c1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 10 Apr 2019 22:25:33 -0400
Subject: [PATCH] Fixed issue with << operator (surfaced in debug mode)

---
 gtsam/slam/InitializePose3.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 2f247811d..a1baab5fa 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations(
   for(const auto& it: relaxedRot3) {
     Key key = it.first;
     if (key != kAnchorKey) {
-      Matrix3 R; R << it.second;
+      Matrix3 R;
+      R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
+
+      // ClosestTo finds rotation matrix closest to H in Frobenius sense
+      // Rot3 initRot = Rot3::ClosestTo(M.transpose());
 
       Matrix U, V; Vector s;
       svd(R.transpose(), U, s, V);