ubuntu | 20 |
---|---|
ros | noetic |
SVO SLAM | |
虚拟机 windows 11 |
cd svo_ws/src
git clone https://github.com/dorian3d/DBoW2.git
tar -cvf DBoW2.tar DBoW2
# 修改svo_ws/src/dbow2_catkin/CMakeLists.txt
ExternalProject_Add(dbow2_src
GIT_REPOSITORY git@github.com:dorian3d/DBoW2.git
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}
BUILD_COMMAND CXXFLAGS=-i${CATKIN_DEVEL_PREFIX}/include make
INSTALL_COMMAND make install
)
# 改为 XXXXXX为DBoW2.tar所在路径
ExternalProject_Add(dbow2_src
URL XXXXXX/DBoW2.tar
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}
BUILD_COMMAND CXXFLAGS=-i${CATKIN_DEVEL_PREFIX}/include make
INSTALL_COMMAND make install
)
??创建工作空间,克隆代码
mkdir -p svo_ws/src && cd svo_ws/src
cd ..
catkin config --init --mkdirs --extend /opt/ros/noetic --cmake-args -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3
cd src
git clone https://github.com/uzh-rpg/rpg_svo_pro_open.git
??依赖下载
# 该步骤任选其一执行
# 1
vcs-import < ./rpg_svo_pro_open/dependencies.yaml
# 1 结束
# 2
git clone https://github.com/catkin/catkin_simple.git &&\
git clone https://github.com/zurich-eye/cmake_external_project_catkin.git &&\
git clone https://github.com/ethz-asl/eigen_catkin.git &&\
git clone https://github.com/ethz-asl/eigen_checks.git &&\
git clone https://github.com/uzh-rpg/fast_neon.git &&\
git clone https://github.com/ethz-asl/gflags_catkin.git &&\
git clone https://github.com/ethz-asl/glog_catkin.git &&\
git clone https://github.com/ethz-asl/minkindr.git &&\
git clone https://github.com/ethz-asl/opengv.git &&\
git clone https://github.com/ethz-asl/minkindr_ros.git &&\
git clone https://github.com/ethz-asl/ceres_catkin.git &&\
git clone https://github.com/uzh-rpg/dbow2_catkin.git &&\
git clone https://github.com/uzh-rpg/rpg_trajectory_evaluation.git
# 2结束
??编译
touch minkindr/minkindr_python/CATKIN_IGNORE
cd rpg_svo_pro_open/svo_online_loopclosing/vocabularies && ./download_voc.sh
cd ../../..
catkin build
??启动SVO核心ROS节点
source XXXXXX/devel/setup.bash
roslaunch svo_ros euroc_mono_frontend_imu.launch
??播放数据集
rosbag play XXXXXX/DataSets/MH_01_easy.bag
??运行图片
??svo轨迹数据通过 /svo/points 话题发布,该话题为visualization_msgs/Marker(中文参考)类型。旋转数据通过 /svo/pose_imu 发布,该话题为geometry_msgs/PoseWithCovarianceStamped(中文参考)类型。
??轨迹路径保存主要分为以下几步:
evo_traj euroc groundtruth.csv --save_as_tum
??/svo/pose_imu原始数据转化为tum格式代码见附录
??获取到真实轨迹与实际轨迹后进行对比,代码与结果如下(感觉结果有点不对,希望知道的人指出):data为真实轨迹,svo_result为ros结果
evo_traj tum data.tum svo_result.txt -p -s --align_origin -a --plot_mode=xyz --ref=data.tum
待补充
in_file_path = "svo_pose_data.txt"
out_file_path = "svo_result.txt"
rfp = open(in_file_path)
wfp = open(out_file_path,"w")
file_end = False
while file_end != True:
time = 0
tx = 0
ty = 0
tz = 0
qx = 0
qy = 0
qz = 0
qw = 0
for i in range(19):
line = rfp.readline()
if line == "":
file_end = True
break
if i == 3:
time += float(line.split(":")[1])
elif i == 4:
time += float(line.split(":")[1])/1e9
elif i == 9:
tx = float(line.split(":")[1])
elif i == 10:
ty = float(line.split(":")[1])
elif i == 11:
tz = float(line.split(":")[1])
elif i == 13:
qx = float(line.split(":")[1])
elif i == 14:
qy = float(line.split(":")[1])
elif i == 15:
qz = float(line.split(":")[1])
elif i == 16:
qw = float(line.split(":")[1])
else:
pass
write_str = "{time} {tx} {ty} {tz} {qx} {qy} {qz} {qw}\n".format(time=time,tx=tx,ty=ty,tz=tz,qx=qx,qy=qy,qz=qz,qw=qw)
# print(write_str)
wfp.write(write_str)
rfp.close()
wfp.close()
print("保存完毕")