slam学习笔记

ubuntu20.04 使用vs code编写

现放cmake文件（记得链接库文件和配置C++版本）

cmake_minimum_required( VERSION 2.8 )

project(learingMatrix)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17")

include_directories("/usr/include/eigen3")
add_executable(learningMatrix eigenMatrix.cpp)
add_executable(learnningGeometry useGeometry.cpp)
add_executable(coordinatetrans coordinateTransform.cpp)
add_executable(plot plotTrajectory.cpp)
add_executable(cube visualizeGeometry.cpp)

include_directories(${Pangolin_INCLUDE_DIRS})
find_package( Pangolin )

target_link_libraries(plot ${Pangolin_LIBRARIES})

target_link_libraries(cube ${Pangolin_LIBRARIES})

以下为eigen的简单运算部分

#include <iostream>
using namespace std;

#include <ctime>

#include <Eigen/Core>           //eigen 核心
#include <Eigen/Dense>          //稠密矩阵运算规则
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char** argv)
{
    Matrix<float,2,3> matrix_23;   //Matrix模板类用三个参数生成一个矩阵 数据类型，行，列

    Vector3d v_3d;                 //Vector3d生成一个三维行向量，d是double的意思,本质上还是Matrix一个特殊形式 
    Matrix<double,3,1>  v1_3d;    //同一个意思

    Matrix3d matrix_33 = Matrix3d::Zero(); //Matrix3d生成一个3*3的方阵， Zero是0阵

    Matrix<double , Dynamic,Dynamic> matrix_dynamic; //定义动态矩阵，行和列随数据变化
    MatrixXd matrix_x;                               //同上

    // ------------------------------------------------------------------------------------

    matrix_23<<1,2,3,4,5,6;        //输入数据（运算符重载）

    cout<<"matrix 2*3 from 1 to 6 : "<<matrix_23<<endl; //输出（运算符重载）

    cout<<"print matrix 2*3: "<<endl;
    for(int i=0;i<2;i++)
    {
        for(int j=0;j<3;j++)
        {
            cout<<matrix_23(i,j)<<"\t";
        }
        cout<<endl;
    }

    //-----------------------------------------------------------------------------------------
    v_3d << 3,2,1;
    v1_3d <<4,5,6;

    //Matrix<double ,2,1> result_wrong_type =matrix_23*v1_3d;(不能混用不同数据类型的矩阵，eigen不会自动升级数据类型)
    Matrix<double , 2, 1> result = matrix_23.cast<double>()*v_3d;           //cast转换数据类型
    cout<<"[1,2,3;4,5,6]*[3,2,1]="<<result.transpose()<<endl;

    Matrix<double , 2,1> result2 = matrix_23.cast<double>()*v1_3d;
    cout<<"[1,2,3;4,5,6]*[4,5,6]="<<result2.transpose()<<endl;              //transpose转置
    //Eigen::Matrix<double , 2,3 > result_wrong_dimension = matrix_23.cast<double>()*v_3d; (不同维度的矩阵不能直接赋值，eigen不会自动填充空白)

    //----------------------------------------------------------------------------------------
    matrix_33 = Matrix3d::Random();                                         //随机填充数
    cout<<"random matrix:\n"<<matrix_33<<endl;
    cout<<"transpose :\n"<<matrix_33.transpose()<<endl;
    cout<<"sum: \n"<<matrix_33.sum()<<endl;
    cout<<"trace: \n"<<matrix_33.trace()<<endl;
    cout<<"10 times: \n"<<10*matrix_33<<endl;
    cout<<"inverse ; \n"<<matrix_33.inverse()<<endl;                        //不推荐直接求逆，维度一大起来，计算量太大
    cout<<"det: \n"<<matrix_33.determinant()<<endl;

    //----------------------------------------------------------------------------------------
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);  //矩阵对角化？
    cout<<"Eigen value =\n"<< eigen_solver.eigenvalues()<<endl;
    cout<<"Eigen vectors = \n"<<eigen_solver.eigenvectors()<<endl;   

    //----------------------------------------------------------------------------------------
    Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN
    = MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double,MATRIX_SIZE,1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);

    clock_t time_stt = clock();
    //----------------------------------------------------------------------------------------
    //直接求逆
    Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    cout<<"time of normal inverse is "
        <<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    //----------------------------------------------------------------------------------------
    //Qr分解
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<"time of Qr decomposition is "
        <<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    //----------------------------------------------------------------------------------------
    //正定阵可以用cholesky分解
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout<<"time of ldlt decomposition is "
        <<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;

    return 0;
}

以下为eigen的几何运算部分

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

int main(int argc,char** argv)
{
    Matrix3d rotation_matrix = Matrix3d::Identity();            //旋转矩阵
    AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1));         //旋转向量      绕Z轴旋转45度
    cout.precision(3);
    cout<<"rotation matrix= \n"<<rotation_vector.matrix()<<endl;//用.matrix（）把轴角式转换成矩阵输出

    rotation_matrix=rotation_vector.toRotationMatrix();         //toRotationmatrix可以直接转换成矩阵并赋值
//用轴角 旋转向量--------------------------------------------------------------------------------------------
    Vector3d v(1,0,0);
    Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation (by angle axis) = "<<v_rotated.transpose()<<endl;
//用旋转矩阵 旋转向量----------------------------------------------------------------------------------------
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation (by matrix) = "<<v_rotated.transpose()<<endl;
//欧拉角转换-----------------------------------------------------------------------------------------------
    Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
//欧式变换(长度、夹角、)
    Isometry3d T = Isometry3d::Identity();                 //4*4的矩阵！！！
    T.rotate(rotation_vector);                             //旋转 
    T.pretranslate(Vector3d(1,3,4));                       //平移
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;
// 用变换矩阵变换
    Vector3d v_transformed = T*v;
    cout<<"v transformed = "<<v_transformed.transpose();

//四元数（四元数和轴角之间可以相互转换）
    Quaterniond q = Quaterniond(rotation_vector);
    cout<<"quaternion from rotation vector = "<<q.coeffs().transpose()<<endl;
    q = Quaterniond(rotation_matrix);
    cout<<"quaternion from rotation matrix = "<<q.coeffs().transpose()<<endl;
//用四元数旋转一个向量
    v_rotated = q * v;                                      
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
//从数学表达式应该是下面的qvq^-1
    cout<<"should be equal to "<<(q*Quaterniond(0,1,0,0) * q.inverse()).coeffs().transpose()<<endl;
    
    return 0;
}

以下为两个小机器人的坐标变换问题

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;


int main(int argc,char** argv)
{
    Quaterniond q1(0.35,0.2,0.3,0.1),q2(-0.5,0.4,-0.1,0.2);       //一号位姿q1，t1，二号位姿q2,t2  四元数表示旋转，t为平移矩阵
    q1.normalize();                                               //归一化数据
    q2.normalize();
    Vector3d t1(0.3,0.1,0.1),t2(-0.1,0.5,0.3);
    Vector3d p1(0.5,0,0.2);                                       //目标在一号机器人坐标系下的坐标

    Isometry3d T1w(q1),T2w(q2);                                   //隐藏了一部旋转向量设定
    // T1w.rotate(q1);
    //T2w.rotate(q2);                                             //旋转向量设定

    T1w.pretranslate(t1);                                         //平移向量设定
    T2w.pretranslate(t2);

    Vector3d p2 =T2w * T1w.inverse()*p1;                          //目标由（一号转世界）（世界转二号）转换至二号机器人坐标系下，得到p2
    cout<<endl<<p2.transpose();
    return 0;
}

以下为pangolin路径绘制部分

#include <iostream>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unistd.h>
using namespace std;
using namespace Eigen;

string trajectory_file = "/home/martin/桌面/code/eigen/trajectory.txt";
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);       //先声明函数

