【SLAM】视觉SLAM十四讲（五：VO）

ORB特征点

使用OpenCV自带的FAST提取算法，再加上一个旋转量，组成一个ORB特征，利用汉明距离进行暴力匹配
代码实现还有问题

从E恢复R,t

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
#include <sophus/so3.h>
#include <iostream>
using namespace std;
int main(int argc, char **argv) {

    // 给定Essential矩阵
    Matrix3d E;
    E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
            0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
            -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;

    // 待计算的R,t
    Matrix3d R;
    Vector3d t;

    // SVD and fix sigular values
    //E = U * S * VT
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, ComputeThinU | ComputeThinV);//初始化jacabisvd对象
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::VectorXd singular_b = svd.singularValues();
    Eigen::Matrix3d S1 = U.inverse() * E * V.transpose().inverse();
    cout << "E = \n" << E << endl <<endl;
    cout << "U*S*VT = \n" << U * S1 * V.transpose() << endl << endl;
    cout << "singular value = \n" << singular_b << endl;
    cout << "U = \n" << U << endl <<endl;
    cout << "S1 = \n" << S1 << endl <<endl;
    cout << "v = \n" << U << endl <<endl;
    Eigen::Vector3d V2 = {(singular_b[0] + singular_b[1]) / 2, (singular_b[0] + singular_b[1]) /
    2, 0};
    Eigen::Matrix3d S =V2.asDiagonal();
    cout << "S update = \n" << S << endl <<endl;

    // set t1, t2, R1, R2 
    Eigen::Matrix3d Rpi1 = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Eigen::Matrix3d Rpi2 = Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Matrix3d t_wedge1 = U * Rpi1 * S * V.transpose();
    Matrix3d t_wedge2 = U * Rpi2 * S * V.transpose();
    
    Matrix3d R1 = U * Rpi1 * V.transpose();
    Matrix3d R2 = U * Rpi2 * V.transpose();

    cout << "R1 = \n" << R1 << endl;
    cout << "R2 = \n" << R2 << endl;
    cout << "t1 = \n" << Sophus::SO3::vee(t_wedge1) << endl;
    cout << "t2 = \n" << Sophus::SO3::vee(t_wedge2) << endl;

    // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "t^R = \n" << tR << endl;

    return 0;
}

用 G-N 实现 Bundle Adjustment

参考https://blog.csdn.net/qq_41814939/article/details/82501664

1. 如何定义重投影误差?重投影误差是将像素坐标与3D点按照当前估计的位姿进行投影得到的像素坐标相比较得到的误差
2. 该误差关于自变量的雅可比矩阵是什么?
3. 解出更新量之后,如何更新至之前的估计上?

#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>
#include "sophus/se3.h"
using namespace std;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "./p3d.txt";
string p2d_file = "./p2d.txt";

int main(int argc, char **argv) {

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d 
    ifstream infile2(p2d_file);
    double u,v;
    string line2;
    if(infile2)
    {
        while(getline(infile2,line2))
        {
            stringstream record(line2);    //从string读取数据
            record>>u>>v;
            Eigen::Vector2d t2(u,v);
            p2d.push_back(t2);
        }
    }
    else
   {
       cout<<"没找到这个文件"<<endl;
   }
    ifstream infile3(p3d_file);
    double tx,ty,tz;
    string line3;
    if(infile3)
    {
        while(getline(infile3,line3))
        {
            stringstream record(line3);    //从string读取数据
            record>>tx>>ty>>tz;
            Eigen::Vector3d t3(tx,ty,tz);
            p3d.push_back(t3);
        }
    }
    else
   {
       cout<<"没找到这个文件"<<endl;
   }
    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti; // estimated pose

    for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
            // compute cost for p3d[I] and p2d[I]
            Vector2d p2 = p2d[i];
            Vector3d p3 = p3d[i];

            Vector3d P = T_esti * p3;
            double x = P[0];
            double y = P[1];
            double z = P[2];

            Vector2d p2_ = { fx * (x/z) + cx, fy * (y/z) + cy};
            Vector2d e = p2 - p2_;
            cost += (e[0]*e[0]+e[1]*e[1]);
            
	    // compute jacobian
            Matrix<double, 2, 6> J;
            J(0,0) = -(fx/z);
            J(0,1) = 0;
            J(0,2) = (fx*x/(z*z));
            J(0,3) = (fx*x*y/(z*z));
            J(0,4) = -(fx*x*x/(z*z)+fx);
            J(0,5) = (fx*y/z);
            J(1,0) = 0;
            J(1,1) = -(fy/z);
            J(1,2) = (fy*y/(z*z));
            J(1,3) = (fy*y*y/(z*z)+fy);
            J(1,4) = -(fy*x*y/(z*z));
            J(1,5) = -(fy*x/z);

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

	// solve dx 
        Vector6d dx;
        dx = H.ldlt().solve(b);

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        T_esti = Sophus::SE3::exp(dx)*T_esti;
        lastCost = cost;
        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}

