c_cpp 从3D矢量的欧拉角度

Posted 2021-05-14

// Vector alignment rotation matrix computation from:
//      https://math.stackexchange.com/a/476311/311746
void eulerAnglesFromVector(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);
}