- 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);
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);
cv::Mat vec_c = vec_a.cross(vec_b);
cv::normalize(vec_c,vec_c);
cv::transpose(vec_c, vec_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
- 从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();
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);