add interface in C++, and helper extractRot2Measurements()

borglab · Jul 9, 2021 · 3c18233 · 3c18233
1 parent 15478bf
commit 3c18233
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
@@ -944,6 +944,20 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
                                      parameters.getUseHuber()),
                          parameters) {}
 
+static ShonanAveraging2::Measurements extractRot2Measurements(
+    const BetweenFactorPose2s &factors) {
+  ShonanAveraging2::Measurements result;
+  result.reserve(factors.size());
+  for (auto f : factors) result.push_back(convert(f));
+  return result;
+}
+
+ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors,
+                                   const Parameters &parameters)
+    : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors),
+                                     parameters.getUseHuber()),
+                         parameters) {}    
+
 /* ************************************************************************* */
 // Explicit instantiation for d=3
 template class ShonanAveraging<3>;