位姿矩阵和四元数在OpenCV、Eigen和Sophus下的表示与转换

1. OpenCV的矩阵和四元数
   opencv的四元数使用需要添加头文件<opencv2/core/quaternion.hpp>，好像4.5.1之后才有四元数

	// 随机生成一个位姿矩阵
	// 随机生成一个旋转矩阵（单位正交矩阵）
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> gauss_dis{0,1};
    // 先随机生成一个轴,并单位化
    std::vector<double> vec_rand = {gauss_dis(gen), gauss_dis(gen), gauss_dis(gen)};
    cv::Mat vec_a(vec_rand);
    cv::normalize(vec_a, vec_a);
    cv::transpose(vec_a, vec_a);

    // 再生成一个垂直于它的轴, 直接绕z轴90度旋转得到一个新向量
    double pi = std::acos(-1.0);
    vec_rand = {
            vec_a.at<double>(0,0)*std::cos(pi/2)-vec_a.at<double>(1,0)*std::sin(pi/2),
            vec_a.at<double>(0,0)*std::sin(pi/2)+vec_a.at<double>(1, 0)*std::cos(pi/2),
            vec_a.at<double>(2,0)
    };
    cv::Mat vec_b(vec_rand);
    cv::normalize(vec_b, vec_b);
    cv::transpose(vec_b, vec_b);

    // 再用vec_a和vec_b的叉积得到一个新向量
    cv::Mat vec_c = vec_a.cross(vec_b);
    cv::normalize(vec_c,vec_c);
    cv::transpose(vec_c, vec_c);

    // 将vec a,b,c组装成旋转矩阵
    cv::Mat r;
    cv::hconcat(vec_a,vec_b,r);
    cv::hconcat(r,vec_c,r);
    
    // 生成一个随机平移向量
    vec_rand = {gauss_dis(gen), gauss_dis(gen), gauss_dis(gen)};
    cv::Mat vec_t(vec_rand);

    // 组装一个位姿
    cv::Matx44d pose;
    cv::hconcat(r, vec_t, pose);
    cv::vconcat(pose, cv::Mat::zeros(1,4, CV_64FC1), pose);

    cv::Quatd pose_quatd(r);        // 从旋转矩阵创建四元数
    pose_quatd.createFromAngle

	// 四元数有多种创建方式
	pose_quatd.createFromAngleAxis();
    pose_quatd.createFromEulerAngles();
    pose_quatd.createFromRotMat();
    pose_quatd.createFromRvec();
    pose_quatd.createFromXRot();
    pose_quatd.createFromYRot();
    pose_quatd.createFromZRot();j

2. 从OpenCV的矩阵和四元数转换到Eigen的矩阵和四元数，Eigen下的四元数和矩阵的相互转换，以及Eigen中的矩阵和Sophus中李群和李代数的相应转换。
   可以用函数cv::eigen2cv和cv::cv2eigen进行从OpenCV的Mat与Eigen的Matrix之间的转换,这个转换需要包含头文件<opencv2/core/eigen.hpp>，

// 将
    // 提取一个旋转矩阵
    Eigen::Matrix3d r_eigen = pose_eigen.block(0,0,3,3);
    Eigen::Quaterniond quat_eigen(r_eigen);     // 从旋转矩阵创建四元数
    r_eigen = quat_eigen.toRotationMatrix();
    Eigen::Vector4d quat_coeffs = quat_eigen.coeffs();  // 获取四元数的四个系数,xyzw


    // 从Eigen转换到Sophus
    // 要包含<sophus/se3.hpp>头文件
    Sophus::SO3d SO3_R(r_eigen);    // 从旋转矩阵到李群
    Sophus::SO3d SO3_q(quat_eigen); // 从四元数到李群 
    SO3_q.setQuaternion(quat_eigen);
    
    Sophus::Vector3d so3 = SO3_R.log(); // 从李群获得相应的李代数
    Sophus::Matrix3d skew_sys_matrix = Sophus::SO3d::hat(so3);  // 获得李代数的反对称矩阵形式
    so3 = Sophus::SO3d::vee(skew_sys_matrix);       // 将李代数从反对称矩阵变成向量形式