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 _apply_prediction_pcl_depImg(pclSource, targetT, targetP, params6=True, **kwargs):
'''
Transform pclSource, Calculate new targetT based on targetP, Create new depth image, and pcl file
Return:
- New pclSource
- New targetT
- New depthImageB
'''
# remove trailing zeros
pclSource = kitti.remove_trailing_zeros(pclSource)
if params6:
tMatP = kitti._get_tmat_from_params(targetP)
tMatT = kitti._get_tmat_from_params(targetT)
else:
tMatP = targetP.reshape([3,4])
tMatT = targetT.reshape([3,4])
# get transformed pclSource based on targetP
pclSourceTransformed = kitti.transform_pcl(pclSource, tMatP)
# get new depth image of transformed pclSource
depthImageB, _ = kitti.get_depth_image_pano_pclView(pclSourceTransformed)
pclSourceTransformed = kitti._zero_pad(pclSourceTransformed, kwargs.get('pclCols')-pclSourceTransformed.shape[1])
# get residual Target
tMatResP2T = kitti.get_residual_tMat_p2t(tMatT, tMatP) # first is source2target, second is source2predicted
if params6:
targetResP2T = kitti._get_tmat_from_params(tMatResP2T)
else:
targetResP2T = tMatResP2T.reshape([12])
return pclSourceTransformed, targetResP2T, depthImageB
def _apply_prediction_periodic(pclA, targetT, targetP, **kwargs):
'''
Transform pclA, Calculate new targetT based on targetP, Create new depth image
Return:
- New PCLA
- New targetT
- New depthImage
'''
# remove trailing zeros
pclA = kitti.remove_trailing_zeros(pclA)
# get transformed pclA based on targetP
tMatP = kitti._get_tmat_from_params(targetP)
pclATransformed = kitti.transform_pcl(pclA, tMatP)
# get new depth image of transformed pclA
depthImageA, _ = kitti.get_depth_image_pano_pclView(pclATransformed)
pclATransformed = kitti._zero_pad(
pclATransformed,
kwargs.get('pclCols') - pclATransformed.shape[1])
# get residual Target
#tMatResA2B = kitti.get_residual_tMat_A2B(targetT, targetP)
targetP[0] = targetP[0] % np.pi
targetP[1] = targetP[1] % np.pi
targetP[2] = targetP[2] % np.pi
targetResP2T = targetT - targetP
return pclATransformed, targetResP2T, depthImageA
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_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
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 GetMapViaPose(poseX2Olist):
"""
Input:
poselist X2O
Output:
Map
"""
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(poseX2Olist)):
pointT = kitti.transform_pcl(origin, poseX2Olist[i])
pathMap = np.append(pathMap, pointT, axis=1)
return pathMap
def GetMapViaTmat(tmatlist):
"""
Input:
tmatList B2A
Output:
Map
"""
'''
Sequencing : P(n) -> P(n-1) -> ... -> P(O)
'''
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(tmatlist) - 1, -1, -1): #last index to 0
pathMap = kitti.transform_pcl(pathMap, tmatlist[i])
pathMap = np.append(pathMap, origin, axis=1)
#pathMap = np.flip(pathMap)
return pathMap
def _apply_prediction(pclA, tMatT, tMatP, **kwargs):
'''
Transform pclA, Calculate new tMatT based on tMatP, Create new depth image
Return:
- New PCLA
- New tMatT
- New depthImage
'''
# remove trailing zeros
pclA = kitti.remove_trailing_zeros(pclA)
# get transformed pclA based on tMatP
pclATransformed = kitti.transform_pcl(pclA, tMatP)
# get new depth image of transformed pclA
_, depthImageA = kitti.get_depth_image_pano_pclView(pclATransformed)
pclATransformed = kitti._zero_pad(
pclATransformed,
kwargs.get('pclCols') - pclATransformed.shape[1])
# get residual tMat
tMatResA2B = kitti.get_residual_tMat_A2B(tMatT, tMatP)
return pclATransformed, tMatResA2B, depthImageA
def _apply_prediction_depImg(pclSource, targetT, targetP, prevP, params6=True, **kwargs):
'''
Transform pclSource, Calculate new targetT based on targetP, Create new depth image
DOESN"T TOUCH THE PCL, BUT USES prevP TO DO TRANSFORMATION FOR CORRECT DEPTH IMAGE
TargetP
is transformation from space pi to space t.
prevP
is transformation from space s to space pi. s to p0 is identity.
targetT
is transformation from space s to space t.
Return:
- New pclSource
- New targetT
- New depthImageB
'''
# remove trailing zeros
pclSource = kitti.remove_trailing_zeros(pclSource)
if params6:
tMatP = kitti._get_tmat_from_params(targetP)
tMatT = kitti._get_tmat_from_params(targetT)
tMatPrevP = kitti._get_tmat_from_params(prevP)
else:
tMatP = targetP.reshape([3,4])
tMatT = targetT.reshape([3,4])
tMatPrevP = prevP.reshape([3,4])
# get the transformation from space s to pi
tMatS2P = np.matmul(kitti._add_row4_tmat(tMatPrevP), kitti._add_row4_tmat(tMatP))
tMatS2P = kitti._remove_row4_tmat(tMatS2P)
# get transformed pclSource based on targetP
pclSourceTransformed = kitti.transform_pcl(pclSource, tMatS2P)
# get new depth image of transformed pclSource
depthImageB, _ = kitti.get_depth_image_pano_pclView(pclSourceTransformed)
# get residual Target
tMatResP2T = kitti.get_residual_tMat_p2t(tMatT, tMatS2P) # first is source2target, second is source2predicted
if params6:
targetResP2T = kitti._get_tmat_from_params(tMatResP2T)
else:
targetResP2T = tMatResP2T.reshape([12])
return tMatS2P, targetResP2T, depthImageB
def _apply_prediction(pclB, targetT, targetP, **kwargs):
'''
Transform pclB, Calculate new targetT based on targetP, Create new depth image
Return:
- New PCLB
- New targetT
- New depthImageB
'''
# remove trailing zeros
pclA = kitti.remove_trailing_zeros(pclB)
# get transformed pclB based on targetP
tMatP = kitti._get_tmat_from_params(targetP)
pclBTransformed = kitti.transform_pcl(pclB, tMatP)
# get new depth image of transformed pclB
depthImageB, _ = kitti.get_depth_image_pano_pclView(pclBTransformed)
pclBTransformed = kitti._zero_pad(
pclBTransformed,
kwargs.get('pclCols') - pclBTransformed.shape[1])
# get residual Target
#tMatResB2A = kitti.get_residual_tMat_Bp2B2A(targetP, targetT) # first is A, second is B
targetResP2T = targetT - targetP
return pclBTransformed, targetResP2T, depthImageB
