基于OptiTrack跟踪系统和Turtlebot机器人的视觉SLAM定位评估

作者丨Mr Qin

来源丨古月居

本文旨在介绍使用OptiTrack光学跟踪系统和Turtlebot机器人进行视觉SLAM定位实验的详细流程，包括实验平台搭建过程、数据处理过程以及SLAM估计评估方法。

由于涉及知识较多，部分内容只给出了相关参考博文链接。

1 实验平台搭建

实验平台包括OptiTrack光学跟踪系统和Turtlebot数据采集平台两部分，OptiTrack系统用于获取机器人运动轨迹真值，Turtlebot数据采集平台用于录制图像数据。

1.1 OptiTrack光学跟踪系统介绍

OptiTrack是美国公司NaturalPoint公司研制的高性能光学跟踪系统，可以获取场地中物体的亚像素级位置，目前TUM等公开数据集的轨迹真值均通过OptiTrack系统获取。

OptiTrack由固定在场地上方的多个Prime相机、荧光标记球、Motive处理软件组成，使用过程中，将多个荧光标记球粘在物体上，Prime相机以每秒数百帧的速率拍摄荧光标记球，Motive实时地解算出物体在场景中的位姿。OptiTrack系统的原理如图所示。

1.2 Turtlebot数据采集平台介绍

本实验所使用的Turtlebot数据采集平台如图所示，共由五部分组成：笔记本、Turtlebot机器人、深度相机、荧光标记球、游戏手柄。

其中笔记本是该平台的核心，用于向各设备发送指令，实现数据采集平台的各项功能；

Turtlebot机器人是一款轻量级机器人开发套件，在实验中作为移动平台；

深度相机是机器人的传感器，在实验中用于录制深度和彩色图像数据；

荧光标记球用于结合OptiTrack系统获取机器人位姿；

无线游戏手柄用来控制机器人的运动。各个设备均通过笔记本相连。

1.3 实验平台搭建：

只有知道轨迹和图像的时间戳，才能知道轨迹与估计轨迹之间的对应关系，进而评估SLAM系统的定位性能。

OptiTrack系统估计的位姿输出在上位机上，而相机图像数据输出在Turtlebot机器人搭载的笔记本上，需要将位姿数据传输到笔记本上，通过ROS统一录制数据，实现图像与位姿之间的匹配。

详细配置过程见：

https://blog.csdn.net/weixin_41536025/article/details/89913961

这里只介绍其工作原理：

上图中，上位机安装有Motive软件，通过交换机获取OptiTrack系统的数据。通过Motive软件中的设置，将机器人位姿数据经过路由器发布在局域网中。

Turtlebot机器人搭载的笔记本通过局域网获取机器人位姿数据，并用ROS的rosbag工具录制深度相机话题和位姿话题。可能用到的命令包括：

配置笔记本防火墙的命令：（只需要配置一次）

sudo ufw status   #查看防火墙
sudo ufw enable   #打开防火墙
sudo ufw disable  #关闭防火墙

开始发布optiTrack位姿的命令，其中192.168.1.104 为路由器的ip地址，笔记本需要通过wifi连接在路由器的局域网中

roslaunch vrpn_client_ros sample.launch  server:=192.168.1.104 #获取optiTrack位姿数据

开始发布Realsense相机的RGB-D图像的命令，其中s_d435_camera_our.launch为编写的launch文件，它记录了各种ros命令以及参数配置信息，需要认真配置该文件，确保发布的数据“深度-彩色匹配”后的图像。

filters:=colorizer是指发布的深度图像的格式，这里是指彩色表示深度值的大小。

roslaunch realsense2_camera rs_d435_camera_our.launch filters:=colorizer #发布realsense相机的图像话题

使用rosbag命令录制图像数据与位姿数据，并保存为test.bag文件。其中/camera/color/image_raw为彩色图像话题名称

/camera/aligned_depth_to_color/image_raw 为深度图像话题名称， /vrpn_client_node/RigidBody1/pose为optiTrack位姿的话题名称，各名称需要根据实际情况修改。

-O test是指将数据保存为test.bag。

rosbag record subset /camera/color/image_raw  /camera/aligned_depth_to_color/image_raw  /vrpn_client_node/RigidBody1/pose -O test

2 实验设备标定

2.1 Optitrack光学跟踪系统标定：

