EuRoC数据集相关

综述

用于室内MAV的双目+IMU数据集，包含两个场景

1. 苏黎世联邦理工学院ETH的一个machine hall
2. 普通房间

硬件设备

• 飞行器机体：AscTec Firefly
• 双目VIO相机：全局快门，单色，相机频率20Hz，IMU频率200Hz，具备相机和IMU的硬件(hw)同步，双目相机型号MT9V034，IMU型号ADIS16448
• VICON0：维肯动作捕捉系统的配套反射标志，叫做marker
• LEICA0：是激光追踪器配套的传感器棱镜，叫做prism
• Leica Nova MS50: 激光追踪器，测量棱镜prism的位置，毫米精度，帧率20Hz，
• Vicon motion capture system: 维肯动作捕捉系统，提供在单一坐标系下的6D位姿测量，测量方式是通过在MAV上贴上一组反射标志，帧率100Hz，毫米精度

坐标系约定和符号

如上图，总共框中四个传感器，对应数据集结构中cam0,cam1,imu0,leica0四个文件夹，其中prism和marker公用一个坐标系，

飞行器的Body Frame是以IMU的中心作为Body Frame的，四个文件夹所有的传感器数据都是相对于各自的传感器坐标系(Sensor Frame)的，当然IMU的Sensor Frame就是飞行器的Body Frame。

数据集结构（ASL Dataset Format）

在每个传感器文件夹里配套一个senor.yaml文件，描述了该传感器相对于Body坐标系的坐标变换情况，以及传感器内参。

其中，T_BS 表示将所有Sensor坐标系下的向量，变为以Body Frame坐标系下的向量。

 

立体矫正程序：（C++实现）

#include 
#include 

int main(int argc, char **argv) {
    int width = 752;
    int height = 480;
    cv::Size image_size(width, height);

    cv::Mat T_bc1 = (cv::Mat_(4, 4) << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
                                               0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
                                               -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
                                               0.0, 0.0, 0.0, 1.0);
    cv::Mat T_bc2 = (cv::Mat_(4, 4) << 0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
                                               0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
                                               -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
                                               0.0, 0.0, 0.0, 1.0);
    cv::Mat T_c2c1 = T_bc2.inv() * T_bc1;
    cv::Mat R_c2c1 = T_c2c1.rowRange(0,3).colRange(0,3);
    cv::Mat t_c2c1 = T_c2c1.rowRange(0,3).col(3);
    std::cout << "t_c2c1:" << std::endl << t_c2c1.t() << std::endl;

    cv::Mat left_k = (cv::Mat_(3, 3) << 458.654, 0.000000, 367.215,
                                                0.000000, 457.296, 248.375,
                                                0.000000, 0.000000, 1.000000);
    cv::Mat left_d = (cv::Mat_(5, 1) << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0);

    cv::Mat right_k = (cv::Mat_(3, 3) << 457.587, 0.000000, 379.999,
                                                 0.000000, 456.134, 255.238,
                                                 0.000000, 0.000000, 1.000000);
    cv::Mat right_d = (cv::Mat_(5, 1) << -0.28368365,  0.07451284, -0.00010473, -3.55590700e-05, 0);

    cv::Mat canvas(image_size.height, image_size.width * 2, CV_8UC3);
    cv::Mat canvas_left = canvas(cv::Rect(0, 0, image_size.width, image_size.height));
    cv::Mat canvas_right = canvas(cv::Rect(image_size.width, 0, image_size.width, image_size.height));

    cv::Mat color_left = cv::imread("../mav0/cam0/data/1403638127345096960.png", cv::IMREAD_COLOR);
    cv::Mat color_right = cv::imread("../mav0/cam1/data/1403638127345096960.png", cv::IMREAD_COLOR);

    color_left.copyTo(canvas_left);
    color_right.copyTo(canvas_right);
    
    cv::namedWindow("canvas", CV_WINDOW_AUTOSIZE);
    cv::imshow("canvas", canvas);
    cv::waitKey(0);


    std::cout << "stereo rectify..." << std::endl;
    cv::Mat R1, R2, P1, P2, Q;
    cv::Rect validROIL; // 图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域, 其内部的所有像素都有效
    cv::Rect validROIR;
    cv::stereoRectify(left_k, left_d, right_k, right_d, image_size, R_c2c1, t_c2c1, R1, R2, P1, P2, Q, 
            cv::CALIB_ZERO_DISPARITY, 0, image_size, &validROIL, &validROIR);

    std::cout << "R1:" << std::endl << R1 << std::endl;
    std::cout << "P1:" << std::endl << P1 << std::endl;
    std::cout << "R2:" << std::endl << R2 << std::endl;
    std::cout << "P2:" << std::endl << P2 << std::endl;
    std::cout << "Q:" << std::endl << Q << std::endl;

    cv::Mat mapLx, mapLy, mapRx, mapRy;
    cv::initUndistortRectifyMap(left_k, left_d, R1, P1.rowRange(0,3).colRange(0,3), image_size, CV_32F, mapLx, mapLy); // CV_16SC2
    cv::initUndistortRectifyMap(right_k, right_d, R2, P2.rowRange(0,3).colRange(0,3), image_size, CV_32F, mapRx, mapRy);

    cv::Mat rectifyImageL, rectifyImageR;
    cv::remap(color_left, rectifyImageL, mapLx, mapLy, cv::INTER_LINEAR);
    cv::remap(color_right, rectifyImageR, mapRx, mapRy, cv::INTER_LINEAR);

    rectifyImageL.copyTo(canvas_left);
    rectifyImageR.copyTo(canvas_right);

    cv::rectangle(canvas_left, validROIL, cv::Scalar(255, 0, 0), 3, 8);
    cv::rectangle(canvas_right, validROIR, cv::Scalar(255, 0, 0), 3, 8);

    for (int i = 0; i <= canvas.rows; i += 10) {
        float b = 255 * float(rand()) / RAND_MAX;
        float g = 255 * float(rand()) / RAND_MAX;
        float r = 255 * float(rand()) / RAND_MAX;
        cv::line(canvas, cv::Point(0, i), cv::Point(canvas.cols, i), cv::Scalar(b, g, r), 1, 8);
    }
    std::cout << "stereo rectify done" << std::endl;
    cv::imshow("canvas", canvas);
    cv::waitKey(0);

    return 0;
}

