mikeferguson/robot_calibration

Showing INITIAL_COSTS as nan while Optimizing

Closed this issue · 4 comments

saurabhbansal90 commented

Hello Mike,
In the process of calibrating my robot, I am facing one more problem.
The scenario is like as below:

  1. There was a problem in using led_finder with Stereo camera (it is not able to measure the depth), so we created one more finder, that we call as mark_finder (concentric circles with different colors).
    So as a whole, we are using our own finder, which seems to be working fine.
  2. I am using manual method to move the arm at different locations.
  3. I am not using it for camera calibration, so I assume, depth_camera_info is not needed. Please correct me if I am wrong.
  4. So from finder, I am providing only 2 points i.e. rgbd_pt (position of mark wrt depth camera frame) and world_pt (position of mark wrt gripper frame)
  5. At now for simplicity, I am using only 1 free_frame and 1 free_param.

But while optimising, I am getting below logs:

[ INFO] [1530684518.983065179]: Done capturing samples
[ INFO] [1530684519.215928914]: Creating chain 'arm' from base_link to target
[ INFO] [1530684519.216580523]: Creating camera3d 'camera' in frame camera_depth_optical_frame
[ INFO] [1530684519.217042632]: optimize: number of offsets: 7
INITIAL COST (0)
x: -nan
y: -nan
z: -nan

INITIAL COST (1)
x: -nan
y: -nan
z: -nan

INITIAL COST (2)
x: -nan
y: -nan
z: -nan

INITIAL COST (3)
x: -nan
y: -nan
z: -nan

INITIAL COST (4)
x: -nan
y: -nan
z: -nan

INITIAL COST (5)
x: -nan
y: -nan
z: -nan

INITIAL COST (6)
x: -nan
y: -nan
z: -nan

INITIAL COST (7)
x: -nan
y: -nan
z: -nan

INITIAL COST (8)
x: -nan
y: -nan
z: -nan

INITIAL COST (9)
x: -nan
y: -nan
z: -nan

INITIAL COST (10)
x: -nan
y: -nan
z: -nan

INITIAL COST (11)
x: -nan
y: -nan
z: -nan

Solver output:
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0704 11:38:39.255892 10933 residual_block.cc:131]

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: -nan -nan -nan

Parameter Block 0, size: 7

       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan 
       0 |         -nan         -nan         -nan

E0704 11:38:39.256040 10933 trust_region_minimizer.cc:73] Terminating: Residual and Jacobian evaluation failed.

Ceres Solver Report: Iterations: 0, Initial cost: -1.000000e+00, Final cost: -1.000000e+00, Termination: FAILURE
Parameter Offsets:
joint5: 0
joint6_x: 0
joint6_y: 0
joint6_z: 0
joint6_a: 0
joint6_b: 0
joint6_c: -1.49012e-08

[ INFO] [1530684519.258365438]: Done calibrating
================================================================================REQUIRED process [robot_calibration-1] has died!
process has finished cleanly
log file: /home/saurabh/.ros/log/bdc8d9aa-7f4f-11e8-a77c-a86bad217c79/robot_calibration-1*.log
Initiating shutdown!
================================================================================
[calibration_bagger-2] killing on exit
[robot_calibration-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

I think nan values in INITIAL_COSTS seems to be the problem. And reason for it seems to be the cost function has not done the evaluation.
Can you please give some input, for possible reason of this error.

Please find the attached calibrate.yaml, capture.yaml, and calibration_data.bag file attached.
Also please let me know if you need some more information.
config.zip

Saurabh

mikeferguson commented

You should never have NANs in the cost -- that will blow things up.

If you're using the standard camera model, you will still need to fill in the P matrix of the camera_info (it looks like it is all zeroes, which causes a divide by zero). We project through P and then back again, even if they are not free parameters (https://github.com/mikeferguson/robot_calibration/blob/master/robot_calibration/src/models.cpp#L175)

I suppose we could also check that all the FX/CX/FY/CY are non-zero, otherwise we skip that portion of computation. It would also speed things up if we check if any of them are free parameters (although the API doesn't allow us to check that too easily on the CalibrationOffsetsParser)

saurabhbansal90 commented

Hello Mike, thanks for response.

  1. My assumption was that camera_info is needed for only camera calibration. So I can avoid it and leave it to be 0.
  2. I think I can remove the camera_info related code from function Camera3dModel::project and project the points though fk only.
    But as you already told, that will result problems in CalibrationOffsetsParser, but I think offset_ is having only free_params and free_frames. So will it be having some issue if I remove camera_info.

Saurabh

mikeferguson commented

Ah -- so, I just realized that we don't need to change any code! What you want to do is use a ChainModel rather than a CameraModel -- even though it is a camera. The ChainModel does the exact same thing as CameraModel (project points in the tip/sensor frame into base frame) but without the camera projection modeling (which requires a camera_info.P matrix). Simply changing the type of model in your calibration.yaml should make the optimization run.

(this is an interesting use case that I hadn't thought of -- we should add something to the documentation that notes that CameraModel NEEDS P matrix, but ChainModel can be used anytime you don't want to calibrate CameraInfo parameters (possibly a speed performance for some users too)).

mikeferguson commented

Documentation updates in #99