在使用OptiTrack光学定位系统前进行标定，标定有几个目的：其一相当于对软件的初始化，为了让系统确定摄像头的采集范围；

其二确定软件内部坐标的精度，使软件根据数据生成一个3D空间；其三对所标记的物体进行空间坐标的计算。

详细标定过程见：

https://blog.csdn.net/zeye5731/article/details/104106696

标定后，在Motive中为机器人建立刚体，即可获取机器人的位姿。

下图为软件运行结果，蓝色为四个Prime相机，绿线为相机对机器人的观测路径，下方为四相机观测到的红外图像。

2.2 相机标定：

深度相机在使用前需要进行标定，需要标定的参数有彩色摄像头的内参、深度摄像头的内参以及两者的外参。

相机在使用前需要进行标定的原因是每个相机在加工制和组装过程中都会存在一定的误差，这使得每个相机的成像平面，相机坐标系都会略有不同，对相机标定后，可校正相机的畸变，获得更为精确的图像，从而能够更为准确的进行定位与建图。

在实际使用过程中，一般只对彩色摄像头进行标定，其他采用厂商提供的默认参数即可。

使用Matlab标定相机的过程可参考：

https://blog.csdn.net/m0_56817529/article/details/127819167

3 实验数据处理

实验使用rosbag工具采集图像和位姿数据，需要将其转换为TUM数据集格式，才能进一步运行SLAM算法和评估轨迹精度。

该过程包括两部分：其一为将图像保存为TUM格式，并使用TUM提供的associate.py实现深度图与彩色图之间的配准；其二为将位姿数据保存为TUM格式的轨迹真值groundtruth.txt。

3.1 RGB-D相机数据处理：

将.bag格式的图像数据转换为TUM格式数据的过程见本人之前的博文：

https://www.guyuehome.com/35920

该博文详细地介绍了转换过程、ROS程序、测试过程。转换结果如下图所示，包括深度图像depth、彩色图像rgb、深度图像路径depth.txt、彩色图像路径rgb.txt。

之后下载TUM提供的associate.py，实现深度图像与彩色图像之间的时间戳对齐，得到associate.txt

python  associate.py rgb.txt  depth.txt > associate.txt

3.2 Optitrack位姿数据处理：

将rosbag数据转换为TUM格式的轨迹真值groundtruth.txt的过程与3.1类似，也是编写一个ROS节点，订阅Optitrack位姿话题，并将数据保存在txt文件中。

具体过程为：

1.编写optitrack2tum 节点，修改下文程序中的路径，并用catkin_make命令编译工作空间

2.运行optitrack2tum节点

rosrun  optitrack2tum opt2tum

3.播放rosbag数据

rosbag play  ./rgbd_dataset_fsdf.bag  -r 0.3  #-r 0.3是指0.3倍速播放数据，太快会导致数据来不及保存

可以在播放数据时同时运行两个节点，保存RGB-D图像和Optitrack位姿数据。此外，也可以将路径作为参数传递给程序，避免处理不同数据时对程序的修改。

optitrackToTUM.cpp文件编写如下：

#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <string>
#include <geometry_msgs/PoseStamped.h>
#include<fstream> 
using namespace std;


string Path = "/home/qin/下载/Dataet/BigPaper/0313/0313_canting/";
string pathGroungTruth = Path + "groundtruth.txt";


string pose_topic = "/vrpn_client_node/RigidBody1/pose";




void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
    ROS_INFO("I heard the pose from the robot"); 
    ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    ROS_INFO("the time we get the pose is %f",  msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);


    std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly




   ofstream of;
   of.open(pathGroungTruth, std::ios_base::app);
     if(of.fail()){
       ROS_INFO("Fail to open groundTruth file!!");
   }else{
     // double time = cv_ptr->header.stamp.toSec(); 
       double time = msg->header.stamp.toSec();
       of<<endl;
       of<<std::fixed<<time<<" "<<msg->pose.position.x <<" "<<msg->pose.position.y<<" "<< msg->pose.position.z<<" ";
       of<<msg->pose.orientation.x<<" "<< msg->pose.orientation.y <<" "<<msg->pose.orientation.z <<" "<<msg->pose.orientation.w ;
   }
}




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


    ofstream of;
    of.open(pathGroungTruth, std::ios_base::app);
    if(of.fail()){
       ROS_INFO("Fail to open file!!");
    }else{
       of<<"#------------start a new groundTruth-----------------"<<endl;; 
      of<<"# timestamp tx ty tz qx qy qz qw";
       of.close();
    }


    ROS_INFO("optitrackToTUM is ready.");
    ros::NodeHandle nodeHandler;
    ros::Subscriber subRGB = nodeHandler.subscribe(pose_topic, 10, &chatterCallback);


    ros::spin();
    return 0;
}

