视觉SLAM十四讲作业练习（5）3D-3D ICP

3D-3D：ICP

（1）SVD方法

首先定义符号：$p_i$ ：第二个图像中的数据；$p_i^{\prime }$ ：第一个图像中的数据。

具体步骤：

（1）定义两组点的质心 $p$与$p^{\prime }$
$$p=\frac{1}{n}\sum _{i=1}^n{p}_i$$

$$p^{\prime }=\frac{1}{n}\sum _{i=1}^n{p}_i^{\prime }$$

（2）计算去质心坐标
$$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$$
（3）定义矩阵W
$$W=\sum _{i=1}^n{q}_i{q}_i^{\prime T}$$
W是一个3*3矩阵，对W进行奇异值分解：

$$W=U\Sigma V^T$$
（4）解得R，t
$$R=U{V}^T$$

$$t=p-R{p}^{\prime }$$

ICP代码

void trans_ICP(const PointType &pts1, const PointType &pts2, Eigen::Matrix3d &R12, Eigen::Vector3d &t12){
    
    Eigen::Vector3d p1, p2;
    int N = pts1.size();
    cout << "N = \n" << N << endl;
    for (int i = 0; i < N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    // 计算质心
    p1 /= N;
    p2 /= N;

    PointType q1(N), q2(N); 

    for (int i = 0; i < N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++)
    {
        W += q1[i] * q2[i].transpose();
    }
    cout << "W = \n" << W << endl;
    cout << "p1 = \n" << p1 << endl;
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    cout << "U = \n" << U << endl;
    cout << "V = \n" << V << endl;

    R12 = U * (V.transpose());
    if (R12.determinant() < 0)
    {
        R12 = -R12;
    }
    t12 = p1 - R12 * p2;
    cout << "R = \n" << R12 << endl;
    cout << "t = \n" << t12 << endl;


}

（2）作业5 用ICP实现轨迹对齐

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
#include <vector>
#include <string>
#include <unistd.h>
#include <fstream>
#include <iostream>
using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>> PointType;
// path to trajectory file
string trajectory_file = "../compare.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(TrajectoryType pose_e,TrajectoryType pose_g);
bool ReadTrajectory(const string &trajectory_file_path, TrajectoryType &poses_e, TrajectoryType &poses_g, 
                    PointType &pts1, PointType &pts2);
void trans_ICP(const PointType &pts1, const PointType &pts2, Eigen::Matrix3d &R12, Eigen::Vector3d &t12);

int main(int argc, char **argv)
{
    TrajectoryType poses_e; 
    TrajectoryType poses_g; 
    PointType pts1;
    PointType pts2;
    // 读取文件
    if (!ReadTrajectory(trajectory_file, poses_e, poses_g, pts1, pts2)) {
        cout << "读取失败...\n";
    } 
    else {
        cout << "读取成功!\n";
    }
    // Use ICP to estimate pose
    Eigen::Matrix3d R_eg;
    Eigen::Vector3d t_eg;
    trans_ICP(pts1, pts2, R_eg, t_eg);

    // Transform trajectory_g to the frame of trajectory_e 
    Sophus::SE3d T_eg(R_eg, t_eg);

    for (Sophus::SE3d &pose: poses_g) {
        pose = T_eg * pose;
    }
    // draw trajectory in pangolin
    DrawTrajectory(poses_e, poses_g);
    return 0;
}

bool ReadTrajectory(const string &trajectory_file_path, TrajectoryType &poses_e, TrajectoryType &poses_g,
                   PointType &pts1, PointType &pts2)
{
    // 读文件
    // 2创建流对象
    ifstream reader;

    // 3.1打开文件
    reader.open(trajectory_file_path, ios::in);
    
    // 3.2判断是否打开成功
    if (!reader.is_open()) {
        cout << "文件打开失败！！！ " << trajectory_file_path << '\n';
        return false;
    }

    // 4读数据
    // 将读到的数据储存在SE(3)矩阵中
    // 估计轨迹 Te
    Eigen::Quaterniond q(1,0,0,0);  //四元数
    Eigen::Vector3d t(0,0,0);       //平移
    double time;

    while (!reader.eof())   //eof() 判断文件是否为空或者是否读到文件结尾
    {   //文件存储的格式为[time_e,t_e,q_e,time_g,t_g,q_g]

        reader >> time >> t[0] >> t[1] >> t[2] >> q.x() >> q.y() >> q.z() >> q.w();
        Sophus::SE3d SE3_e (q,t);
        pts1.push_back(t);
        poses_e.push_back(SE3_e);

        reader >> time >> t[0] >> t[1] >> t[2] >> q.x() >> q.y() >> q.z() >> q.w();
        Sophus::SE3d SE3_g (q,t);
        pts2.push_back(t);
        poses_g.push_back(SE3_g);
    }
    
    reader.close();

    return true;
}

void DrawTrajectory(TrajectoryType poses_e, TrajectoryType poses_g) {
    if (poses_e.empty() || poses_g.empty()) {
        cerr << "Trajectory is empty!\n";
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 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 < poses_e.size() - 1; i++) {
            glColor3f(1 - (float) i / poses_e.size(), 0.0f, (float) i / poses_e.size());
            glBegin(GL_LINES);
            auto p1_e = poses_e[i], p2_e = poses_e[i + 1];
            glVertex3d(p1_e.translation()[0], p1_e.translation()[1], p1_e.translation()[2]);
            glVertex3d(p2_e.translation()[0], p2_e.translation()[1], p2_e.translation()[2]);
            glEnd();
        }
        for (size_t i = 0; i < poses_g.size() - 1; i++) {
            glColor3f(1 - (float) i / poses_g.size(), 0.0f, (float) i / poses_g.size());
            glBegin(GL_LINES);
            auto p1_g = poses_g[i], p2_g = poses_g[i + 1];
            glVertex3d(p1_g.translation()[0], p1_g.translation()[1], p1_g.translation()[2]);
            glVertex3d(p2_g.translation()[0], p2_g.translation()[1], p2_g.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}

void trans_ICP(const PointType &pts1, const PointType &pts2, Eigen::Matrix3d &R12, Eigen::Vector3d &t12){
    
    Eigen::Vector3d p1, p2;
    int N = pts1.size();
    cout << "N = \n" << N << endl;
    for (int i = 0; i < N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    // 计算质心
    p1 /= N;
    p2 /= N;

    PointType q1(N), q2(N); 

    for (int i = 0; i < N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++)
    {
        W += q1[i] * q2[i].transpose();
    }
    cout << "W = \n" << W << endl;
    cout << "p1 = \n" << p1 << endl;
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    cout << "U = \n" << U << endl;
    cout << "V = \n" << V << endl;

    R12 = U * (V.transpose());
    if (R12.determinant() < 0)
    {
        R12 = -R12;
    }
    t12 = p1 - R12 * p2;
    cout << "R = \n" << R12 << endl;
    cout << "t = \n" << t12 << endl;

}