四元数插值 ros tf tf2

http://docs.ros.org/jade/api/tf2/html/classtf2_1_1Quaternion.html

Quaternion tf2::Quaternion::slerp	(	const Quaternion &	q,
		const tf2Scalar &	t
	)		const [inline]

Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion.

Parameters:

q	The other quaternion to interpolate with
t	The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. Slerp interpolates assuming constant velocity.

Definition at line 314 of file Quaternion.h.

 

example:

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <tf2/utils.h>
#include <turtlesim/Spawn.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#define epsilo 0.000001
using namespace std;
using namespace tf2;
#define r2d 57.29577951308232
#define d2r 0.017453292519943295
void seeq(Quaternion q)
{
printf("[%f, %f, %f,%f]\n", q.getX(),q.getY(), q.getZ(), q.getW());
}
void test() {
	Quaternion q,qb,qc;

	float r, p, y;
	float du = 270;
	q.setRPY(0, 0, du*d2r);//
	printf("绕Z旋转270度 ");
	seeq(q);
	y = tf2::getYaw(q);
	printf("%f \n", y);
	du = -90;
	qb.setRPY(0, 0, du*d2r);
	printf("绕Z旋转-90度 ");
	seeq(q);
	y = tf2::getYaw(q);
	printf("%f \n", y);
	printf("end-----四元数每个元素都取反后和自身等价-----\n---四元数插值示例-----\n");

	q.setRPY(0,0,100*d2r);//100 du
	printf("q 100 du  ");
	seeq(q);

	qb.setRPY(0,0,-120*d2r);//-100 du
	printf("qb -120 du");
	seeq(qb);
	qc=q.slerp(qb,0);// qc = q
	printf("插值系数为0,得到qc = q ");
	seeq(qc);

	qc=q.slerp(qb,0.3);// qc = q
	y = tf2::getYaw(qc);
	
	printf("插值系数为0.3,得到qc ");
	seeq(qc);
	printf("插值系数为0.3,得到qc的yaw :%f du\n", y*r2d);

	qc=q.slerp(qb,0.8);// qc = q
	y = tf2::getYaw(qc);
	
	printf("插值系数为0.8,得到qc ");
	seeq(qc);
	printf("插值系数为0.8,得到qc的yaw :%f du\n", y*r2d);
	qc=q.slerp(qb,1);// qc = q
	printf("插值系数为1,得到qc=qb ");
	seeq(qc);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "markerListener");

	ros::NodeHandle node("~");
	test();
	return 0;

}