ORB_SLAM2的Rviz可视化


阅读数: 次   
字数统计: 1.6k字   |   阅读时长: 7分

最近我在考虑能不能使用RVIZ可视化ORB_SLAM2,突然发现一位前辈的分享,为防止以后大佬删除了,我fork到了自己的github,这里做个笔记,记录一下.

这篇博客主要分析一下相关代码

可视化的几个要点

ORB-SLAM中关于Rviz的可视化：

  • ORB-SLAM的Rviz可视化使用单独的一个类来完成可视化信息的发布：MapPublisher类.
  • 所有的可视化信息都是Rviz的Mark类型,根据发布的地图点、关键帧、Covisibility Graph、Spanning Tree和相机轨迹，使用了不同的Mark类型.
  • 所有的可视化信息,包括地图、轨迹等都是从ORB-SLAM中的Map类中获取的.
  • 每次获得一帧图像,进行Track后，利用MapPublisher类发布可视化信息.
  • 在配置相应的Rviz,使其可以接收可视化信息.

明白了这几点之后，在ORB-SLAM2中添加Rviz可视化模块就很简单了，主要对源代码做以下改动：

  • 添加MapPublisher类和配置Rviz,可以直接复用ORB-SLAM中的MapPublisher类和Rviz文件,并在每次Track之后(执行完mpSLAM->TrackStereo()), 利用MapPublisher类发布可视化信息.
  • 为Map类添加返回相关信息的接口.

特别要注意:

ORB-SLAM2的坐标系下，z轴是朝前的，而Rviz的坐标系下，z轴是朝上的，因此要做相应的转换.

ROS包建立

创建相关软件包

在catkin_ws/src目录下新建软件包并编译：

1
2
3
4
catkin_create_pkg my_image_transport image_transport cv_bridge
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="my_image_transport"
source devel/setup.bash

创建图像发布者程序

新建my_image_transport/src/my_publisher.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("camera/image", 1);
  cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  cv::waitKey(30);//不断刷新图像，频率时间为delay，单位为ms
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

  ros::Rate loop_rate(5);
  while (nh.ok()) {
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

代码解释：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
line 1-4：
ros.h头文件是所有的ros节点中必须要包含的，下面三个分别是实现图像的发布和订阅，调用opencv库，完成opencv图像格式转化为ROS图像格式所要用到的头文件；

line 11：
告知结点管理器要在camera/image话题发布图像消息，参数1是话题名称，话题2是缓冲区大小（即消息队列的长度，在发布图像消息时消息队列的长度只能是1）；

line 12：
根据运行时给定的参数（图像文件的路径）读取图像；

line 14：
将opencv格式的图像转化为ROS所支持的消息类型，从而发布到相应的话题上；

line 16-21：
发布图片消息，使消息类型匹配的节点订阅该消息。

创建图像订阅者程序

新建my_image_transport/src/my_subscriber.cpp：

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
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
    cv::waitKey(30);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;
  cv::namedWindow("view");
  cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
  ros::spin();
  cv::destroyWindow("view");
}

代码解释：

1
2
3
line 6：回调函数，当有新的图像消息到达camera/image时，该函数就会被调用；
line 10：显示捕捉到的图像，其中cv_bridge::toCvShare(msg, "bgr8")->image用于将ROS图像消息转化为Opencv支持的图像格式（采用BGR8编码方式）。这部分用法恰好与上一节中发布者节点中的CvImage(std_msgs::Header(), "bgr8", image).toImageMsg(); 的作用相反
line 11：刷新图像的频率，实践过程中发现如果注释这一行图像将无法在窗口的显示

相关配置文件

CMakeLists.txt内容

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
cmake_minimum_required(VERSION 2.8.3)
project(my_image_transport)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
)
find_package(OpenCV REQUIRED)

set(LIBS
 ${OpenCV_LIBS} 
 ${catkin_LIBRARIES})
 
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_image_transport
#  CATKIN_DEPENDS cv_bridge image_transport
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(my_publisher src/my_publisher.cpp)
target_link_libraries(my_publisher ${LIBS})

