diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index b8368c497..4f86bc757 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -44,20 +44,14 @@ Vector CustomFactor::unwhitenedError( * return error * ``` */ - std::pair errorAndJacobian = - this->error_function_(*this, x, H.get_ptr()); + return this->error_function_(*this, x, H.get_ptr()); - Vector error = errorAndJacobian.first; - (*H) = errorAndJacobian.second; - - return error; } else { /* * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. * Users can check for `None` in their callback to determine if the Jacobian is requested. */ - auto errorAndJacobian = this->error_function_(*this, x, nullptr); - return errorAndJacobian.first; + return this->error_function_(*this, x, nullptr); } } else { return Vector::Zero(this->dim()); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 6261636b5..615b5418e 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -35,8 +35,7 @@ class CustomFactor; * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout. * Thus the pointer will never be invalidated. */ -using CustomErrorFunction = std::function( - const CustomFactor &, const Values &, JacobianVector *)>; +using CustomErrorFunction = std::function; /** * @brief Custom factor that takes a std::function as the error @@ -78,7 +77,7 @@ public: * Calls the errorFunction closure, which is a std::function object * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array */ - Vector unwhitenedError(const Values &x, boost::optional&> H = boost::none) const override; + Vector unwhitenedError(const Values &x, boost::optional &> H = boost::none) const override; /** print */ void print(const std::string &s, diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 03e6917f0..a3bb00384 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -33,7 +33,7 @@ class TestCustomFactor(GtsamTestCase): def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]), H + return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -46,7 +46,7 @@ class TestCustomFactor(GtsamTestCase): """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -80,7 +80,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) @@ -103,9 +103,9 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]), H + return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) from gtsam.symbol_shorthand import X @@ -143,7 +143,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) @@ -181,7 +181,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func)