Merge pull request #1618 from cntaylor/develop

Update Python CustomFactor documentation and example

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

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