int main(int argc,char** argv)
{
    vector<Isometry3d,Eigen::aligned_allocator<Isometry3d>> poses;                   //声明
    ifstream fin(trajectory_file);
    if(!fin)
    {
        cout<<"cannot find trajectory file at "<<trajectory_file<<endl;
        return 1;
    }

    while (!fin.eof())
    {
        double time ,tx, ty, tz, qx, qy, qz, qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;                                       //读取时间、平移、旋转
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));                                    //定义旋转向量
        Twr.pretranslate(Vector3d(tx,ty,tz));                                        //定义平移向量
        poses.push_back(Twr);
    }
    cout<<"real total "<<poses.size()<<" pose entries"<<endl;

    DrawTrajectory(poses);
    return 0;
}

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses)        //实例化
{
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024,768 );                         //标题和窗口大小
    glEnable(GL_DEPTH_TEST);                                                               //这三句直接写 这句是启用深度检测，下一句是启用混合
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC0_ALPHA,GL_ONE_MINUS_SRC_ALPHA);                                     //表示颜色的混合方式

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),                     //投影矩阵（前四个是尺寸，然后是near和far的边界值）
        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,0.0,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);                                                             //激活相机s
        glClearColor(1.0f,1.0f,1.0f,1.0f);                                                 //opencv类似设定颜色和线宽
        glLineWidth(2);
        for (size_t i = 0; i < poses.size(); i++)
        {
            Vector3d Ow = poses[i].translation();                                          //平移向量

            Vector3d Xw = poses[i]*(0.1*Vector3d(1,0,0));                                  //旋转向量
            Vector3d Yw = poses[i]*(0.1*Vector3d(0,1,0));
            Vector3d Zw = poses[i]*(0.1*Vector3d(0,0,1));

            glBegin(GL_LINES);                                                             //给各个轴分配不同的颜色
            glColor3f(1.0,0.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Xw[0],Xw[1],Xw[2]);
            glColor3f(0.0,1.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Yw[0],Yw[1],Yw[2]);
            glColor3f(0.0,0.0,1.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Zw[0],Zw[1],Zw[2]);
            glEnd();

        }
        
        for (size_t i = 0; i < poses.size(); i++)                                          //画线
        {
            glColor3f(0.0,0.0,0.0);
            glBegin(GL_LINES);
            auto p1 = poses[i],p2=poses[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();
        usleep(5000);
    }
}

以下为三维集合体可视化部分

#include <iostream>
#include <iomanip>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <pangolin/pangolin.h>

struct RotationMatrix {
  Matrix3d matrix = Matrix3d::Identity();
};

ostream &operator<<(ostream &out, const RotationMatrix &r)                    //重载运算符
{
  out.setf(ios::fixed);
  Matrix3d matrix = r.matrix;
  out << '=';
  out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
      << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
      << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
  return out;
}

istream &operator>>(istream &in, RotationMatrix &r)                           //重载运算符
{
  return in;
}

struct TranslationVector 
{
  Vector3d trans = Vector3d(0, 0, 0);
};

ostream &operator<<(ostream &out, const TranslationVector &t) 
{
  out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
  return out;
}

istream &operator>>(istream &in, TranslationVector &t) 
{
  return in;
}

struct QuaternionDraw {
  Quaterniond q;
};

ostream &operator<<(ostream &out, const QuaternionDraw quat) 
{
  auto c = quat.q.coeffs();
  out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
  return out;
}

istream &operator>>(istream &in, const QuaternionDraw quat) 
{
  return in;
}

int main(int argc, char **argv) 
{
  pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
  glEnable(GL_DEPTH_TEST);
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
    pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
  );

  const int UI_WIDTH = 500;

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

                                    
  pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());                 //绘制左侧提示栏
  pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
  pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
  pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
  pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

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

    d_cam.Activate(s_cam);

    pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
    Matrix<double, 4, 4> m = matrix;

    RotationMatrix R;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++)
        R.matrix(i, j) = m(j, i);
    rotation_matrix = R;

    TranslationVector t;
    t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
    t.trans = -R.matrix * t.trans;
    translation_vector = t;

    TranslationVector euler;
    euler.trans = R.matrix.eulerAngles(2, 1, 0);
    euler_angles = euler;

    QuaternionDraw quat;
    quat.q = Quaterniond(R.matrix);
    quaternion = quat;

    glColor3f(1.0, 1.0, 1.0);

    pangolin::glDrawColouredCube();                   //绘制直线
    // draw the original axis
    glLineWidth(3);
    glColor3f(0.8f, 0.f, 0.f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(10, 0, 0);
    glColor3f(0.f, 0.8f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 10, 0);
    glColor3f(0.2f, 0.2f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 10);
    glEnd();

    pangolin::FinishFrame();
  }
}

自学slam确实挺麻烦的，继续学。