qAutoStereo_20190430/qas/src/main/cpp/btk/b_BitFeatureEm/LocalScanDetector.c
/** applies PCA mapping 
 *  Input and output clusters may be identical
 */
applies PCA mapping
Input and output clusters may be identical
applies PCA mapping Input and output clusters may be identical
void bbf_LocalScanDetector_pcaMap( struct bbs_Context* cpA,
								   const struct bbf_LocalScanDetector* ptrA, 
								   const struct bts_IdCluster2D* inClusterPtrA,
								   struct bts_IdCluster2D* outClusterPtrA )
{
	bbs_DEF_fNameL( "bbf_LocalScanDetector_pcaMap" )
	struct bts_Cluster2D* tmpCl1PtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster1E;
	struct bts_Cluster2D* tmpCl2PtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster2E;
	struct bts_RBFMap2D*  rbfPtrL     = ( struct bts_RBFMap2D* )&ptrA->rbfMapE;
	struct bts_Flt16Alt2D altL;
	uint32 outBbpL = inClusterPtrA->clusterE.bbpE;
	uint32 iL, jL;
	bts_IdCluster2D_convertToEqivalentClusters( cpA, 
												inClusterPtrA,
												&ptrA->pcaClusterE,
												tmpCl1PtrL,
												tmpCl2PtrL );
	altL = bts_Cluster2D_alt( cpA, tmpCl1PtrL, tmpCl2PtrL, bts_ALT_RIGID );
	bts_Cluster2D_transform( cpA, tmpCl1PtrL, altL );
	bts_RBFMap2D_compute( cpA, rbfPtrL, tmpCl2PtrL, tmpCl1PtrL );
	bts_RBFMap2D_mapCluster( cpA, rbfPtrL, &ptrA->pcaClusterE.clusterE, tmpCl1PtrL, 6 );
	{
		const int16* matPtrL = ptrA->pcaMatE.arrPtrE;
		const int16* avgPtrL = ptrA->pcaAvgE.arrPtrE;
		struct bts_Int16Vec2D* vecArrL = tmpCl1PtrL->vecArrE;
		int32 prjVecL[ bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM ];
		uint16 matWidthL = tmpCl1PtrL->sizeE * 2;
		if( ptrA->pcaDimSubSpaceE > bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM )
		{
			bbs_ERROR1( "%s:\nbpi_RF_LANDMARKER_MAX_PCA_DIM exceeded", fNameL );
			return;
		}
		for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
		{
			int32 sumL = 0;
			avgPtrL = ptrA->pcaAvgE.arrPtrE;
			for( jL = 0; jL < tmpCl1PtrL->sizeE; jL++ )
			{
				sumL += matPtrL[ 0 ] * ( vecArrL[ jL ].xE - avgPtrL[ 0 ] );
				sumL += matPtrL[ 1 ] * ( vecArrL[ jL ].yE - avgPtrL[ 1 ] );
				avgPtrL += 2;
				matPtrL += 2;
			}
			prjVecL[ iL ] = ( sumL + 128 ) >> 8;
		}
		matPtrL = ptrA->pcaMatE.arrPtrE;
		avgPtrL = ptrA->pcaAvgE.arrPtrE;
		vecArrL = tmpCl1PtrL->vecArrE;
		for( jL = 0; jL < tmpCl1PtrL->sizeE; jL++ )
		{
			int32 sumL = 0;
			for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
			{
				sumL += matPtrL[ iL * matWidthL + 0 ] * prjVecL[ iL ];
			}
			vecArrL[ jL ].xE = ( ( sumL + 128 ) >> 8 ) + avgPtrL[ 0 ];
			sumL = 0;
			for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
			{
				sumL += matPtrL[ iL * matWidthL + 1 ] * prjVecL[ iL ];
			}
			vecArrL[ jL ].yE = ( ( sumL + 128 ) >> 8 ) + avgPtrL[ 1 ];
			matPtrL += 2;
			avgPtrL += 2;
		}
	}
	bts_IdCluster2D_copy( cpA, outClusterPtrA, &ptrA->pcaClusterE ); 
	bts_Cluster2D_copyTransform( cpA, &outClusterPtrA->clusterE, tmpCl1PtrL, bts_Flt16Alt2D_inverted( &altL ), outBbpL );
}
void bbf_LocalScanDetector_pcaMap ( struct bbs_Context * cpA , const struct bbf_LocalScanDetector * ptrA , const struct bts_IdCluster2D * inClusterPtrA , struct bts_IdCluster2D * outClusterPtrA ) { bbs_DEF_fNameL ( " " ) struct bts_Cluster2D * tmpCl1PtrL = ( struct bts_Cluster2D * ) & ptrA -> tmpCluster1E ; struct bts_Cluster2D * tmpCl2PtrL = ( struct bts_Cluster2D * ) & ptrA -> tmpCluster2E ; struct bts_RBFMap2D * rbfPtrL = ( struct bts_RBFMap2D * ) & ptrA -> rbfMapE ; struct bts_Flt16Alt2D altL ; uint32 outBbpL = inClusterPtrA -> clusterE . bbpE ; uint32 iL , jL ; bts_IdCluster2D_convertToEqivalentClusters ( cpA , inClusterPtrA , & ptrA -> pcaClusterE , tmpCl1PtrL , tmpCl2PtrL ) ; altL = bts_Cluster2D_alt ( cpA , tmpCl1PtrL , tmpCl2PtrL , bts_ALT_RIGID ) ; bts_Cluster2D_transform ( cpA , tmpCl1PtrL , altL ) ; bts_RBFMap2D_compute ( cpA , rbfPtrL , tmpCl2PtrL , tmpCl1PtrL ) ; bts_RBFMap2D_mapCluster ( cpA , rbfPtrL , & ptrA -> pcaClusterE . clusterE , tmpCl1PtrL , 6 ) ; { const int16 * matPtrL = ptrA -> pcaMatE . arrPtrE ; const int16 * avgPtrL = ptrA -> pcaAvgE . arrPtrE ; struct bts_Int16Vec2D * vecArrL = tmpCl1PtrL -> vecArrE ; int32 prjVecL [ bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM ] ; uint16 matWidthL = tmpCl1PtrL -> sizeE * 2 ; if ( ptrA -> pcaDimSubSpaceE > bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM ) { bbs_ERROR1 ( " \n " , fNameL ) ; return ; } for ( iL = 0 ; iL < ptrA -> pcaDimSubSpaceE ; iL ++ ) { int32 sumL = 0 ; avgPtrL = ptrA -> pcaAvgE . arrPtrE ; for ( jL = 0 ; jL < tmpCl1PtrL -> sizeE ; jL ++ ) { sumL += matPtrL [ 0 ] * ( vecArrL [ jL ] . xE - avgPtrL [ 0 ] ) ; sumL += matPtrL [ 1 ] * ( vecArrL [ jL ] . yE - avgPtrL [ 1 ] ) ; avgPtrL += 2 ; matPtrL += 2 ; } prjVecL [ iL ] = ( sumL + 128 ) >> 8 ; } matPtrL = ptrA -> pcaMatE . arrPtrE ; avgPtrL = ptrA -> pcaAvgE . arrPtrE ; vecArrL = tmpCl1PtrL -> vecArrE ; for ( jL = 0 ; jL < tmpCl1PtrL -> sizeE ; jL ++ ) { int32 sumL = 0 ; for ( iL = 0 ; iL < ptrA -> pcaDimSubSpaceE ; iL ++ ) { sumL += matPtrL [ iL * matWidthL + 0 ] * prjVecL [ iL ] ; } vecArrL [ jL ] . xE = ( ( sumL + 128 ) >> 8 ) + avgPtrL [ 0 ] ; sumL = 0 ; for ( iL = 0 ; iL < ptrA -> pcaDimSubSpaceE ; iL ++ ) { sumL += matPtrL [ iL * matWidthL + 1 ] * prjVecL [ iL ] ; } vecArrL [ jL ] . yE = ( ( sumL + 128 ) >> 8 ) + avgPtrL [ 1 ] ; matPtrL += 2 ; avgPtrL += 2 ; } } bts_IdCluster2D_copy ( cpA , outClusterPtrA , & ptrA -> pcaClusterE ) ; bts_Cluster2D_copyTransform ( cpA , & outClusterPtrA -> clusterE , tmpCl1PtrL , bts_Flt16Alt2D_inverted ( & altL ) , outBbpL ) ; }
applies PCA mapping
  Input and output clusters may be identical
applies PCA mapping Input and output clusters may be identical
/* setup two equivalent clusters holding the essential (alt-free) moves to be handled by PCA */ /* ! */ /* PCA projection: cluster1 -> cluster1 */ /* mat elements: 8.8 */ /* same bbp as pca cluster */ /* projected vector */ /* width of matrix */ /* forward trafo */ /* backward trafo */ /* ALT backtransformation */