使用OptiTrack光學(xué)跟蹤系統(tǒng)和Turtlebot機(jī)器人進(jìn)行視覺SLAM定位實(shí)驗(yàn)

本文旨在介紹使用OptiTrack光學(xué)跟蹤系統(tǒng)和Turtlebot機(jī)器人進(jìn)行視覺SLAM定位實(shí)驗(yàn)的詳細(xì)流程，包括實(shí)驗(yàn)平臺(tái)搭建過程、數(shù)據(jù)處理過程以及SLAM估計(jì)評估方法。

由于涉及知識(shí)較多，部分內(nèi)容只給出了相關(guān)參考博文鏈接。

1 實(shí)驗(yàn)平臺(tái)搭建

實(shí)驗(yàn)平臺(tái)包括OptiTrack光學(xué)跟蹤系統(tǒng)和Turtlebot數(shù)據(jù)采集平臺(tái)兩部分，OptiTrack系統(tǒng)用于獲取機(jī)器人運(yùn)動(dòng)軌跡真值，Turtlebot數(shù)據(jù)采集平臺(tái)用于錄制圖像數(shù)據(jù)。

1.1 OptiTrack光學(xué)跟蹤系統(tǒng)介紹

OptiTrack是美國公司NaturalPoint公司研制的高性能光學(xué)跟蹤系統(tǒng)，可以獲取場地中物體的亞像素級位置，目前TUM等公開數(shù)據(jù)集的軌跡真值均通過OptiTrack系統(tǒng)獲取。

OptiTrack由固定在場地上方的多個(gè)Prime相機(jī)、熒光標(biāo)記球、Motive處理軟件組成，使用過程中，將多個(gè)熒光標(biāo)記球粘在物體上，Prime相機(jī)以每秒數(shù)百幀的速率拍攝熒光標(biāo)記球，Motive實(shí)時(shí)地解算出物體在場景中的位姿。OptiTrack系統(tǒng)的原理如圖所示。

1.2 Turtlebot數(shù)據(jù)采集平臺(tái)介紹

本實(shí)驗(yàn)所使用的Turtlebot數(shù)據(jù)采集平臺(tái)如圖所示，共由五部分組成：筆記本、Turtlebot機(jī)器人、深度相機(jī)、熒光標(biāo)記球、游戲手柄。

其中筆記本是該平臺(tái)的核心，用于向各設(shè)備發(fā)送指令，實(shí)現(xiàn)數(shù)據(jù)采集平臺(tái)的各項(xiàng)功能；

Turtlebot機(jī)器人是一款輕量級機(jī)器人開發(fā)套件，在實(shí)驗(yàn)中作為移動(dòng)平臺(tái)；

深度相機(jī)是機(jī)器人的傳感器，在實(shí)驗(yàn)中用于錄制深度和彩色圖像數(shù)據(jù)；

熒光標(biāo)記球用于結(jié)合OptiTrack系統(tǒng)獲取機(jī)器人位姿；

無線游戲手柄用來控制機(jī)器人的運(yùn)動(dòng)。各個(gè)設(shè)備均通過筆記本相連。

1.3 實(shí)驗(yàn)平臺(tái)搭建：

只有知道軌跡和圖像的時(shí)間戳，才能知道軌跡與估計(jì)軌跡之間的對應(yīng)關(guān)系，進(jìn)而評估SLAM系統(tǒng)的定位性能。

OptiTrack系統(tǒng)估計(jì)的位姿輸出在上位機(jī)上，而相機(jī)圖像數(shù)據(jù)輸出在Turtlebot機(jī)器人搭載的筆記本上，需要將位姿數(shù)據(jù)傳輸?shù)焦P記本上，通過ROS統(tǒng)一錄制數(shù)據(jù)，實(shí)現(xiàn)圖像與位姿之間的匹配。

詳細(xì)配置過程見：

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

這里只介紹其工作原理：

