Fix example in docs

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)