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

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

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;
}