From 6f7f3bbd1bc827986fcc0c9e9adebdf98e9fdcb2 Mon Sep 17 00:00:00 2001 From: tony Date: Wed, 3 Jan 2018 14:45:14 +0800 Subject: [PATCH] fix loading rolling shutter read out time bugs; change physical meaning of read out time (per line) --- .gitignore | 3 +++ config/3dm/3dm_config.yaml | 2 +- config/black_box/black_box_config.yaml | 2 +- config/cla/cla_config.yaml | 2 +- config/euroc/euroc_config.yaml | 2 +- config/euroc/euroc_config_no_extrinsic.yaml | 2 +- vins_estimator/src/factor/projection_td_factor.cpp | 12 ++++++------ vins_estimator/src/parameters.cpp | 5 ++--- 8 files changed, 16 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index dcf5b184a..80a59e7ef 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ *.rviz ex_calib_result.yaml vins_result.csv +simulation.launch +simulation/ +data_generator/ diff --git a/config/3dm/3dm_config.yaml b/config/3dm/3dm_config.yaml index 75f48a89b..e53c1cef4 100644 --- a/config/3dm/3dm_config.yaml +++ b/config/3dm/3dm_config.yaml @@ -77,7 +77,7 @@ td: 0.000 # initial value of time offset. unit: s. rea #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera -rolling_shutter_tr: 0 # unit: s. rolling shutter read out time (from data sheet). tr / ROW : read out time of per line. +rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per line (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/config/black_box/black_box_config.yaml b/config/black_box/black_box_config.yaml index 0f6bf1014..aeba54bdc 100644 --- a/config/black_box/black_box_config.yaml +++ b/config/black_box/black_box_config.yaml @@ -80,7 +80,7 @@ td: 0.0 # initial value of time offset. unit: s. rea #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera -rolling_shutter_tr: 0 # unit: s. rolling shutter read out time (from data sheet). tr / ROW : read out time of per line. +rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per line (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/config/cla/cla_config.yaml b/config/cla/cla_config.yaml index 9ddfc42ca..b5cf24404 100644 --- a/config/cla/cla_config.yaml +++ b/config/cla/cla_config.yaml @@ -72,7 +72,7 @@ td: 0.0 # initial value of time offset. unit: s. rea #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera -rolling_shutter_tr: 0 # unit: s. rolling shutter read out time (from data sheet). tr / ROW : read out time of per line. +rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per line (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/config/euroc/euroc_config.yaml b/config/euroc/euroc_config.yaml index 0735c013e..ccfce0eda 100644 --- a/config/euroc/euroc_config.yaml +++ b/config/euroc/euroc_config.yaml @@ -73,7 +73,7 @@ td: 0.0 # initial value of time offset. unit: s. rea #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera -rolling_shutter_tr: 0 # unit: s. rolling shutter read out time (from data sheet). tr / ROW : read out time of per line. +rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per line (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/config/euroc/euroc_config_no_extrinsic.yaml b/config/euroc/euroc_config_no_extrinsic.yaml index 5dd052fea..5431e0617 100644 --- a/config/euroc/euroc_config_no_extrinsic.yaml +++ b/config/euroc/euroc_config_no_extrinsic.yaml @@ -59,7 +59,7 @@ td: 0.0 # initial value of time offset. unit: s. rea #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera -rolling_shutter_tr: 0 # unit: s. rolling shutter read out time (from data sheet). tr / ROW : read out time of per line. +rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per line (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/vins_estimator/src/factor/projection_td_factor.cpp b/vins_estimator/src/factor/projection_td_factor.cpp index 1b144f898..5b2d5fd04 100644 --- a/vins_estimator/src/factor/projection_td_factor.cpp +++ b/vins_estimator/src/factor/projection_td_factor.cpp @@ -48,8 +48,8 @@ bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *resid double td = parameters[4][0]; Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; - pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; + pts_i_td = pts_i - (td - td_i + TR * row_i) * velocity_i; + pts_j_td = pts_j - (td - td_j + TR * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; @@ -179,8 +179,8 @@ void ProjectionTdFactor::check(double **parameters) double td = parameters[4][0]; Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; - pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; + pts_i_td = pts_i - (td - td_i + TR * row_i) * velocity_i; + pts_j_td = pts_j - (td - td_j + TR * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; @@ -236,8 +236,8 @@ void ProjectionTdFactor::check(double **parameters) td += delta.y(); Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; - pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; + pts_i_td = pts_i - (td - td_i + TR * row_i) * velocity_i; + pts_j_td = pts_j - (td - td_j + TR * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; diff --git a/vins_estimator/src/parameters.cpp b/vins_estimator/src/parameters.cpp index ac3802a18..c5b234d9e 100644 --- a/vins_estimator/src/parameters.cpp +++ b/vins_estimator/src/parameters.cpp @@ -121,9 +121,8 @@ void readParameters(ros::NodeHandle &n) ROLLING_SHUTTER = fsSettings["rolling_shutter"]; if (ROLLING_SHUTTER) { - TR = fsSettings["rolling_shutter_tr, please set read out time according to the data sheet."]; - ROS_INFO_STREAM("rolling shutter camera, whole read out time: " << TR); - ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR / ROW); + TR = fsSettings["rolling_shutter_tr"]; + ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); } else {