三维空间坐标的相似变换原理与实现 |
您所在的位置：网站首页 › 什么是相似变换 › 三维空间坐标的相似变换原理与实现 |
三维空间坐标的相似变换原理与实现
|
说到这个博客的题目，可能觉得有点大，在测绘学领域中三维空间坐标的相似变换用得非常多。那么什么是三维坐标的相似变换呢？就是在两个三维直角坐标系中，坐标进行变换，两个坐标系之间变换需要七个参数，即三个平移分量，以及三个旋转参数和一个尺度因子。这里用到的模型采用摄影测量学中的变换模型，具体推导见摄影测量学书籍。数学模型如下： R是旋转矩阵，X0，Y0，Z0是平移量，
常数项如下：
其误差方程式用简化的式子表示如下：
P是权矩阵，这里是单位矩阵 法方程如下：
再考虑坐标重心化，以及将系数矩阵转置后自身相乘，即表达如下：
最后求解法方程式，得到七个参数的改正数，判断改正数是否小于指定的阈值，小于就结束，否则，继续迭代。 综上所述，要求解七个参数，至少要七个方程才能求解，所以至少需要3对控制点，即在两个不同坐标系下的公共点坐标，如果有多余观测，就可以利用最小二乘平差的理论解算七个参数。这个模型只适合旋转角比较小的情况，对于大角度旋转需要其他的模型才能解决。 最后的代码解析如下，首先，定义几个子函数： 1、计算旋转矩阵的函数 //根据三个旋转角计算旋转矩阵 static void CalRotateMatrix(double dbPhi,double dbOmega,double dbKappa,double *pdValues) { pdValues[0] = cos(dbPhi)*cos(dbKappa) - sin(dbPhi)*sin(dbOmega)*sin(dbKappa); pdValues[1] = -cos(dbPhi)*sin(dbKappa) - sin(dbPhi)*sin(dbOmega)*cos(dbKappa); pdValues[2] = -sin(dbPhi)*cos(dbOmega); pdValues[3] = cos(dbOmega)*sin(dbKappa); pdValues[4] = cos(dbOmega)*cos(dbKappa); pdValues[5] = -sin(dbOmega); pdValues[6] = sin(dbPhi)*cos(dbKappa) + cos(dbPhi)*sin(dbOmega)*sin(dbKappa); pdValues[7] = -sin(dbPhi)*sin(dbKappa) + cos(dbPhi)*sin(dbOmega)*cos(dbKappa); pdValues[8] = cos(dbPhi)*cos(dbOmega); } 2、计算算数平均值的函数 static double GetAverage(double *pfValues,int nCount) { double dbSum = 0; for (int i = 0; i < nCount; i ++) { dbSum += pfValues[i]; } return dbSum/nCount; }3、计算系数矩阵转置与常数项乘积矩阵的函数 void CalConstMatrix(double* pGeox1, double* pGeoy1, double* pGeoz1, double* pGeox2, double* pGeoy2, double* pGeoz2, int nPtCount, double *pfRotateMat, double dbK, double *pfCoffMat) { double dbTempX = 0; double dbTempY = 0; double dbTempZ = 0; double dbTempW = 0; for (int i = 0; i < nPtCount; i ++) { double dbTemp1 = pfRotateMat[0]*pGeox2[i] + pfRotateMat[1]*pGeoy2[i] + pfRotateMat[2]*pGeoz2[i]; double dbTemp2 = pfRotateMat[3]*pGeox2[i] + pfRotateMat[4]*pGeoy2[i] + pfRotateMat[5]*pGeoz2[i]; double dbTemp3 = pfRotateMat[6]*pGeox2[i] + pfRotateMat[7]*pGeoy2[i] + pfRotateMat[8]*pGeoz2[i]; double dbLx = pGeox1[i] - dbK*dbTemp1; double dbLy = pGeoy1[i] - dbK*dbTemp2; double dbLz = pGeoz1[i] - dbK*dbTemp3; dbTempX += (pGeox2[i]*dbLx + pGeoy2[i]*dbLy + pGeoz2[i]*dbLz); dbTempY += (pGeox2[i]*dbLz - pGeoz2[i]*dbLx); dbTempZ += (pGeoy2[i]*dbLz - pGeoz2[i]*dbLy); dbTempW += (pGeox2[i]*dbLy - pGeoy2[i]*dbLx); } pfCoffMat[0] = 0; pfCoffMat[1] = 0; pfCoffMat[2] = 0; pfCoffMat[3] = dbTempX; pfCoffMat[4] = dbTempY; pfCoffMat[5] = dbTempZ; pfCoffMat[6] = dbTempW; } 4、计算系数矩阵转置的乘积矩阵的函数 void CalCoffMatrix(double* pGeox1, double* pGeoy1, double* pGeoz1, int nPtCount, double *pfRotateMartix, double *pfCoffMat) { //计算矩阵元素 pfCoffMat[0] = nPtCount; pfCoffMat[8] = nPtCount; pfCoffMat[16] = nPtCount; double dbTemp1 = 0; double dbTemp2 = 0; double dbTemp3 = 0; double dbTemp4 = 0; double dbTemp5 = 0; double dbTemp6 = 0; double dbTemp7 = 0; double dbTemp8 = 0; double dbTemp9 = 0; double dbTemp10 = 0; for (int i = 0; i < nPtCount; i ++) { dbTemp1 += (pGeox1[i]*pGeox1[i] + pGeoy1[i]*pGeoy1[i] + pGeoz1[i]*pGeoz1[i]); dbTemp2 += (pGeox1[i]*pGeox1[i] + pGeoz1[i]*pGeoz1[i]); dbTemp3 += (pGeox1[i]*pGeoy1[i]); dbTemp4 += (pGeoy1[i]*pGeoz1[i]); dbTemp5 += (pGeox1[i]*pGeoy1[i]); dbTemp6 += (pGeoy1[i]*pGeoy1[i] + pGeoz1[i]*pGeoz1[i]); dbTemp7 += (pGeox1[i]*pGeoz1[i]); dbTemp8 += (pGeoy1[i]*pGeoz1[i]); dbTemp9 += (pGeox1[i]*pGeoz1[i]); dbTemp10 += (pGeox1[i]*pGeox1[i] + pGeoy1[i]*pGeoy1[i]); } pfCoffMat[24] = dbTemp1; pfCoffMat[32] = dbTemp2; pfCoffMat[33] = dbTemp3; pfCoffMat[34] = dbTemp4; pfCoffMat[39] = dbTemp5; pfCoffMat[40] = dbTemp6; pfCoffMat[41] = -dbTemp7; pfCoffMat[46] = dbTemp8; pfCoffMat[47] = -dbTemp9; pfCoffMat[48] = dbTemp10; } 5、最后是入口函数： bool SevenParameterSolve(double* pGeox1, double* pGeoy1, double* pGeoz1, double* pGeox2, double* pGeoy2, double* pGeoz2, int nPtCount, double* pdX, double* pdY, double* pdZ, double* pdK, double* pdPhi, double* pdOmige, double* pdKappa) { if (nPtCount < 3) { *pdX = 0; *pdY = 0; *pdZ = 0; *pdK = 1; *pdPhi = 0; *pdOmige = 0; *pdKappa = 0; return false; } //计算坐标的平均值 double dbAvgX1 = 0; double dbAvgY1 = 0; double dbAvgZ1 = 0; double dbAvgX2 = 0; double dbAvgY2 = 0; double dbAvgZ2 = 0; dbAvgX1 = GetAverage(pGeox1,nPtCount); dbAvgY1 = GetAverage(pGeoy1,nPtCount); dbAvgZ1 = GetAverage(pGeoz1,nPtCount); //计算原始坐标系统的中心坐标 dbAvgX2 = GetAverage(pGeox2,nPtCount); dbAvgY2 = GetAverage(pGeoy2,nPtCount); dbAvgZ2 = GetAverage(pGeoz2,nPtCount); //计算重心化的坐标 int i = 0; for (i = 0; i < nPtCount; i ++) { pGeox1[i] -= dbAvgX1; pGeoy1[i] -= dbAvgY1; pGeoz1[i] -= dbAvgZ1; pGeox2[i] -= dbAvgX2; pGeoy2[i] -= dbAvgY2; pGeoz2[i] -= dbAvgZ2; } //变换参数初始值 *pdX = 0; *pdY = 0; *pdZ = 0; *pdK = 1; *pdPhi = 0; *pdOmige = 0; *pdKappa = 0; //增量 double dX = *pdX; double dY = *pdY; double dZ = *pdZ; double dK = *pdK; double dPhi = *pdPhi; double dOmige = *pdOmige; double dKappa = *pdKappa; double adfRotateMatrix[9]; memset(adfRotateMatrix,0,sizeof(double)*9); //计算旋转矩阵 double adfRotateMatrixInit[9]; CalRotateMatrix(dPhi,dOmige,dKappa,adfRotateMatrixInit); //常数项矩阵 double adfConstMatrix[7]; memset(adfConstMatrix,0,sizeof(double)*7); //系数矩阵转置乘积矩阵 7*7 double adfCoffMatrix[49]; memset(adfCoffMatrix,0,sizeof(double)*49); //解矩阵 double adfSolveMat[7]; memset(adfSolveMat,0,sizeof(double)*7); int nCount = 0; while(1) { //计算旋转矩阵 CalRotateMatrix(dPhi,dOmige,dKappa,adfRotateMatrix); //计算常数项 CalConstMatrix(pGeox1,pGeoy1,pGeoz1,pGeox2,pGeoy2,pGeoz2,nPtCount, adfRotateMatrix,dK,adfConstMatrix); //计算系数矩阵 CalCoffMatrix(pGeox2,pGeoy2,pGeoz2,nPtCount,adfRotateMatrix,adfCoffMatrix); //法方程求解 MatrixInverse(adfCoffMatrix,7); MatrixMult(adfCoffMatrix,adfConstMatrix,adfSolveMat,7,7,1); //计算待定参数的新值 dX = dX + adfSolveMat[0]; dY = dY + adfSolveMat[1]; dZ = dZ + adfSolveMat[2]; dK = dK * (1 + adfSolveMat[3]); dPhi = dPhi + adfSolveMat[4]; dOmige = dOmige + adfSolveMat[5]; dKappa = dKappa + adfSolveMat[6]; nCount++; if (fabs(adfSolveMat[4]) < 3e-7 && fabs(adfSolveMat[5]) < 3e-7 && fabs(adfSolveMat[6]) < 3e-7) { break; } memset(adfConstMatrix,0,sizeof(double)*7); memset(adfRotateMatrix,0,sizeof(double)*9); memset(adfCoffMatrix,0,sizeof(double)*49); memset(adfSolveMat,0,sizeof(double)*7); } *pdK = dK; *pdPhi = dPhi; *pdOmige = dOmige; *pdKappa = dKappa; CalRotateMatrix(dPhi,dOmige,dKappa,adfRotateMatrix); double dbTemp1[3]; double dbTemp2[3]; dbTemp1[0] = dbAvgX2; dbTemp1[1] = dbAvgY2; dbTemp1[2] = dbAvgZ2; MatrixMult(adfRotateMatrix,dbTemp1,dbTemp2,3,3,1); //平移量单独计算 dX = dbAvgX1 - dK*dbTemp2[0]; dY = dbAvgY1 - dK*dbTemp2[1]; dZ = dbAvgZ1 - dK*dbTemp2[2]; *pdX = dX; *pdY = dY; *pdZ = dZ; printf("%f,%f,%f,%.15f,%.15f,%.15f,%.15f\n",dX,dY,dZ,dOmige,dPhi,dKappa,dK); printf("迭代次数：%d\n",nCount); return true; }最后忘记说了，空间相似变换可以应用于大地测量、摄影测量、地图投影以及计算机视觉中，主要就是将坐标从一个坐标系转换到另一个坐标系。 注意：代码中有涉及到矩阵求逆和矩阵相乘的操作，这个就没贴出来。 |
是尺度因子，在此只考虑小角度的情况，最后得到的误差方程式如下：
，
【本文地址】