//---------------------------------------------------------
bool CKriging_Simple::Get_Weights(const CSG_Points_Z &Points, CSG_Matrix &W)
{
int n = Points.Get_Count();
if( n > 0 )
{
int n = Points.Get_Count();
W.Create(n, n);
for(int i=0; i<n; i++)
{
W[i][i] = 0.0; // diagonal...
for(int j=i+1; j<n; j++)
{
W[i][j] = W[j][i] = Get_Weight(Points.Get_X(i), Points.Get_Y(i), Points.Get_X(j), Points.Get_Y(j));
}
}
return( W.Set_Inverse(!m_Search.Do_Use_All(), n) );
}
return( false );
}
//---------------------------------------------------------
int CKriging_Universal::Get_Weights(const TSG_Point &p, CSG_Matrix &W, CSG_Points_Z &Points)
{
//-----------------------------------------------------
int n = m_Search.Get_Nearest_Points(Points, p, m_nPoints_Max, m_Radius, m_Direction);
if( n >= m_nPoints_Min )
{
int i, j, k;
int nCoords = m_bCoords ? 2 : 0;
int nGrids = m_pGrids->Get_Count();
W.Create(n + 1 + nGrids + nCoords, n + 1 + nGrids + nCoords);
//-------------------------------------------------
for(i=0; i<n; i++)
{
W[i][i] = 0.0; // diagonal...
W[i][n] = W[n][i] = 1.0; // edge...
for(j=i+1; j<n; j++)
{
W[i][j] = W[j][i] = Get_Weight(Points[i], Points[j]);
}
for(k=0, j=n+1; k<nGrids; k++, j++)
{
W[i][j] = W[j][i] = m_pGrids->asGrid(k)->Get_Value(Points[i].x, Points[i].y, m_Interpolation);
}
for(k=0, j=n+nGrids+1; k<nCoords; k++, j++)
{
W[i][j] = W[j][i] = k == 0 ? Points[i].x : Points[i].y;
}
}
for(i=n; i<=n+nGrids+nCoords; i++)
{
for(j=n; j<=n+nGrids+nCoords; j++)
{
W[i][j] = 0.0;
}
}
if( W.Set_Inverse(true, n + 1 + nGrids + nCoords) )
{
return( n );
}
}
return( 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 );
}
//---------------------------------------------------------
void CWaterRetentionCapacity::Get_WaterRetention(CSG_Matrix &Data, double fC, CSG_Shape *pPoint)
{
int i;
double fWRC = 0, fPerm = 0, fHe = 0, fK = 0, fCCC = 0, fCIL = 0, fTotalDepth = 0;
CSG_Vector CCC (Data.Get_NRows());
CSG_Vector CIL (Data.Get_NRows());
CSG_Vector K (Data.Get_NRows());
CSG_Vector Perm(Data.Get_NRows());
CSG_Vector He (Data.Get_NRows());
CSG_Vector CRA (Data.Get_NRows());
for(i=0; i<Data.Get_NRows(); i++)
{
CCC [i] = Get_CCC(Data[i]);
CIL [i] = Get_CIL(Data[i]);
He [i] = Get_He (Data[i]);
Perm[i] = Get_Permeability(CCC[i], CIL[i]);
if( i > 0 )
{
K[i] = Get_K(Perm[i - 1], Perm[i], fC);
}
CRA[i] = (double)((12.5 * He[i] + 12.5 * (50. - He[i]) * K[i] / 2.) * Data[i][1] / 100.);
fTotalDepth += Data[i][0];
}
for(i=0; i<Data.Get_NRows(); i++)
{
fWRC += Data[i][0] / fTotalDepth * CRA [i];
fCCC += Data[i][0] / fTotalDepth * CCC [i];
fCIL += Data[i][0] / fTotalDepth * CIL [i];
fPerm += Data[i][0] / fTotalDepth * Perm[i];
fHe += Data[i][0] / fTotalDepth * He [i];
fK += Data[i][0] / fTotalDepth * K [i];
}
pPoint->Set_Value(0, fCCC );
pPoint->Set_Value(1, fCIL );
pPoint->Set_Value(2, fPerm);
pPoint->Set_Value(3, fHe );
pPoint->Set_Value(4, fWRC );
}
//---------------------------------------------------------
bool CSG_mRMR::Set_Data(CSG_Matrix &Data, int ClassField, double Threshold)
{
if( !Get_Memory(Data.Get_NCols(), Data.Get_NRows()) )
{
return( false );
}
//-----------------------------------------------------
if( ClassField < 0 || ClassField >= m_nVars )
{
ClassField = 0;
}
for(int iSample=0; iSample<m_nSamples; iSample++)
{
double *pData = m_Samples[iSample] = m_Samples[0] + iSample * m_nVars;
*pData++ = Data[iSample][ClassField];
for(int iVar=0; iVar<m_nVars; iVar++)
{
if( iVar != ClassField )
{
*pData++ = Data[iSample][iVar];
}
}
}
m_VarNames += "CLASS";
for(int iVar=0; iVar<m_nVars; iVar++)
{
if( iVar != ClassField )
{
m_VarNames += CSG_String::Format(SG_T("FEATURE_%02d"), iVar);
}
}
//-----------------------------------------------------
if( Threshold >= 0.0 ) // discretization
{
Discretize(Threshold);
}
return( true );
}
//---------------------------------------------------------
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 CWindeffect_Correction::Fit_Scaling_Factor(int x, int y, double &B, double B_min, double B_max, double B_Step)
{
CSG_Simple_Statistics Statistics[3];
CSG_Matrix Data;
if( !Get_Data(x, y, Data, Statistics[OBS]) )
{
return( false );
}
//-----------------------------------------------------
double dMin = -1.0;
for(double iB=B_min; iB<=B_max; iB+=B_Step)
{
int i;
Statistics[COR].Create(true); // reset
for(i=0; i<Data.Get_NRows(); i++)
{
Statistics[COR] += Get_Wind_Corr(iB, Data[i][BND], Data[i][WND]);
}
Statistics[VAL].Create(false); // reset
for(i=0; i<Data.Get_NRows(); i++)
{
Statistics[VAL] += Statistics[OBS].Get_Mean() * Statistics[COR].Get_Value(i) / Statistics[COR].Get_Mean();
}
double d = fabs(Statistics[VAL].Get_StdDev() - Statistics[OBS].Get_StdDev());
if( dMin < 0.0 || dMin > d )
{
B = iB;
dMin = d;
}
}
return( dMin >= 0.0 );
}
//---------------------------------------------------------
bool CSG_Matrix::Set_Transpose(void)
{
CSG_Matrix m;
if( m.Create(*this) && Create(m_ny, m_nx) )
{
for(int y=0; y<m_ny; y++)
{
for(int x=0; x<m_nx; x++)
{
m_z[y][x] = m.m_z[x][y];
}
}
return( true );
}
return( false );
}
//---------------------------------------------------------
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 );
}
//---------------------------------------------------------
Ptr<TrainData> COpenCV_ML_ANN::Get_Training(const CSG_Matrix &Data)
{
Mat Samples (Data.Get_NRows(), Data.Get_NCols() - 1 , CV_32F);
Mat Response(Data.Get_NRows(), Get_Class_Count() , CV_32F);
for(int i=0; i<Data.Get_NRows(); i++)
{
int j, k = (int)Data[i][Data.Get_NCols() - 1];
for(j=0; j<Response.cols; j++)
{
Response.at<float>(i, j) = j == k ? 1.f : 0.f;
}
for(j=0; j<Samples.cols; j++)
{
Samples.at<float>(i, j) = (float)Data[i][j];
}
}
return( TrainData::create(Samples, ROW_SAMPLE, Response) );
}
CSG_Matrix CSG_Matrix::Multiply(const CSG_Matrix &Matrix) const
{
CSG_Matrix m;
if( m_nx == Matrix.m_ny && m.Create(Matrix.m_nx, m_ny) )
{
for(int y=0; y<m.m_ny; y++)
{
for(int x=0; x<m.m_nx; x++)
{
double z = 0.0;
for(int n=0; n<m_nx; n++)
{
z += m_z[y][n] * Matrix.m_z[n][x];
}
m.m_z[y][x] = z;
}
}
}
return( m );
}
//---------------------------------------------------------
bool CChange_Detection::Get_Changes(CSG_Table &Initial, CSG_Table &Final, CSG_Table *pChanges, CSG_Matrix &Identity)
{
int iInitial, iFinal;
//-----------------------------------------------------
Identity.Create(Final.Get_Count() + 1, Initial.Get_Count() + 1);
for(iInitial=0; iInitial<Initial.Get_Count(); iInitial++)
{
CSG_String s = Initial[iInitial].asString(CLASS_NAM);
for(iFinal=0; iFinal<Final.Get_Count(); iFinal++)
{
Identity[iInitial][iFinal] = s.Cmp(Final[iFinal].asString(CLASS_NAM)) ? 0 : 1;
}
}
Identity[Initial.Get_Count()][Final.Get_Count()] = 1; // unclassified
//-----------------------------------------------------
pChanges->Destroy();
pChanges->Add_Field(_TL("Name"), SG_DATATYPE_String);
for(iFinal=0; iFinal<Final.Get_Count(); iFinal++)
{
pChanges->Add_Field(Final[iFinal].asString(CLASS_NAM), SG_DATATYPE_Double);
}
pChanges->Add_Field(_TL("Unclassified"), SG_DATATYPE_Double);
//-----------------------------------------------------
for(iInitial=0; iInitial<Initial.Get_Count(); iInitial++)
{
pChanges->Add_Record()->Set_Value(0, Initial[iInitial].asString(CLASS_NAM));
}
pChanges->Add_Record()->Set_Value(0, _TL("Unclassified"));
return( true );
}
//---------------------------------------------------------
bool CWindeffect_Correction::Get_Data(int x, int y, CSG_Matrix &Data, CSG_Simple_Statistics &statsObserved)
{
for(int i=0; i<m_Kernel.Get_Count(); i++)
{
int ix = m_Kernel.Get_X(i, x);
int iy = m_Kernel.Get_Y(i, y);
if( m_pBoundary->is_InGrid(ix, iy) && m_pWind->is_InGrid(ix, iy) && m_pObserved->is_InGrid(ix, iy) )
{
CSG_Vector d(2);
d[BND] = m_pBoundary->asDouble(ix, iy);
d[WND] = m_pWind ->asDouble(ix, iy);
Data.Add_Row(d);
statsObserved += m_pObserved->asDouble(ix, iy);
}
}
return( statsObserved.Get_Count() >= 5 );
}
//---------------------------------------------------------
bool CTable_PCA::Get_Matrix(CSG_Matrix &Matrix)
{
int i, j1, j2;
Matrix.Create(m_nFeatures, m_nFeatures);
Matrix.Set_Zero();
switch( m_Method )
{
//-----------------------------------------------------
default:
case 0: // Correlation matrix: Center and reduce the column vectors.
for(j1=0; j1<m_nFeatures; j1++)
{
Matrix[j1][j1] = 1.0;
}
for(i=0; i<m_pTable->Get_Count() && Set_Progress(i, m_pTable->Get_Count()); i++)
{
if( !is_NoData(i) )
{
for(j1=0; j1<m_nFeatures-1; j1++)
{
for(j2=j1+1; j2<m_nFeatures; j2++)
{
Matrix[j1][j2] += Get_Value(j1, i) * Get_Value(j2, i);
}
}
}
}
break;
//-----------------------------------------------------
case 1: // Variance-covariance matrix: Center the column vectors.
case 2: // Sums-of-squares-and-cross-products matrix
for(i=0; i<m_pTable->Get_Count() && Set_Progress(i, m_pTable->Get_Count()); i++)
{
if( !is_NoData(i) )
{
for(j1=0; j1<m_nFeatures; j1++)
{
for(j2=j1; j2<m_nFeatures; j2++)
{
Matrix[j1][j2] += Get_Value(j1, i) * Get_Value(j2, i);
}
}
}
}
break;
}
//-----------------------------------------------------
for(j1=0; j1<m_nFeatures; j1++)
{
for(j2=j1; j2<m_nFeatures; j2++)
{
Matrix[j2][j1] = Matrix[j1][j2];
}
}
return( true );
}
//---------------------------------------------------------
bool CFilter_3x3::On_Execute(void)
{
bool bAbsolute;
CSG_Matrix Filter;
CSG_Grid *pInput, *pResult;
CSG_Table *pFilter;
//-----------------------------------------------------
pInput = Parameters("INPUT" )->asGrid();
pResult = Parameters("RESULT" )->asGrid();
bAbsolute = Parameters("ABSOLUTE" )->asBool();
pFilter = Parameters("FILTER" )->asTable()
? Parameters("FILTER" )->asTable()
: Parameters("FILTER_3X3")->asTable();
if( pFilter->Get_Count() < 1 || pFilter->Get_Field_Count() < 1 )
{
Error_Set(_TL("invalid filter matrix"));
return( false );
}
//-----------------------------------------------------
Filter.Create(pFilter->Get_Field_Count(), pFilter->Get_Count());
{
for(int iy=0; iy<Filter.Get_NY(); iy++)
{
CSG_Table_Record *pRecord = pFilter->Get_Record(iy);
for(int ix=0; ix<Filter.Get_NX(); ix++)
{
Filter[iy][ix] = pRecord->asDouble(ix);
}
}
}
int dx = (Filter.Get_NX() - 1) / 2;
int dy = (Filter.Get_NY() - 1) / 2;
//-----------------------------------------------------
if( !pResult || pResult == pInput )
{
pResult = SG_Create_Grid(pInput);
}
else
{
pResult->Set_Name(CSG_String::Format(SG_T("%s [%s]"), pInput->Get_Name(), _TL("Filter")));
pResult->Set_NoData_Value(pInput->Get_NoData_Value());
}
//-----------------------------------------------------
for(int y=0; y<Get_NY() && Set_Progress(y); y++)
{
#pragma omp parallel for
for(int x=0; x<Get_NX(); x++)
{
double s = 0.0;
double n = 0.0;
if( pInput->is_InGrid(x, y) )
{
for(int iy=0, jy=y-dy; iy<Filter.Get_NY(); iy++, jy++)
{
for(int ix=0, jx=x-dx; ix<Filter.Get_NX(); ix++, jx++)
{
if( pInput->is_InGrid(jx, jy) )
{
s += Filter[iy][ix] * pInput->asDouble(jx, jy);
n += fabs(Filter[iy][ix]);
}
}
}
}
if( n > 0.0 )
{
pResult->Set_Value(x, y, bAbsolute ? s : s / n);
}
else
{
pResult->Set_NoData(x, y);
}
}
}
//-----------------------------------------------------
if( !Parameters("RESULT")->asGrid() || Parameters("RESULT")->asGrid() == pInput )
{
pInput->Assign(pResult);
delete(pResult);
DataObject_Update(pInput);
}
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 );
}
//---------------------------------------------------------
// find_normal() - Function to find the set of normal equations
// that allow a quadratic trend surface to be
// fitted through N points using least squares
// V.1.0, Jo Wood, 27th November, 1994.
//---------------------------------------------------------
bool CParam_Scale::Get_Normal(CSG_Matrix &Normal)
{
double x1, y1, x2, y2, x3, y3, x4, y4, xy2, x2y, xy3, x3y, x2y2, xy, N; // coefficients of X-products.
x1 = y1 = x2 = y2 = x3 = y3 = x4 = y4 = xy2 = x2y = xy3 = x3y = x2y2 = xy = N = 0.0;
// Calculate matrix of sums of squares and cross products
for(int y=0; y<m_Weights.Get_NY(); y++)
{
double dy = Get_Cellsize() * (y - m_Radius);
for(int x=0; x<m_Weights.Get_NX(); x++)
{
double dw = m_Weights[y][x];
double dx = Get_Cellsize() * (x - m_Radius);
x4 += dw * dx * dx * dx * dx;
x2y2 += dw * dx * dx * dy * dy;
x3y += dw * dx * dx * dx * dy;
x3 += dw * dx * dx * dx;
x2y += dw * dx * dx * dy;
x2 += dw * dx * dx;
y4 += dw * dy * dy * dy * dy;
xy3 += dw * dx * dy * dy * dy;
xy2 += dw * dx * dy * dy;
y3 += dw * dy * dy * dy;
y2 += dw * dy * dy;
xy += dw * dx * dy;
x1 += dw * dx;
y1 += dw * dy;
N += dw;
}
}
// Store cross-product matrix elements.
Normal.Create(6, 6);
Normal[0][0] = x4;
Normal[0][1] = Normal[1][0] = x2y2;
Normal[0][2] = Normal[2][0] = x3y;
Normal[0][3] = Normal[3][0] = x3;
Normal[0][4] = Normal[4][0] = x2y;
Normal[0][5] = Normal[5][0] = x2;
Normal[1][1] = y4;
Normal[1][2] = Normal[2][1] = xy3;
Normal[1][3] = Normal[3][1] = xy2;
Normal[1][4] = Normal[4][1] = y3;
Normal[1][5] = Normal[5][1] = y2;
Normal[2][2] = x2y2;
Normal[2][3] = Normal[3][2] = x2y;
Normal[2][4] = Normal[4][2] = xy2;
Normal[2][5] = Normal[5][2] = xy;
Normal[3][3] = x2;
Normal[3][4] = Normal[4][3] = xy;
Normal[3][5] = Normal[5][3] = x1;
Normal[4][4] = y2;
Normal[4][5] = Normal[5][4] = y1;
Normal[5][5] = N;
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 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 CPoint_Trend_Surface::Get_Regression(CSG_Shapes *pPoints, int iAttribute)
{
//-----------------------------------------------------
int i, j, Field;
m_Names.Clear();
m_Names += pPoints->Get_Name();
for(i=1; i<=m_xOrder; i++)
{
m_Names += Get_Power(SG_T("x"), i);
}
for(i=1; i<=m_yOrder; i++)
{
m_Names += Get_Power(SG_T("y"), i);
for(j=1; j<=m_xOrder && i<m_tOrder && j<m_tOrder; j++)
{
m_Names += Get_Power(SG_T("x"), j) + Get_Power(SG_T("y"), i);
}
}
//-----------------------------------------------------
CSG_Vector Y, xPow, yPow;
CSG_Matrix X;
Y.Create(pPoints->Get_Count());
X.Create(m_Names.Get_Count(), pPoints->Get_Count());
xPow.Create(m_xOrder + 1);
yPow.Create(m_yOrder + 1);
xPow[0] = 1.0;
yPow[0] = 1.0;
//-----------------------------------------------------
for(int iShape=0; iShape<pPoints->Get_Count() && Set_Progress(iShape, pPoints->Get_Count()); iShape++)
{
CSG_Shape *pShape = pPoints->Get_Shape(iShape);
if( !pShape->is_NoData(iAttribute) )
{
double zShape = pShape->asDouble(iAttribute);
TSG_Point Point = pShape->Get_Point(0);
Y[iShape] = zShape;
X[iShape][0] = 1.0;
for(i=1, Field=1; i<=m_xOrder; i++)
{
X[iShape][Field++] = xPow[i] = xPow[i - 1] * Point.x;
}
for(i=1; i<=m_yOrder; i++)
{
X[iShape][Field++] = yPow[i] = yPow[i - 1] * Point.y;
for(j=1; j<=m_xOrder && i<m_tOrder && j<m_tOrder; j++)
{
X[iShape][Field++] = xPow[j] * yPow[i];
}
}
}
}
//-----------------------------------------------------
CSG_Matrix Xt, XtXinv;
Xt = X;
Xt .Set_Transpose();
XtXinv = Xt * X;
XtXinv .Set_Inverse();
m_Coefficients = XtXinv * Xt * Y;
return( true );
}
