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 _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(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