1.使用EuRoC MAV Dataset跑该VINS工程
先尝试跑一下 EuRoC MAV Dataset
编译遇到问题:
error: no member named ‘integer_sequence’ in namespace ‘std’
解决方案：将cmake编译选项的C++11换成C++14
运行示例：
2.使用第二章仿真数据跑该VINS工程
运动轨迹是一个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 带噪声
imu_int_pose:积分后得到的imu轨迹， imu_int_pose_noise 带噪声
仿照run_euroc.cpp新建run_simulate.cpp修改部分如下：
void PubImuData()
{
string sImu_data_file = "../data/imu_pose_noise.txt";
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
{
// timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3)
std::istringstream ssImuData(sImu_line);
ssImuData >> dStampNSec;
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);
usleep(5000*nDelayTimes);
}
fsImu.close();
}
void PubImageData()
{
string sImage_file = "../data/cam_pose.txt"; //包含时间戳的文件
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())
{
std::istringstream ssImgData(sImage_line);
ssImgData >> dStampNSec; //读入时间戳
cout<<"cam time: "<<fixed<<dStampNSec<<endl;
// all_points_ 文件存储的是house模型的线特征，每行4个数，对应该线两端点在归一化平面的坐标
string all_points_file_name = "../data/keyframe/all_points_" + to_string(n)+ ".txt"; //第n个相机对应的观测数据的文件名
cout<<"points_file: "<<all_points_file_name<<endl;
vector<cv::Point2f> FeaturePoints;
std::ifstream f;
f.open(all_points_file_name);
// file content in each line: x, y, z, 1, u, v
while(!f.eof())
{
std::string s;
std::getline(f,s);
// 一行两个点连成线，获取每行点判断一下是否之前获取过
if(!s.empty())
{
std::stringstream ss;
ss << s;
double tmp;
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);
usleep(50000*nDelayTimes);
n++;
}
fsImage.close();
}
修改System.cpp中如下：
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();
}
}
}
}
参数文件需要修改的部分：
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
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0, 0, -1,
-1, 0, 0,
0, 1, 0]
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [0.05,0.04,0.03]
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
注意原代码跑出来的pose_output.txt有的数据中间有两个空格使用evo工具不符合tum格式，仿照第二章生成存储cam_pose_tum数据方式改写代码
原始轨迹有漂移，可以使用evo工具中的轨迹对齐（加上 -a 选项）
3.使用不同噪声的imu数据结果
3.1无噪声结果：
jevin@jevin-GS65-Stealth-9SE:~/code/vio-homework/hw7/VINS-Course/data$ evo_ape tum cam_pose_tum.txt pose_output.txt -va --plot --plot_mode xyz
--------------------------------------------------------------------------------
Loaded 600 stamps and poses from: cam_pose_tum.txt
Loaded 587 stamps and poses from: pose_output.txt
Synchronizing trajectories...
Found 587 of max. 587 possible matching timestamps between...
cam_pose_tum.txt
and: pose_output.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.36250998e-02 1.78059217e-05]
[ 3.36250996e-02 9.99434516e-01 1.58890231e-05]
[-1.83301227e-05 -1.52813123e-05 1.00000000e+00]]
Translation of alignment:
[19.9895645 5.66108548 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.062320
mean 0.051179
median 0.050122
min 0.039949
rmse 0.051528
sse 1.558562
std 0.005985
3.2小噪声：
噪声为：
// noise
double gyro_bias_sigma = 1.0e-6;
double acc_bias_sigma = 0.00001;
double gyro_noise_sigma = 0.0015; // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.0019; // m/(s^2) * 1/sqrt(hz)
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 0.307185
mean 0.140351
median 0.129513
min 0.059398
rmse 0.150634
sse 13.319364
std 0.054702
3.3中噪声：
噪声为：
// noise
double gyro_bias_sigma = 5.0e-6;
double acc_bias_sigma = 0.00005;
double gyro_noise_sigma = 0.0075; // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.0095; // m/(s^2) * 1/sqrt(hz)
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 3.136348
mean 1.554474
median 1.506734
min 0.390917
rmse 1.732549
sse 1762.012236
std 0.765071
3.4大噪声：
噪声为：
// noise
double gyro_bias_sigma = 1.0e-5;
double acc_bias_sigma = 0.0001;
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz)
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 58.057148
mean 19.987606
median 16.168900
min 10.046026
rmse 22.975319
sse 309856.927025
std 11.329647
结论：可以看到设置的imu噪声越大通过vins估计出来的轨迹结果越差
2433
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
