NaN values: Error in evaluating the ResidualBlock.

[mzahana]

Hello.

I am trying to use VINS-FUSION with Realsense T265. I followed the following steps to run the setup.

• I recorded a rosbag of the two fisheye cameras at 5Hz rate.
• I did intrinsic camera calibration using Kalibr with omni-radtan models
• I recorded another bag with the imu topic only and used imu_utils to obtain IMU noise parameters
• I used Kalibr to calibrate cameras and IMU
• The resulting parameters were entered in the config file read by VINS, which looks like

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

imu_topic: "/camera/imu"
image0_topic: "/camera/fisheye1/image_raw"
image1_topic: "/camera/fisheye2/image_raw"
output_path: "/home/dji/output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 848
image_height: 800
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

# Left
body_T_cam0: !!opencv-matrix #cam0 coordinate under body coordinate
   rows: 4
   cols: 4
   dt: d
   data: [ -0.99991462,  0.00042657,  0.01306038,  0.01202619,
           -0.0004514,  -0.9999981,  -0.00189783,  0.00503566,
           0.01305954, -0.00190356,  0.99991291,  0.00686882,
           0., 0., 0., 1.]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -0.999985,    0.00234903,  0.00494827, -0.05189573,
           -0.00234375, -0.99999668,  0.00107182,  0.00487799, 
           0.00495077,  0.00106021,  0.99998718,  0.00708981,
           0., 0., 0., 1.]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 0           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.018536          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.00202783         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.00061931 #0.001         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.000014786 #0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.006                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

Parameters of cameras look like,
left.yaml

%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 848
image_height: 800
mirror_parameters:
   xi: 1.9286037242934946
distortion_parameters:
   k1: 0.20381275767624374
   k2: -1.0537353534299863
   p1: 0.001670811101258219
   p2: 0.0028602713865310443
projection_parameters:
   gamma1: 833.6134141376159
   gamma2: 832.8879658406196
   u0: 431.54017928631544
   v0: 396.98924938802116

right.yaml

%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 848
image_height: 800
mirror_parameters:
   xi: 1.8580237672703006
distortion_parameters:
   k1: 0.13359483986040754
   k2: -0.8376114753219499
   p1: 0.0016131745885228281
   p2: 0.003565070055993986
projection_parameters:
   gamma1: 813.8713413792765
   gamma2: 813.3957874200545
   u0: 426.0041925027413
   v0: 408.3802008012069

When I run the vins_node, estimates initially follow the camera motion, but quickly diverges very fast.

The following is the output on the terminal,

[ INFO] [1576862966.629656501]: td 0.011367
time: 1576862966.550582, t: 0.073094 0.154861 -0.026418 q: 0.642189 0.727043 0.167409 0.176000 
[ INFO] [1576862966.695998814]: td 0.010995
time: 1576862966.617249, t: 0.071198 0.123189 -0.025846 q: 0.643381 0.737629 0.134650 0.154382 
[ INFO] [1576862966.759402211]: td 0.010895
time: 1576862966.683916, t: 0.068801 0.085802 -0.027095 q: 0.654060 0.742144 0.086493 0.118098 
[ INFO] [1576862966.826913316]: td 0.010828
time: 1576862966.750583, t: 0.062437 0.047840 -0.029200 q: 0.667815 0.740562 0.028263 0.069225 
[ INFO] [1576862966.900128764]: td 0.010765
time: 1576862966.817250, t: 0.050621 0.019482 -0.029011 q: 0.664513 0.745606 -0.010532 0.048817 
[ INFO] [1576862966.957082326]: td 0.010765
time: 1576862966.883916, t: 0.035719 -0.006006 -0.028085 q: 0.672607 0.737463 -0.056907 0.022570 
WARNING: Logging before InitGoogleLogging() is written to STDERR
W1220 20:29:26.986747 25200 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: 6 parameter blocks x 2 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 

Parameter Block 0, size: 7

   0.0620158 |          nan          nan 
   0.0477048 |          nan          nan 
   -0.029268 |          nan          nan 
    0.740544 |         -nan         -nan 
   0.0282824 |         -nan         -nan 
   0.0692493 |         -nan         -nan 
    0.667832 |            0            0 

Parameter Block 1, size: 7

   0.0180501 |          nan          nan 
  -0.0245418 |          nan          nan 
  -0.0256433 |          nan          nan 
    0.723155 |         -nan         -nan 
  -0.0992446 |         -nan         -nan 
-0.000940254 |          nan          nan 
    0.683518 |            0            0 

Parameter Block 2, size: 7

   0.0104869 |          nan          nan 
  0.00232016 |          nan          nan 
 -0.00557417 |          nan          nan 
  0.00683487 |         -nan         -nan 
 0.000214489 |          nan          nan 
    0.999977 |          nan          nan 
-0.000264728 |            0            0 

Parameter Block 3, size: 7

  -0.0534077 |          nan          nan 
  0.00214623 |          nan          nan 
 -0.00563106 |          nan          nan 
  0.00310245 |          nan          nan 
  0.00172923 |         -nan         -nan 
    0.999993 |          nan          nan 
  -0.0012928 |            0            0 

Parameter Block 4, size: 1

     23.2277 |          nan          nan 

Parameter Block 5, size: 1

   0.0107647 | Not Computed  Not Computed  


E1220 20:29:26.986863 25200 trust_region_minimizer.cc:72] Terminating: Residual and Jacobian evaluation failed.
[ INFO] [1576862966.994982779]: td 0.010765
time: 1576862966.950576, t: 0.018050 -0.024542 -0.025643 q: 0.683518 0.723155 -0.099245 -0.000940 
W1220 20:29:27.055917 25200 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: 6 parameter blocks x 2 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. 

I need help to understand and solve this issue.

Cheers.

[hridaybavle]

Hi,

Where you able to solve the problem? I am facing the same issue.

[mzahana]

@hridaybavle unfortunately, I sopped working on this for now. It's not yet resolved.

[hridaybavle]

Hi @mzahana I was able to solve the issue. You can see this pull request.

[zhangjianglv]

@mzahana Did you solve this problem later? I am also facing this issue.