输出结果如下：

可知，双目基线(base line)为 0.1100738081271868 (m)，bf 为 47.91265992777852

验证立体矫正结果：两张图的对应点在同一水平线上

结果和 ORB_SLAM2/Examples/Stereo 目录下的 EuRoC.yaml 差不多

#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 480
LEFT.width: 752
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]

RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]

groundtruth输出格式

#timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]
1403638128940097024,4.677066,-1.749440,0.568567,0.240749,-0.761130,-0.355916,-0.485843,0.002118,-0.005923,-0.002323,-0.002133,0.021059,0.076659,-0.026895,0.136910,0.059287
......

18位时间戳

• timestamp

p代表position，指的是MAV的空间3D坐标，RS代表这个坐标是在R坐标系的值，也就是LEICA位姿跟踪系统坐标系下测到的值，S指的是原来的值是从Sensor坐标系下得到的，后来又变换到了R坐标系。R可能代表LEICA坐标系，x代表这是3D位置的x轴方向上的真值。单位位米

• p_RS_R_x [m]
• p_RS_R_y [m]
• p_RS_R_z [m]

q代表quaternion四元数，表达了MAV的朝向信息，RS代表是在R坐标系下测到的朝向信息，但是实际上最开始是在Sensor坐标系下的朝向，后来只不过被变换到了R坐标系下,w为四元数的实部，xyz为虚部

• q_RS_w []
• q_RS_x []
• q_RS_y []
• q_RS_z []

v代表这是MAV的速度信息，而且是在R坐标系下的速度信息，单位m/s

• v_RS_R_x [m s^-1]
• v_RS_R_y [m s^-1]
• v_RS_R_z [m s^-1]

w代表这是MAV在R坐标系下的角速度信息，单位rad/s

• b_w_RS_S_x [rad s^-1]
• b_w_RS_S_y [rad s^-1]
• b_w_RS_S_z [rad s^-1]

a代表这是MAV在R坐标系下的线加速度信息，单位m/s^2

• b_a_RS_S_x [m s^-2]
• b_a_RS_S_y [m s^-2]
• b_a_RS_S_z [m s^-2]

绘制groundtruth轨迹

见：用evo工具评估SLAM轨迹(#alan#)

 

参考：

The EuRoC MAV Dataset

EuRoC数据集简介与使用