diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 243611433..47083d5d7 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -290,24 +290,19 @@ struct traits > * Simple binary equality constraint - this constraint forces two variables to * be the same. */ -template -class NonlinearEquality2: public NoiseModelFactor2 { -public: - typedef VALUE X; +template +class NonlinearEquality2 : public NoiseModelFactor2 { + protected: + using Base = NoiseModelFactor2; + using This = NonlinearEquality2; -protected: - typedef NoiseModelFactor2 Base; - typedef NonlinearEquality2 This; - - GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_MANIFOLD_TYPE(T) /// Default constructor to allow for serialization - NonlinearEquality2() { - } + NonlinearEquality2() {} -public: - - typedef boost::shared_ptr > shared_ptr; + public: + typedef boost::shared_ptr> 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::dimension, std::abs(mu)), key1, key2) { - } - ~NonlinearEquality2() override { - } + NonlinearEquality2(Key key1, Key key2, double mu = 1e4) + : Base(noiseModel::Constrained::All(traits::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 H1 = - boost::none, boost::optional H2 = boost::none) const override { - static const size_t p = traits::dimension; - if (H1) *H1 = -Matrix::Identity(p,p); - if (H2) *H2 = Matrix::Identity(p,p); - return traits::Local(x1,x2); + Vector evaluateError( + const T& x1, const T& x2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + static const size_t p = traits::dimension; + if (H1) *H1 = -Matrix::Identity(p, p); + if (H2) *H2 = Matrix::Identity(p, p); + return traits::Local(x1, x2); } GTSAM_MAKE_ALIGNED_OPERATOR_NEW -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); } }; // \NonlinearEquality2 -template -struct traits > : Testable > { +template +struct traits> : Testable> { }; - }// namespace gtsam 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/CMakeLists.txt b/python/CMakeLists.txt index 7b8347da5..bdda15acb 100644 --- a/python/CMakeLists.txt +++ b/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}) 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()