//---------------------------------------------------------
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;
}
