From b156a6498e67e06dafc4015ccc7235fd18bfa99b Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 10 Dec 2019 12:19:38 -0500
Subject: [PATCH] Fix KarcherMeanFactor

---
 gtsam/slam/KarcherMeanFactor-inl.h         | 14 ++++++++++++--
 gtsam/slam/KarcherMeanFactor.h             |  5 ++++-
 gtsam/slam/tests/testKarcherMeanFactor.cpp |  4 +++-
 3 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h
index cfe071ee5..8ff61d796 100644
--- a/gtsam/slam/KarcherMeanFactor-inl.h
+++ b/gtsam/slam/KarcherMeanFactor-inl.h
@@ -26,8 +26,8 @@ using namespace std;
 
 namespace gtsam {
 
-template <class T>
-T FindKarcherMean(const vector<T>& rotations) {
+template <class T, class ALLOC>
+T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
   // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
   // No closed form solution.
   NonlinearFactorGraph graph;
@@ -41,6 +41,16 @@ T FindKarcherMean(const vector<T>& rotations) {
   return result.at<T>(kKey);
 }
 
+template <class T, class ALLOC>
+T FindKarcherMean(const vector<T, ALLOC>& rotations) {
+  return FindKarcherMeanImpl(rotations);
+}
+
+template <class T>
+T FindKarcherMean(std::initializer_list<T>&& rotations) {
+  return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
+}
+
 template <class T>
 template <typename CONTAINER>
 KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h
index c098c9665..f62cb8904 100644
--- a/gtsam/slam/KarcherMeanFactor.h
+++ b/gtsam/slam/KarcherMeanFactor.h
@@ -29,8 +29,11 @@ namespace gtsam {
  * the given rotations, by constructing a factor graph out of simple
  * PriorFactors.
  */
+template <class T, class ALLOC = Eigen::aligned_allocator<T>>
+T FindKarcherMean(const std::vector<T, ALLOC>& rotations);
+
 template <class T>
-T FindKarcherMean(const std::vector<T>& rotations);
+T FindKarcherMean(std::initializer_list<T>&& rotations);
 
 /**
  * The KarcherMeanFactor creates a constraint on all SO(n) variables with
diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp
index a3e52e64d..64cdb1682 100644
--- a/gtsam/slam/tests/testKarcherMeanFactor.cpp
+++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp
@@ -91,7 +91,9 @@ TEST(KarcherMean, FactorSO4) {
   Values initial;
   initial.insert<SO4>(1, Q.inverse());
   initial.insert<SO4>(2, Q);
-  const auto expected = FindKarcherMean<SO4>({Q, Q.inverse()});
+
+  std::vector<SO4, Eigen::aligned_allocator<SO4> > rotations = {Q, Q.inverse()};
+  const auto expected = FindKarcherMean<SO4>(rotations);
 
   auto result = GaussNewtonOptimizer(graph, initial).optimize();
   const auto actual =