Merge branch 'master' of github.com:HKUST-Aerial-Robotics/VINS-PC

seahome45 · Jan 24, 2018 · 5d30745 · 5d30745
2 parents 799412b + 6b9760e
commit 5d30745
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/benchmark_publisher/src/benchmark_publisher_node.cpp b/benchmark_publisher/src/benchmark_publisher_node.cpp
@@ -34,13 +34,15 @@ struct Data
 {
     Data(FILE *f)
     {
-        fscanf(f, " %lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", &t,
+        if (fscanf(f, " %lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", &t,
                &px, &py, &pz,
                &qw, &qx, &qy, &qz,
                &vx, &vy, &vz,
                &wx, &wy, &wz,
-               &ax, &ay, &az);
-        t /= 1e9;
+               &ax, &ay, &az) != EOF)
+        {
+            t /= 1e9;
+        }
     }
     double t;
     float px, py, pz;
@@ -138,7 +140,10 @@ int main(int argc, char **argv)
       return 0;
     }
     char tmp[10000];
-    fgets(tmp, 10000, f);
+    if (fgets(tmp, 10000, f) == NULL)
+    {
+        ROS_WARN("can't load ground truth; no data available");
+    }
     while (!feof(f))
         benchmark.emplace_back(f);
     fclose(f);

diff --git a/vins_estimator/src/estimator_node.cpp b/vins_estimator/src/estimator_node.cpp
@@ -36,10 +36,17 @@ Eigen::Vector3d tmp_Bg;
 Eigen::Vector3d acc_0;
 Eigen::Vector3d gyr_0;
 bool init_feature = 0;
+bool init_imu = 1;
 
 void predict(const sensor_msgs::ImuConstPtr &imu_msg)
 {
     double t = imu_msg->header.stamp.toSec();
+    if (init_imu)
+    {
+        latest_time = t;
+        init_imu = 0;
+        return;
+    }
     double dt = t - latest_time;
     latest_time = t;