建设网站论文,新乡网站关键词优化,昆明做网站外包,做动画 的 网站有哪些软件本章需要掌握的知识点有#xff1a;旋转矩阵#xff0c;变换矩阵#xff0c;四元数#xff0c;欧拉角定义和数学表达#xff1b;同时也要掌握Eigen库关于矩阵、几何模块的使用方法。 文章目录3.1 旋转矩阵3.1.1 点#xff0c;向量和矩阵的关系3.1.2 坐标系间的欧式变换3.… 本章需要掌握的知识点有旋转矩阵变换矩阵四元数欧拉角定义和数学表达同时也要掌握Eigen库关于矩阵、几何模块的使用方法。 文章目录3.1 旋转矩阵3.1.1 点向量和矩阵的关系3.1.2 坐标系间的欧式变换3.1.3 变换矩阵与齐次坐标3.2 Eigen实践3.3 旋转向量和欧拉角理解3.3.1 旋转向量3.3.2 欧拉角3.4 四元数常用3.4.3 用四元数表示旋转3.4.4 四元数到其他旋转表示的转换3.6 Eigen几何模块3.6.1 几何模块数据演示3.6.2 坐标转换3.7 可视化演示本章对应视频为
https://www.bilibili.com/video/BV16t411g7FR?p2【高翔】视觉SLAM十四讲3.1 旋转矩阵
3.1.1 点向量和矩阵的关系
这里相对比较简单只需要理解向量内积和外积的计算方法即可向量内积为 a⃗⋅b⃗a⃗Tb⃗∣a⃗∣∣b⃗∣cosθ\vec{a} \cdot \vec{b} \vec{a}^T\vec{b}|\vec{a}||\vec{b}|cos{\theta}a⋅baTb∣a∣∣b∣cosθ 其中θ\thetaθ为向量a⃗,b⃗\vec{a},\vec{b}a,b之间的夹角向量内积的结果是一个标量。 外积相对比较复杂公式为 a⃗×b⃗e⃗1e⃗2e⃗3a1a2a3b1b2a3[a2b3−a3b2a3b1−a1b3a1b2−a2b1][0−a3a2a30−a1−a2a10]b⃗a⃗∧b⃗{\vec{a}\times\vec{b}}\begin{array} {||ccc||} \vec{e}_{1}\vec{e}_{2}\vec{e}_{3}\\ a_{1}a_{2}a_{3}\\ b_{1}b_{2}a_{3}\\ \end{array} \begin{bmatrix}a_2b_3 -a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1\end{bmatrix}\begin{bmatrix}0 -a_3a_2 \\ a_30-a_1\\-a_2a_10\end{bmatrix}\vec{b}\vec{a}\wedge\vec{b} a×be1a1b1e2a2b2e3a3a3⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤⎣⎡0a3−a2−a30a1a2−a10⎦⎤ba∧b 外积的结果是一个向量方向垂直于这两个向量。大小为∣a⃗∣∣b⃗∣sinθ|\vec{a}||\vec{b}|sin\theta∣a∣∣b∣sinθ。这里引入了反对称矩阵任意向量都有着唯一的一个反对称矩阵这里为 a∧[0−a3a2a30−a1−a2a10]a\wedge\begin{bmatrix}0 -a_3a_2 \\ a_30-a_1\\-a_2a_10\end{bmatrix}a∧⎣⎡0a3−a2−a30a1a2−a10⎦⎤ 3.1.2 坐标系间的欧式变换
这里引入了旋转矩阵R\RR的概念描述了向量从一组基中如何旋转变换到另一组基旋转矩阵是一个行列式为1的正交矩阵反之行列式为1的正交矩阵也是一个旋转矩阵。 除了旋转变换外还有平移因此世界坐标系中的向量a⃗\vec{a}a经过一次旋转用R\RR描述和一次平移ttt后得到了新的向量a′a^{}a′。 a′Rata^{}Rata′Rat 3.1.3 变换矩阵与齐次坐标
这里引入齐次坐标和变换矩阵对于上式可得 [a′1][Rt0T1]T[a1]\begin{bmatrix}a^{}\\1\end{bmatrix}\begin{bmatrix}Rt\\0^{T}1\end{bmatrix}T\begin{bmatrix}a\\1\end{bmatrix}[a′1][R0Tt1]T[a1] 这里将旋转和平移写在了一个矩阵里整个关系变为线性关系矩阵TTT为变换矩阵。变换矩阵的逆矩阵为 T−1[RT−RTt0T1]T^{-1}\begin{bmatrix}R^T-R^Tt\\0^{T}1\end{bmatrix}T−1[RT0T−RTt1] 3.2 Eigen实践
下面介绍了Eigen关于向量矩阵的使用。
#include iostreamusing namespace std;#include ctime
// Eigen 核心部分
#include Eigen/Core
// 稠密矩阵的代数运算逆特征值等
#include Eigen/Denseusing namespace Eigen;#define MATRIX_SIZE 50/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/int main(int argc, char **argv) {// Eigen 中所有向量和矩阵都是Eigen::Matrix它是一个模板类。它的前三个参数为数据类型行列// 声明一个2*3的float矩阵Matrixfloat, 2, 3 matrix_23;// 同时Eigen 通过 typedef 提供了许多内置类型不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrixdouble, 3, 1即三维向量Vector3d v_3d;// 这是一样的Matrixfloat, 3, 1 vd_3d;// Matrix3d 实质上是 Eigen::Matrixdouble, 3, 3Matrix3d matrix_33 Matrix3d::Zero(); //初始化为零// 如果不确定矩阵大小可以使用动态大小的矩阵Matrixdouble, 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里你不能混合两种不同类型的矩阵像这样是错的// Matrixdouble, 2, 1 result_wrong_type matrix_23 * v_3d;// 应该显式转换Matrixdouble, 2, 1 result matrix_23.castdouble() * v_3d;cout [1,2,3;4,5,6]*[3,2,1] result.transpose() endl;Matrixfloat, 2, 1 result2 matrix_23 * vd_3d;cout [1,2,3;4,5,6]*[4,5,6]: result2.transpose() endl;// 同样你不能搞错矩阵的维度// 试着取消下面的注释看看Eigen会报什么错// Eigen::Matrixdouble, 2, 3 result_wrong_dimension matrix_23.castdouble() * 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; // 行列式// 特征值// 实对称矩阵可以保证对角化成功SelfAdjointEigenSolverMatrix3d 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的大小在前边的宏里定义它由随机数生成// 直接求逆自然是最直接的但是求逆运算量大Matrixdouble, MATRIX_SIZE, MATRIX_SIZE matrix_NN MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);matrix_NN matrix_NN * matrix_NN.transpose(); // 保证半正定Matrixdouble, MATRIX_SIZE, 1 v_Nd MatrixXd::Random(MATRIX_SIZE, 1);clock_t time_stt clock(); // 计时// 直接求逆Matrixdouble, 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;
}CMakeLists.txt文件
cmake_minimum_required(VERSION 2.8)
project(useEigen)set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS -O3)# 添加Eigen头文件
include_directories(/usr/include/eigen3)
add_executable(eigenMatrix eigenMatrix.cpp)执行结果
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.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234 0.5661980.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.364595.66198 -6.04897 -4.44451
inverse:
-0.198521 2.22739 2.83571.00605 -0.555135 -1.41603-1.62213 3.59308 3.28973
det: 0.208598
Eigen values
0.02428990.9921541.80558
Eigen vectors
-0.549013 -0.735943 0.3961980.253452 -0.598296 -0.760134
-0.796459 0.316906 -0.514998
time of normal inverse is 0.454ms
x -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.778619.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.078ms
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.021ms
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.7343.3 旋转向量和欧拉角理解
3.3.1 旋转向量
旋转矩阵使用9个变量来描述旋转显得有些冗余同样变换矩阵使用16个变量来描述变换也是很冗余。因此有没有一种方式能够紧凑的描述旋转和平移呢 一次旋转只有3个自由度因此这里引入了旋转向量它的方向与旋转轴一致长度等于旋转角这样就可以使用一个三维向量来描述旋转。 从旋转矩阵到旋转向量的过程需要罗德里格斯公式转换这里直接给出旋转矩阵和旋转向量之间的转换关系 RcosθI(1−cosθ)nnTsinθn∧Rcos{\theta}I(1-cos{\theta})nn^{T}sin{\theta}n\wedgeRcosθI(1−cosθ)nnTsinθn∧ 这里RRR为旋转矩阵nnn为一个单位长度的向量θ\thetaθ为角度。对上式两边取迹可以求出转角 θarccostr(R)−12\thetaarccos{\frac{tr(R)-1}{2}}θarccos2tr(R)−1 因为旋转轴上的向量在旋转后不发生变化可得 RnnRnnRnn 因此nnn为矩阵RRR特征值为1对应的特征向量再归一化即可求出旋转向量。
3.3.2 欧拉角
欧拉角其实就是3个分离的转角常见的有绕物体的ZZZ轴旋转的偏航角(yaw)绕YYY旋转的俯仰角(pitch)绕XXX轴旋转的滚转角(roll)。 工程上常会听到rpyrpyrpy角对应的旋转顺序为ZYXZYXZYX。 3.4 四元数常用
3.4.3 用四元数表示旋转
四元数由实部和虚部组成常见形式为qq0q1iq2jq3kqq_0q_1iq_2jq_3kqq0q1iq2jq3k。关于四元数的运算这里不展开了就是普通复数运算。 那么如何使用四元数来表达一个点的旋转呢假设有一个空间三维点p[x,y,z]p[x,y,z]p[x,y,z]以及一个单位四元数qqq指定的旋转三维点ppp经过旋转之后变为p′pp′。用矩阵描述的话则二者关系为p′RppRpp′Rp用四元数表示则为 p[0,x,y,z]T[0,v]T,p′qpq−1p[0,x,y,z]^{T}[0,v]^{T},pqpq^{-1}p[0,x,y,z]T[0,v]T,p′qpq−1 最后p′pp′的虚部为旋转之后的坐标。
3.4.4 四元数到其他旋转表示的转换
这里总结四元数到旋转矩阵之间的关系设四元数为q[s,v]Tq[s,v]^{T}q[s,v]T则 RvvTs2I2sv∧(v∧)2Rvv^{T}s^2I2sv\wedge(v\wedge)^2RvvTs2I2sv∧(v∧)2 四元数到旋转向量之间的关系 θ2arccosq0[nx,ny,nz]T[q1,q2,q3]T/sinθ2\theta2arccosq_0\\ [n_x,n_y,n_z]^T[q_1,q_2,q_3]^T/sin{\frac{\theta}{2}}θ2arccosq0[nx,ny,nz]T[q1,q2,q3]T/sin2θ 3.6 Eigen几何模块
3.6.1 几何模块数据演示
这里给出如何使用Eigen库进行四元数、旋转矩阵、欧拉角、旋转向量的运算。
#include iostream
#include cmathusing namespace std;#include Eigen/Core
#include Eigen/Geometryusing namespace Eigen;// 本程序演示了 Eigen 几何模块的使用方法int main(int argc, char **argv) {// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fMatrix3d 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顺序即yaw-pitch-roll顺序cout yaw pitch roll euler_angles.transpose() endl;// 欧氏变换矩阵使用 Eigen::IsometryIsometry3d T Isometry3d::Identity(); // 虽然称为3d实质上是44的矩阵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*vtcout 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;
}3.6.2 坐标转换
这是坐标转换的小例子显示了不同坐标系下的点的坐标转换。
#include iostream
#include vector
#include algorithm
#include Eigen/Core
#include Eigen/Geometryusing 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;
}3.7 可视化演示
这是一个显示运动轨迹的程序
#include pangolin/pangolin.h
#include Eigen/Core
#include unistd.h// 本例演示了如何画出一个预先存储的轨迹using namespace std;
using namespace Eigen;// path to trajectory file
string trajectory_file ./examples/trajectory.txt;void DrawTrajectory(vectorIsometry3d, Eigen::aligned_allocatorIsometry3d);int main(int argc, char **argv) {vectorIsometry3d, Eigen::aligned_allocatorIsometry3d 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 pangolinDrawTrajectory(poses);return 0;
}/*******************************************************************************************/
void DrawTrajectory(vectorIsometry3d, Eigen::aligned_allocatorIsometry3d poses) {// create pangolin window and plot the trajectorypangolin::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}
}
