//--------------------------------------------------------- bool CResection::On_Execute(void) { CSG_PointCloud *pPoints; // Input Point Cloud CSG_String fileName; CSG_File *pTabStream = NULL; int n = 6; // Number of unknowns CSG_Vector center(3); CSG_Vector target(3); double c = Parameters("F") ->asDouble(); // Focal Length (mm) double pixWmm = Parameters("W") ->asDouble() / 1000;// Pixel Width (mm) double ppOffsetX = Parameters("ppX") ->asDouble(); // Principal Point Offset X (pixels) double ppOffsetY = Parameters("ppY") ->asDouble(); // Principal Point Offset Y (pixels) pPoints = Parameters("POINTS") ->asPointCloud(); fileName = Parameters("OUTPUT FILE") ->asString(); center[0] = Parameters("Xc") ->asDouble(); center[1] = Parameters("Yc") ->asDouble(); center[2] = Parameters("Zc") ->asDouble(); target[0] = Parameters("Xt") ->asDouble(); target[1] = Parameters("Yt") ->asDouble(); target[2] = Parameters("Zt") ->asDouble(); int pointCount = pPoints->Get_Point_Count(); bool estPPOffsets = false; if ( Parameters("EST_OFFSETS")->asBool() ) { estPPOffsets = true; n = 8; // Increase number of unknowns by 2 } bool applyDistortions = false; CSG_Vector K(3); if ( Parameters("GIVE_DISTORTIONS")->asBool() ) { applyDistortions = true; K[0] = Parameters("K1") ->asDouble(); K[1] = Parameters("K2") ->asDouble(); K[2] = Parameters("K3") ->asDouble(); } double dxapp = center [0] - target [0]; double dyapp = center [1] - target [1]; double dzapp = center [2] - target [2]; double h_d = sqrt (dxapp * dxapp + dyapp * dyapp + dzapp * dzapp); // Distance between Proj. Center & Target (m) double h_dmm = h_d * 1000; // Convert to mm if( fileName.Length() == 0 ) { SG_UI_Msg_Add_Error(_TL("Please provide an output file name!")); return( false ); } pTabStream = new CSG_File(); if( !pTabStream->Open(fileName, SG_FILE_W, false) ) { SG_UI_Msg_Add_Error(CSG_String::Format(_TL("Unable to open output file %s!"), fileName.c_str())); delete (pTabStream); return (false); } CSG_Vector rotns = methods::calcRotations(center,target); // Approx. rotations omega, kappa, alpha CSG_String msg = "********* Initial Approximate Values *********"; pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Rotation Angles:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Omega:\t") + SG_Get_String(rotns[0],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Kappa:\t") + SG_Get_String(rotns[1],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Alpha:\t") + SG_Get_String(rotns[2],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Projection Center:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Xc:\t") + SG_Get_String(center[0],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Yc:\t") + SG_Get_String(center[1],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Zc:\t") + SG_Get_String(center[2],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); if (estPPOffsets) { msg = SG_T("Principal Point Offsets:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("ppX:\t") + SG_Get_String(ppOffsetX,5,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("ppY:\t") + SG_Get_String(ppOffsetY,5,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); } double itrNo = 0; CSG_Matrix invN; while (true) { // Begin Iterations itrNo++; double omega = rotns[0]; double kappa = rotns[1]; double alpha = rotns[2]; CSG_Matrix R = methods::calcRotnMatrix(rotns); // Rotation Matrix from approximate values CSG_Matrix E(3,3); // [w1;w2;w3] = E * [dw;dk;da] E[0][0] = -1; E[0][1] = E[1][0] = E[2][0] = 0; E[0][2] = sin(kappa); E[1][1] = -cos(omega); E[1][2] = -sin(omega) * cos(kappa); E[2][1] = sin(omega); E[2][2] = -cos(omega) * cos(kappa); CSG_Matrix N(n,n); // Transpose(Design Matrix) * I * Design Matrix CSG_Vector ATL(n); // Transpose(Design Matrix) * I * Shortened obs. vector double SS = 0; double sigma_naught = 0; for (int i = 0; i < pointCount; i++) { CSG_Vector pqs(3); // Approx. pi, qi, si for (int j = 0; j < 3; j++) { pqs[j] = R[j][0] * (pPoints->Get_X(i) - center[0]) + R[j][1] * (pPoints->Get_Y(i) - center[1]) + R[j][2] * (pPoints->Get_Z(i) - center[2]); } double p_i = pqs[0]; double q_i = pqs[1]; double s_i = pqs[2]; double dR = 0; // Undistorted double x_u = c * p_i / q_i; double y_u = c * s_i / q_i; double c_hat = c; if (applyDistortions) { double r2 = x_u * x_u + y_u * y_u; dR = K[0] * r2 + K[1] * r2 * r2 + K[2] * r2 * r2 * r2; c_hat = c * (1 - dR); } // Approx. image coordinates (with distortions) double x_i = (1 - dR) * x_u + ppOffsetX * pixWmm; double z_i = (1 - dR) * y_u + ppOffsetY * pixWmm; // Shortened obervation vector: dxi & dzi double dx_i = pPoints->Get_Attribute(i,0) * pixWmm - x_i; double dz_i = pPoints->Get_Attribute(i,1) * pixWmm - z_i; SS += pow(dx_i,2) + pow(dz_i,2); /* x_i, z_i in [mm] p_i,q_i,s_i in [m] h_d in [m] c, c_hat in [mm] h_dmm in [mm] */ CSG_Matrix L(3,2); // CSG_Matrix takes columns first and rows second CSG_Matrix V(3,3); CSG_Matrix LR(3,2); CSG_Matrix LVE(3,2); L[0][0] = L[1][2] = c_hat / (1000 * q_i); L[0][2] = L[1][0] = 0; L[0][1] = -x_u * (1 - dR) / (1000 * q_i); L[1][1] = -y_u * (1 - dR) / (1000 * q_i); V[0][0] = V[1][1] = V[2][2] = 0; V[0][1] = s_i / h_d; V[0][2] = -q_i / h_d; V[1][0] = -s_i / h_d; V[1][2] = p_i / h_d; V[2][0] = q_i / h_d; V[2][1] = -p_i / h_d; LVE = ( L * V ) * E; LR = L * R; // Design Matrix (J) CSG_Matrix design(n,2); for(int j = 0; j < 2; j++) { for(int k = 0; k < 3; k++) { design[j][k] = LVE[j][k]; design[j][k+3] = -LR[j][k]; } } if ( estPPOffsets ) { design[0][6] = design[1][7] = 1.0; } // Build Normal Matrix for(int j = 0; j < n; j++) { for(int k = 0; k < n; k++) { N[j][k] += (design[0][j] * design[0][k] + design[1][j] * design[1][k]); } } // Build Tranpose (J) * I * (Shortened obs. vector) for (int m=0; m < n; m++) { ATL[m] += design[0][m] * dx_i + design[1][m] * dz_i; } L.Destroy(); V.Destroy(); LR.Destroy(); LVE.Destroy(); pqs.Destroy(); design.Destroy(); } // end looping over observations // Eigen values and Eigen Vectors CSG_Vector eigenVals(n); CSG_Matrix eigenVecs(n,n); SG_Matrix_Eigen_Reduction(N, eigenVecs, eigenVals, true); // One of the Eigen Values is 0 if (std::any_of(eigenVals.cbegin(), eigenVals.cend(), [] (double i) { return i == 0; })) { msg = "The Normal Matrix has a rank defect. Please measure more points."; pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); break; } double mx = *std::max_element(eigenVals.cbegin(), eigenVals.cend()); double mn = *std::min_element(eigenVals.cbegin(), eigenVals.cend()); // Ratio of Smallest to the Biggest Eigen value is too small if ((mn / mx) < pow(10,-12.0)) { msg = SG_T("Condition of the Matrix of Normal Equations:\t") + CSG_String::Format(SG_T(" %13.5e"), mn/mx); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = "The Normal Matrix is weakly conditioned. Please measure more points."; pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); break; } // Calculate the adjustments double absMax = 0; invN = N.Get_Inverse(); CSG_Vector est_param_incs = invN * ATL; for (int i = 0; i < n; i++) { if (abs(est_param_incs[i]) > absMax) { absMax = abs(est_param_incs[i]); } } if (absMax < thresh) { msg = "Solution has converged."; pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); break; } for (int a = 0; a < 3; a++) { rotns[a] += est_param_incs[a] / h_dmm; // New Approx. rotations omega, kappa, alpha center[a] += est_param_incs[a+3] / 1000; // New Approx. Projection Center } if ( estPPOffsets ) { ppOffsetX += (est_param_incs[6] / pixWmm); // New Approx. Principal Point ppOffsetY += (est_param_incs[7] / pixWmm); } sigma_naught = sqrt(SS / (2 * pointCount - n)); // Writing To Output File & SAGA Console msg = "********* Iteration: " + SG_Get_String(itrNo,0,false) + " *********"; pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = "Sum of Squared Residuals:\t" + SG_Get_String(SS,5,false); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = "Sigma Naught:\t" + SG_Get_String(sigma_naught,5,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Condition of the Matrix of Normal Equations:\t") + CSG_String::Format(SG_T(" %13.5e"), mn/mx); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); R.Destroy(); E.Destroy(); N.Destroy(); ATL.Destroy(); invN.Destroy(); eigenVals.Destroy(); eigenVecs.Destroy(); est_param_incs.Destroy(); } // end of iterations msg = "********* Final Estimated Parameters *********"; pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Rotation Angles:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Omega:\t") + SG_Get_String(rotns[0],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Kappa:\t") + SG_Get_String(rotns[1],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Alpha:\t") + SG_Get_String(rotns[2],6,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Projection Center:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Xc:\t") + SG_Get_String(center[0],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Yc:\t") + SG_Get_String(center[1],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("Zc:\t") + SG_Get_String(center[2],4,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); if (estPPOffsets) { msg = SG_T("Principal Point Offsets:"); pTabStream->Write(SG_T("\n") + msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("ppX:\t") + SG_Get_String(ppOffsetX,5,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); msg = SG_T("ppY:\t") + SG_Get_String(ppOffsetY,5,false); pTabStream->Write(msg + SG_T("\n")); SG_UI_Msg_Add(msg, true); } K.Destroy(); rotns.Destroy(); center.Destroy(); target.Destroy(); pTabStream->Close(); return true; }
//TODO Must make sure we always use 32F or 64F. void PCAMerge::computeAdd() { // Add checks for errors, Exceptions // TODO : Add checks against model, number of observations. Create eigenVals and eigenVecs accordingly. Refer Matlab code for possible checks to be implemented // // From here, going forward assuming no other errors possible // float eps = 1e-3; // New number of Observations nObs = N + M; // TODO add check for nObs being zero // New model's mean. mean = ( (N * m1.mean) + (M * m2.mean) ) / nObs; // Vector joining the centres cv::Mat dorg = m1.mean - m2.mean; // Note: // dorg : n x 1 // m1.eigenvectors : n x p // m2.eigenvectors : n x q // G : p x q // H : n x q // g : p x 1 // h : n x 1 //Note Eigenvectors from cv::PCA are stored as rows. We need to transpose them. //Note We should probably put the transposed vectors in other matrices, in order to leave the original models untouched (Luca) m1.eigenvectors = m1.eigenvectors.t(); m2.eigenvectors = m2.eigenvectors.t(); //New Basis cv::Mat G = m1.eigenvectors.t() * m2.eigenvectors; cv::Mat H = m2.eigenvectors - ( m1.eigenvectors * G );// H is orthogonal to m1.eigenvectors cv::Mat g = m1.eigenvectors.t() * dorg; cv::Mat h = dorg - (m1.eigenvectors * g); // h is orthogonal to dorg //Some vectors in H can be zero vectors. Must be removed cv::Mat sumH = cv::Mat::zeros( 1, H.cols, CV_64FC1 ); cv::reduce( H.mul(H), sumH, 0, cv::REDUCE_SUM ); // Even h can be a zero vector. Must not be used if so double sumh = 0; sumh = h.dot(h); // // Get indices of sumH > eps. use it to construct vector nu cv::Mat newH; for( int i=0; i < sumH.cols; i++ ) { if( sumH.at<double>(i) > eps ) newH.push_back( H.col(i).t() ); } if (sumh > eps) newH.push_back( h.t() ); newH = newH.t(); // Dimension of newH must be n x t std::cout << newH.size() << std::endl; //TODO : Implement Gram Schmidt Orthonormalization. DONE cv::Mat nu = orth( newH ); //TODO : Forgetting about residues at the moment. //Residues are the eigenvalues which were not used in the model m1 / m2. //The following was used in matlab for including residues //resn1 = size( m1.vct, 1) - size(m1.vct,2 ); /* if resn1 > 0 rpern1 = m1.residue / resn1; else rpern1 = 0; end resn2 = size( m2.vct, 1) - size(m2.vct,2 ); if resn2 > 0 rpern2 = m2.residue / resn2; else rpern2 = 0; end */ //First part of the matrix in equation (20) in paper - Correlation of m1 // int n,p,t,q; n = m1.eigenvectors.rows; // = m2.eigenvectors.rows t = nu.cols; // p = m1.eigenvalues.rows; q = m2.eigenvalues.rows; cv::Mat tempeval = cv::Mat::zeros( (p + t) , 1, m1.eigenvalues.type() ); m1.eigenvalues.copyTo( tempeval.rowRange(0,m1.eigenvalues.rows) ); cv::Mat A1 = ( N / nObs ) * cv::Mat::diag(tempeval); // Correlation of m2 cv::Mat Gamma = nu.t() * m2.eigenvectors; cv::Mat D = G * cv::Mat::diag( m2.eigenvalues ); cv::Mat E = Gamma * cv::Mat::diag( m2.eigenvalues); cv::Mat A2 = cv::Mat::zeros( A1.size(), A1.type() ); A2( cv::Range(0,p), cv::Range(0, p) ) = D * G.t(); A2( cv::Range(0,p), cv::Range(p, A1.cols) ) = D * Gamma.t(); A2( cv::Range(p, A1.rows), cv::Range(0,p) ) = E * G.t(); A2( cv::Range(p, A1.rows), cv::Range(p, A1.cols) ) = E * Gamma.t(); A2 = A2 * ( M / nObs ); //Third Part : term for diff between means cv::Mat gamma = nu.t() * dorg; cv::Mat A3 = cv::Mat( A1.size(), A1.type() ); A3( cv::Range(0,p), cv::Range(0,p) ) = g * g.t(); A3( cv::Range(0,p), cv::Range(p, A1.cols) ) = g * gamma.t(); A3( cv::Range(p, A1.rows), cv::Range(0,p) ) = gamma * g.t(); A3( cv::Range(p, A1.rows), cv::Range(p, A1.cols) ) = gamma * gamma.t(); A3 = ( N * M / (nObs*nObs) ) * A3; // Guard against rounding errors cv::Mat A = A1 + A2 + A3; A = ( A + A.t() ) / 2.0; /* (Luca) Is this step right? Because PCA expects A to have samples inside, not correlation matrices. We should probably call cv::eigen instead [m3.vct m3.val] = eig( A ); % the eigen-solution m3.vct = [m1.vct nu]* m3.vct; % rotate the basis set into place - can fail for v.high dim data m3.val = diag(m3.val); % keep only the diagonal */ m3 = cv::PCA( A, cv::noArray(), cv::PCA::DATA_AS_ROW ); eigenVals = m3.eigenvalues; m3.eigenvectors = m3.eigenvectors.t(); cv::Mat m3Temp = cv::Mat::zeros( n, A.cols, A.type() ); m3Temp( cv::Range::all(), cv::Range(0,p)) = m1.eigenvectors; m3Temp( cv::Range::all(), cv::Range(p, A.cols)) = nu; eigenVecs = m3Temp * m3.eigenvectors; //Look at how many eigenvalues must be returned. Call that function as required. //Calling the function like the matlab code. //I'd try not to transpose the eigenvectors in order to be more OpenCV friendly. int nValsToKeep = keepVals(KEEP_T,eigenVals,eps); eigenVals = eigenVals(cv::Range(0,nValsToKeep),cv::Range::all()).clone(); eigenVecs = eigenVecs(cv::Range::all(),cv::Range(0,nValsToKeep)).clone(); }