diff --git a/python/CustomFactors.md b/python/CustomFactors.md index a6ffa2f360..39a840e349 100644 --- a/python/CustomFactors.md +++ b/python/CustomFactors.md @@ -11,17 +11,18 @@ import gtsam import numpy as np from typing import List -def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): +def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: ... ``` `this` is a reference to the `CustomFactor` object. This is required because one can reuse the same `error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of -**references** to the list of required Jacobians (see the corresponding C++ documentation). +**references** to the list of required Jacobians (see the corresponding C++ documentation). Note that +the error returned must be a 1D numpy array. If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error` method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`, -each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable. +each entry of `H` can be assigned a (2D) `numpy` array, as the Jacobian for the corresponding variable. After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM: @@ -42,7 +43,7 @@ from typing import List expected = Pose2(2, 2, np.pi / 2) -def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): +def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index c7ae8ad21d..a78562ecb7 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -15,7 +15,7 @@ import gtsam import numpy as np -I = np.eye(1) +I = np.eye(1) # Creates a 1-element, 2D array def simulate_car() -> List[float]: @@ -30,7 +30,7 @@ def simulate_car() -> List[float]: def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values: gtsam.Values, - jacobians: Optional[List[np.ndarray]]) -> float: + jacobians: Optional[List[np.ndarray]]) -> np.ndarray: """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -44,12 +44,12 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, if jacobians is not None: jacobians[0] = I - return error + return error # with input types this is a 1D np.ndarray def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values: gtsam.Values, - jacobians: Optional[List[np.ndarray]]) -> float: + jacobians: Optional[List[np.ndarray]]) -> np.ndarray: """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -70,7 +70,7 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values: gtsam.Values, - jacobians: Optional[List[np.ndarray]]) -> float: + jacobians: Optional[List[np.ndarray]]) -> np.ndarray: """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle