《视觉SLAM十四讲从理论到实践第二版》实践部分——ch3

木木夕v

已于 2022-08-12 11:04:21 修改

阅读量570

点赞数 1

分类专栏：视觉SLAM 计算机视觉文章标签：计算机视觉学习

于 2022-08-11 12:14:28 首次发布

本文链接：https://blog.csdn.net/a1446018093/article/details/126265007

版权

视觉SLAM 同时被 2 个专栏收录

11 篇文章 13 订阅

订阅专栏

计算机视觉

11 篇文章 2 订阅

订阅专栏

书籍配套程序

文章目录

课内实践
课后实践
- 实验一
- 实验二

课内实践

useEigen

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(useEigen)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(eigenMatrix eigenMatrix.cpp)

eigenMatrix.cpp

#include <iostream>

using namespace std;
//计时用
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

using namespace Eigen;

#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<double>() * 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;    // 行列式

  // 特征值
  // 实对称矩阵可以保证对角化成功
  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);
  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;
}

结果

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]=32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.0242899
 0.992154
  1.80558
Eigen vectors = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 0.125ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 0.049ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.023ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

若矩阵运算没有考虑矩阵维度，报错信息：
In file included from /usr/include/eigen3/Eigen/Core:347:0,
from /home/mmx/slambook2_2/ch3/useEigen/eigenMatrix.cpp:7:
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h: In instantiation of ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Matrix<double, 2, 3, 0, 2, 3>; Src = Eigen::Product<Eigen::CwiseUnaryOp<Eigen::internal::scalar_cast_op<float, double>, const Eigen::Matrix<float, 2, 3> >, Eigen::Matrix<double, 3, 1>, 0>; Func = Eigen::internal::assign_op<double, double>]’:
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:728:41: required from ‘Derived& Eigen::PlainObjectBase::_set_noalias(const Eigen::DenseBase&) [with OtherDerived = Eigen::Product<Eigen::CwiseUnaryOp<Eigen::internal::scalar_cast_op<float, double>, const Eigen::Matrix<float, 2, 3> >, Eigen::Matrix<double, 3, 1>, 0>; Derived = Eigen::Matrix<double, 2, 3, 0, 2, 3>]’
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:537:19: required from ‘Eigen::PlainObjectBase::PlainObjectBase(const Eigen::DenseBase&) [with OtherDerived = Eigen::Product<Eigen::CwiseUnaryOp<Eigen::internal::scalar_cast_op<float, double>, const Eigen::Matrix<float, 2, 3> >, Eigen::Matrix<double, 3, 1>, 0>; Derived = Eigen::Matrix<double, 2, 3, 0, 2, 3>]’
/usr/include/eigen3/Eigen/src/Core/Matrix.h:379:29: required from ‘Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(const Eigen::EigenBase&) [with OtherDerived = Eigen::Product<Eigen::CwiseUnaryOp<Eigen::internal::scalar_cast_op<float, double>, const Eigen::Matrix<float, 2, 3> >, Eigen::Matrix<double, 3, 1>, 0>; _Scalar = double; int _Rows = 2; int _Cols = 3; int _Options = 0; int _MaxRows = 2; int _MaxCols = 3]’
/home/mmx/slambook2_2/ch3/useEigen/eigenMatrix.cpp:66:83: required from here
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: error: static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES
#define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
^
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:188:3: note: in expansion of macro ‘EIGEN_STATIC_ASSERT’
EIGEN_STATIC_ASSERT(
^~~~~~~~~~~~~~~~~~~
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:833:3: note: in expansion of macro ‘EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE’
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)
^
CMakeFiles/eigenMatrix.dir/build.make:75: recipe for target ‘CMakeFiles/eigenMatrix.dir/eigenMatrix.cpp.o’ failed
make[2]: *** [CMakeFiles/eigenMatrix.dir/eigenMatrix.cpp.o] Error 1
CMakeFiles/Makefile2:82: recipe for target ‘CMakeFiles/eigenMatrix.dir/all’ failed
make[1]: *** [CMakeFiles/eigenMatrix.dir/all] Error 2
Makefile:90: recipe for target ‘all’ failed
make: *** [all] Error 2

useGeometry

