sophus库 so3三维向量在不适用sophus库情况下转换到旋转矩阵

最新推荐文章于 2024-01-25 11:06:40 发布

zhengshunkai

最新推荐文章于 2024-01-25 11:06:40 发布

阅读量1k

点赞数 1

CC 4.0 BY-SA版权

分类专栏：随手记文章标签： sophus so3 罗德里格斯旋转矩阵

本文链接：https://blog.youkuaiyun.com/zhengshunkai/article/details/97787663

随手记专栏收录该内容

6 篇文章

订阅专栏

本文介绍了一种使用罗德里格斯(Rodrigues)公式将角度轴表示转换为旋转矩阵的方法。通过具体的C++ Eigen库代码示例，详细展示了如何基于输入的更新向量计算旋转矩阵。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

实际上就是使用罗德里格斯公式进行转换:
代码如下

Eigen::Matrix3d calR(const Eigen::Matrix<double,6,1>& update)
{
    //cos(the)*I + (1-cos(the))*ano*anot+sin(the)*anohat
    //omega = the * ano
    Eigen::Vector3d omega = update.tail<3>();
    double theta = omega.norm();
    Eigen::Vector3d ano = omega /theta;
    Eigen::Matrix3d aat;
    double a = ano[0];
    double b = ano[1];
    double c = ano[2];
    aat <<  a*a, a*b, a*c
            ,a*b, b*b, b*c
            ,a*c,b*c, c*c;
    Eigen::Matrix3d ahat = hatmat(ano);
    Eigen:: Matrix3d V= Eigen::Matrix3d::Identity() * cos(theta)+
                       (1-cos(theta))*aat+
                       sin(theta)*ahat;
    return V;
}