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