slam课程第二讲作业eigen

#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/Householder>
using namespace std;
using namespace Eigen;
int main()
{
    Matrix<double, Dynamic,Dynamic> matrix_dynamic;
    Matrix<double, Dynamic,Dynamic> matrix_dynamic_d;
    matrix_dynamic = MatrixXd::Random(100,100);
    matrix_dynamic_d = matrix_dynamic.transpose()*matrix_dynamic;
    //cout << "matrix_100 = " << matrix_dynamic << endl;
    Matrix<double, Dynamic,Dynamic> y;
    y = MatrixXd::Random(100,1);
    Matrix<double, Dynamic,Dynamic> x;
    x = matrix_dynamic_d.colPivHouseholderQr().solve(y);
    Matrix<double, Dynamic,Dynamic> x1;
    x1 = matrix_dynamic_d.llt().solve(y);
    //cout << matrix_dynamic*x1 << endl;
    cout << "x = " << x << endl;
    cout << "x = " << x1 << endl;

    //  它要求矩阵的所有特征值必须大于零


    return 0;
}
cholesky分解，为了实现正定矩阵，特征值必须大于0.用到了A‘×A.

#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/Householder>

using namespace std;
int main()
{

    Eigen::Quaternionf q1 = Eigen::Quaternionf(0.35,0.2,0.3,0.1).normalized();
    Eigen::Quaternionf q2 = Eigen::Quaternionf(-0.5,0.4,-0.1,0.2).normalized();



    Eigen::Vector3f t1(0.3,0.1,0.1);
    Eigen::Vector3f t2(-0.1,0.5,0.3);
    Eigen::Isometry3f T1 = Eigen::Isometry3f::Identity();
    T1.rotate(q1);
    T1.pretranslate(t1);

    Eigen::Isometry3f T2 = Eigen::Isometry3f::Identity();
    T2.rotate(q2);
    T2.pretranslate(t2);

    Eigen::Vector4f p1(0.5,0.0,0.2,1);
    Eigen::Vector4f p2;
    Eigen::Vector4f pw;

    pw = T1.inverse()*p1;
    p2 = T2*pw;
    cout << "inverse P2 = " << p2 << endl;


    //QR fenjie
    Eigen::Matrix<float, 4,1> p11;
    Eigen::Matrix<float, 4,1> Pw;
    p11 << 0.5, 0.0, 0.2, 1.0;
    Pw = T1.matrix().colPivHouseholderQr().solve(p11);
    Eigen::Matrix<float, 4,1> P22;
    P22 = T2*Pw;
    cout << "QR P2 = " << P22 << endl;


    //Quatternind
    Eigen::Matrix<float ,3,1> quatation;
    Eigen::Vector3f PQ(Pw(0,0),Pw(1,0),Pw(2,0));
    quatation = q2*PQ+t2;
    cout << "quatation P2= " << quatation << endl;




    return 0;

}