add_executable(my_subscriber src/my_subscriber.cpp)
target_link_libraries(my_subscriber ${LIBS})

package.xml文件中添加

1
2
<build_depend>opencv2</build_depend>
<exec_depend>opencv2</exec_depend>

编译软件包

1
2
cd ~/catkin_ws
catkin_make -DCATKIN_WHITHELIST_PACKAGES="my_image_transport"

运行节点

单独开启一个终端执行roscore,启动ros节点管理器。

开启另一个终端，启动发布者节点：

1
rosrun my_image_transport my_publisher /home/xxx/catkin_ws/src/my_image_transport/000.png

运行订阅者节点：

1
rosrun my_image_transport my_subscriber

ORB_SLAM2 ROS模块结点的编译

在环境变量ROS_PACKAGE_PATH中添加Examples/ROS/ORB_SLAM2的路径：

1
echo "source ~/slam/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.sh" >> ~/.bashrc

在~/slam/ORB_SLAM2目录下执行：

1
2
chmod +x build_ros.sh
./build_ros.sh

等待编译成功.

运行结果

执行命令

1
roslaunch my_image_transport stereo_image_transport.launch

使用PCL库显示地图点

注意：ORB_SLAM2系统Map类中的地图点是世界坐标系中的所有地图点，而不是每个关键

1
ROSpointcloud_to_laserscan包pointcloud_to_laserscan_node`节点将点云`PointCloud`转成`2D激光扫描

订阅节点：cloud_in(sensor_msgs/PointCloud2)

发布节点：scan(sensor_msg/LaserScan)

配置好ROS程序包，在MapPulisher类中稍作修改，参照如下内容：

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
#include<ros/ros.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
  ros::init (argc, argv, "pcl_create");
  ros::NodeHandle nh;
  ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
  pcl::PointCloud<pcl::PointXYZ> cloud;
  sensor_msgs::PointCloud2 output;
  // Fill in the cloud data
  cloud.width = 100;
  cloud.height = 1;
  cloud.points.resize(cloud.width * cloud.height);
  for (size_t i = 0; i < cloud.points.size(); ++i)
  {
    cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  //Convert the cloud to ROS message
  pcl::toROSMsg(cloud, output);
  output.header.frame_id = "odom";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer
  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    pcl_pub.publish(output);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

效果展示:

参考资料

[1] 2D Grid Mapping and Navigation with ORB SLAM. Abhineet Kumar Singh, Ali Jahani Amiri.

[2] 系统Rviz可视化方案

赏

谢谢你请我吃糖果

扫一扫，分享到微信

微信分享二维码
载入天数...载入时分秒...

tag:

    缺失模块。
    1、请确保node版本大于6.2
    2、在博客根目录（注意不是yilia根目录）执行以下命令：
    npm i hexo-generator-json-content --save

    3、在根目录_config.yml里添加配置： 

      jsonContent:
    meta: false
    pages: false
    posts:
      title: true
      date: true
      path: true
      text: false
      raw: false
      content: false
      slug: false
      updated: false
      comments: false
      link: false
      permalink: false
      excerpt: false
      categories: false
      tags: true

计算机科学先驱阿兰·图灵说，“We can only see a short distance ahead, but we can see plenty there that needs to be done.”<br> 我们只能向前看到一小段未来，但我们可以在那里看到足够多可以做的事。很难说当代一个受过良好教育的人，一个理解银河系、理解宇宙演化史、理解现代社会法则的人和16世纪一个仍然相信地球是平的的人有什么本质的区别。看起来16世纪的你一定会比21世纪的你更愚蠢，但从未知的方面看，两者其实同等幼稚。今天宏大的宇宙观可能就是另一个版本的平坦地球，如果被25世纪的人观察的话。正因为如此，图灵的话才会有意义。正是因为所有人都没有大自然聪明，才会有文明的发展，才会有人秉持真理去挑战所谓的人类权威；正是因为问题永远都无法完善，才会有生机与希望，才会有足够多的工作可以做。 所以我想实际上图灵是在说，既然都离不开这艘飞船，那就让我们在这里做一些可以做的事吧，或许会有些惊喜也说不定呢。