search

HKUST-Aerial-Robotics / VINS-Fusion

An optimization-based multi-sensor state estimator
GNU General Public License v3.0
3.42k stars 1.37k forks source link

Optimization nan at vins_estimator #147

Open takmin opened 3 years ago

takmin commented 3 years ago

I encountered similar error to #131 without loop closure.

I tried to run vins_estimator with l515 with the following realsense launch file and vins-fusion config file. These parameters were calibrated with kalibr.

realsense launch file:

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>

  <arg name="fisheye_width"       default="640"/>
  <arg name="fisheye_height"      default="480"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="320"/>
  <arg name="depth_height"        default="240"/>
  <arg name="enable_depth"        default="false"/>

  <arg name="infra_width"        default="640"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra1"       default="false"/>
  <arg name="enable_infra2"       default="false"/>

  <arg name="color_width"         default="960"/>
  <arg name="color_height"        default="540"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"          default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="400"/>
  <arg name="accel_fps"           default="400"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"           default="true"/>
  <arg name="align_depth"           default="true"/>

  <arg name="filters"               default=""/>
  <arg name="clip_distance"         default="-2"/>
  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="initial_reset"         default="false"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="hold_back_imu_for_frames"      default="true"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"             value="$(arg infra_width)"/>
      <arg name="infra_height"            value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"               value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
    </include>
  </group>
</launch>

vins-fusion config file:

%YAML:1.0

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

imu_topic: "/camera/imu"
image0_topic: "/camera/color/image_raw"
output_path: "/home/takuya/output/"

cam0_calib: "camera.yaml"
image_width: 960
image_height: 540

# 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.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.99607877, -0.04560221, 0.07581241, -0.02188346,
          0.04377633, 0.99871395, 0.0255749, -0.19861288,
          -0.07688119, -0.02215582, 0.99679406, -0.19711316,
          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.3441076688576152          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.0229409927751298         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.17075981264651297         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.0028335069421464624       # gyroscope bias random work noise standard deviation.     #4.0e-6
g_norm: 9.805         # gravity magnitude

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: -0.031058326182504023                             # 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/cybermedia/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

I started vins-fusion and encountered error.

$ roslaunch vins vins_rviz.launch &
$ roslaunch realsense2_camera rs_camera_vins_l515.launch
$ $ rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_l515/realsense_mono_imu_config.yaml

init first imu pose
averge acc -3.054389 -9.131524 1.318447
init R0
  0.94933 -0.311055 0.0449115
        0  0.142902  0.989737
-0.314281 -0.939587  0.135661
[ INFO] [1615302142.310426977]: Not enough features or parallax; Move device around
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0310 00:02:22.452554 16369 parameter_block.h:345] Local parameterization Jacobian computation returnedan invalid matrix for x: -nan -nan -nan -nan
 Jacobian matrix : 0nan 0nan 0nan
-nan -nan 0nan
0nan -nan -nan
-nan 0nan -nan
F0310 00:02:22.452662 16369 parameter_block.h:198] Check failed: UpdateLocalParameterizationJacobian() Local parameterization Jacobian computation failed for x: -nan -nan -nan -nan
*** Check failure stack trace: ***
core dump

I changed ros log level and saw the error in detail

$ rosrun vins vins_node src/VINS-Fusion/config/realsense_l515/realsense_mono_imu_config.yaml 
[ INFO] [1615692789.135983499]: init begins
config_file: src/VINS-Fusion/config/realsense_l515/realsense_mono_imu_config.yaml
USE_IMU: 1
IMU_TOPIC: /camera/imu
result path /home/cybermedia/output//vio.csv
[ WARN] [1615692789.138818182]:  Optimize extrinsic param around initial guess!
camera number 1
[ INFO] [1615692789.139549714]: Unsynchronized sensors, online estimate time offset, initial td: 0
[ INFO] [1615692789.139567611]: ROW: 540 COL: 960 
 exitrinsic cam 0
 0.999954 -0.000467 -0.009606
 0.000799    0.9994  0.034635
 0.009584 -0.034641  0.999354
 -0.0126  0.00284 0.006998
