三维点云拼接 标记点拼接 SVD分解法

       在三维重建的过程中每次只能测量有限的区域，那么拼接的操作就再所难免了，最终拼接的效果往往觉得了你做的产品是否真的有价值。很多市面上的产品在比较的时候首先看的是整体的重建效果，而整体的效果就是拼接决定的。拼接的效果由精度和效率决定的，首先是精度主要看最终拼接的点云之间融合的好坏，其次的效率，效率主要考察的是在拼接的过程是否流畅，是否需要很多标记点，好的拼接效率就是在使用少量的标记点也能拼接出高精度的效果。

      拼接的精度是由你使用的算法决定的，拼接的终极思想就是计算出两幅点云的[R,T],其中最方法很多，我介绍的是基于SVD分解的方式，敲公式太麻烦了，还是手写的吧！！

                                 

                                               

CalcRalteR_T(_vPoint3 &vPtsA,_vPoint3 &vPtsB,CvMat *&matR,CvMat *&matT)
{
	CExternalLibrary Clapack;
	int i,j;
	VECTOR3 meanA;
	meanA.x=0;
	meanA.y=0;
	meanA.z=0;
	for (i=0;i<vPtsA.size();i++)
	{
		meanA+=vPtsA[i];
	}
	meanA=meanA/vPtsA.size()
	VECTOR3 meanB;
	meanB.x=0;
	meanB.y=0;
	meanB.z=0;
	for (j=0;j<vPtsB.size();j++)
	{
		meanB+=vPtsB[j];
	}
	meanB=meanB/vPtsB.size();


	_vVector3 substractmeanA;
	_vVector3 substractmeanB;
	for (int m=0;m<vPtsA.size();m++)
	{
		VECTOR3 result=vPtsA[m]-meanA;
		substractmeanA.push_back(result);
	}

	for (int n=0;n<vPtsB.size();n++)
	{

		VECTOR3 result=vPtsB[n]-meanB;
		substractmeanB.push_back(result);
	}

	float matData[9];
	for (int ii=0;ii<9;ii++)
	{
		matData[ii]=0.0f;
	}

	float matData1[3];
	for (int jj=0;jj<3;jj++)
	{
		matData1[jj]=0.0f;
	}

	CvMat matTet=cvMat(3,3,CV_32FC1,matData);
	
	//matR=cvCreateMat(3,3,CV_32FC1);


	CvMat *matTet1=cvCreateMat(3,3,CV_32FC1);
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			cvSetReal2D(matTet1,i,j,0);
		}
	}
	CvMat matA1,matB1,*matB1Transpose;
	matB1Transpose=cvCreateMat(3,1,CV_32FC1);
	for (i=0;i<vPtsA.size();i++)
	{

		float b1[3]={substractmeanB[i].x,substractmeanB[i].y,substractmeanB[i].z};
		float a1[3]={substractmeanA[i].x,substractmeanA[i].y,substractmeanA[i].z};
		matB1=cvMat(1,3,CV_32FC1,b1);
		matA1=cvMat(1,3,CV_32FC1,a1);
	
	}
	cvReleaseMat(&matB1Transpose);
	CvMat *matW,*matU,*matV;
	matW=cvCreateMat(3,1,CV_32FC1);
	matU=cvCreateMat(3,3,CV_32FC1);
	matV=cvCreateMat(3,3,CV_32FC1)
	CvMat *matX;
	CvMat *matUTranspose;
	matX=cvCreateMat(3,3,CV_32FC1);
	matUTranspose=cvCreateMat(3,3,CV_32FC1);
	//matT=cvCreateMat(1,3,CV_32FC1);
	cvSub(&matMeanA,matBMeanMulMatR,matT);
	cvReleaseMat(&matW);
	cvReleaseMat(&matU);
	cvReleaseMat(&matV);
	cvReleaseMat(&matBMeanMulMatR);
	cvReleaseMat(&matUTranspose);
	cvReleaseMat(&matTet1);
	cvReleaseMat(&matX);


}

交流 QQ:1264768501