Merge pull request #1069 from borglab/wrap-karcher-mean-rot3

gtsam/geometry/Rot3.h

@@ -49,16 +49,14 @@
 
 namespace gtsam {
 
-  /**
-   * @brief A 3D rotation represented as a rotation matrix if the preprocessor
-   * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
-   * is defined.
-   * @addtogroup geometry
-   * \nosubgrouping
-   */
-  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
-
-  private:
+/**
+ * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
+ * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
+ * if it is defined.
+ * @addtogroup geometry
+ */
+class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
+ private:
 
 #ifdef GTSAM_USE_QUATERNIONS
     /** Internal Eigen Quaternion */
@@ -67,8 +65,7 @@ namespace gtsam {
     SO3 rot_;
 #endif
 
-  public:
-
+ 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

@@ -323,6 +323,8 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
+gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
+
 #include <gtsam/slam/FrobeniusFactor.h>
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
                                                 size_t d);

python/CMakeLists.txt

@@ -45,6 +45,7 @@ set(ignore
     gtsam::Point3Pairs
     gtsam::Pose3Pairs
     gtsam::Pose3Vector
+    gtsam::Rot3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
     gtsam::DiscreteKey

python/gtsam/preamble/slam.h

@@ -15,3 +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(gtsam::Rot3Vector);

python/gtsam/specializations/slam.h

@@ -12,8 +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<gtsam::Rot3Vector>(m_, "Rot3Vector");

python/gtsam/tests/test_KarcherMeanFactor.py

@@ -15,39 +15,39 @@
 
 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):
 
     def test_find(self):
         # Check that optimizing for Karcher mean (which minimizes Between distance)
         # gets correct result.
-        rotations = {R, R.inverse()}
-        expected = gtsam.Rot3()
-        actual = find_Karcher_mean_Rot3(rotations)
+        rotations = gtsam.Rot3Vector([R, R.inverse()])
+        expected = Rot3()
+        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 @@ def test_factor(self):
         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(
-            [result.atRot3(1), result.atRot3(2)])
+        actual = gtsam.FindKarcherMean(
+            gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
         self.gtsamAssertEquals(expected, actual)
         self.gtsamAssertEquals(
             R12, result.atRot3(1).between(result.atRot3(2)))