Fix wrapping

gtsam/geometry/Rot3.h

@@ -49,15 +49,13 @@
 
 namespace gtsam {
 
-  /**
+/**
-   * @brief A 3D rotation represented as a rotation matrix if the preprocessor
+ * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
-   * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
+ * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
-   * is defined.
+ * if it is defined.
  * @addtogroup geometry
-   * \nosubgrouping
  */
-  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
+class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
-
  private:
 
 #ifdef GTSAM_USE_QUATERNIONS
@@ -68,7 +66,6 @@ namespace gtsam {
 #endif
 
  public:
-
     /// @name Constructors and named constructors
     /// @{
 
@@ -83,7 +80,7 @@ namespace gtsam {
      */
     Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
 
-    /** constructor from a rotation matrix, as doubles in *row-major* order !!! */
+    /// Construct from a rotation matrix, as doubles in *row-major* order !!!
     Rot3(double R11, double R12, double R13,
         double R21, double R22, double R23,
         double R31, double R32, double R33);
@@ -567,6 +564,9 @@ namespace gtsam {
 #endif
   };
 
+  /// std::vector of Rot3s, mainly for wrapper
+  using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
+
   /**
    * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
    * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@@ -585,5 +585,6 @@ namespace gtsam {
 
   template<>
   struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
-}
+  
+}  // namespace gtsam
 

gtsam/slam/slam.i

@@ -316,24 +316,14 @@ class InitializePose3 {
   static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
 };
 
-#include <gtsam/slam/KarcherMeanFactor.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
 template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
 virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
-class Rot3Vector {
+gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
-  Rot3Vector();
-  size_t size() const;
-
-  // structure specific methods
-  gtsam::Rot3 at(size_t i) const;
-  void push_back(const gtsam::Rot3& R);
-};
-gtsam::Rot3 FindKarcherMean<gtsam::Rot3>(const Rot3Vector &rotations);
-typedef FindKarcherMean<gtsam::Rot3> FindKarcherMeanRot3;
-gtsam::Rot3 FindKarcherMeanRot3(const gtsam::Rot3Vector& rotations);
 
 #include <gtsam/slam/FrobeniusFactor.h>
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,

python/gtsam/preamble/slam.h

@@ -15,4 +15,4 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(
     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Rot3, Eigen::aligned_allocator<gtsam::Rot3> >);
+PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector);

python/gtsam/specializations/slam.h

@@ -12,9 +12,9 @@
  */
 
 py::bind_vector<
-    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(
     m_, "BetweenFactorPose3s");
 py::bind_vector<
-    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(
     m_, "BetweenFactorPose2s");
-py::bind_vector<std::vector<gtsam::Rot3, Eigen::aligned_allocator<gtsam::Rot3>>(m_, "Rot3Vector");
+py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector");

python/gtsam/tests/test_KarcherMeanFactor.py

@@ -15,27 +15,15 @@ import unittest
 
 import gtsam
 import numpy as np
+from gtsam import Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY = 0
 MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
-def find_Karcher_mean_Rot3(rotations):
-    """Find the Karcher mean of given values."""
-    # Cost function C(R) = \sum PriorFactor(R_i)::error(R)
-    # No closed form solution.
-    graph = gtsam.NonlinearFactorGraph()
-    for R in rotations:
-        graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
-    initial = gtsam.Values()
-    initial.insert(KEY, gtsam.Rot3())
-    result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
-    return result.atRot3(KEY)
-
-
 # Rot3 version
-R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
+R = Rot3.Expmap(np.array([0.1, 0, 0]))
 
 
 class TestKarcherMean(GtsamTestCase):
@@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase):
     def test_find(self):
         # Check that optimizing for Karcher mean (which minimizes Between distance)
         # gets correct result.
-        rotations = {R, R.inverse()}
+        rotations = gtsam.Rot3Vector([R, R.inverse()])
-        expected = gtsam.Rot3()
+        expected = Rot3()
-        actual = find_Karcher_mean_Rot3(rotations)
+        actual = gtsam.FindKarcherMean(rotations)
         self.gtsamAssertEquals(expected, actual)
 
+    def test_find_karcher_mean_identity(self):
+        """Averaging 3 identity rotations should yield the identity."""
+        a1Rb1 = Rot3()
+        a2Rb2 = Rot3()
+        a3Rb3 = Rot3()
+
+        aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
+        aRb_expected = Rot3()
+
+        aRb = gtsam.FindKarcherMean(aRb_list)
+        self.gtsamAssertEquals(aRb, aRb_expected)
+
     def test_factor(self):
         """Check that the InnerConstraint factor leaves the mean unchanged."""
         # Make a graph with two variables, one between, and one InnerConstraint
@@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase):
         initial = gtsam.Values()
         initial.insert(1, R.inverse())
         initial.insert(2, R)
-        expected = find_Karcher_mean_Rot3([R, R.inverse()])
+        expected = Rot3()
 
         result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
-        actual = find_Karcher_mean_Rot3(
+        actual = gtsam.FindKarcherMean(
-            [result.atRot3(1), result.atRot3(2)])
+            gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
         self.gtsamAssertEquals(expected, actual)
         self.gtsamAssertEquals(
             R12, result.atRot3(1).between(result.atRot3(2)))