书籍配套程序
课内实践
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