Function vis_pose

script/dm/pose_model.py:33–77

visualize predicted pose result vs. gt pose

(vis_info)

    return tuple(g_filtered)

def vis_pose(vis_info):
    '''
    visualize predicted pose result vs. gt pose
    '''
    pdb.set_trace()
    pose = vis_info['pose']
    pose_gt = vis_info['pose_gt']
    theta = vis_info['theta']
    ang_threshold=10
    seq_num = theta.shape[0]
    # # create figure object
    # plot translation traj.
    fig = plt.figure(figsize = (8,6))
    plt.subplots_adjust(left=0, bottom=0, right=1, top=1)
    ax1 = fig.add_axes([0, 0.2, 0.9, 0.85], projection='3d')
    ax1.scatter(pose[10:,0],pose[10:,1],zs=pose[10:,2], c='r', s=3**2,depthshade=0) # predict
    ax1.scatter(pose_gt[:,0], pose_gt[:,1], zs=pose_gt[:,2], c='g', s=3**2,depthshade=0) # GT
    ax1.scatter(pose[0:10,0],pose[0:10,1],zs=pose[0:10,2], c='k', s=3**2,depthshade=0) # predict
    # ax1.plot(pose[:,0],pose[:,1],zs=pose[:,2], c='r') # predict
    # ax1.plot(pose_gt[:,0], pose_gt[:,1], zs=pose_gt[:,2], c='g') # GT
    ax1.view_init(30, 120)
    ax1.set_xlabel('x (m)')
    ax1.set_ylabel('y (m)')
    ax1.set_zlabel('z (m)')
    # ax1.set_xlim(-10, 10)
    # ax1.set_ylim(-10, 10)
    # ax1.set_zlim(-10, 10)

    ax1.set_xlim(-1, 1)
    ax1.set_ylim(-1, 1)
    ax1.set_zlim(-1, 1)

    # ax1.set_xlim(-3, 3)
    # ax1.set_ylim(-3, 3)
    # ax1.set_zlim(-3, 3)

    # plot angular error
    ax2 = fig.add_axes([0.1, 0.05, 0.75, 0.2])
    err = theta.reshape(1, seq_num)
    err = np.tile(err, (20, 1))
    ax2.imshow(err, vmin=0,vmax=ang_threshold, aspect=3)
    ax2.set_yticks([])
    ax2.set_xticks([0, seq_num*1/5, seq_num*2/5, seq_num*3/5, seq_num*4/5, seq_num])
    fname = './vis_pose.png'
    plt.savefig(fname, dpi=50)

def compute_error_in_q(args, dl, model, device, results, batch_size=1):
    use_SVD=True # Turn on for Direct-PN and Direct-PN+U reported result, despite it makes minuscule differences