Skip to content

Commit

Permalink
Publish keyframes (stella-cv#132)
Browse files Browse the repository at this point in the history
  • Loading branch information
@ymd-stella
ymd-stella committed Jun 25, 2023
1 parent fe16542 commit d9062be
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 7 deletions.
55 changes: 48 additions & 7 deletions src/stella_vslam_ros.cc
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include <stella_vslam_ros.h>
#include <stella_vslam/publish/map_publisher.h>
#include <stella_vslam/data/keyframe.h>

#include <chrono>

Expand All @@ -16,6 +17,8 @@ system::system(const std::shared_ptr<stella_vslam::system>& slam,
: slam_(slam), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_default),
mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)),
keyframes_pub_(node_->create_publisher<geometry_msgs::msg::PoseArray>("~/keyframes", 1)),
keyframes_2d_pub_(node_->create_publisher<geometry_msgs::msg::PoseArray>("~/keyframes_2d", 1)),
map_to_odom_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
tf_(std::make_unique<tf2_ros::Buffer>(node_->get_clock())),
transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
Expand All @@ -26,28 +29,27 @@ system::system(const std::shared_ptr<stella_vslam::system>& slam,
std::bind(&system::init_pose_callback,
this, std::placeholders::_1));
setParams();
rot_ros_to_cv_map_frame_ = (Eigen::Matrix3d() << 0, 0, 1,
-1, 0, 0,
0, -1, 0)
.finished();
}

void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp) {
// Extract rotation matrix and translation vector from
Eigen::Matrix3d rot(cam_pose_wc.block<3, 3>(0, 0));
Eigen::Translation3d trans(cam_pose_wc.block<3, 1>(0, 3));
Eigen::Affine3d map_to_camera_affine(trans * rot);
Eigen::AngleAxisd rot_ros_to_cv_map_frame(
(Eigen::Matrix3d() << 0, 0, 1,
-1, 0, 0,
0, -1, 0)
.finished());

// Transform map frame from CV coordinate system to ROS coordinate system
map_to_camera_affine.prerotate(rot_ros_to_cv_map_frame);
map_to_camera_affine.prerotate(rot_ros_to_cv_map_frame_);

// Create odometry message and update it with current camera pose
nav_msgs::msg::Odometry pose_msg;
pose_msg.header.stamp = stamp;
pose_msg.header.frame_id = map_frame_;
pose_msg.child_frame_id = camera_frame_;
pose_msg.pose.pose = tf2::toMsg(map_to_camera_affine * rot_ros_to_cv_map_frame.inverse());
pose_msg.pose.pose = tf2::toMsg(map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse());
pose_pub_->publish(pose_msg);

// Send map->odom transform. Set publish_tf to false if not using TF
Expand All @@ -71,6 +73,33 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time
}
}

void system::publish_keyframes(const rclcpp::Time& stamp) {
Eigen::Vector3d normal_vector;
normal_vector << 0, 0, 1;
geometry_msgs::msg::PoseArray keyframes_msg;
geometry_msgs::msg::PoseArray keyframes_2d_msg;
keyframes_msg.header.stamp = stamp;
keyframes_msg.header.frame_id = map_frame_;
keyframes_2d_msg.header = keyframes_msg.header;
std::vector<std::shared_ptr<stella_vslam::data::keyframe>> all_keyfrms;
slam_->get_map_publisher()->get_keyframes(all_keyfrms);
for (const auto& keyfrm : all_keyfrms) {
if (!keyfrm || keyfrm->will_be_erased()) {
continue;
}
Eigen::Matrix4d cam_pose_wc = keyfrm->get_pose_wc();
Eigen::Matrix3d rot(cam_pose_wc.block<3, 3>(0, 0));
Eigen::Translation3d trans(cam_pose_wc.block<3, 1>(0, 3));
Eigen::Affine3d map_to_camera_affine(trans * rot);
Eigen::Affine3d pose_affine = rot_ros_to_cv_map_frame_ * map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse();
keyframes_msg.poses.push_back(tf2::toMsg(pose_affine));
pose_affine.translation() = pose_affine.translation() - pose_affine.translation().dot(normal_vector) * normal_vector;
keyframes_2d_msg.poses.push_back(tf2::toMsg(pose_affine));
}
keyframes_pub_->publish(keyframes_msg);
keyframes_2d_pub_->publish(keyframes_2d_msg);
}

void system::setParams() {
odom_frame_ = std::string("odom");
odom_frame_ = node_->declare_parameter("odom_frame", odom_frame_);
Expand All @@ -88,6 +117,9 @@ void system::setParams() {
publish_tf_ = true;
publish_tf_ = node_->declare_parameter("publish_tf", publish_tf_);

publish_keyframes_ = true;
publish_keyframes_ = node_->declare_parameter("publish_keyframes", publish_keyframes_);

// Publish pose's timestamp in the future
transform_tolerance_ = 0.5;
transform_tolerance_ = node_->declare_parameter("transform_tolerance", transform_tolerance_);
Expand Down Expand Up @@ -184,6 +216,9 @@ void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
if (cam_pose_wc) {
publish_pose(*cam_pose_wc, msg->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(msg->header.stamp);
}
}

stereo::stereo(const std::shared_ptr<stella_vslam::system>& slam,
Expand Down Expand Up @@ -234,6 +269,9 @@ void stereo::callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const
if (cam_pose_wc) {
publish_pose(*cam_pose_wc, left->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(left->header.stamp);
}
}

rgbd::rgbd(const std::shared_ptr<stella_vslam::system>& slam, const std::string& mask_img_path)
Expand Down Expand Up @@ -280,6 +318,9 @@ void rgbd::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, const
if (cam_pose_wc) {
publish_pose(*cam_pose_wc, color->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(color->header.stamp);
}
}

} // namespace stella_vslam_ros
7 changes: 7 additions & 0 deletions src/stella_vslam_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#include <message_filters/sync_policies/exact_time.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
Expand All @@ -29,6 +30,7 @@ class system {
system(const std::shared_ptr<stella_vslam::system>& slam,
const std::string& mask_img_path);
void publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp);
void publish_keyframes(const rclcpp::Time& stamp);
void setParams();
std::shared_ptr<stella_vslam::system> slam_;
std::shared_ptr<stella_vslam::config> cfg_;
Expand All @@ -38,6 +40,8 @@ class system {
cv::Mat mask_;
std::vector<double> track_times_;
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> pose_pub_;
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseArray>> keyframes_pub_;
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseArray>> keyframes_2d_pub_;
std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>>
init_pose_sub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> map_to_odom_broadcaster_;
Expand All @@ -46,10 +50,13 @@ class system {
std::unique_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
bool publish_tf_;
bool publish_keyframes_;
double transform_tolerance_;

private:
void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

Eigen::AngleAxisd rot_ros_to_cv_map_frame_;
};

class mono : public system {
Expand Down

0 comments on commit d9062be

Please sign in to comment.