SLAM十四讲：第三讲习题

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;
    //transpose
    cout<<M33.sum()<<endl<<endl;
    //sum of all elements;
    cout<<M33.trace()<<endl<<endl;
    //trace
    //cout<<M33.inverse()
    //inverse
    cout<<M33.determinant()<<endl<<endl;
    //determinant
    SelfAdjointEigenSolver<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;
    //QR
    c1=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 matrix
    Isometry3d 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