VINS-Fusion
VINS-Fusion copied to clipboard
Published
20 hours ago •
HKUST-Aerial-Robotics
HKUST-Aerial-Robotics
NaN values: Error in evaluating the ResidualBlock.
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-radtanmodels - I recorded another bag with the imu topic only and used
imu_utilsto 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 unfortunately, I sopped working on this for now. It's not yet resolved.
@mzahana Did you solve this problem later? I am also facing this issue.