用 ICP 实现轨迹对齐

参考https://blog.csdn.net/yzt629/article/details/87967585
步骤：利用ICP求出R,t，转换其中一条轨迹的坐标系

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <unistd.h> 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;

string compare_file = "/home/huangyuan/slam/slambook/pa5/ICP/compare.txt";
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_e,pose_g,pose_g2e;
vector<Eigen::Vector3d> ptse;
vector<Eigen::Vector3d> ptsg;

void readData(string filepath);
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose2,string title);
void computeTge();

int main(int argc, char **argv) {
    
    readData(compare_file);
    cout << "数据读入成功" << endl;
    DrawTrajectory(pose_e,pose_g,"init trace");
    computeTge();
    cout << "转换坐标系完成" << endl;
    DrawTrajectory(pose_g2e,pose_e,"trans trace");
}

/***********************************************************/
void readData(string filepath){
    ifstream infile(filepath);
    double t1,tx1,ty1,tz1,qx1,qy1,qz1,qw1,t2,tx2,ty2,tz2,qx2,qy2,qz2,qw2;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    
            record>>t1>>tx1>>ty1>>tz1>>qx1>>qy1>>qz1>>qw1>>t2>>tx2>>ty2>>tz2>>qx2>>qy2>>qz2>>qw2;
            Eigen::Vector3d te(tx1,ty1,tz1),tg(tx2,ty2,tz2);
            Eigen::Quaterniond qe = Eigen::Quaterniond(qw1,qx1,qy1,qz1).normalized(); 
            Eigen::Quaterniond qg = Eigen::Quaterniond(qw2,qx2,qy2,qz2).normalized(); 
            Sophus::SE3 SE3_qte(qe,te),SE3_qtg(qg,tg);
            ptse.push_back(te);
            ptsg.push_back(tg);
            pose_e.push_back(SE3_qte);
            pose_g.push_back(SE3_qtg);
        }
    }else{cout<<"没找到这个文件"<<endl;}
}
/************************************************************/
void computeTge(){
    Eigen::Vector3d pe,pg;
    int N = ptse.size();
    for (int i=0;i<N;i++) {
       pe += ptse[i];
       pg += ptsg[i];  
    }
    pe /= N;
    pg /= N;
    vector<Eigen::Vector3d> qe(N),qg(N);
    for (int i =0;i<N;i++) {
        qe[i] = ptse[i]-pe;
        qg[i] = ptsg[i]-pg;
    }

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i=0;i<N;i++) {
        W += qe[i] * qg[i].transpose();
    }
    cout << "W = \n" << W << endl;
    
    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0){
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
    }
    
    cout<<"U = \n"<<U<<endl;
    cout<<"V = \n"<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = pe - R_ * pg;
    Sophus::SE3 SE3_Tge(R_,t_);
    for (int i=0;i<N;i++) {
        pose_g2e.push_back( SE3_Tge*pose_g[i] );//生成转换坐标系的pose
    }
}
/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose2,string title) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind(title, 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < pose1.size() - 1; i++) {
            glColor3f(1 - (float) i / pose1.size(), 0.0f, (float) i / pose1.size());
            glBegin(GL_LINES);
            auto p1 = pose1[i], p2 = pose1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        for (size_t i = 0; i < pose2.size() - 1; i++) {
            glColor3f(1 - (float) i / pose2.size(), 0.0f, (float) i / pose2.size());
            glBegin(GL_LINES);
            auto p1 = pose2[i], p2 = pose2[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
    }
}

ch7问题思考

1. 其他特征点与ORB之间的优劣比较
2. 让特征点分布更均匀的方法
3. FLANN等加速匹配的手段
4. 避免误匹配的方法
   回答可参考https://blog.csdn.net/weixin_36448497/article/details/82254832