From 1ebf67520154603279ea05f9ebcbb20326670d23 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Sat, 5 Jun 2021 19:35:32 -0400
Subject: [PATCH] Fix example in docs

---
 python/CustomFactors.md | 21 +++++++++++----------
 1 file changed, 11 insertions(+), 10 deletions(-)

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)