ORB-SLAM3实际数据与OptiTrack真值比较的实现与测试

2023-12-10 17:15:45

ORB-SLAM3实际数据与OptiTrack真值比较的实现与测试

前言
一、新起节点作为中间桥梁
二、ORB-SLAM3节点修改
- 1.数据格式处理
- 2.数据发布
三、cmakelist

前言

在对ORB-SLAM3进行改进时，我们通常会用公开数据集来测试算法精度，但是如果我们接入自己的相机与IMU在实际场景跑，尤其是在室内环境，如何获得grondtruth，成为我们最关心的问题。最近，实验室新引进一套OptiTrack动作捕捉系统，该系统具备亚毫米级精度，能精确采集运动体位置姿态，六*度数据，是现阶段较为精确的真值系统。

一、新起节点作为中间桥梁

要想在实际场景对ORB-SLAM3跑出来的数据与OptiTrack的真值数据进行比较，我们采用evo工具进行评估，因此我们需要将两个数据按照规定的格式保存下来。

这里我们想到重新起一个节点evo_slam_optitrack，原因如下：

该节点同时订阅ORB-SLAM3与OptiTrack发布的里程计数据。这样不用改动ORB-SLAM3的框架，将节点evo_slam_optitrack作为一个中间桥梁即可。
我们所得的两个轨迹需要在时间戳上做同步，在同一个节点下实现较为方便。

//定义时间戳
double trac_stamp;

// 
void odometry_save_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
{
	nav_msgs::Odometry odom = *odom_msg;
	//odom_stamp = odom.header.stamp.toSec();

	ofstream slam_path_file("/home/user/Documents/ORBSLAM3.txt", ios::app);
	slam_path_file.setf(ios::fixed, ios::floatfield);
	slam_path_file.precision(0);
	slam_path_file << trac_stamp * 1e9 << " ";
	slam_path_file.precision(10);
	slam_path_file 	<< odom.pose.pose.position.x << " "
			<< odom.pose.pose.position.y << " "
			<< odom.pose.pose.position.z << " "
			<< odom.pose.pose.orientation.x << " "
			<< odom.pose.pose.orientation.y << " "
			<< odom.pose.pose.orientation.z << " "
			<< odom.pose.pose.orientation.w << endl;
	slam_path_file.close();
}


void optiTrack_save_callback(const opti_track::TrackData::ConstPtr& trac_msg)
{
	opti_track::TrackData trac = *trac_msg;
	trac_stamp = trac.header.stamp.toSec();

	ofstream optitrack_path_file("/home/user/Documents/OptiTrack.txt", ios::app);
	optitrack_path_file.setf(ios::fixed, ios::floatfield);
	optitrack_path_file.precision(0);
	optitrack_path_file << trac_stamp * 1e9 << " ";
	optitrack_path_file.precision(10);
	optitrack_path_file << trac_msg->rigid_bodies[0].pose.position.x << " "
			<< trac_msg->rigid_bodies[0].pose.position.y << " "
			<< trac_msg->rigid_bodies[0].pose.position.z << " "
			<< trac_msg->rigid_bodies[0].pose.orientation.x << " "
			<< trac_msg->rigid_bodies[0].pose.orientation.y << " "
			<< trac_msg->rigid_bodies[0].pose.orientation.z << " "
			<< trac_msg->rigid_bodies[0].pose.orientation.w << endl;
	optitrack_path_file.close();

}


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

	ros::NodeHandle nh("~");


    ros::Subscriber sub_odom = nh.subscribe<nav_msgs::Odometry>("/Mono_Inertial/odometry", 100, odometry_save_callback); 
    ros::Subscriber sub_optiTrack = nh.subscribe<opti_track::TrackData>("/opti_track_node/opti_track_data", 100, optiTrack_save_callback);


	ros::spin();

	return 0;
}

二、ORB-SLAM3节点修改

1.数据格式处理

对ORB-SLAM3源码进行分析，该框架有基于ROS的版本，但并未发布里程计数据，因此需要我们自己去写publisher。首先假设我们跑的是单目+IMU版，在/src下找到Mono_Inertial.cc，这里订阅了相机和IMU话题。找到void ImageGrabber::SyncWithImu() 函数，该函数中调用了TackMonocular()，这里函数返回的为Tcw矩阵，这就是估计的相机在世界系下的表达。矩阵前3x3为旋转，最后一列前三个元素表示x,y,z。这里将Tcw拆解为旋转和平移，调用opencv中的函数，对矩阵进行拆解。ROS中odomtry是以四元数的形式来表示旋转，因此，需要找到工程中实现四元数转换的代码段。此代码在converter.cc中采用Converter::toQuaternion()函数实现的，而该函数中又调用了Converter::toMatrix3d()，于是，为了不引入这个头文件以及改cmakelist，我们直接将这个函数在Mono_Inertial.cc中实现。

待补充

2.数据发布

将上步中的数据处理成odom标准格式，然后publish。

待补充

三、cmakelist

cmakelist.txt中需要增加相应的节点

#Node for evo_slam_optitrack
 add_executable(evo_slam_optitrack src/evo_slam_optitrack.cpp)

 target_link_libraries(evo_slam_optitrack
 ${catkin_LIBRARIES} libNatNet.so
 )

码农公寓