SLAM十四讲：第三讲习题

三维空间刚体的运动

Eigen练习
Eigen几何模块
习题答案
- 习题1 验证旋转矩阵是正交矩阵
- 习题2 Rodrigues' Rotation Formula证明^{[2, 3]}
- 习题3 四元数旋转某个点后，结果是一个虚四元数
- 习题4
- 习题5 用eigen3取一个大矩阵左上角的小$3\times3$矩阵^[4]
- 习题6 一般线性方程的解法
- 习题7 求不同姿态下的坐标。
文献

Eigen练习

#include<iostream>
#include<ctime>
#include<Eigen/Core>
#include<Eigen/Dense>using namespace Eigen;
using namespace std;int main(){Matrix<double,2,3> M23;//comman define;Vector3d V3;//3X1 vector;Matrix3d M33=Matrix3d::Zero();//3X3 matrix, and initialize with 0;Matrix<double, Dynamic,Dynamic> Mtest(3,3);MatrixXd Mtest1(3,3);//float size matrices 3X3 Mtest1 and Mtest;Mtest <<1,2,3,4,5,6,7,8,9;cout<<Mtest<<endl;//It can print the whole Matrix;cout<<M33(0,0)<<endl;//(row,col);//Operation:M33=Matrix3d::Random();//elements of Random between (-1,1)cout<<M33<<endl<<endl;Mtest1=M33*Mtest;cout<<Mtest1<<endl<<endl;//Matrices multiply;cout<<M33.transpose()<<endl<<endl;//transposecout<<M33.sum()<<endl<<endl;//sum of all elements;cout<<M33.trace()<<endl<<endl;//trace//cout<<M33.inverse()//inversecout<<M33.determinant()<<endl<<endl;//determinantSelfAdjointEigenSolver<Matrix3d> eigen_solver(M33.transpose()*M33);//M33.transpose()*M33 is normal matrix.cout<<eigen_solver.eigenvalues()<<endl<<endl;cout<<eigen_solver.eigenvectors()<<endl<<endl;//solve equation set.MatrixXd matrixNN=MatrixXd::Random(50,50);VectorXd v_N=VectorXd::Random(50);clock_t c1=clock();auto x=v_N;x=matrixNN.inverse()*v_N;cout<<"Time:"<<1000*(clock()-c1)/(double)CLOCKS_PER_SEC<<"ms"<<endl;//QRc1=clock();auto v=v_N;v=matrixNN.colPivHouseholderQr().solve(v_N);cout<<"Time:"<<1000*(clock()-c1)/(double)CLOCKS_PER_SEC<<"ms"<<endl;x=x-v;auto c=x.sum();cout<<c<<endl;return 0;
}

Eigen几何模块

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;
using namespace Eigen;int main(int argc,char** argv){Matrix3d testMatrix=Matrix3d::Identity();AngleAxisd rotationV(M_PI_4,Vector3d (0,0,1));//Set a rotation vector: rotate vector pi/4 around Z;cout.precision(3);//Set cout precision;cout<<"Rotation matrix:\n "<<rotationV.matrix()<<endl;cout<<"Rotation matrix:\n "<<rotationV.toRotationMatrix()<<endl;testMatrix=rotationV.matrix();//AngleAxis rotates (1,0,0);Eigen::Vector3d v ( 1,0,0 );v<<1,0,0;Eigen::Vector3d v_rotated = rotationV * v;cout<<"(1,0,0) after rotation = "<<v_rotated<<endl;v_rotated=rotationV.matrix()*v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;//Matrix into Euler angle ZYX;Vector3d eulerAngle=testMatrix.eulerAngles(2,1,0);cout<<"Euler angle (yaw, pitch ,roll )"<<eulerAngle.transpose()<<endl;//Euler transformed matrixIsometry3d T=Isometry3d::Identity();//Isometry3d is a 4X4 matrix!T.rotate(rotationV);T.pretranslate(Vector3d(1,3,4));cout<<"Transform matrix=\n"<<T.matrix()<<endl;//T*vector3d==R*vector3d+pretranslate(t);Vector3d V_T=T*v;cout<<V_T<<endl;//Quaternion//Quaternion can be transformed form AngleAxisd;Quaterniond q=Quaterniond(rotationV);cout<<"Quaternion q ="<<q.coeffs().transpose()<<endl;//q.coeffs=(x*i,y*j,z*k,w);//Quaternion can be transformed from rotation matrix;q=Quaterniond(testMatrix);cout<<"Quaternion q ="<<q.coeffs().transpose()<<endl;//Rotate v with quaternion;V_T=q*v;//In math it's formed q*V*q^(-1);cout<<"Rotation: after multiply (1,0,0): "<<V_T.transpose()<<endl;return 0;
}

