目前编写双目VINS代码,发现双目VINS初始化后的尺度存在问题(理想化双目尺度应该为1),所以想着利用无误差的测试集来测试一下自己写的代码。我先在单目VINS下做了测试,下面是我利用github上贺所长的单目测试集在单目vins上测试的过程:
1.单目VINS测试数据集:https://github.com/HeYijia/vio_data_simulation.
(下载后通过mkdir build;cd build; cmake ..; make ; cd ../bin; ./data_gen 命令生成待读取数据,数据存储在bin/里面)
(1)gener_alldata.cpp :
A. createpointslines(): 通过这个函数取出设计好的房子图形中的map点,之后所有的图像都能观测到这些map点,且顺 序不变;
B. imudata 和 imudata_noise: 只用了imudata;
C. camdata: 完全由imudata 和map点决定,所以没有误差;
(2)imudata的生成:imu.cpp
A. MotionModel: 直接给出imu的线加速度和角加速度 以及imu的R、t;(需要推导)
2.在VINS中新建两个节点分别读取feature数据 和 imu数据:
(1)其中map点在各帧观测的归一化坐标存储在 vio_data_simulation/bin/keyframe/all_points_x.txt中
(6维,map3维+1+归一化坐标点2维),
(2)feature的时间 存储在vio_data_simulation/bin/cam_pose.txt中
(14维,时间1维+q4维+t3维+陀螺仪加速度3维+线加速度3 维),
(3)imu数据存储在vio_data_simulation/bin/imu_pose.txt(无噪声)
( 与cam_pose存储相同,只需要时间和gyro、acc 7维)。
3.节点发布:
(1)feature发布:注意频率为30
ros::init(argc, argv, "test_pub_feature");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
test_s();// 读取feature 和 time 数据
int count = 0;
ros::Rate loop_rate(30);
while(ros::ok())
{
pub_feature(all_points[count],cam_time[count]);
ros::spinOnce();
loop_rate.sleep();
count ++;
}
(2)imu发布:注意频率为200(与上面类似)
4.参数修改:projection_parameters + cam to imu 的 R、t<