利用ICP把自己跑出的数据与开源数据集对齐并显示（二.坐标系对齐）

　　对齐时间戳后，生成文件：GroundAligned.txt和EstimatedAligned.txt。由于两个数据集并不在一个坐标系下进行衡量，所以需要坐标系转换。详见：

　　https://mp.csdn.net/postedit/85006243

　　仍然是以EuRoC数据集V2_02_medium为例进行介绍，并把转换后的相机轨迹保存为AfterAligned.txt：

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <chrono>
#include <pangolin/pangolin.h>
#include <iomanip>
using namespace std;
using namespace cv;
using namespace Sophus;

#define COMPARE 0
#define EXAMPLE 1
//pangolin画图
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses2) {
    if (poses1.empty()||poses2.empty()) {
        cerr << "Trajectory is empty!" << endl;
        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 < poses1.size() - 1; i++) {
            //glColor3f(1 - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());
            glColor3f(1.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex2d(p1.translation()[0], p1.translation()[1]);//p1.translation()[2]
            glVertex2d(p2.translation()[0], p2.translation()[1]);//p2.translation()[2]
            glEnd();
        }
        for (size_t i = 0; i < poses2.size() - 1; i++) {
            //glColor3f(1 - (float) i / poses2.size(), 0.0f, (float) i / poses2.size());
            glColor3f(0.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
            auto p1 = poses2[i], p2 = poses2[i + 1];
            glVertex2d(p1.translation()[0], p1.translation()[1]);//p1.translation()[2]
            glVertex2d(p2.translation()[0], p2.translation()[1]);//p2.translation()[2]
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}
int main ( int argc, char** argv )
{
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ps1,ps2;
#if COMPARE
    ifstream file("../compare.txt");
    string line;
    vector<Point3d> pEstimated,pGroundTruth;
    while(getline(file,line))
    {
        stringstream record(line);
        vector<double> vTmp(16,0);
        for(auto &v:vTmp)
            record>>v;
        Point3d p1{vTmp[1],vTmp[2],vTmp[3]};//位置
        Eigen::Quaterniond q1{vTmp[4],vTmp[5],vTmp[6],vTmp[7]};//旋转四元数
        Sophus::SE3 SE3_qt1(q1,Eigen::Vector3d(p1.x,p1.y,p1.z));//组成李群（欧式变换群）--estimated
        Point3d p2{vTmp[9],vTmp[10],vTmp[11]};
        Eigen::Quaterniond q2{vTmp[12],vTmp[13],vTmp[14],vTmp[15]};
        Sophus::SE3 SE3_qt2(q2,Eigen::Vector3d(p2.x,p2.y,p2.z));//组成李群（欧式变换群）--groundtruth
        pEstimated.push_back(p1);//求估计到真实的变换矩阵只需要用到位置
        pGroundTruth.push_back(p2);
        ps1.push_back(SE3_qt1);
        ps2.push_back(SE3_qt2);
    }
#endif
#if EXAMPLE
    ifstream ifileEstimated("../EstimatedAligned.txt");
    ifstream ifileGround("../GroundAligned.txt");
    string line;
    vector<Point3d> pEstimated,pGroundTruth;//用于求取变换矩阵的相机位置
    while(getline(ifileEstimated,line))
    {
        stringstream record(line);
        double vtmp[8];
        for (auto i = 0; i < 8; i++)
            record >> vtmp[i];
        Point3d p1{vtmp[1],vtmp[2],vtmp[3]};//位置
        pEstimated.push_back(p1);
        Eigen::Quaterniond q(vtmp[4], vtmp[5], vtmp[6], vtmp[7]);
        Sophus::SE3 SE3_qt1(q,Eigen::Vector3d(p1.x,p1.y,p1.z));//组成李群（欧式变换群）--estimated
        ps1.push_back(SE3_qt1);
    }
    while(getline(ifileGround,line))
    {
        stringstream record(line);
        double vtmp[8];
        for (auto i = 0; i < 8; i++)
            record >> vtmp[i];
        Point3d p2{vtmp[1],vtmp[2],vtmp[3]};//位置
        pGroundTruth.push_back(p2);
        Eigen::Quaterniond q(vtmp[4], vtmp[5], vtmp[6], vtmp[7]);
        Sophus::SE3 SE3_qt2(q,Eigen::Vector3d(p2.x,p2.y,p2.z));//组成李群（欧式变换群）--groundtruth
        ps2.push_back(SE3_qt2);
    }
#endif
    //下面是ICP过程，p1=Rp2+t，即为真实到估计之间的变换，之后取逆即为估计到真实之间的变换
    Point3d pMid1, pMid2;     // center of mass
    int N = pEstimated.size();
    for ( int i=0; i<N; i++ )
    {
        pMid1 += pEstimated[i];
        pMid2 += pGroundTruth[i];
    }
    pMid1 = Point3d( pMid1/ N);
    pMid2 = Point3d( pMid2/ N);
    vector<Point3d>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pEstimated[i] - pMid1;
        q2[i] = pGroundTruth[i] - pMid2;
    }
    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W=W+Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<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();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( pMid1.x, pMid1.y, pMid1.z ) - R_ * Eigen::Vector3d ( pMid2.x, pMid2.y, pMid2.z );
    cout<<"R_="<<R_<<endl;
    cout<<"t_="<<t_<<endl;
    //取逆
    Eigen::Matrix3d R=R_.inverse();
    Eigen::Vector3d t=-R*t_;
//    Eigen::Quaterniond q(R_);
//    cout<<"q="<<q.coeffs()<<endl;
    //DrawTrajectory(ps1,ps2);
    SE3 SE3_T(R,t);
#if EXAMPLE
    ofstream ofileEst("../AfterAligned.txt");//保存经过转换后的相机位姿的文件
#endif
    for(auto &p:ps1)
    {
        p=SE3_T*p;
#if EXAMPLE
        Eigen::Quaterniond q(p.rotation_matrix());//把李群变换成位置向量和四元数形式保存
        Eigen::Vector3d vp(p.translation());
　　　　//a按格式写入文件，直接以转置的形式写入，格式有些混乱
        ofileEst<<setiosflags(ios::fixed)<<setprecision(6)<<vp[0]<<' '<<vp[1]<<' '<<vp[2]<<' ';//原样写入（不用科学计数法），保留６位小数
        ofileEst<<setiosflags(ios::fixed)<<setprecision(6)<<q.x()<<' '<<
                q.y()<<' '<<q.z()<<' '<<q.w()<<endl;
    }
    ofileEst.close();
#endif
    DrawTrajectory(ps1,ps2);
    return 0;
}

对齐前的pangolin显示：

 对齐后的pangolin显示：

附：python显示：

import os
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Droid Sans Fallback'] #设置中文字体
plt.rcParams['axes.unicode_minus']=False #正确显示负号
x,y = np.loadtxt('GroundAligned.txt',delimiter=" ",unpack=True,usecols=(1, 2))
x2,y2 = np.loadtxt('AfterAligned.txt',delimiter=" ",unpack=True,usecols=(0,1))
plt.scatter(x,y,s=0.5,c='r',alpha=1)
plt.scatter(x2,y2,s=0.5,c='b',alpha=1)
plt.show()

对齐前：

对齐后：