Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Publish keyframes #71

Merged
merged 1 commit into from
Oct 31, 2021
Merged

Publish keyframes #71

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 42 additions & 0 deletions src/openvslam_ros.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include <openvslam_ros.h>
#include <openvslam/publish/map_publisher.h>
#include <openvslam/data/landmark.h>
#include <openvslam/data/keyframe.h>

#include <chrono>

Expand All @@ -20,6 +21,8 @@ system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string&
mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
pose_pub_(private_nh_.advertise<nav_msgs::Odometry>("camera_pose", 1)),
pc_pub_(private_nh_.advertise<sensor_msgs::PointCloud2>("pointcloud", 1)),
keyframes_pub_(private_nh_.advertise<geometry_msgs::PoseArray>("keyframes", 1)),
keyframes_2d_pub_(private_nh_.advertise<geometry_msgs::PoseArray>("keyframes_2d", 1)),
map_to_odom_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>()),
tf_(std::make_unique<tf2_ros::Buffer>()),
transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
Expand Down Expand Up @@ -87,6 +90,33 @@ void system::publish_pointcloud(const ros::Time& stamp) {
pc_pub_.publish(pcout);
}

void system::publish_keyframes(const ros::Time& stamp) {
Eigen::Vector3d normal_vector;
normal_vector << 0, 0, 1;
geometry_msgs::PoseArray keyframes_msg;
geometry_msgs::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<openvslam::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_cam_pose_inv();
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");
private_nh_.param("odom_frame", odom_frame_, odom_frame_);
Expand All @@ -108,6 +138,9 @@ void system::setParams() {
publish_pointcloud_ = false;
private_nh_.param("publish_pointcloud", publish_pointcloud_, publish_pointcloud_);

publish_keyframes_ = true;
private_nh_.param("publish_keyframes", publish_keyframes_, publish_keyframes_);

// Publish pose's timestamp in the future
transform_tolerance_ = 0.5;
private_nh_.param("transform_tolerance", transform_tolerance_, transform_tolerance_);
Expand Down Expand Up @@ -198,6 +231,9 @@ void mono::callback(const sensor_msgs::ImageConstPtr& msg) {
if (publish_pointcloud_) {
publish_pointcloud(msg->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(msg->header.stamp);
}
}

stereo::stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
Expand Down Expand Up @@ -250,6 +286,9 @@ void stereo::callback(const sensor_msgs::ImageConstPtr& left, const sensor_msgs:
if (publish_pointcloud_) {
publish_pointcloud(left->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(left->header.stamp);
}
}

rgbd::rgbd(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
Expand Down Expand Up @@ -299,5 +338,8 @@ void rgbd::callback(const sensor_msgs::ImageConstPtr& color, const sensor_msgs::
if (publish_pointcloud_) {
publish_pointcloud(color->header.stamp);
}
if (publish_keyframes_) {
publish_keyframes(color->header.stamp);
}
}
} // namespace openvslam_ros
5 changes: 5 additions & 0 deletions src/openvslam_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,15 @@
#include <opencv2/core/core.hpp>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseArray.h>

namespace openvslam_ros {
class system {
public:
system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path);
void publish_pose(const Eigen::Matrix4d& cam_pose_wc, const ros::Time& stamp);
void publish_pointcloud(const ros::Time& stamp);
void publish_keyframes(const ros::Time& stamp);
void setParams();
openvslam::system SLAM_;
std::shared_ptr<openvslam::config> cfg_;
Expand All @@ -39,6 +41,8 @@ class system {
std::vector<double> track_times_;
ros::Publisher pose_pub_;
ros::Publisher pc_pub_;
ros::Publisher keyframes_pub_;
ros::Publisher keyframes_2d_pub_;
ros::Subscriber init_pose_sub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> map_to_odom_broadcaster_;
std::string odom_frame_, map_frame_, base_link_;
Expand All @@ -47,6 +51,7 @@ class system {
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
bool publish_tf_;
bool publish_pointcloud_;
bool publish_keyframes_;
double transform_tolerance_;

private:
Expand Down