diff --git a/python/CustomFactors.md b/python/CustomFactors.md index abba9e00b..a6ffa2f36 100644 --- a/python/CustomFactors.md +++ b/python/CustomFactors.md @@ -40,24 +40,25 @@ import gtsam import numpy as np from typing import List -def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - # Get the variable values from `v` +expected = Pose2(2, 2, np.pi / 2) + +def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """ + Error function that mimics a BetweenFactor + :param this: reference to the current CustomFactor being evaluated + :param v: Values object + :param H: list of references to the Jacobian arrays + :return: the non-linear error + """ key0 = this.keys()[0] key1 = this.keys()[1] - - # Calculate non-linear error gT1, gT2 = v.atPose2(key0), v.atPose2(key1) - error = gtsam.Pose2(0, 0, 0).localCoordinates(gT1.between(gT2)) + error = expected.localCoordinates(gT1.between(gT2)) - # If we need Jacobian if H is not None: - # Fill the Jacobian arrays - # Note we have two vars, so two entries result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - - # Return the error return error noise_model = gtsam.noiseModel.Unit.Create(3)