make报错
CMakeFiles/eigenGeometry.dir/home/mmx/slambook2_2/ch3/examples/coordinateTransform.cpp.o：在函数‘main’中：
coordinateTransform.cpp:(.text+0x0): `main’被多次定义
CMakeFiles/eigenGeometry.dir/eigenGeometry.cpp.o:eigenGeometry.cpp:(.text+0x0)：第一次在此定义
collect2: error: ld returned 1 exit status
CMakeFiles/eigenGeometry.dir/build.make:112: recipe for target ‘eigenGeometry’ failed
make[2]: *** [eigenGeometry] Error 1
CMakeFiles/Makefile2:82: recipe for target ‘CMakeFiles/eigenGeometry.dir/all’ failed
make[1]: *** [CMakeFiles/eigenGeometry.dir/all] Error 2
Makefile:90: recipe for target ‘all’ failed
make: *** [all] Error 2

原因：eigenGeometry.cpp …/examples/coordinateTransform.cpp中均有main函数
解决方法：修改CMakeLists.txt，生成不同的可执行程序

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(geometry)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(eigenGeometry eigenGeometry.cpp)

eigenGeometry.cpp

#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);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 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顺序，即roll pitch yaw顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于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;
}

结果

rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v tranformed = 1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

小萝卜案例（坐标变换）

在这里插入图片描述

coordinateTransform.cpp

#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.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.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}

结果

-0.0309731    0.73499   0.296108

plotTrajectory

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(plotTrajectory)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 寻找并添加Pangolin
FIND_PACKAGE(Pangolin REQUIRED)
INCLUDE_DIRECTORIES(${Pangolin_INCLUDE_DIRS})

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(plotTrajectory plotTrajectory.cpp)

# 添加链接库
TARGET_LINK_LIBRARIES(plotTrajectory ${Pangolin_LIBRARIES})

plotTrajectory.cpp

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

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "../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 << "read total " << poses.size() << " pose entries" << endl;

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

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> 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)
  );

  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);
    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);
      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
  }
}

结果

在这里插入图片描述

visualizeGeometry

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(visualizeGeometry)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 寻找并添加Pangolin
FIND_PACKAGE(Pangolin REQUIRED)
INCLUDE_DIRECTORIES(${Pangolin_INCLUDE_DIRS})

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(visualizeGeometry visualizeGeometry.cpp)

# 添加链接库
TARGET_LINK_LIBRARIES(visualizeGeometry ${Pangolin_LIBRARIES})

visualizeGeometry.cpp

#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));

  // ui
  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();
  }
}

结果

在这里插入图片描述

课后实践

实验一

在这里插入图片描述

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(Eigen)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(changeMatrix changeMatrix.cpp)

changeMatrix.cpp

#include<iostream>
 
//包含Eigen头文件
#include<Eigen/Core>
#include<Eigen/Geometry>
 
#define MATRIX_SIZE 100
using namespace std;
 
int main(int argc,char **argv)
{
    //设置输出小数点后3位
    cout.precision(3);
    Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,3,3>matrix_33 = Eigen::MatrixXd::Random(3,3);//3x3矩阵变量
/*方法1：循环遍历矩阵的三行三列   */
/*
    cout<<"提取出来的矩阵块为:"<<endl;
    for(int i = 0;i < 3; i ++){
        for(int j = 0 ;j < 3;j++){
            matrix_33(i,j) = matrix_NN(i,j);
            cout<<matrix_NN(i,j)<<" ";
        }
              cout<<endl;
    }
    matrix_33 = Eigen::Matrix3d::Identity();
    cout<<"赋值后的矩阵为:"<<endl<<matrix_33<<endl;
    
*/
 
/*方法2：用.block函数   */
 
    //提取后赋值为新的元素
    matrix_33 = matrix_NN.block(0,0,3,3);
    cout<<"提取出来的矩阵块为:"<<endl;
    cout<< matrix_33<<endl;
    matrix_33 = Eigen::Matrix3d::Identity();
    cout<<"赋值后的矩阵为:"<<endl<<matrix_33<<endl;
 
    return 0;
}

结果

提取出来的矩阵块为:
  0.68 -0.035  0.959
-0.211 -0.568  0.488
 0.566  0.901  0.807
赋值后的矩阵为:
1 0 0
0 1 0
0 0 1

实验二

在这里插入图片描述

CMakeLists.txt

# 声明要求的 cmake 最低版本
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

# 声明一个 cmake 工程
PROJECT(Eigen)

# 设置编译模式
SET(CMAKE_BUILD_TYPE "Release")

# 设置编译选项
SET(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
INCLUDE_DIRECTORIES("/usr/include/eigen3")

# 添加一个可执行程序
# 语法：ADD_EXECUTABLE( 程序名 源代码文件 ）
ADD_EXECUTABLE(test2 test2.cpp)

test2.cpp

程序来源

#include<iostream>
#include<ctime>
#include <cmath>
#include <complex>
/*线性方程组Ax = b的解法(直接法（1,2,3,4,5）+迭代法(6))其中只有2 3方法不要求方程组个数与变量个数相等*/
 
//包含Eigen头文件
//#include <Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <Eigen/Eigenvalues>
 
//下面这两个宏的数值一样的时候 方法1 4 5 6才能正常工作
#define MATRIX_SIZE 3   //方程组个数
#define MATRIX_SIZE_ 3  //变量个数
//using namespace std;
typedef  Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_>  Mat_A;
typedef  Eigen::Matrix<double ,MATRIX_SIZE,1 >              Mat_B;
 
//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A   &A,Mat_B   &x_k,int i);
 
//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A   &A,Mat_B   &b,  int &iteration_num, double &accuracy );
 
int main(int argc,char **argv)
{
    //设置输出小数点后3位
    std::cout.precision(3);
    //设置变量
    Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE_);
    Eigen::Matrix<double ,MATRIX_SIZE,1 > v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
 
    //测试用例
    matrix_NN << 10,3,1,2,-10,3,1,3,10;
    v_Nd <<14,-5,14;
 
    //设置解变量
    Eigen::Matrix<double,MATRIX_SIZE_,1>x;
 
    //时间变量
    clock_t tim_stt = clock();
 
/*1、求逆法	很可能没有解 仅仅针对方阵才能计算*/
#if (MATRIX_SIZE == MATRIX_SIZE_)
    x = matrix_NN.inverse() * v_Nd;
    std::cout<<"直接法所用时间和解为："<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        <<"MS"<< std::endl << x.transpose() << std::endl;
#else
    std::cout<<"直接法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif
 
/*2、QR分解解方程组	适合非方阵和方阵 当方程组有解时的出的是真解，若方程组无解得出的是近似解*/
    tim_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    std::cout<<"QR分解所用时间和解为："<<1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl << x.transpose() << std::endl;
 
/*3、最小二乘法	适合非方阵和方阵，方程组有解时得出真解，否则是最小二乘解(在求解过程中可以用QR分解 分解最小二成的系数矩阵) */
    tim_stt = clock();
    x = (matrix_NN.transpose() * matrix_NN ).inverse() * (matrix_NN.transpose() * v_Nd);
    std::cout<<"最小二乘法所用时间和解为:"<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl  << x.transpose() << std::endl;
 
/*4、LU分解方法	只能为方阵（满足分解的条件才行）    */
#if (MATRIX_SIZE == MATRIX_SIZE_)
    tim_stt = clock();
    x = matrix_NN.lu().solve(v_Nd);
    std::cout<< "LU分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl << x.transpose() << std::endl;
#else
    std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif
 
/*5、Cholesky	分解方法  只能为方阵 (结果与其他的方法差好多)*/
#if (MATRIX_SIZE == MATRIX_SIZE_)
    tim_stt = clock();
    x = matrix_NN.llt().solve(v_Nd);
    std::cout<< "Cholesky 分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS"<< std::endl<< x.transpose()<<std::endl;
#else
    std::cout<< "Cholesky法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif
 
/*6、Jacobi迭代法  */
#if (MATRIX_SIZE == MATRIX_SIZE_)
    int Iteration_num = 10 ;
    double Accuracy =0.01;
    tim_stt = clock();
    x= Jacobi(matrix_NN,v_Nd,Iteration_num,Accuracy);
    std::cout<< "Jacobi 迭代法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS"<< std::endl<< x.transpose()<<std::endl;
#else
    std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif
 
    return 0;
}
 
//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A  &A,Mat_B  &b, int &iteration_num, double &accuracy ) {
    Mat_B x_k = Eigen::MatrixXd::Zero(MATRIX_SIZE_,1);//迭代的初始值
    Mat_B x_k1;         //迭代一次的解向量
    int k,i;            //i,k是迭代算法的循环次数的临时变量
    double temp;        //每迭代一次解向量的每一维变化的模值
    double R=0;         //迭代一次后，解向量每一维变化的模的最大值
    int isFlag = 0;     //迭代要求的次数后，是否满足精度要求
 
    //判断Jacobi是否收敛
    Mat_A D;            //D矩阵
    Mat_A L_U;          //L+U
    Mat_A temp2 = A;    //临时矩阵获得A矩阵除去对角线后的矩阵
    Mat_A B;            //Jacobi算法的迭代矩阵
    Eigen::MatrixXcd EV;//获取矩阵特征值
    double maxev=0.0;   //最大模的特征值
    int flag = 0;       //判断迭代算法是否收敛的标志 1表示Jacobi算法不一定能收敛到真值
 
    std::cout<<std::endl<<"欢迎进入Jacobi迭代算法！"<<std::endl;
    //對A矩陣進行分解 求取迭代矩陣 再次求取譜半徑 判斷Jacobi迭代算法是否收斂
    for(int l=0 ;l < MATRIX_SIZE;l++){
        D(l,l) = A(l,l);
        temp2(l,l) = 0;
        if(D(l,l) == 0){
            std::cout<<"迭代矩阵不可求"<<std::endl;
            flag =1;
            break;
        }
    }
    L_U = -temp2;
    B = D.inverse()*L_U;
 
    //求取特征值
    Eigen::EigenSolver<Mat_A>es(B);
    EV = es.eigenvalues();
//    cout<<"迭代矩阵特征值为:"<<EV << endl;
 
    //求取矩陣的特征值 然後獲取模最大的特徵值 即爲譜半徑
    for(int index = 0;index< MATRIX_SIZE;index++){
        maxev = ( maxev > __complex_abs(EV(index)) )?maxev:(__complex_abs(EV(index)));
    }
    std::cout<< "Jacobi迭代矩阵的谱半径为："<< maxev<<std::endl;
 
    //谱半径大于1 迭代法则发散
    if(maxev >= 1){
        std::cout<<"Jacobi迭代算法不收敛！"<<std::endl;
        flag =1;
    }
 
    //迭代法收敛则进行迭代的计算
    if (flag == 0 ){
        std::cout<<"Jacobi迭代算法谱半径小于1,该算法收敛"<<std::endl;
        std::cout<<"Jacobi迭代法迭代次数和精度： "<< std::endl << iteration_num<<" "<<accuracy<<std::endl;
 
        //迭代计算
        for( k = 0 ;k < iteration_num ; k++ ){
            for(i = 0;i< MATRIX_SIZE_ ; i++){
                x_k1(i) = x_k(i) + ( b(i) - Jacobi_sum(A,x_k,i) )/A(i,i);
                temp = fabs( x_k1(i) - x_k(i) );
                if( fabs( x_k1(i) - x_k(i) ) > R )
                    R = temp;
            }
 
            //判断进度是否达到精度要求 达到进度要求后 自动退出
            if( R < accuracy ){
                std::cout <<"Jacobi迭代算法迭代"<< k << "次达到精度要求."<< std::endl;
                isFlag = 1;
                break;
            }
 
            //清零R，交换迭代解
            R = 0;
            x_k = x_k1;
        }
        if( !isFlag )
            std::cout << std::endl <<"迭代"<<iteration_num<<"次后仍然未达到精度要求，若不满意该解，请再次运行加大循环次数！"<< std::endl;
        return x_k1;
    }
    //否则返回0
    return  x_k;
}
 
//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A  &A,Mat_B &x_k,int i) {
    double sum;
    for(int j = 0; j< MATRIX_SIZE_;j++){
        sum += A(i,j)*x_k(j);
    }
    return sum;
}

结果

直接法所用时间和解为：0.017MS
1 1 1
QR分解所用时间和解为：0.004MS
1 1 1
最小二乘法所用时间和解为:0.001MS
1 1 1
LU分解方法所用时间和解为:0.002MS
1 1 1
Cholesky 分解方法所用时间和解为:0.001MS
    1.4 -0.0472   0.103

欢迎进入Jacobi迭代算法！
Jacobi迭代矩阵的谱半径为：0.387
Jacobi迭代算法谱半径小于1,该算法收敛
Jacobi迭代法迭代次数和精度： 
10 0.01
Jacobi迭代算法迭代6次达到精度要求.
Jacobi 迭代法所用时间和解为:0.056MS
0.998     1 0.998