no.kartverket.positionorientation

Class Transform

java.lang.Object

no.kartverket.positionorientation.Transform

public class Transform
extends java.lang.Object

Builds and maintains a transform between two coordinate systems by a robust estimation of transform parameters based on various observations of common objects in both coordinate systems. Currently the transform is limited to a 3 dimensional translation and a rotation around the z-axis. Other rotations, scale changes and non-linear deformations may be implemented later.

The transform parameters are estimated by minimizing the weighted differences between the observations and an idealized model of the system, a "least squares adjustment".

The model equations can generally be written as A * (l + v) + B * dx = d where A and B are the model matrixes for the observations and paramaeters respectively (or the matrixes of the partial derivatives, the jacobi matrixes in case of non-linear model equations). l is the vector of observations, v the vector of residuals (the difference between the observations and least-squares adusted observations), dx the corrections to the approximated parameters added to x0 the vector of approximated parameters (necessary when solving non-linear model equations by iteration) and d a vector of constants.

The problem is solved by finding the vector of corrections to parameters that minimizes the weighted sum squared of the residuals transpose(v) * inverse(Q) * v, subject to the constraints of the model equations above, which results in the solution dx = inverse(N) * t where N = transpose(B) * We * B, t = transpose(B) * We * f, We = inverse(A * Q * transpose(A)) and f = d - A * l. Q is the matrix of cofactors, which is equal to the a priori covariance matrix of the observations, possibly scaled my a constant.

For more information about the mathematics behind the formulas please consult a standard text on least squares adjustment for geodesy and photogrammetry, for example "Observations and least squares", by Edward M: Mikhail, 1976

Created by runaas on 11.05.2017.

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
private class	Transform.ModelEquations All matrixes of the model equations for a set of observations.
static interface	Transform.Observation Generic observation
class	Transform.OrientationObservation Orientation obeservation, typically from a compass
private class	Transform.Parameters Block of estimated transform parameters
class	Transform.PointInPlaneObservation observation of a tango point that should lie in a plane described by world coordinates
class	Transform.PointInTerrainObservation observation of a tango point that should lie on a terrain model surface
class	Transform.PositionObservation2D A 2D position observation, contains a "tango" and a "world" position describing the same point, and their respecive a-priori standard deviations
class	Transform.PositionObservation3D A 3D position observation, contains a "tango" and a "world" position describing the same point, and their respecive a-priori standard deviations

Field Summary

Fields

Modifier and Type	Field and Description
private org.ejml.data.DMatrixRMaj	N Normal equations system matrix
private org.ejml.data.DMatrixRMaj	Ninv Inverted normal equations system matrix
(package private) static int	num_parameters Number of estimated parameters, currently 4 (x, y, z and orientation)
private Transform.ModelEquations	orientEquations
private Transform.Parameters[]	parameters Array of three parameter blocks, distinct for each of the three computation steps in the adjust(List) method
private Transform.ModelEquations	planeEquations
private Transform.ModelEquations	posEquations2D Model equations for observations
private Transform.ModelEquations	posEquations3D
private org.ejml.data.DMatrixRMaj	t Normal equations system vector
private org.ejml.data.DMatrixRMaj	tmp1 Temp matrix for internal use
private org.ejml.data.DMatrixRMaj	tmp2
private org.ejml.data.DMatrixRMaj	tmp3
private org.ejml.data.DMatrixRMaj	vWv Computed weighted sum squared of residuals
private org.ejml.data.DMatrixRMaj	x Computed adjustments to parameters (Ninv = inverse(N); x = Ninv * t;)

Constructor Summary

Constructors

Constructor and Description
Transform() Create and initialize a transform object with all relevant temp structures

Method Summary

Modifier and Type	Method and Description
boolean	adjust(java.util.List<Transform.Observation> observations) Recompute the transform from a list of observation data.
double	az() Rotation around z axis
double	azSigma2()
private void	computeQvv(org.ejml.data.DMatrixRMaj Q, org.ejml.data.DMatrixRMaj A, org.ejml.data.DMatrixRMaj We, org.ejml.data.DMatrixRMaj B, org.ejml.data.DMatrixRMaj Ninv, org.ejml.data.DMatrixRMaj Qvv) Compute cofactor matrix for residuals Qvv = Q * A' * (We - We * B * Ninv * B' * We) * A * Q
private void	computeV(org.ejml.data.DMatrixRMaj Q, org.ejml.data.DMatrixRMaj A, org.ejml.data.DMatrixRMaj We, org.ejml.data.DMatrixRMaj f, org.ejml.data.DMatrixRMaj B, org.ejml.data.DMatrixRMaj x, org.ejml.data.DMatrixRMaj v) Compute residual vector v = Q * A' * We * (f - B * x)
private static void	computeVWV(org.ejml.data.DMatrixRMaj x, org.ejml.data.DMatrixRMaj t, org.ejml.data.DMatrixRMaj vWv) Update the weighted sum of residuals with the corrections to the parameters vWv=vWv-x'*t
double	cosAz() Precomputed cosine of roatation around z-axis
static double	normalizeAngle(double a) Normalize an angle to the -pi, pi interval
double	sigma2() Observation sample variance of the unit weight (translation in meters)
double	sinAz() Precomputed sine of roatation around z-axis
private static boolean	solveNormal(org.ejml.data.DMatrixRMaj N, org.ejml.data.DMatrixRMaj t, org.ejml.data.DMatrixRMaj Ninv, org.ejml.data.DMatrixRMaj x) Solve normal equations Ninv = inv(N) x = Ninv * t
private void	updateN(org.ejml.data.DMatrixRMaj B, org.ejml.data.DMatrixRMaj We, org.ejml.data.DMatrixRMaj N) Update normal equation matrix N with observation matrixes B and weights We N = N + B' * We * B
private void	updateT(org.ejml.data.DMatrixRMaj B, org.ejml.data.DMatrixRMaj We, org.ejml.data.DMatrixRMaj f, org.ejml.data.DMatrixRMaj t) Update normal equation vector t with observation t = t + B' * We * f
double	x0() x translation
double	xySigma2() Estimated variance of the horizontal (xy) translation component
double	y0() y translation
double	z0() z translation
double	zSigma2() Estimated variance of the vertical (z) translation component

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

