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