c_cpp 如何从3D矢量计算轴旋转或旋转矩阵

Posted 2021-05-14

// Vector alignment rotation matrix computation from:
//      https://math.stackexchange.com/a/476311/311746
void axisRotationFromVector(const cv::Vec3f &_vec, cv::Vec3f *rot,
                           const bool normalize = false) {
    cv::Vec3f vec = _vec;
    if (normalize) {
        double l2 = cv::norm(vec);
        vec /= l2;
    }

    const cv::Vec3f refVec(0, 0, 1);

    cv::Vec3f v = refVec.cross(vec);  // cross product

    double s = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);

    double c = refVec.dot(vec);

    cv::Matx33f vx(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0);

    cv::Matx33f R;
    cv::add(cv::Mat::eye(3, 3, CV_32F), vx, R);
    cv::Matx33f tmp = (vx * vx) * (1/(1+c));
    cv::add(R, tmp, R);

    cv::Rodrigues(R, *rot);

//    cv::Mat rotVector(1, 3, CV_32F);
//    rotVector.at<float>(0,0) = -vec[1];
//    rotVector.at<float>(0,1) =  vec[0];
//    rotVector.at<float>(0,2) =  0;
//    cv::Mat rotMat;
//    cv::Rodrigues(rotVector, rotMat);

//    (*rot)[1]  = asin (-rotMat.at<float>(2,0));
//    (*rot)[0]  = atan2( rotMat.at<float>(2,1),
//                        rotMat.at<float>(2,2));
//    (*rot)[2]  = atan2( rotMat.at<float>(1,0),
//                        rotMat.at<float>(0,0));
}