本文介绍了如何修改ORB_SLAM2的ROS Demo，使其能够在建图的同时发布位姿。本文只涉及ROS RGB-D的代码修改。

修改代码

直接进入正题，需要修改的代码文件是Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc这个demo文件。ORB_SLAM2本身其实已经存放了相机的位姿和路径数据了，也提供了获取数据的接口，我们要做的就是将其转成ROS能够支持的格式并通过Topic发布出去。

首先添加头文件

1
2
3
#include <tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include <nav_msgs/Path.h>

然后在ImageGrabber类后方，main函数之前全局声明下面三个变量，注意是全局声明。

1
2
3
ros::Publisher pose_pub;
nav_msgs::Path rgbd_path;
ros::Publisher rgbd_path_pub;

main函数中ros::spin();之前新增如下代码，这里设置了我们发布位姿和路径的两个Topic，后续需要在rviz里面订阅这两个Topic来看到发布的位姿数据。

1
2
3
4
// 发布位姿
pose_pub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);
// 发布路径
rgbd_path_pub = nh.advertise<nav_msgs::Path>("ORB_SLAM/path", 10);

在ImageGrabber::GrabRGBD函数的最后修改，修改和添加如下内容。这里是用了一个cv::Mat存放了mpSLAM->TrackRGBD函数的返回结果（之前是没有用变量存放的）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
// 发布位姿
cv::Mat Tcw;
Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
if (!Tcw.empty()) // 注意需要判断是否为空，可能为空
{
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "map"; // 注意这里，必须是map而不是path

    cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); // Rotation information
    cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3);      // translation information
    vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

    tf::Transform new_transform;
    new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
    tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
    new_transform.setRotation(quaternion);
    tf::poseTFToMsg(new_transform, pose.pose);
    pose_pub.publish(pose); // 发布位姿

    // 发布路径
    rgbd_path.header.frame_id = "map";
    rgbd_path.header.stamp = ros::Time::now();
    rgbd_path.poses.push_back(pose);
    rgbd_path_pub.publish(rgbd_path);
}

到这里就修改完成了，先使用项目根目录下的build.sh脚本编译ORB_SLAM2普通版本，然后使用build_ros.sh脚本编译ROS版本。

1
2
3
4
cd ORB_SLAM2 # 先进入文件夹
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$PWD/Examples/ROS/ORB_SLAM2
./build.sh
./build_ros.sh

下面给出完整的demo代码，也可以在Github查看。

修改后ros_rgbd.cc的完整代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv2/core/core.hpp>

#include "../../../include/System.h"

#include <tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include <nav_msgs/Path.h>

using namespace std;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System *pSLAM) : mpSLAM(pSLAM) {}

    void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD);

    ORB_SLAM2::System *mpSLAM;
};

ros::Publisher pose_pub;
nav_msgs::Path rgbd_path;
ros::Publisher rgbd_path_pub;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if (argc != 3)
    {
        cerr << endl
             << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;
        ros::shutdown();
        return 1;
    }

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
    SLAM.LoadMap("/home/bill/work/3dslam/orb_slam2/MapPointandKeyFrame.bin");

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub, depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2));
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5); // 发布位姿
    rgbd_path_pub = nh.advertise<nav_msgs::Path>("ORB_SLAM/path", 10);       // 发布路径

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    SLAM.SaveMap("MapPointandKeyFrame.bin");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    // 发布位姿
    cv::Mat Tcw;
    Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
    if (!Tcw.empty())
    {
        geometry_msgs::PoseStamped pose;
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "map"; // 注意这里，必须是map而不是path

        cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); // Rotation information
        cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3);      // translation information
        vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

        tf::Transform new_transform;
        new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
        tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
        new_transform.setRotation(quaternion);
        tf::poseTFToMsg(new_transform, pose.pose);
        pose_pub.publish(pose); // 发布位姿

        // 发布路径
        rgbd_path.header.frame_id = "map";
        rgbd_path.header.stamp = ros::Time::now();
        rgbd_path.poses.push_back(pose);
        rgbd_path_pub.publish(rgbd_path);
    }
}

截个图，证明一下俺的代码是能正常编译的。

image.png

1
2
3
4
5
6
[ 77%] Linking CXX executable ../Stereo
[ 88%] Linking CXX executable ../RGBD
[ 88%] Built target Stereo
[ 88%] Built target RGBD
[100%] Linking CXX executable ../MonoAR
[100%] Built target MonoAR

测试效果

先在一个终端里面执行roscore命令，在另外一个终端里面使用rviz命令启动前端查看器。

另外再开一个终端，以RGBD模式启动ORB_SLAM2程序，注意需要修改TUM1.yaml文件中的DepthMapFactor为1.0，该参数ROS需要修改为1.0，直接运行是5000.0。

1
2
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$PWD/Examples/ROS/ORB_SLAM2
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

在左下角add里面选择by topic，订阅我们刚刚在ORB_SLAM2中发布的两个topic，注意需要启动ORB_SLAM2之后才能看到这两个topic。

下图是从参考博客里面借来的，因为我自己忘记截图了。

image.png

rviz订阅完毕这两个topic之后，就可以开始播放tum数据集的bag文件了

1
2
3
4
rosbag play \
    datasets/TUM/rgbd_dataset_freiburg1_desk.bag \
    /camera/rgb/image_color:=/camera/rgb/image_raw \
    /camera/depth/image:=/camera/depth/image_raw

能够观察到红色的箭头代表相机位姿，绿色线条代表相机走过的路径。

image.png

image.png

至此，位姿发布功能测试成功。

参考博客

这里要提醒一下，参考博客中给出的代码是错的，主要是frame_id设置的不对，在我的测试环境中frame_id必须要设置为map，而不是path。如果你在修改了代码之后遇到了Fix Frame [map] does not exist的报错，就说明你的frame_id设置错误了。

image.png

参考博客：