上圖中，上位機(jī)安裝有Motive軟件，通過交換機(jī)獲取OptiTrack系統(tǒng)的數(shù)據(jù)。通過Motive軟件中的設(shè)置，將機(jī)器人位姿數(shù)據(jù)經(jīng)過路由器發(fā)布在局域網(wǎng)中。

Turtlebot機(jī)器人搭載的筆記本通過局域網(wǎng)獲取機(jī)器人位姿數(shù)據(jù)，并用ROS的rosbag工具錄制深度相機(jī)話題和位姿話題。可能用到的命令包括：

配置筆記本防火墻的命令：（只需要配置一次）

sudo ufw status  #查看防火墻
sudo ufw enable  #打開防火墻
sudo ufw disable #關(guān)閉防火墻

開始發(fā)布o(jì)ptiTrack位姿的命令，其中192.168.1.104 為路由器的ip地址，筆記本需要通過wifi連接在路由器的局域網(wǎng)中

roslaunch vrpn_client_ros sample.launch server:=192.168.1.104 #獲取optiTrack位姿數(shù)據(jù)

開始發(fā)布Realsense相機(jī)的RGB-D圖像的命令，其中s_d435_camera_our.launch為編寫的launch文件，它記錄了各種ros命令以及參數(shù)配置信息，需要認(rèn)真配置該文件，確保發(fā)布的數(shù)據(jù)“深度-彩色匹配”后的圖像。

filters:=colorizer是指發(fā)布的深度圖像的格式，這里是指彩色表示深度值的大小。

roslaunch realsense2_camera rs_d435_camera_our.launch filters:=colorizer #發(fā)布realsense相機(jī)的圖像話題

使用rosbag命令錄制圖像數(shù)據(jù)與位姿數(shù)據(jù)，并保存為test.bag文件。其中/camera/color/image_raw為彩色圖像話題名稱

/camera/aligned_depth_to_color/image_raw 為深度圖像話題名稱， /vrpn_client_node/RigidBody1/pose為optiTrack位姿的話題名稱，各名稱需要根據(jù)實(shí)際情況修改。

-O test是指將數(shù)據(jù)保存為test.bag。

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

2 實(shí)驗(yàn)設(shè)備標(biāo)定

2.1 Optitrack光學(xué)跟蹤系統(tǒng)標(biāo)定：

在使用OptiTrack光學(xué)定位系統(tǒng)前進(jìn)行標(biāo)定，標(biāo)定有幾個(gè)目的：其一相當(dāng)于對軟件的初始化，為了讓系統(tǒng)確定攝像頭的采集范圍；

其二確定軟件內(nèi)部坐標(biāo)的精度，使軟件根據(jù)數(shù)據(jù)生成一個(gè)3D空間；其三對所標(biāo)記的物體進(jìn)行空間坐標(biāo)的計(jì)算。

詳細(xì)標(biāo)定過程見：

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

標(biāo)定后，在Motive中為機(jī)器人建立剛體，即可獲取機(jī)器人的位姿。

下圖為軟件運(yùn)行結(jié)果，藍(lán)色為四個(gè)Prime相機(jī)，綠線為相機(jī)對機(jī)器人的觀測路徑，下方為四相機(jī)觀測到的紅外圖像。

2.2 相機(jī)標(biāo)定：

深度相機(jī)在使用前需要進(jìn)行標(biāo)定，需要標(biāo)定的參數(shù)有彩色攝像頭的內(nèi)參、深度攝像頭的內(nèi)參以及兩者的外參。

相機(jī)在使用前需要進(jìn)行標(biāo)定的原因是每個(gè)相機(jī)在加工制和組裝過程中都會(huì)存在一定的誤差，這使得每個(gè)相機(jī)的成像平面，相機(jī)坐標(biāo)系都會(huì)略有不同，對相機(jī)標(biāo)定后，可校正相機(jī)的畸變，獲得更為精確的圖像，從而能夠更為準(zhǔn)確的進(jìn)行定位與建圖。

在實(shí)際使用過程中，一般只對彩色攝像頭進(jìn)行標(biāo)定，其他采用廠商提供的默認(rèn)參數(shù)即可。

