First shot
-
Copy the .launch file in package VINS-Fusion to the directory of realsense2_cameara/launch
$ cp /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/rs_camera.launch /home/william/catkin_ws/src/realsense-2.1.3/realsense2_camera/launch/rs_imu_camear.launch
roslaunch vins vins_rviz.launch rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
roslaunch realsense2_camera rs_imu_camera.launch
then error occurred
[ERROR] [1555901595.625850921]: Hardware Notification:Right MIPI error,1.5559e+12,Error,Hardware Error
change the USB and Type-C hardware interface to another one because sometimes someone might don't work.
1rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml 2[ INFO] [1555916680.872819604]: init begins 3config_file: /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml 4USE_IMU: 1 5IMU_TOPIC: /camera/imu 6result path /home/william/Documents/output/vinsFusion//vio.csv 7[ WARN] [1555916726.571724172]: Optimize extrinsic param around initial guess! 8camera number 2 9[ INFO] [1555916726.571853778]: Unsynchronized sensors, online estimate time offset, initial td: 0 10[ INFO] [1555916726.571872262]: ROW: 480 COL: 640 11 exitrinsic cam 0 12-0.00575863 -0.00404633 0.999975 13 -0.999983 -0.00102246 -0.00576281 14 0.00104575 -0.999991 -0.00404037 15 0.0203293 0.00793252 0.00285598 16 exitrinsic cam 1 17-0.00100218 0.000363135 0.999999 18 -0.999992 -0.00383034 -0.00100078 19 0.00382998 -0.999993 0.000366971 20 0.0152858 -0.0524358 0.00869313 21set g 0 0 9.805 22[ INFO] [1555916726.572012229]: reading paramerter of camera /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/left.yaml 23[ INFO] [1555916726.572176400]: reading paramerter of camera /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/right.yaml 24MULTIPLE_THREAD is 1 25[ WARN] [1555916726.572294355]: waiting for image and imu... 26throw img0 27throw img1 28init first imu pose 29averge acc -1602603144.533333 534233088.000000 534354329.600000 30init R0 31 0.426462 0.639509 0.639655 32 0 0.707187 -0.707027 33-0.904506 0.30152 0.301588 34feature tracking not enough, please slowly move your device! 35feature tracking not enough, please slowly move your device! 36feature tracking not enough, please slowly move your device! 37feature tracking not enough, please slowly move your device! 38feature tracking not enough, please slowly move your device! 39[ WARN] [1555916728.459035454]: gyroscope bias initial calibration -0.0933654 0.265827 -0.0616124 40[ WARN] [1555916728.463995949]: numerical unstable in preintegration 41[ WARN] [1555916728.464149835]: numerical unstable in preintegration 42[ WARN] [1555916728.470874921]: numerical unstable in preintegration 43...
update the latest version Realsense SDK (2.21.0) and ROS package (2.2)
merge the rs_camera.launch in VINS-Fusion and Realsense, put it in the directory of realsense2_cameara/launch
looks like this:
1<launch> 2 <arg name="serial_no" default=""/> 3 <arg name="json_file_path" default=""/> 4 <arg name="camera" default="camera"/> 5 <arg name="tf_prefix" default="$(arg camera)"/> 6 7 <arg name="fisheye_width" default="640"/> 8 <arg name="fisheye_height" default="480"/> 9 <arg name="enable_fisheye" default="true"/> 10 11 <arg name="depth_width" default="640"/> 12 <arg name="depth_height" default="480"/> 13 <arg name="enable_depth" default="true"/> 14 15 <arg name="infra_width" default="640"/> 16 <arg name="infra_height" default="480"/> 17 <arg name="enable_infra1" default="true"/> 18 <arg name="enable_infra2" default="true"/> 19 20 <arg name="color_width" default="640"/> 21 <arg name="color_height" default="480"/> 22 <arg name="enable_color" default="true"/> 23 24 <arg name="fisheye_fps" default="30"/> 25 <arg name="depth_fps" default="30"/> 26 <arg name="infra_fps" default="30"/> 27 <arg name="color_fps" default="30"/> 28 <arg name="gyro_fps" default="200"/> 29 <arg name="accel_fps" default="250"/> 30 <arg name="enable_gyro" default="true"/> 31 <arg name="enable_accel" default="true"/> 32<!-- 33 <arg name="enable_imu" default="true"/> 34--> 35 36 <arg name="enable_pointcloud" default="false"/> 37 <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/> 38 <arg name="pointcloud_texture_index" default="0"/> 39 40 <arg name="enable_sync" default="true"/> 41 <arg name="align_depth" default="true"/> 42 43 <arg name="filters" default=""/> 44 <arg name="clip_distance" default="-2"/> 45 <arg name="linear_accel_cov" default="0.01"/> 46 <arg name="initial_reset" default="false"/> 47 <arg name="unite_imu_method" default="linear_interpolation"/> 48 <arg name="topic_odom_in" default="odom_in"/> 49 <arg name="calib_odom_file" default=""/> 50 <arg name="publish_odom_tf" default="true"/> 51 <arg name="hold_back_imu_for_frames" default="true"/> 52 53 <group ns="$(arg camera)"> 54 <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml"> 55 <arg name="tf_prefix" value="$(arg tf_prefix)"/> 56 <arg name="serial_no" value="$(arg serial_no)"/> 57 <arg name="json_file_path" value="$(arg json_file_path)"/> 58 59 <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/> 60 <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/> 61 <arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/> 62 <arg name="enable_sync" value="$(arg enable_sync)"/> 63 <arg name="align_depth" value="$(arg align_depth)"/> 64 65 <arg name="fisheye_width" value="$(arg fisheye_width)"/> 66 <arg name="fisheye_height" value="$(arg fisheye_height)"/> 67 <arg name="enable_fisheye" value="$(arg enable_fisheye)"/> 68 69 <arg name="depth_width" value="$(arg depth_width)"/> 70 <arg name="depth_height" value="$(arg depth_height)"/> 71 <arg name="enable_depth" value="$(arg enable_depth)"/> 72 73 <arg name="color_width" value="$(arg color_width)"/> 74 <arg name="color_height" value="$(arg color_height)"/> 75 <arg name="enable_color" value="$(arg enable_color)"/> 76 77 <arg name="infra_width" value="$(arg infra_width)"/> 78 <arg name="infra_height" value="$(arg infra_height)"/> 79 <arg name="enable_infra1" value="$(arg enable_infra1)"/> 80 <arg name="enable_infra2" value="$(arg enable_infra2)"/> 81 82 <arg name="fisheye_fps" value="$(arg fisheye_fps)"/> 83 <arg name="depth_fps" value="$(arg depth_fps)"/> 84 <arg name="infra_fps" value="$(arg infra_fps)"/> 85 <arg name="color_fps" value="$(arg color_fps)"/> 86 <arg name="gyro_fps" value="$(arg gyro_fps)"/> 87 <arg name="accel_fps" value="$(arg accel_fps)"/> 88 <arg name="enable_gyro" value="$(arg enable_gyro)"/> 89 <arg name="enable_accel" value="$(arg enable_accel)"/> 90<!-- 91 <arg name="enable_imu" value="$(arg enable_imu)"/> 92 --> 93 94 <arg name="filters" value="$(arg filters)"/> 95 <arg name="clip_distance" value="$(arg clip_distance)"/> 96 <arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/> 97 <arg name="initial_reset" value="$(arg initial_reset)"/> 98 <arg name="unite_imu_method" value="$(arg unite_imu_method)"/> 99 <arg name="topic_odom_in" value="$(arg topic_odom_in)"/> 100 <arg name="calib_odom_file" value="$(arg calib_odom_file)"/> 101 <arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/> 102 103 </include> 104 </group> 105</launch>
then roslaunch realsense2_camera rs_imu_camera.launch
error
1unused args [enable_imu] for include of [/home/william/catkin_ws/src/realsense-2.2/realsense2_camera/launch/includes/nodelet.launch.xml] 2The traceback for the exception was written to the log file
Delete everything related to enable_imu.
Seems like the same problem. Change the USB and Type-C hardware interface to another one because sometimes someone might don't work.
Error again.
1$ rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml 2[ INFO] [1556005316.069145892]: init begins 3config_file: /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml 4USE_IMU: 1 5IMU_TOPIC: /camera/imu 6result path /home/william/Documents/output/vinsFusion//vio.csv 7[ WARN] [1556005316.073310738]: Optimize extrinsic param around initial guess! 8camera number 2 9[ INFO] [1556005316.073429273]: Unsynchronized sensors, online estimate time offset, initial td: 0 10[ INFO] [1556005316.073443294]: ROW: 480 COL: 640 11 exitrinsic cam 0 12-0.00575863 -0.00404633 0.999975 13 -0.999983 -0.00102246 -0.00576281 14 0.00104575 -0.999991 -0.00404037 15 0.0203293 0.00793252 0.00285598 16 exitrinsic cam 1 17-0.00100218 0.000363135 0.999999 18 -0.999992 -0.00383034 -0.00100078 19 0.00382998 -0.999993 0.000366971 20 0.0152858 -0.0524358 0.00869313 21set g 0 0 9.805 22[ INFO] [1556005316.073557518]: reading paramerter of camera /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/left.yaml 23[ INFO] [1556005316.073686245]: reading paramerter of camera /home/william/catkin_ws/src/VINS-Fusion/config/realsense_d435i/right.yaml 24MULTIPLE_THREAD is 1 25[ WARN] [1556005316.073774275]: waiting for image and imu... 26wait for imu ... 27wait for imu ... 28...
grep -rn * -ie wait
1src/estimator/estimator.cpp:141: printf("wait for imu\n"); 2src/estimator/estimator.cpp:172: printf("wait for imu ... \n"); 3src/KITTIGPSTest.cpp:181: // cv::waitKey(2); 4src/KITTIOdomTest.cpp:117: //cv::waitKey(2); 5src/rosNodeTest.cpp:223: ROS_WARN("waiting for image and imu..."); 6src/featureTracker/feature_tracker.cpp:395: // cv::waitKey(0); 7src/featureTracker/feature_tracker.cpp:508: // cv::waitKey(2);
gedit src/estimator/estimator.cpp
1bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, 2 vector<pair<double, Eigen::Vector3d>> &gyrVector) 3{ 4 if(accBuf.empty()) 5 { 6 printf("not receive imu\n"); 7 return false; 8 } 9 //printf("get imu from %f %f\n", t0, t1); 10 //printf("imu fornt time %f imu end time %f\n", accBuf.front().first, accBuf.back().first); 11 if(t1 <= accBuf.back().first) 12 { 13 while (accBuf.front().first <= t0) 14 { 15 accBuf.pop(); 16 gyrBuf.pop(); 17 } 18 while (accBuf.front().first < t1) 19 { 20 accVector.push_back(accBuf.front()); 21 accBuf.pop(); 22 gyrVector.push_back(gyrBuf.front()); 23 gyrBuf.pop(); 24 } 25 accVector.push_back(accBuf.front()); 26 gyrVector.push_back(gyrBuf.front()); 27 } 28 else 29 { 30 printf("wait for imu\n"); 31 return false; 32 } 33 return true; 34}
in this function, t1 <= accBuf.back().first. Search the Internet, nothing similar like this.
3rd shot
Refer to blog 如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据, this guy had run VINS-MONO with D435i.
Modify realsense_color_config.yaml, then it looks like:
1%YAML:1.0 2 3#common parameters 4imu_topic: "/camera/imu" 5image_topic: "/camera/color/image_raw" 6output_path: "/home/william/Documents/output/vinsMono/" 7 8#camera calibration 9model_type: PINHOLE 10camera_name: camera 11image_width: 640 12image_height: 480 13distortion_parameters: 14 k1: 9.2615504465028850e-02 15 k2: -1.8082438825995681e-01 16 p1: -6.5484100374765971e-04 17 p2: -3.5829351558557421e-04 18projection_parameters: 19 fx: 6.0970550296798035e+02 20 fy: 6.0909579671294716e+02 21 cx: 3.1916667152289227e+02 22 cy: 2.3558360480225772e+02 23 24# Extrinsic parameter between IMU and Camera. 25estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 28#If you choose 0 or 1, you should write down the following matrix. 29#Rotation from camera frame to imu frame, imu^R_cam 30extrinsicRotation: !!opencv-matrix 31 rows: 3 32 cols: 3 33 dt: d 34 data: [ 0.99964621, 0.01105994, 0.02418954, 35 -0.01088975, 0.9999151, -0.00715601, 36 -0.02426663, 0.00689006, 0.99968178] 37#Translation from camera frame to imu frame, imu^T_cam 38extrinsicTranslation: !!opencv-matrix 39 rows: 3 40 cols: 1 41 dt: d 42 data: [0.07494282, -0.01077138, -0.00641822] 43 44#feature traker paprameters 45max_cnt: 150 # max feature number in feature tracking 46min_dist: 25 # min distance between two features 47freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48F_threshold: 1.0 # ransac threshold (pixel) 49show_track: 1 # publish tracking image as topic 50equalize: 0 # if image is too dark or light, trun on equalize to find enough features 51fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 53#optimization parameters 54max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55max_num_iterations: 8 # max solver itrations, to guarantee real time 56keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 58#imu parameters The more accurate parameters you provide, the better performance 59acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 60gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 61acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.02 62gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 63g_norm: 9.80 # gravity magnitude 64 65#loop closure parameters 66loop_closure: 1 # start loop closure 67fast_relocalization: 1 # useful in real-time and large project 68load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 69pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 70 71#unsynchronization parameters 72estimate_td: 0 # online estimate time offset between camera and imu 73td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 75#rolling shutter parameters 76rolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera 77rolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet). 78 79#visualization parameters 80save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82visualize_camera_size: 0.4 # size of camera marker in RVIZ
Error [ WARN] [1556065325.371664369]: imu message in disorder!
According to issues getting ros warn imu message in disorder #204 and Doubts about camera-imu temporal calibration #270 in GitHub, definitely it's the problem of unsynchroned image and imu datas.
Reduce fps of imu stream < 100 hz