CMakeList.txt文件编写如下：

cmake_minimum_required(VERSION 3.0.2)
project(optitrack2tum)


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


## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
)




## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)




## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()


################################################
## Declare ROS messages, services and actions ##
################################################


## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)


## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )


## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )


## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )


## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   sensor_msgs#   std_msgs
# )


################################################
## Declare ROS dynamic reconfigure parameters ##
################################################


## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed


## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )


###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
  CATKIN_DEPENDS  roscpp    geometry_msgs
#  DEPENDS system_lib
)


###########
## Build ##
###########


## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${OpenCV_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)




## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})


## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide






add_executable(opt2tum src/optitrackToTUM.cpp)
target_link_libraries(opt2tum ${catkin_LIBRARIES} ${OpenCV_LIBS})
 #add_dependencies(opt2tum  ${PROJECT_NAME}_gencpp)

4 SLAM轨迹评估

使用EVO轨迹轨迹的详细过程可参考博文：

https://blog.csdn.net/xiaojinger_123/article/details/120136618

例如，使用以下命令绘制SLAM算法轨迹轨迹与真实轨迹的对比图。

其中evo_ape用来计算绝对轨迹误差，tum为所使用的数据格式，ourslam.txt为SLAM算法估计的轨迹，groundtruth.txt为3,2节获取到的轨迹真值，-p表示绘制误差曲线，-va表示详细模式，且使用SE(3)对齐。

evo_ape tum ourslam.txt groundtruth.txt -p -va

—END—

---

高效学习3D视觉三部曲

第一步 加入行业交流群，保持技术的先进性

目前工坊已经建立了3D视觉方向多个社群，包括SLAM、工业3D视觉、自动驾驶方向，细分群包括：[工业方向]三维点云、结构光、机械臂、缺陷检测、三维测量、TOF、相机标定、综合群；[SLAM方向]多传感器融合、ORB-SLAM、激光SLAM、机器人导航、RTK|GPS|UWB等传感器交流群、SLAM综合讨论群；[自动驾驶方向]深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器讨论群、多传感器标定、自动驾驶综合群等。除了这些，还有求职、硬件选型、视觉产品落地等交流群。大家可以添加小助理微信: dddvisiona，备注：加群+方向+学校|公司, 小助理会拉你入群。

添加小助理微信：dddvisiona, 拉你入群

第二步 加入知识星球，问题及时得到解答

针对3D视觉领域的视频课程（三维重建、三维点云、结构光、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业、项目对接为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：「3D视觉从入门到精通」

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

第三步 系统学习3D视觉，对模块知识体系，深刻理解并实践

如果大家对3D视觉某一个细分方向想系统学习[从理论、代码到实战],推荐3D视觉精品课程学习网址：www.3dcver.com

工业3D视觉方向课程：

[1]机械臂抓取从入门到实战课程（理论+源码）

[2]从零搭建一套结构光3D重建系统理论+源码+实践

[3]三维点云处理：算法与实战汇总

[4]彻底搞懂基于Open3D的点云处理教程！

[5]3D视觉缺陷检测教程：理论与实战！

[6]如何学习相机模型与标定？（代码+实战）

SLAM方向课程:

[1]如何高效学习基于LeGo-LOAM框架的激光SLAM?

[2]彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战

[3]（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

[4]彻底搞懂视觉-惯性SLAM：VINS-Fusion原理精讲与源码剖析

[5]彻底剖析室内、室外激光SLAM关键算法和实战(cartographer+LOAM+LIO-SAM)

[6]ORB-SLAM3理论讲解与代码精析（第2期）

[7]ROS2从入门到精通：理论与实战

视觉三维重建

[1]彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进)

自动驾驶方向课程：

[1]目标检测中的视觉Transformer

[2]单目深度估计方法：算法梳理与代码实现

[3]面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)

[4]如何将深度学习模型部署到实际工程中？（分类+检测+分割）