//---------------------------------------------------------
double CSG_Vector::Get_Angle(const CSG_Vector &Vector) const
{
if( Get_N() > Vector.Get_N() )
{
return( Vector.Get_Angle(*this) );
}
int i;
double A, B, z, *Z = Get_Data();
if( (A = Get_Length()) > 0.0 && (B = Vector.Get_Length()) > 0.0 )
{
for(i=0, z=0.0; i<Get_N(); i++)
{
z += Vector[i] * Z[i];
}
for(i=Get_N(); i<Vector.Get_N(); i++)
{
z += Vector[i];
}
return( acos(z / (A * B)) );
}
return( 0.0 );
}
//---------------------------------------------------------
int CGW_Multi_Regression_Grid::Get_Variables(int x, int y, CSG_Vector &z, CSG_Vector &w, CSG_Matrix &Y)
{
TSG_Point Point = m_dimModel.Get_Grid_to_World(x, y);
int nPoints = m_Search.is_Okay() ? (int)m_Search.Select_Nearest_Points(Point.x, Point.y, m_nPoints_Max, m_Radius, m_Direction) : m_Points.Get_Count();
z.Create(nPoints);
w.Create(nPoints);
Y.Create(1 + m_nPredictors, nPoints);
for(int iPoint=0; iPoint<nPoints; iPoint++)
{
double ix, iy, iz;
CSG_Shape *pPoint = m_Search.is_Okay() && m_Search.Get_Selected_Point(iPoint, ix, iy, iz)
? m_Points.Get_Shape((int)iz)
: m_Points.Get_Shape(iPoint);
z[iPoint] = pPoint->asDouble(0);
w[iPoint] = m_Weighting.Get_Weight(SG_Get_Distance(Point, pPoint->Get_Point(0)));
Y[iPoint][0] = 1.0;
for(int iPredictor=1; iPredictor<=m_nPredictors; iPredictor++)
{
Y[iPoint][iPredictor] = pPoint->asDouble(iPredictor);
}
}
return( nPoints );
}
//---------------------------------------------------------
bool CSG_Vector::Assign(const CSG_Vector &Vector)
{
if( Create(Vector.Get_N()) )
{
memcpy(Get_Data(), Vector.Get_Data(), Get_N() * sizeof(double));
return( true );
}
return( false );
}
//---------------------------------------------------------
CSG_Vector CSG_Matrix::Get_Row(int iRow) const
{
CSG_Vector Row;
if( iRow >= 0 && iRow < m_ny )
{
Row.Create(m_nx, m_z[iRow]);
}
return( Row );
}
//---------------------------------------------------------
bool CSG_Vector::Subtract(const CSG_Vector &Vector)
{
if( Get_N() == Vector.Get_N() && Get_N() > 0 )
{
for(int i=0; i<Get_N(); i++)
{
Get_Data()[i] -= Vector.Get_Data()[i];
}
return( true );
}
return( false );
}
//---------------------------------------------------------
CSG_Vector CSG_Matrix::Get_Col(int iCol) const
{
CSG_Vector Col;
if( iCol >= 0 && iCol < m_nx )
{
Col.Create(m_ny);
for(int y=0; y<m_ny; y++)
{
Col[y] = m_z[y][iCol];
}
}
return( Col );
}
//---------------------------------------------------------
bool CSG_Vector::is_Equal(const CSG_Vector &Vector) const
{
if( Get_N() == Vector.Get_N() )
{
for(int i=0; i<Get_N(); i++)
{
if( Get_Data(i) != Vector.Get_Data(i) )
{
return( false );
}
}
return( true );
}
return( false );
}
//---------------------------------------------------------
int CGWR_Grid_Downscaling::Get_Variables(int x, int y, CSG_Vector &z, CSG_Vector &w, CSG_Matrix &Y)
{
int n = 0;
z.Create(m_Search.Get_Count());
w.Create(m_Search.Get_Count());
Y.Create(1 + m_nPredictors, m_Search.Get_Count());
//-----------------------------------------------------
for(int i=0, ix, iy; i<m_Search.Get_Count(); i++)
{
double id, iw;
if( m_Search.Get_Values(i, ix = x, iy = y, id, iw, true) && m_pDependent->is_InGrid(ix, iy) )
{
z[n] = m_pDependent->asDouble(ix, iy);
w[n] = iw;
Y[n][0] = 1.0;
for(int j=0; j<m_nPredictors && iw>0.0; j++)
{
if( !m_pPredictors[j]->is_NoData(ix, iy) )
{
Y[n][j + 1] = m_pPredictors[j]->asDouble(ix, iy);
}
else
{
iw = 0.0;
}
}
if( iw > 0.0 )
{
n++;
}
}
}
//-----------------------------------------------------
z.Set_Rows(n);
w.Set_Rows(n);
Y.Set_Rows(n);
return( n );
}
//---------------------------------------------------------
bool SG_Matrix_Solve(CSG_Matrix &Matrix, CSG_Vector &Vector, bool bSilent)
{
bool bResult = false;
int n = Vector.Get_N();
if( n > 0 && n == Matrix.Get_NX() && n == Matrix.Get_NY() )
{
int *Permutation = (int *)SG_Malloc(n * sizeof(int));
if( SG_Matrix_LU_Decomposition(n, Permutation, Matrix.Get_Data(), bSilent) )
{
SG_Matrix_LU_Solve(n, Permutation, Matrix, Vector.Get_Data(), bSilent);
bResult = true;
}
SG_Free(Permutation);
}
return( bResult );
}
CSG_Vector CSG_Matrix::Multiply(const CSG_Vector &Vector) const
{
CSG_Vector v;
if( m_nx == Vector.Get_N() && v.Create(m_ny) )
{
for(int y=0; y<m_ny; y++)
{
double z = 0.0;
for(int x=0; x<m_nx; x++)
{
z += m_z[y][x] * Vector(x);
}
v[y] = z;
}
}
return( v );
}
//---------------------------------------------------------
double CSG_Vector::Multiply_Scalar(const CSG_Vector &Vector) const
{
double z = 0.0;
if( Get_N() == Vector.Get_N() )
{
for(int i=0; i<Get_N(); i++)
{
z += Get_Data()[i] * Vector[i];
}
}
return( z );
}
//---------------------------------------------------------
bool CSG_Vector::Multiply(const CSG_Vector &Vector)
{
if( Get_N() == Vector.Get_N() && Get_N() == 3 )
{
CSG_Vector v(*this);
Get_Data()[0] = v[1] * Vector[2] - v[2] * Vector[1];
Get_Data()[1] = v[2] * Vector[0] - v[0] * Vector[2];
Get_Data()[2] = v[0] * Vector[1] - v[1] * Vector[0];
return( true );
}
return( false );
}
//---------------------------------------------------------
std::pair<double,double> minmax(CSG_Vector v)
{
double min = v[0];
double max = v[0];
for (int i = 0; i < v.Get_Length(); i++) {
if (v[i] > v[0]) {
max = v[i];
}
else if (v[i] < v[0]) {
min = v[i];
}
}
return std::make_pair(min, max);
}
//---------------------------------------------------------
// find_obs() - Function to find the observed vector as part of
// the set of normal equations for least squares.
// V.1.0, Jo Wood, 11th December, 1994.
//---------------------------------------------------------
bool CParam_Scale::Get_Observed(int x, int y, CSG_Vector &Observed, bool bConstrain)
{
if( m_pDEM->is_NoData(x, y)
|| x < m_Radius || x > Get_NX() - m_Radius
|| y < m_Radius || y > Get_NY() - m_Radius )
{
return( false );
}
//-----------------------------------------------------
int ix, iy, jx, jy;
double dx, dy, dz, z;
Observed.Create(6);
z = m_pDEM->asDouble(x, y);
for(iy=0, jy=y-m_Radius, dy=-m_Radius*Get_Cellsize(); iy<m_Weights.Get_NY(); iy++, jy++, dy+=Get_Cellsize())
{
for(ix=0, jx=x-m_Radius, dx=-m_Radius*Get_Cellsize(); ix<m_Weights.Get_NX(); ix++, jx++, dx+=Get_Cellsize())
{
dz = m_pDEM->is_InGrid(jx, jy) ? m_pDEM->asDouble(jx, jy) - z : 0.0;
if( dz )
{
dz *= m_Weights[iy][ix];
Observed[0] += dz * dx * dx;
Observed[1] += dz * dy * dy;
Observed[2] += dz * dx * dy;
Observed[3] += dz * dx;
Observed[4] += dz * dy;
if( !bConstrain ) // if constrained, should remain 0.0
{
Observed[5] += dz;
}
}
}
}
return( true );
}
bool CSG_Matrix::Set_Row(int iRow, const CSG_Vector &Data)
{
return( m_nx == Data.Get_N() ? Set_Row(iRow, Data.Get_Data()) : false );
}
bool CSG_Matrix::Ins_Row(int iRow, const CSG_Vector &Data)
{
return( m_ny == 0 ? Add_Row(Data) : m_nx == Data.Get_N() ? Ins_Row(iRow, Data.Get_Data()) : false );
}
//---------------------------------------------------------
bool SG_Matrix_LU_Decomposition(int n, int *Permutation, double **Matrix, bool bSilent)
{
int i, j, k, iMax;
double dMax, d, Sum;
CSG_Vector Vector;
Vector.Create(n);
for(i=0, iMax=0; i<n && (bSilent || SG_UI_Process_Set_Progress(i, n)); i++)
{
dMax = 0.0;
for(j=0; j<n; j++)
{
if( (d = fabs(Matrix[i][j])) > dMax )
{
dMax = d;
}
}
if( dMax <= 0.0 ) // singular matrix !!!...
{
return( false );
}
Vector[i] = 1.0 / dMax;
}
for(j=0; j<n && (bSilent || SG_UI_Process_Set_Progress(j, n)); j++)
{
for(i=0; i<j; i++)
{
Sum = Matrix[i][j];
for(k=0; k<i; k++)
{
Sum -= Matrix[i][k] * Matrix[k][j];
}
Matrix[i][j] = Sum;
}
for(i=j, dMax=0.0; i<n; i++)
{
Sum = Matrix[i][j];
for(k=0; k<j; k++)
{
Sum -= Matrix[i][k] * Matrix[k][j];
}
Matrix[i][j] = Sum;
if( (d = Vector[i] * fabs(Sum)) >= dMax )
{
dMax = d;
iMax = i;
}
}
if( j != iMax )
{
for(k=0; k<n; k++)
{
d = Matrix[iMax][k];
Matrix[iMax][k] = Matrix[j ][k];
Matrix[j ][k] = d;
}
Vector[iMax] = Vector[j];
}
Permutation[j] = iMax;
if( Matrix[j][j] == 0.0 )
{
Matrix[j][j] = M_TINY;
}
if( j != n )
{
d = 1.0 / (Matrix[j][j]);
for(i=j+1; i<n; i++)
{
Matrix[i][j] *= d;
}
}
}
return( bSilent || SG_UI_Process_Get_Okay(false) );
}
bool CSG_Matrix::Ins_Col(int iCol, const CSG_Vector &Data)
{
return( m_nx == 0 ? Add_Col(Data) : m_ny == Data.Get_N() ? Ins_Col(iCol, Data.Get_Data()) : false );
}
bool CSG_Matrix::Add_Row(const CSG_Vector &Data)
{
return( m_ny == 0 ? Create(Data.Get_N(), 1, Data.Get_Data()) : m_nx == Data.Get_N() ? Add_Row(Data.Get_Data()) : false );
}
bool CSG_Matrix::Set_Col(int iCol, const CSG_Vector &Data)
{
return( m_ny == Data.Get_N() ? Set_Col(iCol, Data.Get_Data()) : false );
}
bool SG_Matrix_Triangular_Decomposition(CSG_Matrix &A, CSG_Vector &d, CSG_Vector &e)
{
if( A.Get_NX() != A.Get_NY() )
{
return( false );
}
int l, k, j, i, n;
double scale, hh, h, g, f;
n = A.Get_NX();
d.Create(n);
e.Create(n);
for(i=n-1; i>=1; i--)
{
l = i - 1;
h = scale = 0.0;
if( l > 0 )
{
for(k=0; k<=l; k++)
{
scale += fabs(A[i][k]);
}
if( scale == 0.0 )
{
e[i] = A[i][l];
}
else
{
for(k=0; k<=l; k++)
{
A[i][k] /= scale;
h += A[i][k] * A[i][k];
}
f = A[i][l];
g = f > 0.0 ? -sqrt(h) : sqrt(h);
e[i] = scale * g;
h -= f * g;
A[i][l] = f - g;
f = 0.0;
for(j=0; j<=l; j++)
{
A[j][i] = A[i][j]/h;
g = 0.0;
for(k=0; k<=j; k++)
{
g += A[j][k] * A[i][k];
}
for(k=j+1; k<=l; k++)
{
g += A[k][j] * A[i][k];
}
e[j] = g / h;
f += e[j] * A[i][j];
}
hh = f / (h + h);
for(j=0; j<=l; j++)
{
f = A[i][j];
e[j] = g = e[j] - hh * f;
for(k=0; k<=j; k++)
{
A[j][k] -= (f * e[k] + g * A[i][k]);
}
}
}
}
else
{
e[i] = A[i][l];
}
d[i] = h;
}
d[0] = 0.0;
e[0] = 0.0;
for(i=0; i<n; i++)
{
l = i - 1;
if( d[i] )
{
for(j=0; j<=l; j++)
{
g = 0.0;
for(k=0; k<=l; k++)
{
g += A[i][k] * A[k][j];
}
for(k=0; k<=l; k++)
{
A[k][j] -= g * A[k][i];
}
}
}
d[i] = A[i][i];
A[i][i] = 1.0;
for(j=0; j<=l; j++)
{
A[j][i] = A[i][j] = 0.0;
}
}
return( true );
}
bool SG_Matrix_Tridiagonal_QL(CSG_Matrix &Q, CSG_Vector &d, CSG_Vector &e)
{
if( Q.Get_NX() != Q.Get_NY() || Q.Get_NX() != d.Get_N() || Q.Get_NX() != e.Get_N() )
{
return( false );
}
int m, l, iter, i, k, n;
double s, r, p, g, f, dd, c, b;
n = d.Get_N();
for(i=1; i<n; i++)
{
e[i - 1] = e[i];
}
e[n - 1] = 0.0;
for(l=0; l<n; l++)
{
iter = 0;
do
{
for(m=l; m<n-1; m++)
{
dd = fabs(d[m]) + fabs(d[m + 1]);
if( fabs(e[m]) + dd == dd )
{
break;
}
}
if( m != l )
{
if( iter++ == 30 )
{
return( false ); // erhand("No convergence in TLQI.");
}
g = (d[l+1] - d[l]) / (2.0 * e[l]);
r = sqrt((g * g) + 1.0);
g = d[m] - d[l] + e[l] / (g + M_SET_SIGN(r, g));
s = c = 1.0;
p = 0.0;
for(i = m-1; i >= l; i--)
{
f = s * e[i];
b = c * e[i];
if (fabs(f) >= fabs(g))
{
c = g / f;
r = sqrt((c * c) + 1.0);
e[i+1] = f * r;
c *= (s = 1.0/r);
}
else
{
s = f / g;
r = sqrt((s * s) + 1.0);
e[i+1] = g * r;
s *= (c = 1.0/r);
}
g = d[i+1] - p;
r = (d[i] - g) * s + 2.0 * c * b;
p = s * r;
d[i+1] = g + p;
g = c * r - b;
for(k=0; k<n; k++)
{
f = Q[k][i+1];
Q[k][i+1] = s * Q[k][i] + c * f;
Q[k][i] = c * Q[k][i] - s * f;
}
}
d[l] = d[l] - p;
e[l] = g;
e[m] = 0.0;
}
}
while( m != l );
}
return( true );
}
//---------------------------------------------------------
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 CParam_Scale::On_Execute(void)
{
//-----------------------------------------------------
bool bConstrain;
int Index[6];
double zScale, Tol_Slope, Tol_Curve;
CSG_Matrix Normal;
//-----------------------------------------------------
bConstrain = Parameters("CONSTRAIN")->asBool();
zScale = Parameters("ZSCALE" )->asDouble(); if( zScale <= 0.0 ) { zScale = 1.0; }
Tol_Slope = Parameters("TOL_SLOPE")->asDouble();
Tol_Curve = Parameters("TOL_CURVE")->asDouble();
m_pDEM = Parameters("DEM" )->asGrid();
//-----------------------------------------------------
CSG_Grid *pFeature = Parameters("FEATURES" )->asGrid();
CSG_Grid *pElevation = Parameters("ELEVATION")->asGrid();
CSG_Grid *pSlope = Parameters("SLOPE" )->asGrid();
CSG_Grid *pAspect = Parameters("ASPECT" )->asGrid();
CSG_Grid *pProfC = Parameters("PROFC" )->asGrid();
CSG_Grid *pPlanC = Parameters("PLANC" )->asGrid();
CSG_Grid *pLongC = Parameters("LONGC" )->asGrid();
CSG_Grid *pCrosC = Parameters("CROSC" )->asGrid();
CSG_Grid *pMiniC = Parameters("MINIC" )->asGrid();
CSG_Grid *pMaxiC = Parameters("MAXIC" )->asGrid();
//-----------------------------------------------------
if( !Get_Weights() )
{
return( false );
}
if( !Get_Normal(Normal) )
{
return( false );
}
// To constrain the quadtratic through the central cell, ignore the calculations involving the
// coefficient f. Since these are all in the last row and column of the matrix, simply redimension.
if( !SG_Matrix_LU_Decomposition(bConstrain ? 5 : 6, Index, Normal.Get_Data()) )
{
return( false );
}
//-----------------------------------------------------
for(int y=0; y<Get_NY() && Set_Progress(y); y++)
{
#pragma omp parallel for
for(int x=0; x<Get_NX(); x++)
{
CSG_Vector Observed;
double elevation, slope, aspect, profc, planc, longc, crosc, minic, maxic;
if( Get_Observed(x, y, Observed, bConstrain)
&& SG_Matrix_LU_Solve(bConstrain ? 5 : 6, Index, Normal, Observed.Get_Data()) )
{
Get_Parameters(zScale, Observed.Get_Data(), elevation, slope, aspect, profc, planc, longc, crosc, minic, maxic);
GRID_SET_VALUE(pFeature , Get_Feature(slope, minic, maxic, crosc, Tol_Slope, Tol_Curve));
GRID_SET_VALUE(pElevation, elevation + m_pDEM->asDouble(x, y)); // Add central elevation back
GRID_SET_VALUE(pSlope , slope);
GRID_SET_VALUE(pAspect , aspect);
GRID_SET_VALUE(pProfC , profc);
GRID_SET_VALUE(pPlanC , planc);
GRID_SET_VALUE(pLongC , longc);
GRID_SET_VALUE(pCrosC , crosc);
GRID_SET_VALUE(pMiniC , minic);
GRID_SET_VALUE(pMaxiC , maxic);
}
else
{
GRID_SET_NODATA(pFeature);
GRID_SET_NODATA(pElevation);
GRID_SET_NODATA(pSlope);
GRID_SET_NODATA(pAspect);
GRID_SET_NODATA(pProfC);
GRID_SET_NODATA(pPlanC);
GRID_SET_NODATA(pLongC);
GRID_SET_NODATA(pCrosC);
GRID_SET_NODATA(pMiniC);
GRID_SET_NODATA(pMaxiC);
}
}
}
//-----------------------------------------------------
CSG_Parameter *pLUT = DataObject_Get_Parameter(pFeature, "LUT");
if( pLUT && pLUT->asTable() )
{
pLUT->asTable()->Del_Records();
LUT_SET_CLASS(FLAT , _TL("Planar" ), SG_GET_RGB(180, 180, 180));
LUT_SET_CLASS(PIT , _TL("Pit" ), SG_GET_RGB( 0, 0, 0));
LUT_SET_CLASS(CHANNEL, _TL("Channel" ), SG_GET_RGB( 0, 0, 255));
LUT_SET_CLASS(PASS , _TL("Pass (saddle)"), SG_GET_RGB( 0, 255, 0));
LUT_SET_CLASS(RIDGE , _TL("Ridge" ), SG_GET_RGB(255, 255, 0));
LUT_SET_CLASS(PEAK , _TL("Peak" ), SG_GET_RGB(255, 0, 0));
DataObject_Set_Parameter(pFeature, pLUT);
DataObject_Set_Parameter(pFeature, "COLORS_TYPE", 1); // Color Classification Type: Lookup Table
}
//-----------------------------------------------------
DataObject_Set_Colors(pSlope , 11, SG_COLORS_YELLOW_RED);
DataObject_Set_Colors(pAspect, 11, SG_COLORS_ASPECT_3);
DataObject_Set_Colors(pProfC , 11, SG_COLORS_RED_GREY_BLUE, true);
DataObject_Set_Colors(pPlanC , 11, SG_COLORS_RED_GREY_BLUE, false);
DataObject_Set_Colors(pLongC , 11, SG_COLORS_RED_GREY_BLUE, true);
DataObject_Set_Colors(pCrosC , 11, SG_COLORS_RED_GREY_BLUE, true);
DataObject_Set_Colors(pMiniC , 11, SG_COLORS_RED_GREY_BLUE, true);
DataObject_Set_Colors(pMaxiC , 11, SG_COLORS_RED_GREY_BLUE, true);
//-----------------------------------------------------
return( true );
}