Fix KarcherMeanFactor

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)

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

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 =