Re: [ros-users] rotation problem on published tf links relat…

int getQuaternion(const cv::Mat_<double> &rvec, cv::Mat_<double> &quat) {
    cv::Mat_<double> R(3, 3);
    cv::Rodrigues(rvec, R);

    if ((quat.rows == 4) && (quat.cols == 1)) {
        //Mat size OK
    } else {
        quat = cv::Mat_<double>::eye(4,1);
    }
    double   w;

    w = R(0,0) + R(1,1)+ R(2,2) + 1;
    if ( w < 0.0 ) return 1;

    w = sqrt( w );
    quat(0,0) = (R(2,1) - R(1,2)) / (w*2.0);
    quat(1,0) = (R(0,2) - R(2,0)) / (w*2.0);
    quat(2,0) = (R(1,0) - R(0,1)) / (w*2.0);
    quat(3,0) = w / 2.0;
    return 0;
}