Merge pull request #856 from borglab/feature/wrap-nonlinearequality2

Wrap NonlinearEquality2

gtsam/nonlinear/NonlinearEquality.h

@@ -290,24 +290,19 @@ struct traits<NonlinearEquality1<VALUE> >
  * Simple binary equality constraint - this constraint forces two variables to
  * be the same.
  */
-template<class VALUE>
-class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {
-public:
-  typedef VALUE X;
+template <class T>
+class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
+ protected:
+  using Base = NoiseModelFactor2<T, T>;
+  using This = NonlinearEquality2<T>;
 
-protected:
-  typedef NoiseModelFactor2<VALUE, VALUE> Base;
-  typedef NonlinearEquality2<VALUE> This;
-
-  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
+  GTSAM_CONCEPT_MANIFOLD_TYPE(T)
 
   /// Default constructor to allow for serialization
-  NonlinearEquality2() {
-  }
+  NonlinearEquality2() {}
 
-public:
-
-  typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
+ public:
+  typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
 
   /**
    * Constructor
@@ -315,11 +310,10 @@ public:
    * @param key2 the key for the second unknown variable to be constrained
    * @param mu a parameter which really turns this into a strong prior
    */
-  NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
-      Base(noiseModel::Constrained::All(traits<X>::dimension, std::abs(mu)), key1, key2) {
-  }
-  ~NonlinearEquality2() override {
-  }
+  NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
+      : Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
+             key1, key2) {}
+  ~NonlinearEquality2() override {}
 
   /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
@@ -328,32 +322,30 @@ public:
   }
 
   /// g(x) with optional derivative2
-  Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
-    static const size_t p = traits<X>::dimension;
-    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 =  Matrix::Identity(p,p);
-    return traits<X>::Local(x1,x2);
+  Vector evaluateError(
+      const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    static const size_t p = traits<T>::dimension;
+    if (H1) *H1 = -Matrix::Identity(p, p);
+    if (H2) *H2 = Matrix::Identity(p, p);
+    return traits<T>::Local(x1, x2);
   }
 
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
-private:
-
+ private:
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar
-        & boost::serialization::make_nvp("NoiseModelFactor2",
-            boost::serialization::base_object<Base>(*this));
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
   }
 };
 // \NonlinearEquality2
 
-template<typename VALUE>
-struct traits<NonlinearEquality2<VALUE> > : Testable<NonlinearEquality2<VALUE> > {
+template <typename VALUE>
+struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
 };
 
-
 }// namespace gtsam

gtsam/nonlinear/nonlinear.i

@@ -824,4 +824,17 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
+               gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
+               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
+               gtsam::PinholeCamera<gtsam::Cal3_S2>,
+               gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+               gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+               gtsam::PinholeCamera<gtsam::Cal3Unified>,
+               gtsam::imuBias::ConstantBias}>
+virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
+  NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
+  gtsam::Vector evaluateError(const T& x1, const T& x2);
+};
+
 }  // namespace gtsam

python/CMakeLists.txt

@@ -165,6 +165,5 @@ add_custom_target(
         COMMAND
           ${CMAKE_COMMAND} -E env # add package to python path so no need to install
           "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
-          ${PYTHON_EXECUTABLE} -m unittest discover
-          DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
-        WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests)
+          ${PYTHON_EXECUTABLE} -m unittest discover -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests"
+          DEPENDS ${GTSAM_PYTHON_DEPENDENCIES})

python/gtsam/tests/test_Factors.py

@@ -0,0 +1,34 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for various factors.
+
+Author: Varun Agrawal
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestNonlinearEquality2Factor(GtsamTestCase):
+    """
+    Test various instantiations of NonlinearEquality2.
+    """
+    def test_point3(self):
+        """Test for Point3 version."""
+        factor = gtsam.NonlinearEquality2Point3(0, 1)
+        error = factor.evaluateError(gtsam.Point3(0, 0, 0),
+                                     gtsam.Point3(0, 0, 0))
+
+        np.testing.assert_allclose(error, np.zeros(3))
+
+
+if __name__ == "__main__":
+    unittest.main()