set g     0     0 9.805
[ INFO] [1615692789.139658584]: reading paramerter of camera src/VINS-Fusion/config/realsense_l515/camera.yaml
MULTIPLE_THREAD is 1
[ WARN] [1615692789.139785403]: waiting for image and imu...
[DEBUG] [1615693018.245505633]: Connection::drop(2)
[DEBUG] [1615693018.245632462]: TCP socket [11] closed
[DEBUG] [1615693018.245688544]: Connection::drop(0)
[DEBUG] [1615693018.245748109]: Connection::drop(2)
[DEBUG] [1615693018.245864205]: Connection::drop(2)
[DEBUG] [1615693025.433634019]: Received update for topic [/camera/imu] (1 publishers)
[DEBUG] [1615693025.433699295]: Publisher update for [/camera/imu]: http://Cybermedia-Vostro-5490:37351/,  already have these connections: 
[DEBUG] [1615693025.433798612]: Began asynchronous xmlrpc connection to [Cybermedia-Vostro-5490:37351]
[DEBUG] [1615693025.434406530]: Received update for topic [/camera/color/image_raw] (1 publishers)
[DEBUG] [1615693025.434436251]: Publisher update for [/camera/color/image_raw]: http://Cybermedia-Vostro-5490:37351/,  already have these connections: 
[DEBUG] [1615693025.434533030]: Began asynchronous xmlrpc connection to [Cybermedia-Vostro-5490:37351]
[DEBUG] [1615693025.635336647]: Connecting via tcpros to topic [/camera/color/image_raw] at host [Cybermedia-Vostro-5490:35243]
[DEBUG] [1615693025.635574265]: Resolved publisher host [Cybermedia-Vostro-5490] to [127.0.1.1] for socket [14]
[DEBUG] [1615693025.635677997]: Adding tcp socket [14] to pollset
[DEBUG] [1615693025.635711832]: Async connect() in progress to [Cybermedia-Vostro-5490:35243] on socket [14]
[DEBUG] [1615693025.635768525]: Connected to publisher of topic [/camera/color/image_raw] at [Cybermedia-Vostro-5490:35243]
[DEBUG] [1615693025.635805844]: Connecting via tcpros to topic [/camera/imu] at host [Cybermedia-Vostro-5490:35243]
[DEBUG] [1615693025.635903787]: Resolved publisher host [Cybermedia-Vostro-5490] to [127.0.1.1] for socket [15]
[DEBUG] [1615693025.635944477]: Adding tcp socket [15] to pollset
[DEBUG] [1615693025.635969039]: Async connect() in progress to [Cybermedia-Vostro-5490:35243] on socket [15]
[DEBUG] [1615693025.636011347]: Connected to publisher of topic [/camera/imu] at [Cybermedia-Vostro-5490:35243]
init first imu pose
averge acc -3.393105 -9.384964 0.686468
init R0 
 0.940712 -0.338302 0.0247452
        0 0.0729506  0.997336
-0.339205 -0.938206 0.0686255
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0314 12:37:06.469364 14526 residual_block.cc:130] 

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: 3 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:              inf         -inf 

Parameter Block 0, size: 4

    0.999955 |      169.162     -88.6992 
  0.00257361 |      11460.9        -6036 
 -0.00908786 |      21866.2     -11512.2 
 -0.00133184 |     -50.5089      294.072 

Parameter Block 1, size: 3

   0.0806101 |     -5.21405           -0 
  -0.0280095 |           -0     -5.21405 
    0.119354 |      543.872     -286.398 

Parameter Block 2, size: 3

    -20.1169 |      4.66792     -5.18916 
     10.5083 |      2.79882     -6.69498 
 3.46882e-12 |       543.87      -286.32 

E0314 12:37:06.469475 14526 trust_region_minimizer.cc:73] Terminating: Residual and Jacobian evaluation failed.
[ WARN] [1615693026.469850170]: gyroscope bias initial calibration -0.000474674  -0.00118822 -0.000186036
[ INFO] [1615693026.470229516]: misalign visual structure with IMU
[ WARN] [1615693026.598303594]: gyroscope bias initial calibration -nan -nan -nan
[ INFO] [1615693026.714800860]: Initialization finish!
[ INFO] [1615693026.714975415]: td 0.000000
time: 1615300596.271713, t: nan nan -nan q: nan -nan -nan -nan 
[ WARN] [1615693026.726596187]: Marker 'CameraPoseVisualization/1': invalid point[0] (nan, nan, nan)
[ WARN] [1615693026.726629238]: Marker 'CameraPoseVisualization/1': invalid point[1] (nan, nan, nan)
[ WARN] [1615693026.726644708]: Marker 'CameraPoseVisualization/1': invalid point[2] (nan, nan, nan)
[ WARN] [1615693026.726659906]: Marker 'CameraPoseVisualization/1': invalid point[3] (nan, nan, nan)
.
.
.

It seems something wrong around imu, but I have no idea. I appreciate if anybody gives me any hints to solve this problem.

takmin commented 3 years ago

I carefully traced the error and found the reason:

In FeatureTracker::undistortedPts(), tracking points are projected to new coordinates which eliminates lens distortion effect., however some points are projected to inf. I set "distortion_parameters:" to zero in camera.txt, then it worked.

takmin commented 3 years ago

This error is caused in PinholeCamera::liftProjective(). At line 501 in PinholeCamera.cc, parameter d_u is amplified by calling distortion() function recursively. I could avoid error by changing the number of iteration to 1. I don't know why the distortion() function is called iteratively, so I wonder if it is the right solution.

ahjcahkl commented 7 months ago

I also encountered a similar problem. I set the iteration count of this function to 1 and it can run normally, up to a maximum of 3. However, if I add it up again, there will be an issue with nan, which I am very confused about. I have recalibrated the parameters of the IMU and camera, and successfully performed Kalibr joint calibration. Do you have any idea about this? I would greatly appreciate it if you could answer me.