习题答案

习题1 验证旋转矩阵是正交矩阵

旋转矩阵定义：旋转矩阵乘以一个向量，只改变向量的方向不改变其大小，即等价于两个向量的内积不变(因为两向量的夹角不变)，所以有：
α ⃗ ⋅ β ⃗ = R α ⃗ ⋅ R β ⃗ = ( R ⋅ α ⃗ ) T R β ⃗ = α ⃗ T R T R β ⃗ \vec \alpha \cdot \vec \beta =R\vec \alpha \cdot R \vec \beta=(R\cdot \vec \alpha)^T R \vec \beta=\vec \alpha ^T R^TR\vec \beta α ⋅β =Rα ⋅Rβ =(R⋅α )TRβ =α TRTRβ
所以即有 R T R = I = R − 1 R R^TR=I=R^{-1}R RTR=I=R−1R，所以 R R R是正交矩阵。

习题2 Rodrigues’ Rotation Formula证明^{[2, 3]}

Rodrigues’ Rotation Formula在Wikipedia的第一种证明比较详细了，我就不再贴上面了。
指数扩展证明方法《视觉SLAM十四讲》第4讲上面也有证明。

习题3 四元数旋转某个点后，结果是一个虚四元数

设 p = ( 0 , x ⃗ ) p=(0,\vec x) p=(0,x )为坐标点 p p p， q = ( q 0 , y ⃗ ) q=(q_0,\vec y) q=(q0,y )为一个旋转，所以有：
p ′ = q p q − 1 = q p q ∗ ∣ ∣ q ∣ ∣ = ( − x ⃗ ⋅ y ⃗ , q 0 x ⃗ + x ⃗ × y ⃗ ) ⋅ ( q 0 , − y ⃗ ) ∣ ∣ q ∣ ∣ p^\prime=qpq^{-1}=\frac {qpq^*}{||q||}=\frac{(-\vec x \cdot \vec y, q_0\vec x+\vec x\times \vec y)\cdot (q_0,-\vec y)}{||q||} p′=qpq−1=∣∣q∣∣qpq∗=∣∣q∣∣(−x ⋅y ,q0x +x ×y )⋅(q0,−y )
计算得到：
p ′ = ( 0 , y ⃗ T x ⃗ ⋅ y ⃗ + q 0 2 x ⃗ q 0 2 + ∣ ∣ y ⃗ ∣ ∣ 2 ) p^\prime=(0,\frac{\vec y^T \vec x \cdot \vec y+q_0^2\vec x}{\sqrt{q_0^2+||\vec y||^2}}) p′=(0,q02+∣∣y ∣∣2 y Tx ⋅y +q02x )

习题4

略

习题5 用eigen3取一个大矩阵左上角的小 3 × 3 3\times3 3×3矩阵^[4]

程序如下：

#include<iostream>
#include<Eigen/Core>using namespace std;
using namespace Eigen;int main(int argc,char** argv){MatrixXd test;test=MatrixXd::Zero(10,10);Matrix3d test2;test2=test.block(0,0,3,3);//test.block(i,j,p,q):begin(i,j),size(p,q);test2=Matrix3d::Identity();cout<<test2<<endl;return 0;
}

