SFM

最新推荐文章于 2024-04-23 09:53:39 发布

darkerJ

最新推荐文章于 2024-04-23 09:53:39 发布

阅读量572

点赞数

分类专栏： SLAM

本文链接：https://blog.csdn.net/jsf921942722/article/details/95511440

版权

SLAM 专栏收录该内容

8 篇文章 0 订阅

订阅专栏

SFM

文章目录

SFM
- 五点法计算RT 改进
- SFM 流程

五点法计算RT 改进

VINS 根据匹配点对计算位姿，使用的五点法，由于五点法存在一些限制并且陀螺仪的数据相对比较准确，因此采用本质矩阵+陀螺仪数据的方式，根据匹配点对计算 $t$ 。
已知：

匹配点对
陀螺仪数据计算出的 $R$
根据本质矩阵的性质满足方程： $P_2^T t^{\wedge}R P_1 = 0$

根据满足的方程式展开得到：
$\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$
令 $\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;
}