num_parameters

static final int num_parameters

Number of estimated parameters, currently 4 (x, y, z and orientation)

See Also:

Constant Field Values

parameters

private final Transform.Parameters[] parameters

Array of three parameter blocks, distinct for each of the three computation steps in the adjust(List) method

N

private final org.ejml.data.DMatrixRMaj N

Normal equations system matrix

Ninv

private final org.ejml.data.DMatrixRMaj Ninv

Inverted normal equations system matrix

t

private final org.ejml.data.DMatrixRMaj t

Normal equations system vector

x

private final org.ejml.data.DMatrixRMaj x

Computed adjustments to parameters (Ninv = inverse(N); x = Ninv * t;)

vWv

private final org.ejml.data.DMatrixRMaj vWv

Computed weighted sum squared of residuals

posEquations2D

private final Transform.ModelEquations posEquations2D

Model equations for observations

posEquations3D

private final Transform.ModelEquations posEquations3D

orientEquations

private final Transform.ModelEquations orientEquations

planeEquations

private final Transform.ModelEquations planeEquations

tmp1

private final org.ejml.data.DMatrixRMaj tmp1

Temp matrix for internal use

tmp2

private final org.ejml.data.DMatrixRMaj tmp2

tmp3

private final org.ejml.data.DMatrixRMaj tmp3

Constructor Detail

Transform

public Transform()

Create and initialize a transform object with all relevant temp structures

Method Detail

x0

public double x0()

x translation

y0

public double y0()

y translation

z0

public double z0()

z translation

az

public double az()

Rotation around z axis

sinAz

public double sinAz()

Precomputed sine of roatation around z-axis

cosAz

public double cosAz()

Precomputed cosine of roatation around z-axis

sigma2

public double sigma2()

Observation sample variance of the unit weight (translation in meters)

xySigma2

public double xySigma2()

Estimated variance of the horizontal (xy) translation component

zSigma2

public double zSigma2()

Estimated variance of the vertical (z) translation component

azSigma2

public double azSigma2()

adjust

public boolean adjust(java.util.List<Transform.Observation> observations)

Recompute the transform from a list of observation data.

Parameters:

observations - the list of observations

Returns:

false if the computations failed (singular equation system, fail to converge)

normalizeAngle

public static double normalizeAngle(double a)

Normalize an angle to the -pi, pi interval

Parameters:

a - a possibly un-normalized angle

Returns:

the normalized angle int the -pi, pi interval

updateN

private void updateN(org.ejml.data.DMatrixRMaj B,
                     org.ejml.data.DMatrixRMaj We,
                     org.ejml.data.DMatrixRMaj N)

Update normal equation matrix N with observation matrixes B and weights We N = N + B' * We * B

Parameters:

B -

We -

N -

updateT

private void updateT(org.ejml.data.DMatrixRMaj B,
                     org.ejml.data.DMatrixRMaj We,
                     org.ejml.data.DMatrixRMaj f,
                     org.ejml.data.DMatrixRMaj t)

Update normal equation vector t with observation t = t + B' * We * f

Parameters:

B -

We -

f -

t -

solveNormal

private static boolean solveNormal(org.ejml.data.DMatrixRMaj N,
                                   org.ejml.data.DMatrixRMaj t,
                                   org.ejml.data.DMatrixRMaj Ninv,
                                   org.ejml.data.DMatrixRMaj x)

Solve normal equations Ninv = inv(N) x = Ninv * t

Parameters:

N -

t -

Ninv -

x -

Returns:

true if N is non-singular

computeVWV

private static void computeVWV(org.ejml.data.DMatrixRMaj x,
                               org.ejml.data.DMatrixRMaj t,
                               org.ejml.data.DMatrixRMaj vWv)

Update the weighted sum of residuals with the corrections to the parameters vWv=vWv-x'*t

Parameters:

x -

t -

vWv -

computeV

private void computeV(org.ejml.data.DMatrixRMaj Q,
                      org.ejml.data.DMatrixRMaj A,
                      org.ejml.data.DMatrixRMaj We,
                      org.ejml.data.DMatrixRMaj f,
                      org.ejml.data.DMatrixRMaj B,
                      org.ejml.data.DMatrixRMaj x,
                      org.ejml.data.DMatrixRMaj v)

Compute residual vector v = Q * A' * We * (f - B * x)

Parameters:

Q -

A -

We -

f -

B -

x -

v -

computeQvv

private void computeQvv(org.ejml.data.DMatrixRMaj Q,
                        org.ejml.data.DMatrixRMaj A,
                        org.ejml.data.DMatrixRMaj We,
                        org.ejml.data.DMatrixRMaj B,
                        org.ejml.data.DMatrixRMaj Ninv,
                        org.ejml.data.DMatrixRMaj Qvv)

Compute cofactor matrix for residuals Qvv = Q * A' * (We - We * B * Ninv * B' * We) * A * Q

Parameters:

Q -

A -

We -

B -

Ninv -

Qvv -