Support evaluation on residuals and Jacobian from pyceres.Problem

[B1ueber2y]

Also support partial evaluation.

Example with the NormalCost:

x1 = np.zeros(3)
x2 = np.ones(3)
x1_prior = np.arange(3)
covar = np.eye(3)
prob = pyceres.Problem()
loss = pyceres.TrivialLoss()
prob.add_residual_block(pyceres.factors.NormalPrior(x1_prior, covar), loss, [x1])
prob.add_residual_block(pyceres.factors.NormalError(covar), loss, [x1, x2])
print(prob.evaluate_residuals())
# Output:
# [0.0, -1.0, -2.0, -1.0, -1.0, -1.0]

options = pyceres.SolverOptions()
summary = pyceres.SolverSummary()
pyceres.solve(options, prob, summary)

# Example: full evaluation
eval_options = pyceres.EvaluateOptions()
jacobian = prob.evaluate_jacobian(eval_options)
rows, cols, values = jacobian.to_tuple()
sparse_matrix = scipy.sparse.coo_matrix((values, (rows, cols)), shape=(6, 6))
print(sparse_matrix.toarray())
# Output:
# [[ 1.  0.  0.  0.  0.  0. ]
#  [ 0.  1.  0.  0.  0.  0. ]
#  [ 0.  0.  1.  0.  0.  0. ]
#  [ 1.  0.  0. -1.  0.  0. ]
#  [ 0.  1.  0.  0. -1.  0. ]
#  [ 0.  0.  1.  0.  0. -1. ]]

 # Example: partial evaluation
eval_options.set_parameter_blocks([x2])
jacobian = prob.evaluate_jacobian(eval_options)
rows, cols, values = jacobian.to_tuple()
sparse_matrix = scipy.sparse.coo_matrix((values, (rows, cols)), shape=(6, 3))
print(sparse_matrix.toarray())
# Output:
# [[  0.  0.  0. ]
#  [  0.  0.  0. ]
#  [  0.  0.  0. ]
#  [ -1.  0.  0. ]
#  [  0. -1.  0. ]
#  [  0.  0. -1. ]]

[B1ueber2y]

Thanks. I have made an update.