位姿的插值怎么做

平移：线性插值
旋转：球面线性插值（slerp），可以调用Eigen的slerp()函数，示例代码：

Eigen::Quaterniond q1(w1,x1,y1,z1);
Eigen::Quaterniond q2(w2,x2,y2,z2);
double t = 0.5;
Eigen::Quanterniond q3 = q1.slerp(t,q2);

slerp函数具体公式如下：
$$\text{SLERP}(t,q_1,q_2)=\frac{\sin ((1-t)\theta )}{\sin (\theta )}{q}_1+\frac{\sin (t\theta )}{\sin (\theta )}{q}_2$$