From 4bf2308ec595a96270952d81ce25e8d2d2d447fa Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:48:26 -0400
Subject: [PATCH] add conversion function for Pose2 -> BinaryMeasurement<Rot2>

---
 gtsam/sfm/ShonanAveraging.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 0de37be1e6..7810d0d470 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
                                      parameters.getUseHuber()),
                          parameters) {}
 
+// Extract Rot2 measurement from Pose2 betweenfactors
+// Modeled after similar function in dataset.cpp
+static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
+    const BetweenFactor<Pose2>::shared_ptr &f) {
+  auto gaussian =
+      boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
+  if (!gaussian)
+    throw std::invalid_argument(
+        "parseMeasurements<Rot2> can only convert Pose2 measurements "
+        "with Gaussian noise models.");
+  const Matrix6 M = gaussian->covariance();
+  auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2));
+  return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
+                                 model);
+}
+    
 static ShonanAveraging2::Measurements extractRot2Measurements(
     const BetweenFactorPose2s &factors) {
   ShonanAveraging2::Measurements result;
   result.reserve(factors.size());
-  for (auto f : factors) result.push_back(convert(f));
+  for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f));
   return result;
 }