我需要以四元数表示的角速度,以便在 OpenCV 中使用以下表达式更新每帧的四元数:
q(k)=q(k-1)*qwt;
我的角速度是
Mat w; //1x3
我想获得角度的四元数形式
Mat qwt; //1x4
我找不到这方面的信息,有什么想法吗?
我需要以四元数表示的角速度,以便在 OpenCV 中使用以下表达式更新每帧的四元数:
q(k)=q(k-1)*qwt;
我的角速度是
Mat w; //1x3
我想获得角度的四元数形式
Mat qwt; //1x4
我找不到这方面的信息,有什么想法吗?
如果我理解正确,您想从这个Axis Angle 形式传递到 quaternion。
如链接所示,首先您需要计算角速度的模块(乘以帧之间的 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;
}
}
一个小技巧可以摆脱那些 cos 和 sin 函数。四元数 q(t) 的时间导数为:
dq(t)/dt = 0.5 * x(t) * q(t)
其中,如果角速度为 {w0, w1, w2},则 x(t) 是 {0, w0, w1, w2} 的四元数。请参阅 David H Eberly 的书第 10.5 节以获得证明