Fixed Jacobian in python CustomFactorExample

[ulterzlw]

The original Jacobian does not meet the error function.
The modified error function and Jacobian are the correct ones.
They also match the order of unknown[k], unknown[k + 1] and o[k] when constructing the factors.

[dellaert]

Thanks! It looks correct to me now, but could you paste a before/after in the PR comment to assert something that was fixed. I.e., how did you diagnose?

[ulterzlw]

The correction doesn't change the solution value of the state but the internal error term in the factor graph.

Here's a program to test:

def main():
    factor_graph = gtsam.NonlinearFactorGraph()

    gps_model = gtsam.noiseModel.Isotropic.Sigma(1, 0.1)
    gf = gtsam.CustomFactor(gps_model, [0],
                                partial(error_gps, np.array([0])))
    factor_graph.add(gf)

    odo_model = gtsam.noiseModel.Isotropic.Sigma(1, 0.1)
    odof = gtsam.CustomFactor(odo_model, [0, 1],
                                  partial(error_odom, np.array([1])))
    factor_graph.add(odof)

    v = gtsam.Values()
    v.insert(0, np.array([0.0]))                      # optimal value 0.0
    v.insert(1, np.array([0.0]))                      # optimal value 1.0
    params = gtsam.GaussNewtonParams()
    optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)

    result = optimizer.optimize()
    print(result, factor_graph.error(result))

Result after the fix:

Values with 2 values:
Value 0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
        0
]

Value 1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
        1
]

 0.0                                                   <------ correct error

Result before the fix:

Values with 2 values:
Value 0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
        0
]

Value 1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
        1
]

 200.0                                                 <------ wrong error

Similarly, if we print the variable error in class error_odom, it is not decreasing during optimization.

[dellaert]

LGTM, I'll let @ProfFan check to be sure as well as merge

[ProfFan]

Yes that is correct :) Thank you @ulterzlw !