第七题(python版)
令p点在世界坐标系下的点为Pw,点p从世界坐标系转换到小萝卜一号坐标系有:
p = q1 x Pw + t2
解出Pw
那么令p在小萝卜二号坐标系下的点为p2,有:
p2 = q2 x Pw +t
#!/usr/bin/python
# -*- coding: UTF-8 -*-
#slam习题
#numpy里矩阵X乘用.dot,点乘用*
import numpy as np
q1 = (0.35, 0.2, 0.3, 0.1)
q2 = (-0.5, 0.4, -0.1, 0.2)
t = np.array([-0.1, 0.5, 0.3])
t2 = np.array([0.3, 0.1, 0.1])
p = np.array([0.5, 0, 0.2])
def quaternion_rotate(q): #四元素转换为旋转矩阵
r = np.array([[1-2*q[2]*q[2]-2*q[3]*q[3], 2*q[1]*q[2]-2*q[0]*q[3], 2*q[1]*q[3]+2*q[0]*q[2]],
[2*q[1]*q[2]+2*q[0]*q[3], 1-2*q[1]*q[1]-2*q[3]*q[3], 2*q[2]*q[3]-2*q[0]*q[1]],
[2*q[1]*q[3]-2*q[0]*q[2], 2*q[2]*q[3]+2*q[0]*q[1], 1-2*q[1]*q[1]-2*q[2]*q[2]]])
return r
def quaternion_normalized(q): #归一化
length = (q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3])**0.5
q_one = (round(q[0]/length, 9), round(q[1]/length, 9), round(q[2]/length, 9), round(q[3]/length, 9))
#round(a, b) 保留a的b位小数
return q_one
q1_one = quaternion_normalized(q1)
q2_one = quaternion_normalized(q2)
r1 = quaternion_rotate(q1_one)
r2 = quaternion_rotate(q2_one)
pw = np.linalg.inv(r1).dot((p-t2))
p2 = r2.dot(pw)+t
print '-----------answer---------'
print p2