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