实际应用场景：
在世界坐标系中，机器人运动到位姿pose1(position=px1,py1,pz1,orientation=x1,y1,z1,w1)处，机器人在此处开始启动carto建图；
在carto坐标系中，机器人运动到位姿pose2(position=px2,py2,pz2,orientation=x2,y2,z2,w2)处，机器人在此处结束建图；
请问机器人在结束建图的位姿在世界坐标系中的表示pose3(position=px3,py3,pz3,orientation=x3,y3,z3,w3)是多少？
求解思路：
首先通过pose1，求出世界坐标系原点->carto坐标系原点的转移关系T；
然后，利用pose3.position与pose2.position之间存在的转移关系T，可以求出pose3.position；
然后，利用四元数的乘法pose3.orientation=pose1.orientation * pose2.orientation，直接求出pose3.orientation；
最后，将pose3.position与pose3.orientation拼起来就得到要求解的pose3。
python程序实现：
1 from numpy import * 2 3 def pose_transform(self,pose1,pose2): 4 ''' 5 tf(pose3)=tf(pose1)+tf(pose2) 6 ''' 7 px1,py1,pz1,x1,y1,z1,w1=pose1 8 px2,py2,pz2,x2,y2,z2,w2=pose2 9 ### Pworld=T*Pcarto ### 10 #quart -> R 11 w=w1 12 x=x1 13 y=y1 14 z=z1 15 R=mat(zeros((3,3))) 16 R[0,0]=w*w+x*x-y*y-z*z 17 R[0,1]=2*x*y-2*w*z 18 R[0,2]=2*x*z+2*w*y 19 R[1,0]=2*x*y+2*w*z 20 R[1,1]=w*w-x*x+y*y-z*z 21 R[1,2]=2*y*z-2*w*x 22 R[2,0]=2*x*z-2*w*y 23 R[2,1]=2*y*z+2*w*x 24 R[2,2]=w*w-x*x-y*y+z*z 25 #R,t -> T 26 t=mat(zeros((3,1))) 27 t[0,0]=px1 28 t[1,0]=py1 29 t[2,0]=pz1 30 block1=hstack((R,t)) 31 block2=hstack((mat([0.,0.,0.]),mat([1.]))) 32 T=vstack((block1,block2)) 33 print 'T=',T 34 #cacul px3,py3,pz3 35 Pcarto=mat([[px2],[py2],[pz2],[1.]]) 36 Pworld=T*Pcarto 37 print 'Pworld=',Pworld 38 px3=Pworld[0,0] 39 py3=Pworld[1,0] 40 pz3=Pworld[2,0] 41 ### quart3=quart1*quart2 ### 42 x3 = w1*x2 + x1*w2 + y1*z2 - z1*y2 43 y3 = w1*y2 - x1*z2 + y1*w2 + z1*x2 44 z3 = w1*z2 + x1*y2 - y1*x2 + z1*w2 45 w3 = w1*w2 - x1*x2 - y1*y2 - z1*z2 46 ### return pose3 ### 47 pose3=[px3,py3,pz3,x3,y3,z3,w3] 48 print 'pose3=',pose3 49 return pose3