四元素插值

SLAM 同时被 2 个专栏收录
20 篇文章 3 订阅
11 篇文章 2 订阅

四元数插值常见的有线性插值、球面线性插值等

线性插值(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;
}

在这里插入图片描述

  • 0
    点赞
  • 2
    评论
  • 2
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

©️2020 CSDN 皮肤主题: 编程工作室 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值