使用Matlab標(biāo)定相機(jī)的過程可參考：

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

3 實(shí)驗(yàn)數(shù)據(jù)處理

實(shí)驗(yàn)使用rosbag工具采集圖像和位姿數(shù)據(jù)，需要將其轉(zhuǎn)換為TUM數(shù)據(jù)集格式，才能進(jìn)一步運(yùn)行SLAM算法和評估軌跡精度。

該過程包括兩部分：其一為將圖像保存為TUM格式，并使用TUM提供的associate.py實(shí)現(xiàn)深度圖與彩色圖之間的配準(zhǔn)；其二為將位姿數(shù)據(jù)保存為TUM格式的軌跡真值groundtruth.txt。

3.1 RGB-D相機(jī)數(shù)據(jù)處理：

將.bag格式的圖像數(shù)據(jù)轉(zhuǎn)換為TUM格式數(shù)據(jù)的過程見本人之前的博文：

https://www.guyuehome.com/35920

該博文詳細(xì)地介紹了轉(zhuǎn)換過程、ROS程序、測試過程。轉(zhuǎn)換結(jié)果如下圖所示，包括深度圖像depth、彩色圖像rgb、深度圖像路徑depth.txt、彩色圖像路徑rgb.txt。

之后下載TUM提供的associate.py，實(shí)現(xiàn)深度圖像與彩色圖像之間的時(shí)間戳對齊，得到associate.txt

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

3.2 Optitrack位姿數(shù)據(jù)處理：

將rosbag數(shù)據(jù)轉(zhuǎn)換為TUM格式的軌跡真值groundtruth.txt的過程與3.1類似，也是編寫一個(gè)ROS節(jié)點(diǎn)，訂閱Optitrack位姿話題，并將數(shù)據(jù)保存在txt文件中。

具體過程為：

1.編寫optitrack2tum 節(jié)點(diǎn)，修改下文程序中的路徑，并用catkin_make命令編譯工作空間

2.運(yùn)行optitrack2tum節(jié)點(diǎn)

rosrun optitrack2tum opt2tum

3.播放rosbag數(shù)據(jù)

 rosbag play ./rgbd_dataset_fsdf.bag -r 0.3 #-r 0.3是指0.3倍速播放數(shù)據(jù)，太快會(huì)導(dǎo)致數(shù)據(jù)來不及保存

可以在播放數(shù)據(jù)時(shí)同時(shí)運(yùn)行兩個(gè)節(jié)點(diǎn)，保存RGB-D圖像和Optitrack位姿數(shù)據(jù)。此外，也可以將路徑作為參數(shù)傳遞給程序，避免處理不同數(shù)據(jù)時(shí)對程序的修改。

optitrackToTUM.cpp文件編寫如下：

#include 
#include 
#include 
#include 
#include 
#include 
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::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<<"
 
"<header.stamp.toSec(); 
    double time = msg->header.stamp.toSec();
    of<pose.position.x <<" "<pose.position.y<<" "<< msg->pose.position.z<<" ";
 ? ? ? of<pose.orientation.x<<" "<< msg->pose.orientation.y <<" "<pose.orientation.z <<" "<pose.orientation.w ;
  }
}




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


  ofstream of;
  of.open(pathGroungTruth, std::app);
  if(of.fail()){
    ROS_INFO("Fail to open file!!");
  }else{
    of<<"#------------start a new groundTruth-----------------"<

	

	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軌跡軌跡的詳細(xì)過程可參考博文：

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

	

	例如，使用以下命令繪制SLAM算法軌跡軌跡與真實(shí)軌跡的對比圖。

	其中evo_ape用來計(jì)算絕對軌跡誤差，tum為所使用的數(shù)據(jù)格式，ourslam.txt為SLAM算法估計(jì)的軌跡，groundtruth.txt為3,2節(jié)獲取到的軌跡真值，-p表示繪制誤差曲線，-va表示詳細(xì)模式，且使用SE(3)對齊。

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

	

	

	    責(zé)任編輯：彭菁