| 274 | } |
| 275 | |
| 276 | VioNavState RosbagDataProvider::getGroundTruthVioNavState( |
| 277 | const size_t& k_frame) const { |
| 278 | CHECK_LE(k_frame, rosbag_data_.gt_odometry_.size()); |
| 279 | nav_msgs::Odometry gt_odometry = *(rosbag_data_.gt_odometry_.at(k_frame)); |
| 280 | // World to Body rotation |
| 281 | gtsam::Rot3 W_R_B = gtsam::Rot3::Quaternion( |
| 282 | gt_odometry.pose.pose.orientation.w, |
| 283 | gt_odometry.pose.pose.orientation.x, |
| 284 | gt_odometry.pose.pose.orientation.y, |
| 285 | gt_odometry.pose.pose.orientation.z); |
| 286 | gtsam::Point3 position (gt_odometry.pose.pose.position.x, |
| 287 | gt_odometry.pose.pose.position.y, |
| 288 | gt_odometry.pose.pose.position.z); |
| 289 | gtsam::Vector3 velocity (gt_odometry.twist.twist.linear.x, |
| 290 | gt_odometry.twist.twist.linear.y, |
| 291 | gt_odometry.twist.twist.linear.z); |
| 292 | VioNavState gt_init; |
| 293 | gt_init.pose_ = gtsam::Pose3(W_R_B, position); |
| 294 | gt_init.velocity_ = velocity; |
| 295 | // TODO(Toni): how can we get the ground-truth biases? For sim, ins't it 0? |
| 296 | gtsam::Vector3 gyro_bias (0.0, 0.0, 0.0); |
| 297 | gtsam::Vector3 acc_bias (0.0, 0.0, 0.0); |
| 298 | gt_init.imu_bias_ = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias); |
| 299 | return gt_init; |
| 300 | } |
| 301 | |
| 302 | void RosbagDataProvider::publishClock(const Timestamp& timestamp) const { |
| 303 | rosgraph_msgs::Clock clock; |