习题6 一般线性方程的解法

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>
using namespace std;
using namespace Eigen;
int main(){MatrixXd A=MatrixXd::Random(10,10);VectorXd v=VectorXd::Random(10);VectorXd x=v;cout<<v.transpose()<<endl;x=A.ldlt().solve(v);cout<<x.transpose()<<endl;x=A.llt().solve(v);cout<<x.transpose()<<endl;//Both .ldlt() and .llt() are inlcuded in <Eigen/Cholesky>//Only for positive definite matrices.x=A.lu().solve(v);  //Lower-Uppwer decomposition Stable and fast.// #include <Eigen/LU>cout<<x.transpose()<<endl;x=A.colPivHouseholderQr().solve(v);//QR decompositioncout<<x.transpose()<<endl;JacobiSVD<MatrixXd> svd(A,ComputeThinU|ComputeThinV);cout<<svd.singularValues().transpose()<<endl;cout<<"U:"<<endl<<svd.matrixU()<<endl;cout<<"V:"<<endl<<svd.matrixV()<<endl;//SVD Stable, slowest. #include <Eigen/SVD>x=svd.solve(v);cout<<x.transpose()<<endl;return 0;
}

LLT分解：
Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零，故分解的下三角的对角元也是大于零的（LU三角分解法的变形）。
LDLT分解法：
若A为一对称矩阵且其任意一k阶主子阵均不为零，则A有如下惟一的分解形式：
其中L为一下三角形单位矩阵（即主对角线元素皆为1），D为一对角矩阵（只在主对角线上有元素，其余皆为零），L^T为L的转置矩阵。
LDLT分解法实际上是Cholesky分解法的改进，因为Cholesky分解法虽然不需要选主元，但其运算过程中涉及到开方问题，而LDLT分解法则避免了这一问题，可用于求解线性方程组。
llt和ldlt分解是针对正定实对称矩阵成立的，一般的矩阵用这个方法是不能求解的。

习题7 求不同姿态下的坐标。

令 p p p点在一号的齐次坐标为 p c 1 p_{c1} pc1，世界坐标为 p w p_w pw，在二号的齐次坐标为 p c 2 p_{c2} pc2。
计算如下：
p c 2 = T 2 ⋅ p w = T 2 ⋅ ( T 1 − 1 ⋅ p c 1 ) p_{c2}=T_2 \cdot p_w=T_2 \cdot (T_1^{-1}\cdot p_{c1}) pc2=T2⋅pw=T2⋅(T1−1⋅pc1)
程序如下：

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<Eigen/QR>using namespace std;
using namespace Eigen;int main(int argc,char** argv){Quaterniond q1(0.35,0.2,0.3,0.1);//(w,x,y,z);//Constructor of Quternion://Quaternion (const Scalar &w, const Scalar &x, //const Scalar &y, const Scalar &z)//But when it was stored, Quaternion stores as (const Scalar &x,//const Scalar &y, const Scalar &z,const Scalar &w,) cout<<"(x,y,z,w):"<<q1.coeffs().transpose()<<endl;q1.normalize();//Normalize quaternion.Vector3d t1(0.3,0.1,0.1);Isometry3d T1;T1.rotate(q1);T1.pretranslate(t1);//--------------------------------------Quaterniond q2(-0.5,0.4,-0.1,0.2);q2.normalize();Vector3d t2(-0.1,0.5,0.3);Isometry3d T2;T2.rotate(q2);T2.pretranslate(t2);Vector3d pc1(0.5,0,0.2);Vector3d pc2=T2*T1.inverse()*pc1;cout<<pc2.transpose()<<endl;
}

注意：
Eigen中quaternion的构造函数为：

 Quaternion (const Scalar &w, const Scalar &x,const Scalar &y,const Scalar &z)

注意 w w w在前。然而在内部存储时eigen将四元数的 w w w放在最后。
例如通过Eigen::Vector4d q = Q.coeffs();访问时， q q q中的最后一个元素才是 w w w。

文献

[1] Wikipedia, “Cross product”:https://en.wikipedia.org/wiki/Cross_product
[2] Wikipedia, “Rodrigues’ Rotation Formula”:https://en.wikipedia.org/wiki/Rodrigues’_rotation_formula
[3] Wikipedia, “Rotation matrix”:https://en.wikipedia.org/wiki/Rotation_matrix
[4]子矩阵操作:https://www.cnblogs.com/yabin/p/6473654.html?utm_source=itdadao&utm_medium=referral