wrap NonlinearEquality2 + python unit test

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/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()