12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1618 from cntaylor/develop 

Update Python CustomFactor documentation and example
release/4.3a0
Frank Dellaert 2023-09-01 23:26:59 -07:00 committed by GitHub
parent 98d1ceedc6 039f5cf711
commit 5d44dbbd47
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
python/CustomFactors.md
View File

@ -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

10
python/gtsam/examples/CustomFactorExample.py
View File

@ -15,7 +15,7 @@ from typing import List, Optional
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