在OpenCV中将角速度转换为四元数

Question

我需要表示为四元数的角速度,以便在OpenCV中使用以下表达式更新每一帧的四元数:

q(k)=q(k-1)*qwt;

我的角速度是

Mat w;  //1x3

我想获得四角形的角度

Mat qwt;   //1x4

我无法找到有关此信息的任何想法？

Answer 1

如果我理解正确,你想从这个Axis Angle形式传递给四元数.

如链接所示,首先需要计算角速度的模块(乘以帧之间的delta(t)),然后应用公式.

这样的示例函数将是

// w is equal to angular_velocity*time_between_frames
void quatFromAngularVelocity(Mat& qwt,const Mat&w)
{
    const float x = w.at<float>(0);
    const float y = w.at<float>(1);
    const float z = w.at<float>(2);
    const float angle = sqrt(x*x + y*y + z*z);  //module of angular velocity

    if (angle > 0.0) //the formulas from the link
    {
        qwt.at<float>(0)= x*sin(angle/2.0f)/angle;
        qwt.at<float>(1)= y*sin(angle/2.0f)/angle;
        qwt.at<float>(2)= z*sin(angle/2.0f)/angle;
        qwt.at<float>(3)= cos(angle/2.0f);
    }else    //to avoid illegal expressions
    {
        qwt.at<float>(0)=qwt.at<float>(0)=qwt.at<float>(0)=0.0f;
        qwt.at<float>(3)=1.0f;
    }
}