SLAM基础代码

一、编译方式

1、g++命令编译.cpp文件，生成各种各样的文件，如可执行文件，库文件等。
g++ 的编译指令大全：https://blog.csdn.net/zhubaohua_bupt/article/details/52763639
2、CMake编译
建立遵循语法的CMakeLists.txt文件，语法链接：
https://blog.csdn.net/afei__/article/details/81201039#t9
https://blog.csdn.net/yiminghd2861/article/details/85413408#t1
之后使用命令，cmake . 代替输入的g++编译指令，make来进行编译
3、使用IDE来进行编译

二、使用Eigen做矩阵的旋转变换

坐标系1、坐标系2，向量a在两个坐标系下的坐标为a1,a2,两个坐标通过欧式变换连接到一起。
欧式变换由旋转和平移组成，旋转由旋转矩阵SO（n），（特殊正交群）元素R来表示，平移由t表示。
a1=R12a2+t12
R12表示2到1的旋转矩阵，t12表示在坐标系1下，从1指向2的向量。
通过引入变换矩阵T（SE（n）：特殊欧式群），将上式进一步简化为：
$\begin{array}{c}a1\\ 1\end{array}$= $\begin{array}{cc}R& t\\ 0& 1\end{array}$ * $\begin{array}{c}a2\\ 1\end{array}$
左边引入了齐次坐标转换，方便起见可以写为a1=Ta2

#include < iostream>
#include < ctime>
为了调用clock函数，函数返回值是硬件滴答数，要换算成秒或者毫秒，需要除以CLOCKS_PER_SEC。比如，在VC++6.0下，这两个量的值都是1000，这表示硬件滴答1000下是1秒，因此要计算一个进程的时间，用clock()除以1000即可。
using namespace std;
using namespace Eigen;
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#define MATRIX_SIZE 50

/****************************

• 本程序演示了 Eigen 基本类型的使用
  ****************************/

int main(int argc, char **argv) {
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Matrix<float, 2, 3> matrix_23;

// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Vector3d v_3d;
// 这是一样的
Matrix<float, 3, 1> vd_3d;

// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零
// 如果不确定矩阵大小，可以使用动态大小的矩阵
Matrix<double, Dynamic, Dynamic> matrix_dynamic;
// 更简单的
MatrixXd matrix_x;
// 这种类型还有很多，我们不一一列举

// 下面是对Eigen阵的操作
// 输入数据（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 输出
cout << “matrix 2x3 from 1 to 6: \n” << matrix_23 << endl;

// 用()访问矩阵中的元素
cout << "print matrix 2x3: " << 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;
vd_3d << 4, 5, 6;

// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// 应该显式转换
Matrix<double, 2, 1> result = matrix_23.cast< double>() * v_3d;
cout << “[1,2,3;4,5,6]*[3,2,1]=” << result.transpose() << endl;

Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast() * v_3d;

// 一些矩阵运算
// 四则运算就不演示了，直接用±*/即可。
matrix_33 = Matrix3d::Random(); // 随机数矩阵
cout << “random matrix: \n” << matrix_33 << endl;
cout << “transpose: \n” << matrix_33.transpose() << endl; // 转置
cout << "sum: " << matrix_33.sum() << endl; // 各元素和
cout << "trace: " << matrix_33.trace() << endl; // 迹
cout << “times 10: \n” << 10 * matrix_33 << endl; // 数乘
cout << “inverse: \n” << matrix_33.inverse() << endl; // 逆
cout << "det: " << matrix_33.determinant() << endl; // 行列式

// 特征值，特征值要先进行At*A的过程
// 实对称矩阵可以保证对角化成功
SelfAdjointEigenSolver< Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
cout << “Eigen values = \n” << eigen_solver.eigenvalues() << endl;
cout << “Eigen vectors = \n” << eigen_solver.eigenvectors() << endl;

// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大

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); QR分解
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做矩阵旋转
^表示把向量变为反对称矩阵，反之相反；任意一个向量都有唯一的反对称矩阵
除了R表示旋转之外，还可以用旋转向量，欧拉角，四元数表示旋转
旋转向量：theta后面紧跟a；旋转轴为单位长度1的向量a,旋转角度为theta

欧拉角：最常用的是绕ZYX轴的旋转，Z偏航角：yaw,Y俯仰角：pich,X滚转角:roll
四元数：q=q0+q1i+q2j+q3k, q=[s,v]实部为零，虚四元数，虚部为零，实四元数
旋转后的p’=qpq-1，四元数到R的变换关系，不作要求。
#include < iostream>
#include < cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry> 做旋转变换

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

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

// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Matrix3d rotation_matrix = Matrix3d::Identity();单位矩阵
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));旋转向量 //沿 Z 轴旋转 45 度
cout.precision(3)输出时小数点后保留3位;
cout << “rotation matrix =\n” << rotation_vector.matrix() << endl; //用matrix()转换成矩阵
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix()旋转向量转为R;
// 用 AngleAxis 可以进行坐标变换
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); // ZYX顺序，即yaw-pitch-roll顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

// 欧氏变换矩阵使用 Eigen::Isometry
Isometry3d T = Isometry3d::Identity();使用变换矩阵T做旋转 // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector); T=四元数q1 // 指定旋转的R
T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)
cout << “Transform matrix = \n” << T.matrix() << endl;

// 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v; 注意要先进行T.matrix() // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;

// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

// 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);旋转向量转为四元数
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
<< endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);旋转矩阵转为四元数
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q * v;使用四元数旋转 // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
// 用常规向量乘法表示，则应该如下计算
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

return 0;
}

四、使用pangolin做3D绘图

https://blog.csdn.net/weixin_43991178/article/details/105119610?utm_medium=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.edu_weight&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.edu_weight

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹,轨迹按照行存放，每行格式为time,tx,ty,tz,qx,qy,qz,qw；存放在.txt文件中

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = “/home/hotfinda/example_slambook/slambook2-master/ch3/examples/trajectory.txt”;

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator< Isometry3d>>);这才是定义容器的正确方法，只是一般定义的都是c++的类型，所以才省略了后面的一长串，而Isometry不是c++的类型

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

vector<Isometry3d, Eigen::aligned_allocator> poses;
ifstream fin(trajectory_file);打开文件
if (!fin) {
cout << "cannot find trajectory file at " << trajectory_file << endl;
return 1;
}

while (!fin.eof()) 成员函数eof()用来检测是否到达文件尾，如果到达文件尾返回非0值，否则返回0
{
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 << “read total " << poses.size() << " pose entries” << endl;

// draw trajectory in pangolin
DrawTrajectory(poses);
return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator> poses) {
// 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)
);
ProjectMatrix(int h, int w, int fu, int fv, int cu, int cv, int znear, int zfar)
参数依次为观察相机的图像高度、宽度、4个内参以及最近和最远视距

ModelViewLookAt(double x, double y, double z,double lx, double ly, double lz, AxisDirection Up)
参数依次为相机所在的位置，以及相机所看的视点位置(一般会设置在原点)

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));
创建一个交互式视图（view）用于显示上一步摄像机所“拍摄”到的内容，setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右），可以采用相对坐标（0~1）以及绝对坐标（使用Attach对象）。
handler为交互相机视图句柄

while (pangolin::ShouldQuit() == false) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);清空颜色和深度缓存
d_cam.Activate(s_cam); d_cam视图，启动s_cam相机
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
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);与glEnd结合使用
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); // sleep 5 ms
}
}