| 544 | } // namespace VIO |
| 545 | |
| 546 | void RosBaseDataProvider::publishState( |
| 547 | const SpinOutputPacket& vio_output) const { |
| 548 | // Get latest estimates for odometry. |
| 549 | const gtsam::Pose3& pose = vio_output.getEstimatedPose(); |
| 550 | const gtsam::Vector3& velocity = vio_output.getEstimatedVelocity(); |
| 551 | const Timestamp& ts = vio_output.getTimestamp(); |
| 552 | const gtsam::Matrix6& pose_cov = vio_output.getEstimatedPoseCov(); |
| 553 | const gtsam::Matrix3& vel_cov = vio_output.getEstimatedVelCov(); |
| 554 | |
| 555 | // First publish odometry estimate |
| 556 | nav_msgs::Odometry odometry_msg; |
| 557 | |
| 558 | // Create header. |
| 559 | odometry_msg.header.stamp.fromNSec(ts); |
| 560 | odometry_msg.header.frame_id = world_frame_id_; |
| 561 | odometry_msg.child_frame_id = base_link_frame_id_; |
| 562 | |
| 563 | // Position |
| 564 | odometry_msg.pose.pose.position.x = pose.x(); |
| 565 | odometry_msg.pose.pose.position.y = pose.y(); |
| 566 | odometry_msg.pose.pose.position.z = pose.z(); |
| 567 | |
| 568 | // Orientation |
| 569 | const gtsam::Rot3& rotation = pose.rotation(); |
| 570 | const gtsam::Quaternion& quaternion = rotation.toQuaternion(); |
| 571 | odometry_msg.pose.pose.orientation.w = quaternion.w(); |
| 572 | odometry_msg.pose.pose.orientation.x = quaternion.x(); |
| 573 | odometry_msg.pose.pose.orientation.y = quaternion.y(); |
| 574 | odometry_msg.pose.pose.orientation.z = quaternion.z(); |
| 575 | |
| 576 | // Remap covariance from GTSAM convention |
| 577 | // to odometry convention and fill in covariance |
| 578 | static const std::vector<int> remapping{3, 4, 5, 0, 1, 2}; |
| 579 | |
| 580 | // Position covariance first, angular covariance after |
| 581 | DCHECK_EQ(pose_cov.rows(), remapping.size()); |
| 582 | DCHECK_EQ(pose_cov.rows() * pose_cov.cols(), |
| 583 | odometry_msg.pose.covariance.size()); |
| 584 | for (int i = 0; i < pose_cov.rows(); i++) { |
| 585 | for (int j = 0; j < pose_cov.cols(); j++) { |
| 586 | odometry_msg.pose |
| 587 | .covariance[remapping[i] * pose_cov.cols() + remapping[j]] = |
| 588 | pose_cov(i, j); |
| 589 | } |
| 590 | } |
| 591 | |
| 592 | // Linear velocities, trivial values for angular |
| 593 | const gtsam::Matrix3& inversed_rotation = rotation.transpose(); |
| 594 | const Vector3 velocity_body = inversed_rotation * velocity; |
| 595 | odometry_msg.twist.twist.linear.x = velocity_body(0); |
| 596 | odometry_msg.twist.twist.linear.y = velocity_body(1); |
| 597 | odometry_msg.twist.twist.linear.z = velocity_body(2); |
| 598 | |
| 599 | // Velocity covariance: first linear |
| 600 | // and then angular (trivial values for angular) |
| 601 | const gtsam::Matrix3 vel_cov_body = |
| 602 | inversed_rotation.matrix() * vel_cov * rotation.matrix(); |
| 603 | DCHECK_EQ(vel_cov_body.rows(), 3); |