示例#1
0
//---------------------------------------------------------
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 );
}
示例#3
0
//---------------------------------------------------------
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 );
}
示例#4
0
//---------------------------------------------------------
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 );
}
示例#5
0
//---------------------------------------------------------
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 );
}
示例#6
0
//---------------------------------------------------------
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 );
}
示例#7
0
//---------------------------------------------------------
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 );
}
示例#9
0
//---------------------------------------------------------
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 );
}
示例#10
0
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 );
}
示例#11
0
//---------------------------------------------------------
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 );
}
示例#12
0
//---------------------------------------------------------
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 );
}
示例#13
0
//---------------------------------------------------------
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);
}
示例#14
0
//---------------------------------------------------------
// 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 );
}
示例#15
0
bool CSG_Matrix::Set_Row(int iRow, const CSG_Vector &Data)
{
	return( m_nx == Data.Get_N() ? Set_Row(iRow, Data.Get_Data()) : false );
}
示例#16
0
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 );
}
示例#17
0
//---------------------------------------------------------
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) );
}
示例#18
0
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 );
}
示例#19
0
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 );
}
示例#20
0
bool CSG_Matrix::Set_Col(int iCol, const CSG_Vector &Data)
{
	return( m_ny == Data.Get_N() ? Set_Col(iCol, Data.Get_Data()) : false );
}
示例#21
0
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 );
}
示例#22
0
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 );
}
示例#23
0
//---------------------------------------------------------
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;
}
示例#24
0
//---------------------------------------------------------
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 );
}