SFM

文章目錄

五點法計算RT 改進

VINS 根據匹配點對計算位姿，使用的五點法，由於五點法存在一些限制並且陀螺儀的數據相對比較準確，因此採用本質矩陣+陀螺儀數據的方式，根據匹配點對計算 $t$ 。
已知：

匹配點對
陀螺儀數據計算出的 $R$
根據本質矩陣的性質滿足方程： $P_2^T t^{\wedge}R P_1 = 0$

根據滿足的方程式展開得到：
$t = \begin{bmatrix} t_1 \\ t_2 \\ t_3 \end{bmatrix}, \quad P_1 = \begin{bmatrix} x_1 \\ y_1 \\ z_1 \end{bmatrix}, \quad P_2 = \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix}, \quad {P_2}' = RP_1 = \begin{bmatrix} {x_1}' \\ {y_1}' \\ {z_1}' \end{bmatrix}$
$P_2^T t^{\wedge}{P_1}' = -P_2^T {{P_1}'}^{\wedge} t = \begin{bmatrix} x_2 & y_2 & z_2 \end{bmatrix} \begin{bmatrix} 0 & -t_3 & t_2 \\ t_3 & 0 & -t_1 \\ -t_2 & t_1 & 0 \end{bmatrix} \begin{bmatrix} {x_1}' \\ {y_1}' \\ {z_1}' \end{bmatrix} = 0$
$\Rightarrow \left(z_2 {y_1}' - y_2 {z_1}'\right)t_1 + \left(x_2 {z_1}' - z_2 {x_1}'\right)t_2 + \left(y_2 {x_1}' - x_2 {y_1}'\right)t_3 = 0$
令：
$A_1^T = \begin{bmatrix} z_2 {y_1}' - y_2 {z_1}' & x_2 {z_1}' - z_2 {x_1}' & y_2 {x_1}' - x_2 {y_1}' \end{bmatrix}$
則：
$A_1^T t = 0$
求解 $t$
$A_1^T A_1 t = A t = 0$
因爲會有多組點：
$\sum_{i \in N}{A_i^T A_i} \ \ \ t = 0$
令 $M = \sum_{i \in N}{A_i^T A_i}$
則矩陣 $M$ 最小特徵值對應的特徵向量即爲 $t$ 或 $-t$
對於 $t$ 和 $-t$ 還需要三角化計算出三維點，然後投影帶匹配的兩幀上，判斷z軸的方向是否在同一側，因爲兩幀本來距離就近，理論上應該在同一側。
代碼：

bool VINS::computeT(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d> > &corres, Eigen::Matrix3d &Rotation, Eigen::Vector3d &Translation) {

    Eigen::Matrix<double, 3, 3> M;
    M.setZero();
    for (int i = 0; i < int(corres.size()); i++) {
        Eigen::Vector3d v3l(corres[i].first.x(), corres[i].first.y(), corres[i].first.z());
        Eigen::Vector3d R_P = Rotation * v3l;
        double x_ = corres[i].second.x();
        double y_ = corres[i].second.y();
        double z_ = corres[i].second.z();
        Eigen::Matrix<double, 3, 1> Z_3_1;
        Z_3_1(0) = R_P(1) * z_ - R_P(2) * y_;
        Z_3_1(1) = R_P(2) * x_ - R_P(0) * z_;
        Z_3_1(2) = R_P(0) * y_ - R_P(1) * x_;

        Eigen::Matrix<double, 3, 3> Z_3_3;
        Z_3_3 = Z_3_1 * Z_3_1.transpose();
        M = M + Z_3_3;
    }

    Eigen::EigenSolver<Eigen::Matrix<double, 3, 3>> es(M);
    double eigenValues[3] = { 0. };
    eigenValues[0] = es.eigenvalues()(0).real();
    eigenValues[1] = es.eigenvalues()(1).real();
    eigenValues[2] = es.eigenvalues()(2).real();

    int minIndex = 0;
    minIndex = eigenValues[0] < eigenValues[1] ? (0) : (1);
    minIndex = eigenValues[minIndex] < eigenValues[2] ? (minIndex) : (2);

    Translation(0) = es.eigenvectors().col(minIndex)(0, 0).real();
    Translation(1) = es.eigenvectors().col(minIndex)(1, 0).real();
    Translation(2) = es.eigenvectors().col(minIndex)(2, 0).real();

    vector<cv::Point2d> ll, rr;

    for (int i = 0; i < int(corres.size()); ++i) {
        ll.push_back(cv::Point2d(corres[i].first(0), corres[i].first(1)));
        rr.push_back(cv::Point2d(corres[i].second(0), corres[i].second(1)));
    }

    double triangulation[2] = { 0. };
    cv::Mat_<double> cv_R(3, 3), cv_T(3, 1), cv_T_(3, 1);
    for (int i = 0; i < 3; i++) {
        cv_T(i) = Translation(i);
        cv_T_(i) = -Translation(i);
        for (int j = 0; j < 3; j++) {
            cv_R(i, j) = Rotation(i, j);
        }
    }

    triangulation[0] = E_TestTriangulation(ll, rr, cv_R, cv_T);
    triangulation[1] = E_TestTriangulation(ll, rr, cv_R, cv_T_);

    int max_index = triangulation[0] > triangulation[1] ? 0 : 1;
    std::cout << "ok point score : " << triangulation[max_index] << std::endl;
    if (triangulation[max_index] < 9.0 / int(corres.size())) {
        return false;
    }

    cv::Mat_<double> R_cv, T_cv;
    switch (max_index) {
    case 0:
        break;
    case 1:
        Translation = -Translation;
        break;
    }
    return true;
}

