李代數和旋轉矩陣及變換矩陣的轉換關系

李代數可以與李群相互轉化

Sophus::SO3 SO3;
Eigen::Vector3d so3 = SO3.log();
Sophus::SO3 SO3_R = Sophus::SO3::exp( so3 );

但是Sophus::SO3 SO3;不能用於表示旋轉矩陣，實際上他是旋轉矩陣的向量形式，轉化為旋轉矩陣要通過

    // 使用對數映射獲得它的李代數
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 為向量到反對稱矩陣
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相對的，vee為反對稱到向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose純粹是為了輸出美觀一些

同樣的對於SE3,

cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

SE3_updated.matrix()並不是變化矩陣T,要通過hat得到

   // 對SE(3)操作大同小異
    Eigen::Vector3d t(1,0,0);           // 沿X軸平移1
    Sophus::SE3 SE3_Rt(R, t);           // 從R,t構造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 從q,t構造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代數se(3) 是一個六維向量，方便起見先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 觀察輸出，會發現在Sophus中，se(3)的平移在前，旋轉在后.
    // 同樣的，有hat和vee兩個算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
    
    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;