空间中位姿矩阵的插补算法代码，旋转中采用四元数进行插补

//  五次多项式生成，用于后面的插补间距
void  Five_ploynomial(double t0, double tf, double q0, double qf, double d_q0, double d_qf, double dd_q0, double  dd_qf, VectorXd & qt)
{
	double A, B, C, D, E, F;

	A = (double(12.00 * (qf - q0)) - 6 * (d_qf + d_q0)*tf - (dd_q0 - dd_qf)*pow(tf, 2)) / (2 * pow(tf, 5));
	B = (double(-30 * (qf - q0) + (16 * d_q0 + 14 * d_qf)*tf + (3 * dd_q0 - 2 * dd_qf)*pow(tf, 2))) / (2 * pow(tf, 4));
	C = (double(20 * (qf - q0) - (12 * d_q0 + 8 * d_qf)*tf - (3 * dd_q0 - dd_qf)*pow(tf, 2))) / (2 * pow(tf, 3));
	D = dd_q0 / 2;
	E = d_q0;
	F = q0;

	double i = 0;
	for (double t = 0; t < tf + 0.5; t = t + 0.5)
	{
		qt(i) = A*pow(t, 5) + B*pow(t, 4) + C*pow(t, 3) + D*pow(t, 2) + E*t + F;
		i = i + 1;
	}

}

// 矩阵转四元数  ， 实部在前，虚部在后
void quaternion(Matrix4d &T, Vector4d &q_quaternion)
{
	Matrix3d R = Matrix3d::Zero();
	double w, x, y, z;

	R(0, 0) = T(0, 0);
	R(0, 1) = T(0, 1);
	R(0, 2) = T(0, 2);
	R(1, 0) = T(1, 0);
	R(1, 1) = T(1, 1);
	R(1, 2) = T(1, 2);
	R(2, 0) = T(2, 0);
	R(2, 1) = T(2, 1);
	R(2, 2) = T(2, 2);

	double trace = R(0, 0) + R(1, 1) + R(2, 2);
	double epsilon = 1E-12;
	if (trace > epsilon)
	{
		double s = 0.5 / sqrt(trace + 1.0);
		w = 0.25 / s;
		x = -(R(2, 1) - R(1, 2)) * s;
		y = -(R(0, 2) - R(2, 0)) * s;
		z = -(R(1, 0) - R(0, 1)) * s;

	}
	else
	{
		if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2))
		{
			double s = 2.0 * sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
			w = -(R(2, 1) - R(1, 2)) / s;
			x = 0.25 * s;
			y = (R(0, 1) + R(1, 0)) / s;
			z = (R(0, 2) + R(2, 0)) / s;

		}
		else if (R(1, 1) > R(2, 2))
		{
			double s = 2.0 * sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
			w = -(R(0, 2) - R(2, 0)) / s;
			x = (R(0, 1) + R(1, 0)) / s;
			y = 0.25 * s;
			z = (R(1, 2) + R(2, 1)) / s;
		}
		else
		{
			double s = 2.0 * sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
			w = -(R(1, 0) - R(0, 1)) / s;
			x = (R(0, 2) + R(2, 0)) / s;
			y = (R(1, 2) + R(2, 1)) / s;
			z = 0.25 * s;

		}

	}
	q_quaternion(0) = w;
	q_quaternion(1) = x;
	q_quaternion(2) = y;
	q_quaternion(3) = z;

}


// 四元数转旋转矩阵
void quatern2rotMat(Vector4d &q_quaternion, Matrix3d &R)
{

	R(0, 0) = 2.0000 * pow(q_quaternion(0), 2) - 1.0000 + 2.0000 * pow(q_quaternion(1), 2);
	R(0, 1) = 2.0000 * (q_quaternion(1)*q_quaternion(2) + q_quaternion(0)*q_quaternion(3));
	R(0, 2) = 2.0000 * (q_quaternion(1)*q_quaternion(3) - q_quaternion(0)*q_quaternion(2));
	R(1, 0) = 2.0000* (q_quaternion(1)*q_quaternion(2) - q_quaternion(0)*q_quaternion(3));
	R(1, 1) = 2.0000 * pow(q_quaternion(0), 2) - 1.0000 + 2.0000 * pow(q_quaternion(2), 2);
	R(1, 2) = 2.0000* (q_quaternion(2)*q_quaternion(3) + q_quaternion(0)*q_quaternion(1));
	R(2, 0) = 2.0000 * (q_quaternion(1)*q_quaternion(3) + q_quaternion(0)*q_quaternion(2));
	R(2, 1) = 2.0000 * (q_quaternion(2)*q_quaternion(3) - q_quaternion(0)*q_quaternion(1));
	R(2, 2) = 2.0000 * pow(q_quaternion(0), 2) - 1.0000 + 2.0000 * pow(q_quaternion(3), 2);

}

 

