fix loading rolling shutter read out time bugs; change physical meani…

…ng of read out time (per line)
sheng00125 · Jan 3, 2018 · 6f7f3bb · 6f7f3bb
1 parent 9b724db
commit 6f7f3bb
Show file tree

Hide file tree

Showing 8 changed files with 16 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
     {