| 57 | RosbagDataProvider::~RosbagDataProvider() {} |
| 58 | |
| 59 | bool RosbagDataProvider::parseRosbag(const std::string& bag_path, |
| 60 | const std::string& left_imgs_topic, |
| 61 | const std::string& right_imgs_topic, |
| 62 | const std::string& imu_topic, |
| 63 | const std::string& gt_odom_topic, |
| 64 | RosbagData* rosbag_data) { |
| 65 | CHECK_NOTNULL(rosbag_data); |
| 66 | |
| 67 | // Fill in rosbag to data_ |
| 68 | rosbag::Bag bag; |
| 69 | bag.open(bag_path, rosbag::bagmode::Read); |
| 70 | |
| 71 | // Generate list of topics to parse: |
| 72 | std::vector<std::string> topics; |
| 73 | topics.push_back(left_imgs_topic); |
| 74 | topics.push_back(right_imgs_topic); |
| 75 | topics.push_back(imu_topic); |
| 76 | if (!gt_odom_topic.empty()) { |
| 77 | CHECK(pipeline_params_.backend_params_->autoInitialize_ == 0) |
| 78 | << "Provided a gt_odom_topic; but autoInitialize is not set to 0," |
| 79 | " meaning no ground-truth initialization will be done... " |
| 80 | "Are you sure you don't want to use gt? "; |
| 81 | topics.push_back(gt_odom_topic); |
| 82 | } else { |
| 83 | CHECK(pipeline_params_.backend_params_->autoInitialize_ != 0); |
| 84 | ROS_DEBUG("Not parsing ground truth data."); |
| 85 | } |
| 86 | |
| 87 | // Query rosbag for given topics |
| 88 | rosbag::View view(bag, rosbag::TopicQuery(topics)); |
| 89 | |
| 90 | // Keep track of this since we expect IMU data before an image. |
| 91 | bool start_parsing_stereo = false; |
| 92 | // For some datasets, we have duplicated measurements for the same time. |
| 93 | Timestamp last_imu_timestamp = 0; |
| 94 | for (const rosbag::MessageInstance& msg : view) { |
| 95 | // Get topic. |
| 96 | const std::string& msg_topic = msg.getTopic(); |
| 97 | |
| 98 | // IMU |
| 99 | sensor_msgs::ImuConstPtr imu_msg = msg.instantiate<sensor_msgs::Imu>(); |
| 100 | if (imu_msg != nullptr && msg_topic == imu_topic) { |
| 101 | gtsam::Vector6 imu_accgyr; |
| 102 | imu_accgyr(0) = imu_msg->linear_acceleration.x; |
| 103 | imu_accgyr(1) = imu_msg->linear_acceleration.y; |
| 104 | imu_accgyr(2) = imu_msg->linear_acceleration.z; |
| 105 | imu_accgyr(3) = imu_msg->angular_velocity.x; |
| 106 | imu_accgyr(4) = imu_msg->angular_velocity.y; |
| 107 | imu_accgyr(5) = imu_msg->angular_velocity.z; |
| 108 | const Timestamp& imu_data_timestamp = imu_msg->header.stamp.toNSec(); |
| 109 | if (imu_data_timestamp > last_imu_timestamp) { |
| 110 | imu_data_.imu_buffer_.addMeasurement(imu_data_timestamp, imu_accgyr); |
| 111 | last_imu_timestamp = imu_data_timestamp; |
| 112 | } else { |
| 113 | ROS_FATAL( |
| 114 | "IMU timestamps in rosbag are out of order: consider re-ordering " |
| 115 | "rosbag."); |
| 116 | } |