【文章內容簡介】 ()。 return 0。 } //空間后方交會算法主體函數(shù) void HFJHDialog::HFJH() { //結果值 double Xs=Xs0, Ys=Ys0, Zs=Zs0。 //定義 double phi=phi0, omega=omega0, kappa=kappa0。 //定義 double x, y, X, Y, Z。 //定義 X,Y,Z double a1,a2,a3,b1,b2,b3,c1,c2,c3。 double X_,Y_,Z_。 double a11,a12,a13,a14,a15,a16。 double a21,a22,a23,a24,a25,a26。 double lx,ly。 double r_r, dx, dy。 cv::Mat A, L, dX。 //, A0, L0, A1。 //迭代終止閾值 double e = / 206265。 //單位權中誤差 double m0。 int index = 1。 //求取主點坐標 double W = 。 double H = 。 x0 = W / 2 + x0。 y0 = H / 2 y0。 do { for (size_t i = 0。 i != ()。 ++i) { 19 x = pic[i].x。 y = pic[i].y。 X = 0。 Y = 0。 Z = 0。 for (size_t j = 0。 j ()。 j++) { if (obj[j].index == pic[i].index) { X = obj[j].X。 Y = obj[j].Y。 Z = obj[j].Z。 break。 } } if (abs(x) 1) { char *arg1 = 像點 。 char *arg2 = 沒有找到對應的控制點 。 uitextBrowserappend(QString(%1%2%3).arg(QString::fromLocal8Bit(arg1)).arg(pic[i].index).arg(QString::fromLocal8Bit(arg2)))。 continue。 } //求觀測值的改正 r_r = (x x0)*(x x0) + (y y0)*(y y0)。 dx = (x x0)*(k1*r_r + k2*r_r*r_r) + p1*(r_r + 2 * (x x0)*(x x0)) 2 * p2*(x x0)*(y y0)。 dy = (y y0)*(k1*r_r + k2*r_r*r_r) + p2*(r_r + 2 * (y y0)*(y y0)) 2 * p1*(x x0)*(y y0)。 //經(jīng)過畸變改正后的像點坐標觀測值 x = (x x0) dx。 y = (y y0) dy。 //求矩陣 R各元素的值 a1 = cos(phi)*cos(kappa) sin(phi)*sin(omega)*sin(kappa)。 a2 = cos(phi)*sin(kappa) sin(phi)*sin(omega)*cos(kappa)。 a3 = sin(phi)*cos(omega)。 b1 = cos(omega)*sin(kappa)。 b2 = cos(omega)*cos(kappa)。 b3 = sin(omega)。 c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa)。 c2 = sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa)。 c3 = cos(phi)*cos(omega)。 //求 X_， Y_， Z_ X_ = a1*(X Xs) + b1*(Y Ys) + c1*(Z Zs)。 Y_ = a2*(X Xs) + b2*(Y Ys) + c2*(Z Zs)。 Z_ = a3*(X Xs) + b3*(Y Ys) + c3*(Z Zs)。 //求 x， y的 近似值 double x_ = fx*X_ / Z_。 double y_ = fy*Y_ / Z_。 //求矩陣 L各元素的值 lx = x x_。 ly = y y_。 //求矩陣 A各元素的值 a11 = 1 / Z_*(a1*fx + a3*x)。 a12 = 1 / Z_*(b1*fx + b3*x)。 a13 = 1 / Z_*(c1*fx + c3*x)。 a21 = 1 / Z_*(a2*fy + a3*y)。 a22 = 1 / Z_*(b2*fy + b3*y)。 a23 = 1 / Z_*(c2*fy + c3*y)。 a14 = y*sin(omega) (x / fx*(x*cos(kappa) y*sin(kappa)) + fx*cos(kappa))*cos(omega)。 a15 = fx*sin(kappa) x / fx*(x*sin(kappa) + y*cos(kappa))。 a16 = y。 a24 = x*sin(omega) (y / fy*(x*cos(kappa) y*sin(kappa)) fy*sin(kappa))*cos(omega)。 a25 = fy*cos(kappa) y / fy*(x*sin(kappa) + y*cos(kappa))。 a26 = x。 //合成矩陣 double a[12] = { a11, a12, a13, a14, a15, a16, a21, a22, a23, a24, a25, 20 a26 }。 double l[2] = { lx, ly }。 cv::Mat Ai(2, 6, CV_64FC1, a)。 cv::Mat Li(2, 1, CV_64FC1, l)。 (Ai)。 (Li)。 } //計算 dX = (() * A).inv() * () * L。 Xs+=double(0,0)。 Ys+=double(1,0)。 Zs+=double(2,0)。 phi+=double(3,0)。 omega+=double(4,0)。 kappa+=double(5,0)。 uitextBrowserappend(QString(%1: %2 %3 %4 %5 %6 %7).arg(index).arg(tdouble(0)).arg(double(1)).arg(double(2)).arg(double(3)).arg(double(4)).arg(double(5)))。 index++。 cv::Mat M0 = (A*dX L).t()*(A*dX L)。 m0 = sqrt(double(0) / (2 * () 6))。 ()。 ()。 if (index 100) break。 } while(fabs(double(0))||fabs(double(3))e)。 uitextBrowserappend(QString(Result:))。 uitextBrowserappend(QString(Xs:%1 Ys:%2 Zs:%3 phi:%4 omega:%5 kappa:%6).arg(Xs).arg(Ys).arg(Zs).arg(phi).arg(omega).arg(kappa))。 char *log = 單位權中誤差： 。 uitextBrowserappend(QString(%1%2).arg(QString::fromLocal8Bit(log)).arg(m0))。 } void HFJHDialog::on_pushButton_clicked() { QString qpath = QFileDialog::getOpenFileName(this)。 string path = ()。 if(readxy(path) == 1) { char *log =讀取像點坐標錯誤 XXXXXX。 uitextBrowserappend(QString::fromLocal8Bit(log))。 return。 } char *log = 讀取像點坐標結束 OOOOOO。 uitextBrowserappend(QString::fromLocal8Bit(log))。 uilineEditsetText(qpath)。 } void HFJHDialog::on_pushButton_2_clicked() { QString qpath = QFileDialog::getOpenFileName(this)。 string path = ()。 if(readXYZ(path) == 1) { char *log = 讀取控制點坐標錯誤 XXXXXX。 uitextBrowserappend(QString::fromLocal8Bit(log))。 return。 } char *log = 讀取控制點坐標結束 OOOOOO。 uitextBrowserappend(QString::fromLocal8Bit(log))。 uilineEdit_2setText(qpath)