double VINS::E_TestTriangulation(std::vector<cv::Point2d> &ll, std::vector<cv::Point2d> &rr,
    cv::Mat_<double> &R, cv::Mat_<double> &T) {
    // ll 對應的相機外參爲 單位陣
    cv::Matx34d P_ll = cv::Matx34d(1., 0., 0., 0.,
                                   0., 1., 0., 0.,
                                   0., 0., 1., 0.);
    // rr 對應的相機外參
    cv::Matx34d P_rr = cv::Matx34d(R(0, 0), R(0, 1), R(0, 2), T(0),
                                   R(1, 0), R(1, 1), R(1, 2), T(1),
                                   R(2, 0), R(2, 1), R(2, 2), T(2));

    cv::Mat pointcloud;
    cv::triangulatePoints(P_ll, P_rr, ll, rr, pointcloud);

    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++) {
        double normal_factor = pointcloud.col(i).at<double>(3);
        cv::Mat_<double> p_3d_l = cv::Mat(P_ll) * (pointcloud.col(i) / normal_factor);
        cv::Mat_<double> p_3d_r = cv::Mat(P_rr) * (pointcloud.col(i) / normal_factor);
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0) {
            front_count++;
        }
    }
    return 1.0 * front_count / pointcloud.cols;
}

SFM 流程

本節主要闡述SFM的流程， SFM（Structure from motion）用到的算法：

三角化
PNP
BA
以上三種具體算法其它文檔中有提到，可以查看其它文檔。

已知劃窗內第L幀與劃窗的最後一幀有足夠的匹配點，然後根據“五點法計算RT 改進”計算出兩幀的位姿，其中第L幀的位姿爲單位陣，最後一幀爲RT。
由於匹配點對是根據光流跟蹤計算出的，因此L幀到End幀之間的幀與最後一幀同樣有足夠的匹配點對。

通過三角法可以計算出L幀與End幀之間共有的三維點座標，然後根據PnP就可以計算出L幀到End幀之間幀的某一幀的位姿，然後再通過三角法計算出更多的三維點，依次計算出L幀到End幀之間幀的位姿和三維點。
由於L幀與End幀之間的三維點足夠多，因此這些三維點中包含第L-1幀的三維點，然後PnP計算出L-1幀的位姿，再通過三角法計算出L-1幀與L幀之間的三維點，然後再計算L-2幀的位姿，依次把0幀到L幀的位姿和三維點計算出。

通過上面兩步，就可以計算出劃窗內的位姿和三維點，在通過BA優化，這樣就完成了SFM。

代碼：

bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
{
    feature_num = sfm_f.size();
    q[l].w() = 1;
    q[l].x() = 0;
    q[l].y() = 0;
    q[l].z() = 0;
    T[l].setZero();
    q[frame_num - 1] = q[l] * Quaterniond(relative_R);
    T[frame_num - 1] = relative_T;
    //rotate to cam frame
    Matrix3d *c_Rotation = new Matrix3d[frame_num];
    Vector3d *c_Translation = new Vector3d[frame_num];
    Quaterniond *c_Quat = new Quaterniond[frame_num];

    double(*c_rotation)[4] = new double[frame_num][4];
    double(*c_translation)[3] = new double[frame_num][3];
    Eigen::Matrix<double, 3, 4> *Pose = new Eigen::Matrix<double, 3, 4>[frame_num];

    c_Quat[l] = q[l].inverse();
    c_Rotation[l] = c_Quat[l].toRotationMatrix();
    c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
    Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
    Pose[l].block<3, 1>(0, 3) = c_Translation[l];

    c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
    c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
    c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
    Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
    Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];


    //1: trangulate between l ----- frame_num - 1
    //2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;
    //   ...   solve pnp l + 1; trigangulate 0 -----1;
    for (int i = l; i < frame_num - 1; i++)
    {
        // solve pnp
        if (i > l)
        {
            Matrix3d R_initial = c_Rotation[i - 1];
            Vector3d P_initial = c_Translation[i - 1];
            if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f)) {
                delete[] c_Rotation;
                delete[] c_Translation;
                delete[] c_Quat;
                delete[] c_rotation;
                delete[] c_translation;
                delete[] Pose;
                return false;
            }
            c_Rotation[i] = R_initial;
            c_Translation[i] = P_initial;
            c_Quat[i] = c_Rotation[i];
            Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
            Pose[i].block<3, 1>(0, 3) = c_Translation[i];
        }

        // triangulate point based on the solve pnp result
        triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
    }
    //3: triangulate l-----l+1 l+2 ... frame_num -2
    for (int i = l + 1; i < frame_num - 1; i++)
        triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
    //4: solve pnp l-1; triangulate l-1 ----- l
    //             l-2              l-2 ----- l
    for (int i = l - 1; i >= 0; i--)
    {
        //solve pnp
        Matrix3d R_initial = c_Rotation[i + 1];
        Vector3d P_initial = c_Translation[i + 1];
        solveFrameByPnP(R_initial, P_initial, i, sfm_f);
        c_Rotation[i] = R_initial;
        c_Translation[i] = P_initial;
        c_Quat[i] = c_Rotation[i];
        Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
        Pose[i].block<3, 1>(0, 3) = c_Translation[i];
        //triangulate
        triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
    }
    //5: triangulate all other points
    for (int j = 0; j < feature_num; j++)
    {
        if (sfm_f[j].state == true)
            continue;
        if ((int)sfm_f[j].observation.size() >= 2)
        {
            Vector2d point0, point1;
            int frame_0 = sfm_f[j].observation[0].first;
            point0 = sfm_f[j].observation[0].second;
            int frame_1 = sfm_f[j].observation.back().first;
            point1 = sfm_f[j].observation.back().second;
            Vector3d point_3d;
            triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
            sfm_f[j].state = true;
            sfm_f[j].position[0] = point_3d(0);
            sfm_f[j].position[1] = point_3d(1);
            sfm_f[j].position[2] = point_3d(2);
        }
    }


    //full BA
    ceres::Problem problem;
    ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
    for (int i = 0; i < frame_num; i++)
    {
        //double array for ceres
        c_translation[i][0] = c_Translation[i].x();
        c_translation[i][1] = c_Translation[i].y();
        c_translation[i][2] = c_Translation[i].z();
        c_rotation[i][0] = c_Quat[i].w();
        c_rotation[i][1] = c_Quat[i].x();
        c_rotation[i][2] = c_Quat[i].y();
        c_rotation[i][3] = c_Quat[i].z();
        problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
        problem.AddParameterBlock(c_translation[i], 3);
        if (i == l)
        {
            // 固定第 l幀的旋轉 R
            problem.SetParameterBlockConstant(c_rotation[i]);
        }
        if (i == l || i == frame_num - 1)
        {
            // 固定第 l和最後一幀的  T
            problem.SetParameterBlockConstant(c_translation[i]);
        }
    }

    delete[] c_Rotation;
    delete[] c_Translation;
    delete[] c_Quat;
    delete[] Pose;

    for (int i = 0; i < feature_num; i++)
    {
        if (sfm_f[i].state != true)
            continue;
        for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
        {
            int l = sfm_f[i].observation[j].first;
            ceres::CostFunction* cost_function = ReprojectionError3D::Create(
            sfm_f[i].observation[j].second.x(),
            sfm_f[i].observation[j].second.y());

            problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
            sfm_f[i].position);
        }

    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.minimizer_progress_to_stdout = true;
    options.max_solver_time_in_seconds = 0.3;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";
    if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 3e-03) {
        cout << "vision only BA converge" << endl;
    }        
    else
    {
        delete[] c_rotation;
        delete[] c_translation;
        cout << "vision only BA not converge " << endl;
        return false;
    }
    for (int i = 0; i < frame_num; i++)
    {
        q[i].w() = c_rotation[i][0];
        q[i].x() = c_rotation[i][1];
        q[i].y() = c_rotation[i][2];
        q[i].z() = c_rotation[i][3];
        q[i] = q[i].inverse();
    }
    for (int i = 0; i < frame_num; i++)
    {

        T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
    }
    for (int i = 0; i < (int)sfm_f.size(); i++)
    {
        if (sfm_f[i].state)
            sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
    }
    delete[] c_rotation;
    delete[] c_translation;
    return true;
}

SFM

SFM

文章目錄

五點法計算RT 改進

SFM 流程

VINS 優化

VINS 預積分雅各比矩陣和協方差矩陣推導過程

視覺與IMU對齊

SFM

三角化

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結