视觉slam14讲第三讲习题

第七题（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