四元数插值常见的有线性插值、球面线性插值等
线性插值(Linear Interpolation,简称Lerp)
四元数表示旋转时是单位四元数,这种插值方式,相当于我们是沿着一条直线(也就是圆上的一个弦)进行插值的,这样插值出来的四元数不是单位四元数
归一化线性插值(Normalized LinearInterpolation,简称Nlerp)
有其他问题的。如下图所示,在同等时间内, vt 扫过的⻆度是不同的, vt 扫过的速度(或者说⻆速度)首先会不断地增加,到t = 0.50之后会开始减速,所以Nlerp插值不能保证均匀的⻆速度。
球面线性插值(Spherical Linear Interpolation,简称Slerp)
为了解决这个问题,我们可以转而对⻆度进行线性插值。这就要使用更复杂一些的插值方法了,比如常用的球面线性插值(Spherical Linear Interpolation),简称Slerp。Slerp插值可以解决前面的均匀角速度问题,它能够保证每两个四元数之间的⻆速度是固定的,这就从原理上保证了插值的效果。如下图所示,如果 v1 和 v2 之间的夹⻆为 w,那么:
V
t
=
K
0
V
0
+
K
1
V
1
V_t=K_0V_0+K_1V_1
Vt=K0V0+K1V1
推导比较简单,利用三角形、三角函数性质可得:
简化版求解方法
实践
/****************************
* 题目:四元数球面线性插值
* 我们用智能手机采集了图像序列和IMU数据,由于IMU帧率远大于图像帧率,需要你用Slerp方法进行四元数插值,使得插值后的IMU和图像帧对齐
* 已知某帧图像的时间戳为:t =700901880170406,离该图像帧最近的前后两个时刻IMU时间戳为:
* t1 = 700901879318945,t2 = 700901884127851
* IMU在t1, t2时刻测量得的旋转四元数为:
* q1x=0.509339, q1y=0.019188, q1z=0.049596, q1w=0.858921;
* q2x=0.509443, q2y=0.018806, q2z=0.048944,q2w=0.858905
* 根据上述信息求IMU对齐到图像帧的插值后的四元数
****************************/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
using namespace Eigen;
// 四元数球面线性插值简化方法:v'=v1*cosθ' + v⊥*sinθ',原理见公众号推送文章
Quaterniond slerp(double t, Quaterniond &q1, Quaterniond &q2)
{
// ---- 开始你的代码 ----- //
//归一化四元数
q1.normalize();
q2.normalize();
//四元数变为Mat: 方法 1
// double q1_array[4]={q1.w(),q1.x(),q1.y(),q1.z()};
// double q2_array[4]={q2.w(),q2.x(),q2.y(),q2.z()};
// Mat q1_Mat(4,1,CV_64F,q1_array);
// Mat q2_Mat(4,1,CV_64F,q2_array);
// 四元数变为Mat: 方法 2
Mat q1_Mat=(Mat_<double>(4,1)<<q1.w(),q1.x(),q1.y(),q1.z());
Mat q2_Mat=(Mat_<double>(4,1)<<q2.w(),q2.x(),q2.y(),q2.z());
double dotProd=q1_Mat.dot(q2_Mat);
double norm2=norm(q1_Mat,NORM_L2)*norm(q2_Mat,NORM_L2);
double costTheta=dotProd/norm2;
Mat result;
Quaterniond result_quat;
if(costTheta>0.9995f){
result=(1.0f-t)*q1_Mat+t*q2_Mat;
}
else
{
double theta=acosf(costTheta);
double thetaT=theta*t;
Mat qperp=q2_Mat-cos(theta)*q1_Mat;
//Mat qperp=q2_Mat-costTheta*q1_Mat;
result=q1_Mat*cos(thetaT)+qperp*sin(thetaT);
}
result=result/norm(result,NORM_L2);
// Mat 转化为四元数
result_quat.w() = result.at<double>(0, 0);
result_quat.x() = result.at<double>(1, 0);
result_quat.y() = result.at<double>(2, 0);
result_quat.z() = result.at<double>(3, 0);
return result_quat;
// ---- 结束你的代码 ----- //
}
int main ( int argc, char** argv )
{
double t_img(700901880170406), t1_imu(700901879318945), t2_imu(700901884127851);
Quaterniond q1 = Quaterniond(0.858921, 0.509339, 0.019188, 0.049596);
Quaterniond q2 = Quaterniond(0.858905, 0.509443, 0.018806, 0.048944);
double t = (t_img - t1_imu) / (t2_imu - t1_imu);
Quaterniond q_slerp = slerp(t, q1, q2);
cout<<"插值后的四元数:q_slerp =\n"<< q_slerp.coeffs() <<endl; //coeffs的顺序是(x,y,z,w)
return 0;
}