Vins-mono
ros
添加清华大学ros源，选择自己的系统版本
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
# 更新
sudo apt update
sudo apt-get upgrade -y
安装ros前记得查询自己的Ubuntu对应什么ros版本，18.04对应的ros版本为ROS Melodic
sudo apt-get install ros-melodic-cv-bridge ros-melodic-tf ros-melodic-message-filters ros-melodic-image-transport ros-melodic-desktop-full -y
接下来的命令需要用到rosdep，但是这个在国内使用存在问题，将所有rosdep改为rosdepc，感谢大佬本文之后，世上再无rosdep更新失败问题！如果有....小鱼就...
sudo apt-get install python3-pip -y
sudo pip3 install rosdepc
# 安装评估工具
pip3 install evo --upgrade
sudo apt-get install python3-tk -y
# 初始化空间
sudo rosdepc init
# 更新空间
rosdepc update
# 添加环境变量
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc
打开3个终端进行测试
# 打开主进程
roscore
# 运行turtlesim 包中的turtlesim_node节点，打开小乌龟控制台
rosrun turtlesim turtlesim_node
# 运行turtlesim 包中的turtle_teleop_key节点，可以使用键盘方向键移动小乌龟
rosrun turtlesim turtle_teleop_key
如果打开主线程报错ModuleNotFoundError: No module named 'defusedxml'，这是因为python版本不兼容，ROS Melodic 依赖 Python 2.7
Ceres Solver 1.14.0
sudo apt-get install libgoogle-glog-dev libgflags-dev libatlas-base-dev -y
mkdir -p ~/code
cd ~/code && wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar zxf ceres-solver-1.14.0.tar.gz
cd ceres-solver-1.14.0
mkdir build
cd build
cmake ..
make -j4
sudo make install
Vins-mono
# 创建工作空间目录
mkdir -p ~/catkin_ws/src
# 进入 src 目录
cd ~/catkin_ws/src
# 克隆 VINS-Mono 仓库
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
修改保存位置,我选择的是**/root/result/vins-mono**
mkdir -p ~/result/vins-mono
nano ~/catkin_ws/src/VINS-Mono/config/euroc/euroc_config.yaml
# 设置轨迹保存位置
output_path:/root/result/vins-mono/
# 设置位姿图保存位置
pose_graph_save_path:/root/result/vins-mono/pose_graph/
由于vins-mono保存的轨迹既不符合tum数据集的格式，又不符合euroc数据集的格式。我们需要对其进行修改。
# 第一个文件
nano ~/catkin_ws/src/VINS-Mono/vins_estimator/src/utility/visualization.cpp
# 修改前
// write result to file
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << ","
<< estimator.Ps[WINDOW_SIZE].y() << ","
<< estimator.Ps[WINDOW_SIZE].z() << ","
<< tmp_Q.w() << ","
<< tmp_Q.x() << ","
<< tmp_Q.y() << ","
<< tmp_Q.z() << ","
<< estimator.Vs[WINDOW_SIZE].x() << ","
<< estimator.Vs[WINDOW_SIZE].y() << ","
<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
foutC.close();
# 修改后
// write result to file
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(9);
foutC << header.stamp.toSec() << " ";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << " "
<< estimator.Ps[WINDOW_SIZE].y() << " "
<< estimator.Ps[WINDOW_SIZE].z() << " "
<< tmp_Q.x() << " "
<< tmp_Q.y() << " "
<< tmp_Q.z() << " "
<< tmp_Q.w() << endl;
//<< estimator.Vs[WINDOW_SIZE].x() << ","
//<< estimator.Vs[WINDOW_SIZE].y() << ","
//<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
foutC.close();
# 第二个文件
nano ~/catkin_ws/src/VINS-Mono/pose_graph/src/pose_graph.cpp
# 第一个位置在函数addKeyFrame()里
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
# 修改为
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(9);
loop_path_file << cur_kf->time_stamp << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
loop_path_file.close();
}
# 第二个位置在函数updatePath()里
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << (*it)->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
# 修改为
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(9);
loop_path_file << (*it)->time_stamp << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << ","
<< P.z() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< Q.w() << endl;
loop_path_file.close();
}
具体来说这3个地方改的方式是一样的
- 第一个.precision(0)改为.precision(9)，第二个.precision(5)则维持不变
- (*it)->time_stamp * 1e9改为(*it)->time_stamp
- 分隔符由**,**改为空格
- 输出格式由xyzwxyz改为xyzxyzw，记住w后面需要接换行符。
完成修改后进行编译
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
数据集测试并评估
EUROC数据集官网：kmavvisualinertialdatasets – ASL Datasets (ethz.ch)
# 下载数据集
mkdir -p ~/dataset/EUROC && cd ~/dataset/EUROC
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.bag
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.zip
unzip MH_01_easy.zip -d MH01
# 打开3个终端
# 第一个
cd ~/catkin_ws && source devel/setup.bash
roslaunch vins_estimator euroc.launch
# 第二个
cd ~/catkin_ws && source devel/setup.bash
roslaunch vins_estimator vins_rviz.launch
# 第三个
cd ~/catkin_ws && source devel/setup.bash
rosbag play ~/dataset/EUROC/MH_01_easy.bag
运行结束后进入刚刚设置的保存位置，里面会有2个文件（分别表示有回环和无回环的结果）加上1个位姿图文件夹,将groundtruth的csv文件转化为tum格式。
cd ~/result/vins-mono
evo_traj euroc ~/dataset/EUROC/MH01/mav0/state_groundtruth_estimate0/data.csv --save_as_tum
# 无回环
# ATE
evo_ape euroc ~/dataset/EUROC/MH01/mav0/state_groundtruth_estimate0/data.csv vins_result_no_loop.csv -va --plot --plot_mode xyz --save_results vins_no_loop_ate.zip --save_plot vins_no_loop_ate_plot.pdf
# RPE
evo_rpe euroc ~/dataset/EUROC/MH01/mav0/state_groundtruth_estimate0/data.csv vins_result_no_loop.csv -va --plot --plot_mode xyz --save_results vins_no_loop_rpe.zip --save_plot vins_no_loop_rpe_plot.pdf
# 轨迹图
evo_traj tum vins_result_no_loop.csv --ref=data.tum -p --plot_mode=xyz --align --correct_scale --save_plot vins_no_loop_traj_plot.pdf
# 有回环
# ATE
evo_ape euroc ~/dataset/EUROC/MH01/mav0/state_groundtruth_estimate0/data.csv vins_result_loop.csv -va --plot --plot_mode xyz --save_results vins_loop_ate.zip --save_plot vins_loop_ate_plot.pdf
# RPE
evo_rpe euroc ~/dataset/EUROC/MH01/mav0/state_groundtruth_estimate0/data.csv vins_result_loop.csv -va --plot --plot_mode xyz --save_results vins_loop_rpe.zip --save_plot vins_loop_rpe_plot.pdf
# 轨迹图
evo_traj tum vins_result_loop.csv --ref=data.tum -p --plot_mode=xyz --align --correct_scale --save_plot vins_loop_traj_plot.pdf
评论区