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

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=
"<<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;

總結

以上是生活随笔為你收集整理的李代数和旋转矩阵及变换矩阵的转换关系的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。