焦作 网站建设,做网站用域名不备案怎么弄,网站建设衣服,网站qq微信分享怎么做的前言 小编研究生的研究方向是视觉SLAM#xff0c;目前在自学#xff0c;本篇文章为初学高翔老师课的第二次作业。 文章目录 前言1.熟悉 Eigen 矩阵运算2.几何运算练习3.旋转的表达4.罗德里格斯公式的证明5.四元数运算性质的验证6.熟悉 C11 1.熟悉 Eigen 矩阵运算 设线性⽅程 …前言 小编研究生的研究方向是视觉SLAM目前在自学本篇文章为初学高翔老师课的第二次作业。 文章目录 前言1.熟悉 Eigen 矩阵运算2.几何运算练习3.旋转的表达4.罗德里格斯公式的证明5.四元数运算性质的验证6.熟悉 C11 1.熟悉 Eigen 矩阵运算 设线性⽅程 Ax b在 A 为⽅阵的前提下请回答以下问题 在什么条件下 x 有解且唯⼀⾼斯消元法的原理是什么QR 分解的原理是什么Cholesky 分解的原理是什么编程实现 A 为 100 × 100 随机矩阵时⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。 1.当r(A)r([A|b])n时也就是A满秩A可逆方程组有唯一解。
2.高斯消元法如图所示 3.QR的分解原理如图所示: 4.Cholesky分解的原理如下图 5. 编程实现 A 为 100 × 100 随机矩阵时⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。提⽰你可能需要参考相关的数学书籍或⽂章。请善⽤搜索引擎。 Eigen 固定⼤⼩矩阵最⼤⽀持到 50所以你会⽤到动态⼤⼩的矩阵。 其实这题就是让我们自己建立一个AXB利用Eigen的内置函数求解X 代码如下 #include iostream
using namespace std;
#include ctime
#include Eigen/Core
#include Eigen/Dense
using namespace Eigen;#define MATRIX_SIZE 100//宏定义int main(int argc, char **argv)
{//建立一个动态矩阵AMatrixXd A;//建立一个100*100的随机矩阵A A MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); //保证A为正定矩阵AA*A的转置正定A A * A.transpose(); //建立一个float类型的动态矩阵BMatrixdouble, Dynamic, 1 B; //建立一个100*1的随机矩阵BB MatrixXd::Random(MATRIX_SIZE, 1); //建立一个动态矩阵XMatrixdouble, Dynamic, 1 X;//建立一个100*1的随机矩阵X X MatrixXd::Random(MATRIX_SIZE, 1); //QR分解X A.colPivHouseholderQr().solve(B); cout QR: X X.transpose() endl;//cholesky分解X A.ldlt().solve(B); cout cholesky: X X.transpose() endl;return 0;
}运行结果如下
Eigen的语法规则及其用法参考如下
#include iostream
using namespace std;
#include ctime
// Eigen 部分
#include Eigen/Core
// 稠密矩阵的代数运算逆特征值等
#include Eigen/Dense#define MATRIX_SIZE 50/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/int main( int argc, char** argv )
{// Eigen 中所有向量和矩阵都是Eigen::Matrix它是一个模板类。它的前三个参数为数据类型行列// 声明一个2*3的float矩阵Eigen::Matrixfloat, 2, 3 matrix_23;// 同时Eigen 通过 typedef 提供了许多内置类型不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrixdouble, 3, 1即三维向量Eigen::Vector3d v_3d;// 这是一样的Eigen::Matrixfloat,3,1 vd_3d;// Matrix3d 实质上是 Eigen::Matrixdouble, 3, 3Eigen::Matrix3d matrix_33 Eigen::Matrix3d::Zero(); //初始化为零// 如果不确定矩阵大小可以使用动态大小的矩阵Eigen::Matrix double, Eigen::Dynamic, Eigen::Dynamic matrix_dynamic;// 更简单的Eigen::MatrixXd matrix_x;// 这种类型还有很多我们不一一列举// 下面是对Eigen阵的操作// 输入数据初始化matrix_23 1, 2, 3, 4, 5, 6;// 输出cout matrix_23 endl;// 用()访问矩阵中的元素for (int i0; i2; i) {for (int j0; j3; j)coutmatrix_23(i,j)\t;coutendl;}// 矩阵和向量相乘实际上仍是矩阵和矩阵v_3d 3, 2, 1;vd_3d 4,5,6;// 但是在Eigen里你不能混合两种不同类型的矩阵像这样是错的// Eigen::Matrixdouble, 2, 1 result_wrong_type matrix_23 * v_3d;// 应该显式转换Eigen::Matrixdouble, 2, 1 result matrix_23.castdouble() * v_3d;cout result endl;Eigen::Matrixfloat, 2, 1 result2 matrix_23 * vd_3d;cout result2 endl;// 同样你不能搞错矩阵的维度// 试着取消下面的注释看看Eigen会报什么错// Eigen::Matrixdouble, 2, 3 result_wrong_dimension matrix_23.castdouble() * v_3d;// 一些矩阵运算// 四则运算就不演示了直接用-*/即可。matrix_33 Eigen::Matrix3d::Random(); // 随机数矩阵cout matrix_33 endl endl;cout matrix_33.transpose() endl; // 转置cout matrix_33.sum() endl; // 各元素和cout matrix_33.trace() endl; // 迹cout 10*matrix_33 endl; // 数乘cout matrix_33.inverse() endl; // 逆cout matrix_33.determinant() endl; // 行列式// 特征值// 实对称矩阵可以保证对角化成功Eigen::SelfAdjointEigenSolverEigen::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的大小在前边的宏里定义它由随机数生成// 直接求逆自然是最直接的但是求逆运算量大Eigen::Matrix double, MATRIX_SIZE, MATRIX_SIZE matrix_NN;matrix_NN Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );Eigen::Matrix double, MATRIX_SIZE, 1 v_Nd;v_Nd Eigen::MatrixXd::Random( MATRIX_SIZE,1 );clock_t time_stt clock(); // 计时// 直接求逆Eigen::Matrixdouble,MATRIX_SIZE,1 x matrix_NN.inverse()*v_Nd;cout time use in normal inverse is 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC ms endl;// 通常用矩阵分解来求例如QR分解速度会快很多time_stt clock();x matrix_NN.colPivHouseholderQr().solve(v_Nd);cout time use in Qr decomposition is 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC ms endl;return 0;
}
2.几何运算练习 下⾯我们来练习如何使⽤ Eigen/Geometry 计算⼀个具体的例⼦。 设有⼩萝⼘ 1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为 q1 [0:55; 0:3; 0:2; 0:2]; t1 [0:7;1:1; 0:2]Tq 的第⼀项为实部。这⾥的 q 和 t 表达的是 Tcw也就是世界到相机的变换关系。⼩萝⼘⼆号的位姿为 q2 [−0:1; 0:3; −0:7; 0:2]; t2 [−0:1; 0:4;0:8]T。现在⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下坐标为 p1 [0:5; −0:1; 0:2]T求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事并提交你的程序。 提⽰ 四元数在使⽤前需要归⼀化。请注意 Eigen 在使⽤四元数时的虚部和实部顺序。参考答案为 p2 [1:08228; 0:663509; 0:686957]T。你可以⽤它验证程序是否正确。 #include iostream
using namespace std;
#include Eigen/Core
#include Eigen/Geometry
using namespace Eigen;
int main(int argc, char ** argv)
{//创建小萝卜1和2的位姿q1和q2Quaterniond q1(0.55, 0.3, 0.2, 0.2), q2(-0.1, 0.3, -0.7, 0.2);//四元数归一化(高博的SLAM视屏中的代码解释没有归一化)q1.normalize();q2.normalize();//创建小萝卜1和2的另一个位姿量t1和t2Vector3d t1(0.7, 1.1, 0.2), t2(-0.1, 0.4, 0.8);//创建p1坐标Vector3d p1(0.5, -0.1, 0.2); //欧式变换矩阵Tc1w和Tc2wIsometry3d Tc1w(q1), Tc2w(q2);Tc1w.pretranslate(t1);// 把平移向量设成(t1)Tc2w.pretranslate(t2);// 把平移向量设成(t2)//计算p2Vector3d p2 Tc2w*Tc1w.inverse()*p1;cout p2.transpose() endl;return 0;
}运行结果如下我们可以看到答案正确
Eigen/四元数的语法规则及其用法参考如下
#include iostream
#include cmath
using namespace std;
#include Eigen/Core
// Eigen 几何模块
#include Eigen/Geometry/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/int main ( int argc, char** argv )
{// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fEigen::Matrix3d rotation_matrix Eigen::Matrix3d::Identity();// 旋转向量使用 AngleAxis, 它底层不直接是Matrix但运算可以当作矩阵因为重载了运算符Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度cout .precision(3);coutrotation matrix \nrotation_vector.matrix() endl; //用matrix()转换成矩阵// 也可以直接赋值rotation_matrix rotation_vector.toRotationMatrix();// 用 AngleAxis 可以进行坐标变换Eigen::Vector3d v ( 1,0,0 );Eigen::Vector3d v_rotated rotation_vector * v;cout(1,0,0) after rotation v_rotated.transpose()endl;// 或者用旋转矩阵v_rotated rotation_matrix * v;cout(1,0,0) after rotation v_rotated.transpose()endl;// 欧拉角: 可以将旋转矩阵直接转换成欧拉角Eigen::Vector3d euler_angles rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序即roll pitch yaw顺序coutyaw pitch roll euler_angles.transpose()endl;// 欧氏变换矩阵使用 Eigen::IsometryEigen::Isometry3d TEigen::Isometry3d::Identity(); // 虽然称为3d实质上是44的矩阵T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)cout Transform matrix \n T.matrix() endl;// 用变换矩阵进行坐标变换Eigen::Vector3d v_transformed T*v; // 相当于R*vtcoutv tranformed v_transformed.transpose()endl;// 对于仿射和射影变换使用 Eigen::Affine3d 和 Eigen::Projective3d 即可略// 四元数// 可以直接把AngleAxis赋值给四元数反之亦然Eigen::Quaterniond q Eigen::Quaterniond ( rotation_vector );coutquaternion \nq.coeffs() endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部前三者为虚部// 也可以把旋转矩阵赋给它q Eigen::Quaterniond ( rotation_matrix );coutquaternion \nq.coeffs() endl;// 使用四元数旋转一个向量使用重载的乘法即可v_rotated q*v; // 注意数学上是qvq^{-1}cout(1,0,0) after rotation v_rotated.transpose()endl;return 0;
}3.旋转的表达 1. 设有旋转矩阵 R证明 RT R I 且 det R 12。 2. 设有四元数 q我们把虚部记为ε实部记为 η那么 q (εη)。请说明 ε 和 η 的维度。 四元数q有三个虚部和一个实部。 即qq0q1iq2jq3k 因此ε的维度为3 η的维度为1。 3.第三问
4.罗德里格斯公式的证明
罗德⾥格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为 θ⽅向为 n那么旋转矩阵 R 为 R cos θI − (1 − cos θ)nnT sin θn^. ------------------------------------------------------------(4) 我们在课程中仅指出了该式成⽴但没有给出证明。请你证明此式。 参考 链接: 罗德里格斯公式的证明
5.四元数运算性质的验证 6.熟悉 C11 请注意本题为附加题。 C 是⼀门古⽼的语⾔但它的标准⾄今仍在不断发展。在 2011 年、 2014 年和 2017 年 C的标准又进⾏了更新被称为 C11 C14 C17。其中 C11 标准是最重要的⼀次更新让C发⽣了重要的改变也使得近年来的 C 程序与你在课本上⽐如谭浩强学到的 C 程序有很⼤的不同。你甚⾄会惊叹这是⼀种全新的语⾔。 C14 和 C17 则是对 11 标准的完善与扩充。 越来越多的程序开始使⽤11 标准它也会让你在写程序时更加得⼼应⼿。本题中你将学习⼀些 11标准下的新语法。请参考本次作业 books/⽬录下的两个pdf并回答下⾯的问题。 设有类 A并有 A 类的⼀组对象组成了⼀个 vector。现在希望对这个 vector进⾏排序但排序的⽅式由 A.index 成员⼤⼩定义。 那么在 C11 的语法下程序写成 1#include iostream
2#include vector
3#include algorithm
4
5 using namespace std;
6
7 class A {
8 public:
9 A(const int i ) : index(i) {}
10 int index 0;
11};
12
13 int main() {
14 A a1(3), a2(5), a3(9);
15 vectorA avec{a1, a2, a3};
16 std::sort(avec.begin(), avec.end(), [](const Aa1, const Aa2) {return a1.indexa2.index;});
17 for ( auto a: avec ) couta.index ;
18 coutendl;
19 return 0;
20 }请说明该程序中哪些地⽅⽤到了 C11 标准的内容。提⽰请关注范围 for 循环、⾃动类型推导、 lambda表达式等内容。
该程序中使用了C11标准的以下内容
iostream使用了C11中引入的iostream库用于输入输出流操作。
vector使用了C11中引入的vector容器用于存储和操作A类的实例。
algorithm使用了C11中引入的algorithm库其中的sort函数用于对vector容器中的元素进行排序。
using namespace std;使用了C11中引入的namespace别名声明用于简化对std命名空间的使用。
class A使用了C11中引入的类初始化列表语法用于对A类的成员变量进行初始化。
vector avec{a1, a2, a3}; 使用了C11中引入的列表初始化语法用于初始化vector容器avec并添加元素。
[] (const Aa1, const Aa2) { return a1.index a2.index; }使用了C11中引入的lambda表达式用作sort函数的排序准则。其中const Aa1, const Aa2是参数列表return a1.indexa2.index;是函数体返回值是布尔型的大小比较结果。
for (auto a : avec)使用了C11中引入的范围for循环语法用于遍历vector容器avec中的元素。用auto关键字实现了自动类型推导让编译器自动设置变量a的类型C引入了基于范围的for循环不用下标就能访问元素
