Representation and conversion of pose matrix and quaternion under OpenCV, Eigen and Sophus

1. OpenCV's matrix and quaternion
   The use of opencv's quaternion needs to add the header file <opencv2/core/quaternion.hpp>, it seems that there are quaternions only after 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. Convert from OpenCV's matrix and quaternion to Eigen's matrix and quaternion, the mutual conversion of quaternion and matrix under Eigen, and the corresponding conversion of matrix in Eigen and Lie group and Lie algebra in Sophus.
   You can use the functions cv::eigen2cv and cv::cv2eigen to convert between OpenCV's Mat and Eigen's Matrix. This conversion needs to include the header file <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);       // 将李代数从反对称矩阵变成向量形式