//四元数插值计算
void  qinterp(Vector4d &Q1, Vector4d & Q2, double r, Vector4d &q_quaternion_interpolation)
{

	double k0, k1, theta, sin_theta, cos_theta;
	Vector4d q_temp;
	if ((r<0) || (r>1))
		cout << " R out of rang" << endl;

	cos_theta = Q1(0)*Q2(0) + Q1(1)*Q2(1) + Q1(2)*Q2(2) + Q1(3)*Q2(3);

	if (cos_theta < 0)
	{
		Q2 = -Q2;
		cos_theta = -cos_theta;
	}



	if ((cos_theta) > 0.9999999999)
	{
		k0 = 1.00 - r;
		k1 = r;

	}
	else
	{
		sin_theta = sqrt(1.00 - pow(cos_theta, 2));
		theta = atan2(sin_theta, cos_theta);
		k0 = sin((1.000 - r)*theta) / sin(theta);
		k1 = sin((r)*theta) / sin(theta);

	}
	q_quaternion_interpolation = k0* Q1 + k1 * Q2;

}


// 两个矩阵之间插值计算
void trinterp(Matrix4d &T0, Matrix4d &T1, double r, Matrix4d &T)
{
	Vector4d q0, q1, qr;
	Vector3d p0, p1, pr;
	Matrix3d R, R0;


	quaternion(T0, q0);  // 位姿矩阵转换为四元数
	quaternion(T1, q1);



	p0 << T0(0, 3), T0(1, 3), T0(2, 3);       // 提取出位置矩阵
	p1 << T1(0, 3), T1(1, 3), T1(2, 3);

	qinterp(q0, q1, r, qr);   // 进行四元数插值
	pr = p0*(1 - r) + r*p1;      // 进行位置插值

	quatern2rotMat(qr, R);      // 四元数转旋转矩阵

	T(0, 0) = R(0, 0); T(0, 1) = R(0, 1); T(0, 2) = R(0, 2); T(0, 3) = pr(0);
	T(1, 0) = R(1, 0); T(1, 1) = R(1, 1); T(1, 2) = R(1, 2); T(1, 3) = pr(1);
	T(2, 0) = R(2, 0); T(2, 1) = R(2, 1); T(2, 2) = R(2, 2); T(2, 3) = pr(2);
	T(3, 0) = 0;       T(3, 1) = 0;       T(3, 2) = 0;       T(3, 3) = 1;
}

上述中的一些代码都是根据相应的函数公式进行相应的程序编写，
下面的是一些Eigen 库中的相应的库函数

    // 旋转矩阵转为四元数
     q = Eigen::Quaterniond(R);
	 //Eigen 库中四元数转化为旋转矩阵，
	 Eigen::Matrix3d R = Eigen::Matrix3d(q);
	 cout << "R=\n" << R << endl << endl;
	// 旋转轴和旋转向量转换为旋转矩阵
	Eigen::AngleAxisd rotationVector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
	Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity();
	rotationMatrix = rotationVector.toRotationMatrix();
	cout << "rotationMatrix \n" << rotationMatrix << endl;

// 采用Eigen 库 将旋转轴和旋转向量转换为 四元数，其中四元数 中的 虚部在前，室部在后
Eigen::Quaterniond q = Eigen::Quaterniond(rotationVector);
cout << "rotation quaternion \n" << q.coeffs() << endl;
// 各项输出 四元数中每一个数
cout << q.x() << "	" << q.y() << "	" << q.z() << "	" << q.w() << endl;
// 可以采用下面的方法对Eigen 库生成的四元数进行赋值
	//q.x() = 3;
	//cout << q.x() << endl;
	
   //将四元数转化为旋转向量
	 Eigen::AngleAxisd  v = Eigen::AngleAxisd(q);
	 cout << v.angle() << "	" << v.axis() << endl;
	 cout << v.matrix() << endl << endl;