Skip to content

Python cost function only works for scalar #70

@nmerrillq

Description

@nmerrillq

I tried a Python cost function using the following script:

import numpy as np

np.random.seed(123)

class HelloCostFunction(pyceres.CostFunction):
    def __init__(self, A, gt):
        pyceres.CostFunction.__init__(self)
        self.A = A
        self.gt = gt
        self.set_num_residuals(A.shape[0])
        self.set_parameter_block_sizes([A.shape[1]])

    def Evaluate(self, parameters, residuals, jacobians):
        x = parameters[0]
        r = gt - A @ x
        print("r =", r)
        residuals[:] = r
        if jacobians is not None and jacobians[0] is not None:
            J = -A
            print("J =", J)
            jacobians[0][:] = J.flatten()

        return True
    
    
if __name__ == "__main__":
    A = np.random.randn(3, 3)
    x = np.random.randn(3)
    gt = A @ x
    x_ptb = x + np.random.randn(3)
    x_ptb_init = x_ptb.copy()
    prob = pyceres.Problem()
    cost = HelloCostFunction(A, gt)
    prob.add_residual_block(cost, None, [x_ptb])
    options = pyceres.SolverOptions()
    options.minimizer_progress_to_stdout = True
    summary = pyceres.SolverSummary()
    pyceres.solve(options, prob, summary)
    print(summary.BriefReport())
    print(f"{x_ptb_init} -> {x_ptb} -> {x}")

and it gave the following output:

r = [0.11546025 1.8127238  3.26756541]
J = [[ 1.0856306  -0.99734545 -0.2829785 ]
 [ 1.50629471  0.57860025 -1.65143654]
 [ 2.42667924  0.42891263 -1.26593626]]
W20250805 16:34:43.433236 130239502679616 residual_block.cc:129] 

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all    
residual and jacobians that were requested or there was a non-finite value (nan/infinite)
generated during the or jacobian computation. 

Residual Block size: 1 parameter blocks x 3 residuals

For each parameter block, the value of the parameters are printed in the first column   
and the value of the jacobian under the corresponding residual. If a ParameterBlock was 
held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry 
of the Jacobian/residual array was requested but was not written to by user code, it is 
indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating 
to Inf or NaN is also an error.  

Residuals:          3.26757 Uninitialized Uninitialized 

Parameter Block 0, size: 3

    0.624649 |     -1.26594 Uninitialized Uninitialized 
    -1.31779 | Uninitialized Uninitialized Uninitialized 
   -0.538691 | Uninitialized Uninitialized Uninitialized 


E20250805 16:34:43.433461 130239502679616 trust_region_minimizer.cc:71] Terminating: Residual and Jacobian evaluation failed.
Ceres Solver Report: Iterations: 0, Initial cost: -1.000000e+00, Final cost: -1.000000e+00, Termination: FAILURE
[ 0.62464922 -1.31778815 -0.53869093] -> [ 0.62464922 -1.31778815 -0.53869093] -> [-0.8667404  -0.67888615 -0.09470897]

Only the first element of residuals and Jacobians gets set whenever a numpy array is inserted. I also tried setting each element separately e.g.,

        residuals[0] = r[0]
        residuals[1] = r[1]
        residuals[2] = r[2]

and the result was the same.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions