def _apply_prediction_colorOnly(imgColor, targetT, targetP, prevP, params6=True, **kwargs):
'''
Transform imgColor, Calculate new targetT based on targetP, Create new depth image, and pcl file
Return:
- New imgColor
- New targetT
- New depthImageB
'''
# target is B2A (next2prev)
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 color image based on iterative target
imgColorB = kitti.transform_image(imgColorB, tMatS2P)
# get residual Target OR [NEW 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 tMatS2P, targetResP2T, imgColorB
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 _get_pose_from_param(pParam):
poses = list()
for i in range(pParam.shape[0]):
pposep = pParam[i]
#print(pposep)
poses.append(kitti._get_tmat_from_params(pposep).reshape(3 * 4))
return poses
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 _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
