//--------------------------------------------------------- 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; }
//--------------------------------------------------------- bool CPC_Drop_Attribute::On_Execute(void) { //------------------------------------------------- int *Features = (int *)Parameters("FIELDS")->asPointer(); int nFeatures = Parameters("FIELDS")->asInt (); if( !Features || nFeatures <= 0 ) { Error_Set(_TL("You must specify at least one attribute to drop!")); return( false ); } //----------------------------------------------------- CSG_PointCloud *pInput = Parameters("INPUT" )->asPointCloud(); CSG_PointCloud *pOutput = Parameters("OUTPUT")->asPointCloud(), Output; if( !pOutput || pOutput == pInput ) { pOutput = &Output; } pOutput->Create(pInput); //----------------------------------------------------- int i; std::set<int> setCols; std::set<int>::iterator it; setCols.clear(); for(i=0; i<nFeatures; i++) { setCols.insert(Features[i]); } for(i=0, it=setCols.begin(); it!=setCols.end(); i++, it++) { pOutput->Del_Field(*it - i); } //----------------------------------------------------- for(i=0; i<pInput->Get_Point_Count() && SG_UI_Process_Set_Progress(i, pInput->Get_Count()); i++) { pOutput->Add_Point(pInput->Get_X(i), pInput->Get_Y(i), pInput->Get_Z(i)); for(int j=0, k=0; j<pInput->Get_Attribute_Count(); j++, k++) { it = setCols.find(j + 3); if( it != setCols.end() ) { k--; continue; } switch (pInput->Get_Attribute_Type(j)) { default: pOutput->Set_Attribute(k, pInput->Get_Attribute(i, j)); break; case SG_DATATYPE_Date: case SG_DATATYPE_String: CSG_String sAttr; pInput->Get_Attribute(i, j, sAttr); pOutput->Set_Attribute(k, sAttr); break; } } } //----------------------------------------------------- if( pOutput == &Output ) { CSG_MetaData History = pInput->Get_History(); CSG_String Name = pInput->Get_Name (); pInput->Assign(pOutput); pInput->Get_History() = History; pInput->Set_Name(Name); Parameters("OUTPUT")->Set_Value(pInput); } else { pOutput->Fmt_Name("%s [%s]", pInput->Get_Name(), _TL("Dropped Attributes")); } //----------------------------------------------------- return( true ); }