The trajectory is normal when VI sensor is static , but have big drift when moving VI sensor.
Closed this issue · 9 comments
Dear author 👍
The trajectory is normal when VI sensor is static,like the follow picture
But when I moving the sensor , the trajectory have very big drift, Could you give me a hint about the reason ?
Thank you very much!
Can you show screenshots showing the feature statistics when the algorithm is running?
@ke-sun Thank you !
The first image the feature statistics when sensor is static, (feature statistics from ImageProcessor::trackFeatures() function )
The second image the feature statistics when sensor is moving,
the third is the trajectory around xyz directions,(sensor from static to moving)
It seems that the cause is either the initial calibration between IMU and camera is very off or the IMU and cameras are heavily out of synchronization, since most of the features are removed because of the ransac.
Are you using the VI sensor or the ZED camera? If you are using the VI sensor, you can directly use the calibration file for the EuRoC dataset in the config folder.
Thank you , I use zed mini, the manufacturers say IMU and cameras are synchronized , and I use kalibr calibrate zed mini , the report show result is good . the feature statistics in the picture I post ,
[ INFO] [1542771576.482308989]: candidates: 66; track: 66; match: 65; ransac: 62/66=0.939394
I think the ransac: 62/66=0.939394 means after ransac ,most of the features are tracked, not "most of the features are removed because of the ransac" you said. the follow is my calibration result:
results-imucam-srckalibrmutil_camera_2018-11-15-12-25-21.txt
Calibration results
Normalized Residuals
Reprojection error (cam0): mean 0.372595798113, median 0.328318327917, std: 0.227928053606
Reprojection error (cam1): mean 0.364592237435, median 0.315327544554, std: 0.239616626685
Gyroscope error (imu0): mean 0.810287449069, median 0.598746436272, std: 0.719915443382
Accelerometer error (imu0): mean 0.219529698952, median 0.186459513873, std: 0.142965700748
Residuals
Reprojection error (cam0) [px]: mean 0.372595798113, median 0.328318327917, std: 0.227928053606
Reprojection error (cam1) [px]: mean 0.364592237435, median 0.315327544554, std: 0.239616626685
Gyroscope error (imu0) [rad/s]: mean 0.134599166894, median 0.0994594839097, std: 0.119587214419
Accelerometer error (imu0) [m/s^2]: mean 0.256497394035, median 0.217858356431, std: 0.167040404343
Transformation (cam0):
T_ci: (imu0 to cam0):
[[ 0.34789196 -0.9139593 -0.20892482 0.0006666 ]
[ 0.34950669 0.33321157 -0.87567981 -0.00053826]
[ 0.86995187 0.23162134 0.43535652 0.00017377]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.34789196 0.34950669 0.86995187 -0.00019495]
[-0.9139593 0.33321157 0.23162134 0.00074835]
[-0.20892482 -0.87567981 0.43535652 -0.00040773]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.027951306754672903
Transformation (cam1):
T_ci: (imu0 to cam1):
[[ 0.34565937 -0.91444538 -0.21049762 -0.06853095]
[ 0.34722197 0.33304714 -0.87665073 0.00216617]
[ 0.87175484 0.22993315 0.43263639 0.00135753]
[ 0. 0. 0. 1. ]]
T_ic: (cam1 to imu0):
[[ 0.34565937 0.34722197 0.87175484 0.02175279]
[-0.91444538 0.33304714 0.22993315 -0.06370138]
[-0.21049762 -0.87665073 0.43263639 -0.01311394]
[ 0. 0. 0. 1. ]]
timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.039862741914442626
Baselines:
Baseline (cam0 to cam1):
[[ 0.99999615 0.000435 -0.00273956 -0.06919683]
[-0.00044171 0.99999691 -0.00244839 0.00270515]
[ 0.00273848 0.00244959 0.99999325 0.00118325]
[ 0. 0. 0. 1. ]]
baseline norm: 0.06925979775659971 [m]
Gravity vector in target coords: [m/s^2]
[8.63921022 1.39029533 4.42713773]
Calibration configuration
cam0
Camera model: pinhole
Focal length: [1645.2017281774333, 1653.407780277082]
Principal point: [1043.9465784280121, 519.3912838783154]
Distortion model: radtan
Distortion coefficients: [-0.1690224067734941, -0.009015979735236051, -0.004619769664047008, -0.0046167247713686495]
Type: aprilgrid
Tags:
Rows: 8
Cols: 5
Size: 0.02 [m]
Spacing 0.005 [m]
cam1
Camera model: pinhole
Focal length: [1647.932344708378, 1658.51992787514]
Principal point: [1076.7749429562316, 512.2088602013155]
Distortion model: radtan
Distortion coefficients: [-0.20798229154042852, 0.038733808294745924, -0.0019899794376111303, -0.008572244066561187]
Type: aprilgrid
Tags:
Rows: 8
Cols: 5
Size: 0.02 [m]
Spacing 0.005 [m]
IMU configuration
IMU0:
Model: calibrated
Update rate: 500.0
Accelerometer:
Noise density: 0.0522522111452
Noise density (discrete): 1.16839496095
Random walk: 0.0018082934935
Gyroscope:
Noise density: 0.00742879301007
Noise density (discrete): 0.166112861613
Random walk: 7.54124934431e-05
T_i_b
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
It seems the imu to camera rotation does not make sense. The rotation matrix translates to (28.01, -60.45, 45.13) in roll, pitch and yaw. It is possible that yaw is not 0. But the roll and pitch is quite large assuming the PCB for the zed camera is flat.
Thank you , I will check this problem. "The rotation matrix translates to (28.01, -60.45, 45.13) in roll, pitch and yaw", how did you (28.01, -60.45, 45.13)? Transform T(imu to cam) to Euler Angle?
@chenwinki The top left 3x3 matrix of your homogenous transform matrix is the rotation matrix.
i.e.
T_ci: (imu0 to cam1):
[[ 0.34565937 -0.91444538 -0.21049762 -0.06853095]
[ 0.34722197 0.33304714 -0.87665073 0.00216617]
[ 0.87175484 0.22993315 0.43263639 0.00135753]
[ 0. 0. 0. 1. ]]
Take the top left 3x3
R =
[[ 0.34565937 -0.91444538 -0.21049762]
[ 0.34722197 0.33304714 -0.87665073]
[ 0.87175484 0.22993315 0.43263639]]
And apply the equations here https://www.coursera.org/lecture/spacecraft-dynamics-kinematics/7-euler-angle-dcm-relation-WXF1B on slide 27. In matlab you can use the rotm2eul function. Just make sure you follow the ZYX convention.
>> R = [[ 0.34565937 -0.91444538 -0.21049762]
[ 0.34722197 0.33304714 -0.87665073]
[ 0.87175484 0.22993315 0.43263639]];
>> rad2deg(rotm2eul(R))
ans =
45.1292 -60.6632 27.9893
@andre-nguyen Thank you for your help!
@ke-sun Thank you, I have another question, Now I got another calibration result , the transformation between imu and camera seems good,
imu to cam0 yaw pitch roll = 1.52 -1.61 0.0407
imu to cam1 yaw pitch roll = 1.77 -1.61 -0.205
cam0 to cam1 yaw pitch roll = 0.000188 0.00876 -0.00593
But msckf result is still bad. about the covariance ,I use the value in your code , could you give me some hint how can I get these param? or just try some other number?
**// The initial covariance of orientation and position can be
// set to 0. But for velocity, bias and extrinsic parameters,
// there should be nontrivial uncertainty.
double gyro_bias_cov, acc_bias_cov, velocity_cov;
nh.param("initial_covariance/velocity",
velocity_cov, 0.25);
nh.param("initial_covariance/gyro_bias",
gyro_bias_cov, 1e-4);
nh.param("initial_covariance/acc_bias",
acc_bias_cov, 1e-2);
double extrinsic_rotation_cov, extrinsic_translation_cov;
nh.param("initial_covariance/extrinsic_rotation_cov",
extrinsic_rotation_cov, 3.0462e-4);
nh.param("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);**