def _get_param_from_pose(poselist):
params = list()
for i in range(len(poselist) - 1):
poseA2B = kitti._get_tMat_A_2_B(kitti._get_3x4_tmat(poselist[i]),
kitti._get_3x4_tmat(poselist[i + 1]))
params.append(kitti._get_params_from_tmat(poseA2B))
return params
def _get_p_map_w_orig(pPoseAB, gPose2o, numTuple):
"""
get the predicted truth path map
poses are w.r.t. previous frame
number of tuples in the batch. numTuple-1 transformation matrices. numTuple-2 will be added from gPose2o
"""
## origin = np.array([[0], [0], [0]], dtype=np.float32)
## pathMap = np.ndarray(shape=[3,0], dtype=np.float32)
## pathMap = np.append(pathMap, origin, axis=1)
## # Sequential transformations that takes points form frame i to i+1
## for i in range(len(pPoseAB)-1,-1,-1):
## poseA2B = kitti._get_3x4_tmat(np.array(pPoseAB[i]))
## pathMap = kitti.transform_pcl(pathMap, poseA2B)
## pathMap = np.append(pathMap, origin, axis=1)
## #### PathMap consists of all points transformed to the last frame coordinates
## # transform points at last frame coordinates to origin frame
## #pathMap = kitti.transform_pcl(pathMap, gPose2o[len(gPose2o)-1])
## pathMap = kitti.transform_pcl(pathMap, gPose2o[0])
origin = np.array([[0], [0], [0]], dtype=np.float32)
pathMap = np.ndarray(shape=[3, 0], dtype=np.float32)
poseA2o = kitti._get_3x4_tmat(gPose2o[0])
# because inv(OrigTo1)*OrigTo1*(0,0,0)=(0,0,0) so simply append (0,0,0)
#gtLoc = kitti.transform_pcl(origin, gPose2o[0])
#pathMap = np.append(pathMap, gtLoc, axis=1)
pathMap = np.append(pathMap, origin, axis=1)
for i in range(len(pPoseAB)):
poseA2B = kitti._get_3x4_tmat(np.array(pPoseAB[i]))
poseB2O = _get_tMat_B_2_O(poseA2o, poseA2B)
oAtNextFrame = kitti.transform_pcl(origin, poseA2B)
oAtOrigFrame = kitti.transform_pcl(origin, poseB2O)
pathMap = np.append(pathMap, oAtOrigFrame, axis=1)
poseA2o = poseB2O
#### PathMap consists of all points in the origin frame coordinates
return pathMap
def _get_gt_map_backwards(gtPose):
"""
iterate backwards to transform step by step backwards
"""
origin = np.array([[0], [0], [0]], dtype=np.float32)
pathMap = np.array([[0], [0], [0]], dtype=np.float32)
for i in range(gtPose.shape[0] - 2, -1, -1):
poseA = kitti._get_3x4_tmat(gtPose[i])
poseB = kitti._get_3x4_tmat(gtPose[i + 1])
poseB2A = _get_tMat_A_2_B(poseB, poseA)
pathMap = kitti.transform_pcl(pathMap, poseB2A)
pathMap = np.append(pathMap, origin, axis=1)
#### PathMap consists of all points transformed to the frame 0 coordinates
# transform them to origin access
pathMap = kitti.transform_pcl(pathMap, gtPose[0])
# add final origin
pathMap = np.append(pathMap, origin, axis=1)
return pathMap
def _get_p_map(pPose):
"""
get the predicted truth path map
poses are w.r.t. previous frame
"""
origin = np.array([[0], [0], [0]], dtype=np.float32)
pathMap = np.ndarray(shape=[3, 0], dtype=np.float32)
pathMap = np.append(pathMap, origin, axis=1)
for i in range(len(pPose)):
pose = kitti._get_3x4_tmat(np.array(pPose[i]['tmat']))
origin = kitti.transform_pcl(origin, pose)
pathMap = np.append(pathMap, origin, axis=1)
return pathMap
def _get_gt_map(gtPose):
"""
get the ground truth path map
pose are w.r.t. the origin
"""
origin = np.array([[0], [0], [0]], dtype=np.float32)
pathMap = np.ndarray(shape=[3, 0], dtype=np.float32)
pathMap = np.append(pathMap, origin, axis=1)
for i in range(len(gtPose)):
pose = kitti._get_3x4_tmat(gtPose[i])
pointT = kitti.transform_pcl(origin, pose)
pathMap = np.append(pathMap, pointT, axis=1)
return pathMap
def GetGtPoseTmat(gtPoseDir, seqID):
'''
'''
gtPosePath = kitti.get_pose_path(gtPoseDir, seqID)
gtPoseList = kitti.get_pose_data(gtPosePath) # gtpose -> X2O (12 values)
gtPose = list()
for i in range(0, len(gtPoseList)):
gtPose.append(kitti._get_3x4_tmat(gtPoseList[i]))
#### gtpose -> prediction like
gtTmat = list()
# ntuple == 2, so I am appending the first X2O and the rest are B2A
gtTmat.append(gtPose[0])
for i in range(1, len(gtPose)):
gtTmat.append(kitti._get_tMat_B_2_A(gtPose[i - 1], gtPose[i]))
####
return gtPose, gtTmat
def _get_p_map_w_orig_points(pPoseAB, gPose2o):
"""
Original Coordinates are used and only transformation for each frame is plotted
len(pPoseAB) == len(gPose2o)+1
get the predicted truth path map
poses are w.r.t. previous frame
"""
origin = np.array([[0], [0], [0]], dtype=np.float32)
pathMap = np.ndarray(shape=[3, 0], dtype=np.float32)
gtLoc = kitti.transform_pcl(origin, gPose2o[0])
pathMap = np.append(pathMap, gtLoc, axis=1)
# Sequential transformations that takes points form frame i to i+1
for i in range(len(pPoseAB)):
poseA2B = kitti._get_3x4_tmat(np.array(pPoseAB[i]))
oAtNextFrame = kitti.transform_pcl(origin, poseA2B)
oAtOrigFrame = kitti.transform_pcl(oAtNextFrame, gPose2o[i + 1])
pathMap = np.append(pathMap, oAtOrigFrame, axis=1)
#### PathMap consists of all points in the origin frame coordinates
return pathMap
