反余弦函数的值域在 [0, pi]
斜体样式
cam_pose = self._cameras['hand_realsense'].camera.get_model_matrix() # cam2world# 物体到相机的向量
obj_tcp_vec = cam_pose[:3, 3] - self.obj_pose.p
dist = np.linalg.norm(obj_tcp_vec)
# 物体位姿的旋转矩阵
obj_rot_mat = self.obj_pose.to_transformation_matrix()[:3, :3]
x, y, z = obj_rot_mat[:, 0], obj_rot_mat[:, 1], obj_rot_mat[:, 2]# 计算角度 theta 和 phi
# dot_value = norm（obj_tcp_vec）*norm（z）*cos (theta)
vec_z_dot = np.dot(obj_tcp_vec, z)
# 向量vec_z_dot 与 平面 xy 的夹角 （等价于与z 轴）
theta = np.pi / 2 - np.arccos(vec_z_dot / dist)
# 向量在 z 轴的投影
obj_tcp_vec_proj_z = vec_z_dot * z
# 向量在 平面 xy 的投影
obj_tcp_vec_proj_xy = obj_tcp_vec - obj_tcp_vec_proj_z
dist_xy = np.linalg.norm(obj_tcp_vec_proj_xy) + 0.001
# 向量在 xy 平面投影 与 x 轴的内积
vecxy_x_dot = np.dot(obj_tcp_vec_proj_xy, x)
# 把空间一分为二，在两边使得 值 分别 [0, pi) (-pi,0]可以一一对应# vecxy_x_dot 是 向量在 xy平面投影 与 x 轴的内积
# 向量在 第一，第二象限 （x轴朝向东边，y轴朝向北边）
if np.dot(obj_tcp_vec_proj_xy, y) > 0:phi = np.pi - np.arccos(vecxy_x_dot / dist_xy)
# 向量在 第三，第四象限
else:phi = np.arccos(vecxy_x_dot / dist_xy) - np.pi
theta: 向量 obj_tcp_vec 与 平面 xy 所成的角度，俯仰角，等价于与 z 轴所成角度。
phi：向量obj_tcp_vec ，与 x 轴所成角度相关
# 将 phi 和 theta 离散化为索引
phi_min, phi_max = -180, 180 * 5 / 6
phi_d = (phi_max - phi_min) // 11
phi_t = (phi / np.pi * 180 - phi_min) / phi_d # [0, 12)# theta 是与 平面 xy 的夹角
theta_min, theta_max = 10, 80
theta_d = (theta_max - theta_min) // 5
theta_t = (theta / np.pi * 180 - theta_min) / theta_d # [0, 5)
