《视觉SLAM进阶：从零开始手写VIO》 第7讲作业 超详细运行步骤
文章目录
前言：VINS-Course代码解析
参考：
[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——run_euroc前端的数据处理(内容|代码)，写得特别详细，不管是前端，后端以及初始化部分都有详细的解析（后端及初始化部分可以根据这篇文章的链接顺序看下去）！！
关于第七节VINS的初始化，其内容和代码均整理在下面列出的博客中。视频的主要讲解内容在初始化1，初始化3。
run_euroc前端的数据处理(内容|代码)
初始化1外参标定(内容|代码)
初始化2视觉初始化(内容|代码)
初始化3视觉IMU对齐(内容|代码)
初始化4visualInitialAlign()(内容|代码)
VINS_Mono知识点解析
1.使用EuRoC MAV Dataset跑本VINS工程
1.1 下载数据集： EuRoC MAV Dataset
1.2 修改数据集路径
将文件test/run_euroc.cpp的如下
string sData_path = "/home/dataset/EuRoC/MH-05/mav0/";
改为自己的路径，我的数据集路径如下：
string sData_path = "/home/cgm/MH_05/mav0/";
1.3 编译
cd build
cmake ..
make
1.4 报错及解决
因我当前环境opencv版本过高问题，需要针对每个报错点进行相应版本的匹配修改
-
报错1：
error: ‘CV_BGR2GRAY’ was not declared in this scope解决方法：将 CV_BGR2GRAY 全部替换为 cv::COLOR_BGR2GRAY
-
报错2：
error: ‘CV_GRAY2BGR’ was not declared in this scope
error: ‘CV_GRAY2RGB’ was not declared in this scope解决方法：
将 CV_GRAY2BGR 全部替换为 cv::COLOR_GRAY2BGR
将 CV_GRAY2RGB 全部替换为 cv::COLOR_GRAY2RGB
报错3： error: ‘CV_CALIB_CB_ADAPTIVE_THRESH’ was not declared in this scope
error: ‘CV_CALIB_CB_NORMALIZE_IMAGE’ was not declared in this scope
error: ‘CV_CALIB_CB_FILTER_QUADS’ was not declared in this scope
error: ‘CV_CALIB_CB_FAST_CHECK’ was not declared in this scope
**解决方法：**将CV_CALIB_CB_ADAPTIVE_THRESH、CV_CALIB_CB_NORMALIZE_IMAGE、CV_CALIB_CB_FILTER_QUADS和CV_CALIB_CB_FAST_CHECK
修改为 cv::CALIB_CB_ADAPTIVE_THRESH 、 cv::CALIB_CB_NORMALIZE_IMAGE 、cv::CALIB_CB_FILTER_QUADS 和cv::CALIB_CB_FAST_CHECK
其他3个替换同理
报错4：error: ‘CV_ADAPTIVE_THRESH_MEAN_C’ was not declared in this scope
解决办法：
在报错文件上添加头文件 #include <opencv2/imgproc/imgproc_c.h>
单独遇到CV_AA的报错时，也可以将 CV_AA 改为 cv::LINE_AA
报错5：/home/cgm/VINS-Course/test/run_euroc.cpp:10:10: fatal error: cv.h: 没有那个文件或目录 10 | #include <cv.h>
解决办法：
#include <cv.h>
#include <highgui.h>
改为
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
报错6：
error: ‘QuitAll’ is not a member of ‘pangolin’; did you mean ‘Quit’? 34 | pangolin::QuitAll();
解决办法：
pangolin::QuitAll();改为 pangolin::Quit();
报错7：
error: ‘CV_WINDOW_AUTOSIZE’ was not declared in this scope 164 | cv::namedWindow("IMAGE", CV_WINDOW_AUTOSIZE);
解决办法：
CV_WINDOW_AUTOSIZE 改为 cv::WINDOW_AUTOSIZE
可以参考一下链接的报错解决办法：
源码安装vins-mono算法问题整理（ROS Melodic + opencv 4.1.1）
1.5 运行 run_euroc
cd bin
./run_euroc /home/cgm/MH_05/mav0/ ../config/
可以在VINS-Course目录下创建build.sh脚本文件，以后测试只需要运行 ./build.sh 就可以了。
build.sh
#!/bin/bash
echo "Configuring and building ..."
if [ ! -d "build" ]; then
mkdir build
fi
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
cd ../bin
./run_euroc /home/cgm/MH_05/mav0/ ../config/
1.6 结果
2.使用第二章仿真数据跑该VINS工程
2.1 第二章的仿真数据
数据在vio_data_simulation/bin 文件下
第二章的运动轨迹是一个xy平面椭圆运动，z正弦运动。
house_model：
保存了原始设置的3d房屋关键点（一行两个点连成线，保存点前判断一下是否之前获取过）
运行后生成如下数据：
keyframe文件夹：
每一帧相机观测到的特征点，代码中设定相机帧率为30Hz，仿真20s，对应一共产生600帧观测
all_points：
保存了所有特征点（对比房屋关键点可以有所增加）文件存储的是house模型的线特征，每行4个数，对应 该线两端点 在归一化平面的坐标，一共23行对应23条线段
cam_pose：
所有相机位姿， cam_pose_tum只是存储顺序和变量个数有所不同
imu_pose：
所有imu位姿， imu_pose_noise 带噪声
mu_int_pose：
积分后得到的imu轨迹， imu_int_pose_noise 带噪声
2.2 新建run_simulate.cpp
在VINS-Course下的test文件下：复制 run_euroc.cpp 并重命名为 run_simulate.cpp
修改部分：imu位姿的路径；cam位姿的路径；void PubImuData()函数；void PubImageData()函数；
我的imu位姿和cam位姿还是在第二章的代码 vio_data_simulation/bin 文件下
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <thread>
#include <iomanip>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include "System.h"
using namespace std;
using namespace cv;
using namespace Eigen;
const int nDelayTimes = 2;
string sData_path = "/home/cgm/vio_data_simulation/bin";//修改imu位姿文件的路径
string sConfig_path = "/home/cgm/vio_data_simulation/bin";//修改cam位姿文件的路径
std::shared_ptr<System> pSystem;
void PubImuData()
{
string sImu_data_file = sConfig_path + "imu_pose.txt";//修改要测试的imu位姿文件 //不带噪声的IMU数据的路径
cout << "1 PubImuData start sImu_data_filea: " << sImu_data_file << endl;
ifstream fsImu;//文件流对象
fsImu.open(sImu_data_file.c_str());
if (!fsImu.is_open())
{
cerr << "Failed to open imu file! " << sImu_data_file << endl;
return;
}
std::string sImu_line;
double dStampNSec = 0.0;//时间戳
double tmp;
Vector3d vAcc;//加速度
Vector3d vGyr;
while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data sImu_line获得每行的文件流
{
// timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3)
std::istringstream ssImuData(sImu_line);//ssImuData得到每行文件的内容
ssImuData >> dStampNSec;//时间戳
//利用循环跳过imu quaternion(4)，imu position(3)
for(int i=0;i<7;i++)
ssImuData>>tmp;
ssImuData>>vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z();
// 时间单位为 s
pSystem->PubImuData(dStampNSec, vGyr, vAcc);//PubImuData不需要改变 用来将不同时刻下的IMU数据放进VINS系统
usleep(5000*nDelayTimes);
}
fsImu.close();
}
void PubImageData()
{
string sImage_file = sConfig_path + "cam_pose.txt";//修改要测试的cam位姿文件
cout << "1 PubImageData start sImage_file: " << sImage_file << endl;
ifstream fsImage;//文件流对象
fsImage.open(sImage_file.c_str());
if (!fsImage.is_open())
{
cerr << "Failed to open image file! " << sImage_file << endl;
return;
}
std::string sImage_line;
double dStampNSec;
string sImgFileName;
int n=0;
// cv::namedWindow("SOURCE IMAGE", CV_WINDOW_AUTOSIZE);
//这个循环是遍历所有的相机
while (std::getline(fsImage, sImage_line) && !sImage_line.empty())//sImage_line是cam_pose每行的数据流
{
std::istringstream ssImgData(sImage_line);//是cam_pose每行的内容
ssImgData >> dStampNSec; //读入时间戳
cout<<"cam time: "<<fixed<<dStampNSec<<endl;
// all_points_ 文件存储的是house模型的线特征，每行4个数，对应该线两端点在归一化平面的坐标
//all_points_ 文件每行的内容是 x, y, z, 1, u, v 这里的u v是归一化下的x ,y 不是像素坐标
//在函数PubSimImageData中会算出具体特征点的像素坐标
string all_points_file_name = "/home/cgm/vio_data_simulation/bin/keyframe/all_points_" + to_string(n)+ ".txt"; //第n个相机对应的观测数据的文件名
cout<<"points_file: "<<all_points_file_name<<endl;
vector<cv::Point2f> FeaturePoints;//容器FeaturePoints存放一个相机的特征点(归一化坐标)
std::ifstream f;
f.open(all_points_file_name);
//这个循环是遍历每个相机的特征点信息
// file content in each line: x, y, z, 1, u, v
//经过这个循环把all_points_的特征点都放在FeaturePoints了
while(!f.eof())
{
std::string s;
std::getline(f,s);//得到all_points_的文件流s
// 一行两个点连成线，获取每行点判断一下是否之前获取过
if(!s.empty())
{
std::stringstream ss;//
ss << s;//ss得到每行的内容
double tmp;//跳过 x y z 1
for(int i=0;i<4;i++)
ss>>tmp;
float px,py;
ss >> px;
ss >> py;
cv::Point2f pt( px, py);//归一化坐标
FeaturePoints.push_back(pt);
}
}
// cout << "All points:" << endl;
// for(auto point : FeaturePoints){
// cout << point << " ";
// }
// cout << endl;
pSystem->PubSimImageData(dStampNSec, FeaturePoints);//把每一个图片的特征点 放进VINS系统里
usleep(50000*nDelayTimes);
n++;
}
fsImage.close();
}
//后面代码没变化，就不复制了。
//后面代码没变化，就不复制了。
//后面代码没变化，就不复制了。
2.3 修改CMakeLists.txt
在CMakeLists.txt 末尾增加代码
add_executable(run_simulate test/run_simulate.cpp)
target_link_libraries(run_simulate
MyVio
-lpthread)
2.4 修改System.h
增加 第14行 代码
class System
{
public:
System(std::string sConfig_files);
~System();
void PubImageData(double dStampSec, cv::Mat &img);
void PubImuData(double dStampSec, const Eigen::Vector3d &vGyr,
const Eigen::Vector3d &vAcc);
//添加声明
void PubSimImageData(double dStampSec, const vector<cv::Point2f> &FeaturePoints);
2.5 修改System.cpp
System.cpp末尾增加如下代码
这个函数是根据 System.cpp的 void System::PubImageData(double dStampSec, Mat &img)进行修改得到的。
void System::PubSimImageData(double dStampSec, const vector<cv::Point2f> &FeaturePoints)
{
if (!init_feature)
{
cout << "1 PubImageData skip the first detected feature, which doesn't contain optical flow speed" << endl;
init_feature = 1;
return;
}
if (first_image_flag)
{
cout << "2 PubImageData first_image_flag" << endl;
first_image_flag = false;
first_image_time = dStampSec;
last_image_time = dStampSec;
return;
}
// detect unstable camera stream 发现时间戳不连续甚至倒退，提示重新输入
if (dStampSec - last_image_time > 1.0 || dStampSec < last_image_time)
{
cerr << "3 PubImageData image discontinue! reset the feature tracker!" << endl;
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
return;
}
last_image_time = dStampSec;
// frequency control 控制频率设定小于某一阈值
// if (round(1.0 * pub_count / (dStampSec - first_image_time)) <= FREQ)
// {
// PUB_THIS_FRAME = true;
// // reset the frequency control TODO question:若当前连续图像序列的频率与 FREQ=10 误差在一定范围内重置？
// if (abs(1.0 * pub_count / (dStampSec - first_image_time) - FREQ) < 0.01 * FREQ)
// {
// first_image_time = dStampSec;
// pub_count = 0;
// }
// }
// else
// {
// PUB_THIS_FRAME = false;
// }
PUB_THIS_FRAME = true;
TicToc t_r;
// cout << "3 PubImageData t : " << dStampSec << endl;
// TODO Bookmark:获取图像特征点
// trackerData[0].readImage(img, dStampSec);
// trackerData[0].readPoints(FeaturePoints, dStampSec);
// for (unsigned int i = 0;; i++)
// {
// bool completed = false;
// completed |= trackerData[0].updateID(i);
//
// if (!completed)
// break;
// }
if (PUB_THIS_FRAME)
{
pub_count++;
shared_ptr<IMG_MSG> feature_points(new IMG_MSG());
feature_points->header = dStampSec;
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
// auto &un_pts = trackerData[i].cur_un_pts;// 去畸变的归一化图像坐标
// auto &cur_pts = trackerData[i].cur_pts;// 当前追踪到的特征点
// auto &ids = trackerData[i].ids;
// auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < FeaturePoints.size(); j++)
{
// if (trackerData[i].track_cnt[j] > 1)
// {
// int p_id = ids[j];
int p_id = j;
hash_ids[i].insert(p_id);
double x = FeaturePoints[j].x;
double y = FeaturePoints[j].y;
double z = 1;
feature_points->points.push_back(Vector3d(x, y, z));
feature_points->id_of_point.push_back(p_id * NUM_OF_CAM + i);
// feature_points->u_of_point.push_back(cur_pts[j].x); // 像素坐标
// feature_points->v_of_point.push_back(cur_pts[j].y);
// // TODO Bookmark:速度项用于对齐imu时间戳 作业不考虑可以设为0
// feature_points->velocity_x_of_point.push_back(pts_velocity[j].x);
// feature_points->velocity_y_of_point.push_back(pts_velocity[j].y);
cv::Point2f pixel_point;
pixel_point.x = 460 * x + 255;
pixel_point.y = 460 * y + 255;
feature_points->u_of_point.push_back(pixel_point.x); // 像素坐标
feature_points->v_of_point.push_back(pixel_point.y);
feature_points->velocity_x_of_point.push_back(0);
feature_points->velocity_y_of_point.push_back(0);
// }
}
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
cout << "4 PubImage init_pub skip the first image!" << endl;
init_pub = 1;
}
else
{
m_buf.lock();
feature_buf.push(feature_points);
// cout << "5 PubImage t : " << fixed << feature_points->header
// << " feature_buf size: " << feature_buf.size() << endl;
m_buf.unlock();
con.notify_one();
}
}
}
}
2.6 修改配置文件
复制VINS-Course/config下的euroc_config_sim.yaml 并重命名为 euroc_config_sim.yaml
euroc_config_sim.yaml
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/home/hyj/slam_course_vins/"
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 640
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 460
fy: 460
cx: 255
cy: 255
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0, 0, -1,
-1, 0, 0,
0, 1, 0]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [0.05,0.04,0.03]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.019 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.015 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.0001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 0 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/cgm/VINS-Course/" # save and load path
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
修改**System.cpp**配置文件的路径，下面代码的第12行改为刚刚的euroc_config_sim.yaml文件
#include "System.h"
#include <pangolin/pangolin.h>
using namespace std;
using namespace cv;
using namespace pangolin;
System::System(string sConfig_file_)
:bStart_backend(true)
{
string sConfig_file = "../config/euroc_config_sim.yaml";
2.6 使用脚本运行run_simulate
复制VINS-Course目录下创建的build.sh脚本文件，并修改最后一行。
#!/bin/bash
echo "Configuring and building ..."
if [ ! -d "build" ]; then
mkdir build
fi
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
cd ../bin
./run_simulate /home/cgm/vio_data_simulation/bin/ /home/cgm/vio_data_simulation/bin/
注意自己的 imu位姿 和 cam位姿 的数据位置
结果：
生成的 pose_output.txt 在 VINS-Course/bin 目录下
3.使用evo评估工具
3.1 evo评估pose_output.txt
代码生成的 pose_output.txt 在 VINS-Course/bin 目录下。
但是我们使用evo工具
evo_traj tum pose_output.txt --plot
却报错：
[ERROR] TUM trajectory files must have 8 entries per row and no trailing delimiter at the end of the rows (space)
**原因：**pose_output.txt有的数据中间有两个空格，使用evo工具，不符合tum格式
3.2 pose_output.txt存储为TUM数据集格式
解决办法：
方法1：把
pose_output.txt里面的 2个空格 全部替换成 1个空格。方法2：修改保存数据的代码-------void System::ProcessBackEnd()函数。
将 void System::ProcessBackEnd() 函数的如下2行
cout << "1 BackEnd processImage dt: " << fixed << t_processImage.toc() << " stamp: " << dStamp << " p_wi: " << p_wi.transpose() << endl; ofs_pose << fixed << dStamp << " " << p_wi.transpose() << " " << q_wi.coeffs().transpose() << endl; 修改为下面的代码
//修改保存数据的代码如下： ofs_pose.setf(std::ios::fixed, std::ios::floatfield); ofs_pose.precision(9); ofs_pose <<dStamp<<" ";//时间戳保存为9位数精度 ofs_pose.precision(5);//位姿保存位5位数的精度 ofs_pose <<p_wi(0)<<" " <<p_wi(1)<<" " <<p_wi(2)<<" " <<q_wi.coeffs().x()<<" " <<q_wi.coeffs().y()<<" " <<q_wi.coeffs().z()<<" " <<q_wi.coeffs().w()<<std::endl; cout << "1 BackEnd processImage dt: " << " stamp: " << dStamp << " p_wi: " << p_wi.transpose() << endl;
把之前的 pose_output.txt文件 和 可执行文件run_simulate 删除；
再次运行 ./build_sim.sh 查看一下 pose_output.txt 为 TUM数据集格式:
timestamp tx ty tz qx qy qz qw
(每行有８个元素，元素之间只有1个空格，结尾没有空格, 时间以秒为单位
3.3 画出轨迹
evo_traj tum pose_output.txt --plot
4.对无噪声和默认噪声的IMU数据和相机数据仿真
4.1生成无噪声的 pose_output_without_noise.txt
因为我们的run_simulate.cpp用的是没有噪声的数据：
string sImu_data_file = sConfig_path + "imu_pose.txt";
string sImage_file = sConfig_path + "cam_pose.txt";
所以为了测试，我们修改System.cpp
ofs_pose.open("./pose_output.txt",fstream::app | fstream::out);
修改导出文件的名字
ofs_pose.open("./pose_output_without_noise.txt",fstream::app | fstream::out);
运行./build_sim.sh 生成 pose_output_without_noise.txt
4.2生成有噪声（默认）的 pose_output_with_noise.txt
这里的 imu_pose_noise.txt 是第二讲默认的噪声生成的
// 第二讲默认的噪声 default noise
double gyro_bias_sigma = 1.0e-5; //陀螺仪的 bias 随机游走噪声
double acc_bias_sigma = 0.0001; //加速度 bias 的随机游走噪声
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声
修改
string sImu_data_file = sConfig_path + "imu_pose.txt";
改为
string sImu_data_file = sConfig_path + "imu_pose_noise.txt";
修改
ofs_pose.open("./pose_output_without_noise.txt",fstream::app | fstream::out);
改为
ofs_pose.open("./pose_output_with_noise.txt",fstream::app | fstream::out);
运行./build_sim.sh 生成 pose_output_with_noise.txt
4.3 对比无噪声
我们把 vio_data_simulation/bin 里的 cam_pose_tum.txt 复制到
命令：
evo_ape tum cam_pose_tum.txt pose_output_without_noise.txt -va --plot --plot_mode xyz --save_results without_noise.zip
这里我们使用evo_ape(cam_pose_tum.txt是参考) 计算来自 pose_output_without_noise.txt 的轨迹的绝对位姿误差，并将各个结果绘制并保存到 without_nois.zip 文件中
cgm@ubuntu:~/VINS-Course/bin$ evo_ape tum cam_pose_tum.txt pose_output_without_noise.txt -va --plot --plot_mode xyz --save_results without_noise.zip
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 1761 stamps and poses from: pose_output_without_noise.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 587 of max. 600 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output_without_noise.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 9.99434516e-01 -3.36250974e-02 1.78172351e-05]
[ 3.36250971e-02 9.99434516e-01 1.59104301e-05]
[-1.83421495e-05 -1.53023267e-05 1.00000000e+00]]
Translation of alignment:
[19.9895646 5.66108537 5.33820265]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 587 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 0.062322
mean 0.051179
median 0.050118
min 0.039944
rmse 0.051528
sse 1.558558
std 0.005985
--------------------------------------------------------------------------------
Plotting results...
APE w.r.t. translation part (m)( with SE(3) Umeyama alignment) raw
APE w.r.t. translation part (m )(with SE(3) Umeyama alignment) map
4.4 对比有噪声（第二讲默认的噪声）
命令
evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise.zip
cgm@ubuntu:~/VINS-Course/bin$ evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise.zip
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 1312 stamps and poses from: pose_output_with_noise.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 587 of max. 600 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output_with_noise.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 0.998435 -0.05419753 -0.0137906 ]
[ 0.05373543 0.9980448 -0.03192294]
[ 0.01549378 0.03113193 0.99939519]]
Translation of alignment:
[21.15684026 4.41788869 4.27253012]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 587 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 3.300167
mean 1.750798
median 1.577023
min 0.954810
rmse 1.856596
sse 2023.358491
std 0.617783
--------------------------------------------------------------------------------
Plotting results...
APE w.r.t. translation part (m)( with SE(3) Umeyama alignment) raw
APE w.r.t. translation part (m )(with SE(3) Umeyama alignment) map
5.不同大小噪声的imu数据仿真结果
5.1 改为更小的噪声
-
步骤1：修改第二讲的代码（
vio_data_simulation/src/param.h里的噪声部分），并运行。我第一次将噪声改小：
double gyro_bias_sigma = 1.0e-5;//陀螺仪的 bias 随机游走噪声 double acc_bias_sigma = 0.0001;//加速度 bias 的随机游走噪声 double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声 double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声 改为 //smaller nosie double gyro_bias_sigma = 1.0e-6; //陀螺仪的 bias 随机游走噪声 double acc_bias_sigma = 0.00001; //加速度 bias 的随机游走噪声 double gyro_noise_sigma = 0.0015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声 double acc_noise_sigma = 0.0019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声 -
步骤2：使用脚本运行run_simulate
./build._sim.sh -
步骤3：在目录VINS-Course/bin 下打开终端进行对比。
命令
evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_smaller.zip
结果：
cgm@ubuntu:~/VINS-Course/bin$ evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_smaller.zip
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 587 stamps and poses from: pose_output_with_noise.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 587 of max. 587 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output_with_noise.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 9.99528250e-01 -3.07127532e-02 6.32727881e-05]
[ 3.07127431e-02 9.99528240e-01 1.55148469e-04]
[-6.80079752e-05 -1.53131997e-04 9.99999986e-01]]
Translation of alignment:
[19.94065242 5.7381364 5.37494616]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 587 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 0.143044
mean 0.082941
median 0.076755
min 0.051445
rmse 0.085677
sse 4.308874
std 0.021479
--------------------------------------------------------------------------------
Plotting results...
==提示：==如果画的轨迹不对，关闭终端重新打开再输入。或者把之前的数据删除，比如 imu_pose_noise.txt 和 pose_output_with_noise.txt
5.2 改为中等的噪声
建议把之前的数据 imu_pose_noise.txt 和 pose_output_with_noise.txt 删除，重新生成。
（1）修改vio_data_simulation/src/param.h里的噪声部分
// medium noise
double gyro_bias_sigma = 5.0e-6; //陀螺仪的 bias 随机游走噪声
double acc_bias_sigma = 0.00005; //加速度 bias 的随机游走噪声
double gyro_noise_sigma = 0.0075; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声
double acc_noise_sigma = 0.0095; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声
（2）运行vio_data_simulation工程
（3）运行VINS-Course工程
（4）在目录VINS-Course/bin 下打开终端进行对比
（5）在终端输入命令
evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_medium.zip
（6）结果：
cgm@ubuntu:~/VINS-Course/bin$ evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_medium.zip
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 587 stamps and poses from: pose_output_with_noise.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 587 of max. 587 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output_with_noise.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 9.99477227e-01 -3.23186345e-02 -8.82261297e-04]
[ 3.23234921e-02 9.99458238e-01 6.19858381e-03]
[ 6.81453557e-04 -6.22386113e-03 9.99980399e-01]]
Translation of alignment:
[19.69164372 6.83792945 5.06506652]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 587 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 1.257529
mean 0.634277
median 0.652913
min 0.217522
rmse 0.663024
sse 258.045365
std 0.193115
--------------------------------------------------------------------------------
Plotting results...
5.3 改为更大的噪声
建议把之前的数据 imu_pose_noise.txt 和 pose_output_with_noise.txt 删除，重新生成。
（1）修改vio_data_simulation/src/param.h里的噪声部分
// larger noise
double gyro_bias_sigma = 1.0e-5; //陀螺仪的 bias 随机游走噪声
double acc_bias_sigma = 0.0001; //加速度 bias 的随机游走噪声
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声
（2）运行vio_data_simulation工程
（3）运行VINS-Course工程
（4）在目录VINS-Course/bin 下打开终端进行对比
（5）在终端输入命令
evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_larger.zip
（6）结果：
cgm@ubuntu:~/VINS-Course/bin$ evo_ape tum cam_pose_tum.txt pose_output_with_noise.txt -va --plot --plot_mode xyz --save_results with_noise_larger.zip
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 587 stamps and poses from: pose_output_with_noise.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 587 of max. 587 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output_with_noise.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 0.99155227 -0.10602052 0.07472451]
[ 0.09982098 0.99159396 0.08232364]
[-0.08282437 -0.07416912 0.99380031]]
Translation of alignment:
[17.64534845 13.25170598 7.44484511]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 587 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 10.887888
mean 6.548863
median 5.900705
min 4.079211
rmse 6.794114
sse 27095.909198
std 1.808969
--------------------------------------------------------------------------------
Plotting results...
6.可视化总结
结论：可以看到设置的imu噪声越大通过vins估计出来的轨迹结果越差
扫一扫
257
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
