diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 91525c5fd..61f164b43 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -824,4 +824,17 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { void serialize() const; }; +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + 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 diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py new file mode 100644 index 000000000..3ec8648a4 --- /dev/null +++ b/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()