A Robust Solution of  Similarity Transformation
via Dual Quaternions

Béla Paláncz
Department of Photogrammetry and Geoinformatics
Budapest University of Technology and Economics, H- 1521, Hungary
e-mail: palancz@epito.bme.hu

Abstract

A RANSAC type method with a core algorithm based on dual quaternions is presented for similarity transformation. Although dual - quaternion technique basically represents rigid transformation - rotation and translation, -  introducing a simple additional  transformation for the scale parameter, one can successfully use it for similarity transformation too, where scaling is also important. For 3-point problem a determined multivariate polynomial system with 7 unknowns can be developed and easily solved by numerical Groebner basis. Since this computation is fast, it can be efficiently integrated it into a RANSAC type method as a core algorithm, where it is solved repeatedly. For n-point problem we employ Gauss-Jacobi method solving the 3-point subsystem simultaneously.
This contribution will guide the reader through the different  phases of the development of the method step by step illustrated by numerical examples. Finally, example with real world data representing an overdetermined n-point problem is solved. All symbolic and numeric computations have been carried out with Mathematica 10.

Keywords

Dual- quaternions, similarity transformation, RANSAC, numerical Groebner basis, Gauss-Jacobi solution

1- Introduction

RANSAC is one of the most proper methods to carry out robust parameter estimation, see i.e. Zuliani  (2012) and  Yaniv  (2010). In order to use this method efficiently, one needs a fast core algorithm integrated into RANSAC, which solves the actual subsystems of  an n-point overdetermined system, repeatedly. The efficiency of the RANSAC method can be improved to parallelize the repetitive solution of the subsystems.
To solve such nonlinear  determined subsystems -usually 3-point problems in case of similarity transformation one can use local methods (linearization or  iterative solution), but these require initial values for the parameters, which are rarely are available, see Sjöberg (2013). There are also exist standard non-iterative solutions like Procrustes solution employing SVD see. Awange et al. (2010), or need  tedious algebraic preprocessing, see Paláncz and Zaletnyik (2011).
In this study we introduce dual-quaternions for solving such subsystems in non-iterative way. We have to mention that dual quaternions have been successfully applied already to non-robust similarity transform, see Proskova (2011) or more recently Wang et al (2014) using global numerical optimization techniques.

 Dual numbers are akin to complex numbers and application dual number theory led to dual quaternions, which allows the unification of translation and rotation. In this way a rigid transformation - a rotation about an axis followed by a translation in the direction of this axis - can be represented by dual quaternions, see i.e. Kenwright (2012).

The inverse problem of the rigid transformation means to find the parameters of the transformation when corresponding data pairs are known. This problem which is equivalent to computing the components of the dual quaternion representing the transformation, leads to solving a multivariate polynomial system. 

It can be done without iteration by using numerical Groebner basis, which is a fast, global method. In practice the utilization of this technique is two-folded. One can employ it in non-robust case using Gauss-Jacobi combinatorial technique see i.e. Awange et al (2010) or in robust case as a core algorithm of a robust shell method, like RANSAC.  In this study we concentrate on the robust application.

The structure of this presentation is the following: in Section 2, we introduce quaternions and they application to rigid rotation. Here we also give the matrix equivalent of quaternions as well as illustrate the inverse problem, namely the parameter estimation of the quaternions. In Section 3, the dual quaternions are introduced and their application to rigid transformation -rotation and translation- is demonstrated. In this section we solve the inverse problem for the rigid transformation, too. In Section 4, employing a simple transformation method for scale parameter, we extend the application of dual-quaternions to similarity transformation where scaling is important. The inverse problem of similarity transformation is again illustrated. In Section 5, we introduce the RANSAC method with this extended dual quaternion algorithm as its core algorithm, and illustrate the application with real-word data.

2- Quaternion algebra

2- 1-  Definition of quaternion

Quaternions can be considered as an extension of the complex numbers, namely

Q = (q0, q1, q2, q3) = q0 + i q1 + J q2 + K q3 = (q0, q)

where q0 is a scalar real part and  q = (q1, q2, q3) is an imaginary vector. In Mathematica we can work with quaternions as a special data type:

<<Quaternions`

Example 1. Let us define a quaternion with the following values for its component,

q0=1;q1=2;q2=3;q3=4;

Then the quaternion type variable Q with theses data is

Q=Quaternion[q0,q1,q2,q3]

indeed Q is quaternion data type

Head[Q]

2- 2-  Quaternion of rotation

Given an angle θ and the axis of rotation represented by a unit-vector n = ((Subscript[n, x]), (Subscript[n, y]), (Subscript[n, z])). Then we can construct a quaternion as

Q = (cos ((θ/2)), n sin((θ/2)))

or

q0 = cos ((θ/2)), q1= (Subscript[n, x]) sin((θ/2)), q2= (Subscript[n, y]) sin((θ/2)), q3= (Subscript[n, z]) sin((θ/2))

Sometimes having the quaternion, we need to know the angle, then

θ =2 Arccos(q0)

and the vector of rotation axis is

(Subscript[n, x])= (q1/(sin((θ/2)))), (Subscript[n, y])= (q2/(sin((θ/2)))), (Subscript[n, z])= (q3/(sin((θ/2)))),

Example 2. Let us develop the quaternion of rotation when  θ = (π/4) and the axis of rotation is  n = ( (1/(Sqrt[3])),(1/(Sqrt[3])), (1/(Sqrt[3]))).

θ=(π/4);n={(1/(Sqrt[3])),(1/(Sqrt[3])),(1/(Sqrt[3]))};

where n is a unit vector. Indeed

Norm[n]

The corresponding quaternion is

Q=Quaternion[Cos[(θ/2)],Sin[(θ/2)]n[[1]],Sin[(θ/2)]n[[2]],Sin[(θ/2)]n[[3]]]

Using built-in function, we can get the elements of the quaternion, since

wxyz=FromQuaternion[Q]

Then the angel of the rotation is

θ=2ArcCos[Re[wxyz]/.{J->0,K->0}]

or more directly

2ArcCos[Re[Q]]

and the unit vector of the rotation axis is

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}/Sin[(θ/2)]

The norm of the quaternion is,

Norm[Q]//TrigReduce

2- 3-  Rotation transformation with quaternion

Let p a position vector of a point in 3D. We can rotate this vector counter-clockwise with the quaternion of rotation Q as

(Subscript[P, Q])= Q P (Subscript[Q, c])

where P is a pure quaternion, P = (0, p) and (Subscript[Q, c]) is the conjugate quaternion of Q. (Subscript[P, Q]) is quaternion of the rotated vector, which is also a pure quaternion (Subscript[P, Q]) = (0, (Subscript[p, Q]) ) and (Subscript[p, Q]) is the position vector of the rotated point.

Example 3. Let us compute the conjugate of the quaternion of rotation Q . We can use built in function,

Qc=Conjugate[Q]

You can realize that, similarly to complex numbers, the conjugate can be expressed as

(Subscript[Q, c]) = (({{ q0}, {-q1}, {-q2}, {-q3}}))

We can consider the conjugate of a quaternion as the inverse of the quaternion,

FromQuaternion[Q**Qc]

Example 4. Let a position  vector be p = {2, 0, 1}, which is rotated by  θ = (π/2) about the axis given by the vector n . Let us compute the image of p.

p={2,0,1};

P=Quaternion[0,p[[1]],p[[2]],p[[3]]]

Then according to Eq.(6)

PQ=Q**P**Qc

The result is a pure quaternion

wxyz=FromQuaternion[PQ]//TrigReduce

which means its real part is zero. The rotated vector, the vector of the image point is

pQ={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}//Simplify

Let us check the result employing built in function of the Mathematica. The matrix of the rotation is

R=RotationTransform[θ,n]

Then the rotated vector

R[p]//Simplify

2- 4-  Quaternion to matrix

In order to avoid using quaternion as a new data type, we can employ the matrix form of a quaternion. A quaternion can be expressed by a 4 × 4 matrix,

Q =(q0, q1, q2, q3)= (({{q0, -q1, -q2,  -q3}, {q1,     q0, -q3,      q2}, {q2,    q3,     q0, -q1}, {q3, -q2,     q1,     q0}}))

Let us define a new function, which creates the matrix form of a quaternion from its components,

QuaternionMatrixForm[{q0_,q1_,q2_,q3_}]:={{q0,-q1,-q2,-q3},{q1,q0,-q3,q2},{q2,q3,q0,-q1},{q3,-q2,q1,q0}}

Example 5. Do carry out the multiplication (Subscript[P, Q])=Q**P**(Subscript[Q, c]) in matrix form! We should create the matrix form of the quaternions Q and P, namely (Subscript[M, Q]) and (Subscript[M, P]), and the vector form of  (Subscript[Q, c]). Then the vector form of the rotated vector  is a pure quaternion

(Subscript[p, Q])= (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c])= (Subscript[M, Q]) (Subscript[M, P]) (({{    q0}, {-q1}, {-q2}, {-q3}}))=(({{0}, {(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

Consequently the rotated vector is

(Subscript[p, r])=(({{(Subscript[p, r1])}, {(Subscript[P, r2])}, {(Subscript[P, r3])}}))= (({{(Subscript[p, Q1])}, {(Subscript[P, Q2])}, {(Subscript[P, Q3])}}))

Let us construct the matrix form of the rotation quaternion, (Subscript[M, Q]). Since

wxyz=FromQuaternion[Q]

and

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

Therefore the matrix form of Q is,

MQ=QuaternionMatrixForm[Join[{Re[wxyz/.{J->0,K->0}]},n]]

or

MatrixForm[MQ]

Similarly, the matrix form of the position vector (Subscript[M, P]) can be computed as

wxyz=FromQuaternion[P]

and

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

Then

MP=QuaternionMatrixForm[Join[{Re[wxyz/.{J->0,K->0}]},n]];

MatrixForm[MP]

or in more simple  way,

QuaternionMatrixForm[{0,2,0,1}]//MatrixForm

The matrix  (vector) form of the conjugate of the rotation quaternion, (Subscript[M, Q]) can be similarly computed

wxyz=FromQuaternion[Qc]

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

MQc=Join[{Re[wxyz/.{J->0,K->0}]},n];MatrixForm[MQc]

Then the rotated vector, see Eq. (9)

pQ=MQ.MP.MQc//TrigReduce//Expand

It is really a pure quaternion.

2- 5-  Inverse problem of rotation

Now, we assume that the coordinates of the original points - let us call them as a source points - and the coordinates of their corresponding transformed points - image points- are known and we should like to get the vector of the rotation axis and the angle of the rotation. This means that we are looking for q0, q1, q2 and q3, the components of the quaternion of the rotation. We have seen that the image point of a the rotated source point is a pure quaternion, see Eq.(9)

(Subscript[p, Q])= (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c])= (Subscript[M, Q]) (Subscript[M, P]) (({{    q0}, {-q1}, {-q2}, {-q3}}))=(({{0}, {(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

and the position vector of the rotated point is, see Eq. (10)

(Subscript[p, r])=(({{(Subscript[p, r1])}, {(Subscript[p, r2])}, {(Subscript[p, r3])}}))= (({{(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

Consequently, these relations between a source and an corresponding image point coordinates provide only 3 equations for the 4 unknowns. So we need 2 points to solve the inverse problem - more precisely, in addition  one more corresponding coordinate pair  is needed. Let us consider the equations from the terms (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c]) which provide 4 equations,

Clear[q0,q1,q2,q3]

eqs=QuaternionMatrixForm[{q0,q1,q2,q3}].QuaternionMatrixForm[{0,p1,p2,p3}].{q0,-q1,-q2,-q3}

But the first one is zero as it is expected,

eqs[[1]]//Simplify

Therefore we have 3 prototype equations between the source point p = (p1, p2, p3) and its image point (Subscript[p, r]) = ((Subscript[p, r1]), (Subscript[p, r2]), (Subscript[p, r3])), namely

eq1=eqs[[2]]-pr1

eq2=eqs[[3]]-pr2

eq3=eqs[[4]]-pr3

We have already one data pair, now we consider a second one, see Table 1.

Table 1.

Coordinates of source and image points A and B

({{Point, p1, p2, p3, pr1, pr2, pr3}, {A, 2, 0, 1, 1+(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(Sqrt[(2/3)])}, {B, 1, 2, -1, (1/6) (4+(Sqrt[2])-3 (Sqrt[6])), (1/3) (2+2 (Sqrt[2])+(Sqrt[6])), (1/6) (4-5 (Sqrt[2])+(Sqrt[6]))}})

Let us develop the equations with these numerical data. The data of the 2 corresponding pairs of points  are

dataA={p1->2,p2->0,p3->1,pr1->1+(1/(Sqrt[2]))+(1/(Sqrt[6])),pr2->1-(1/(Sqrt[2]))+(1/(Sqrt[6])),pr3->1-(Sqrt[(2/3)])};
dataB={p1->1,p2->2,p3->-1,pr1->(1/6) (4+(Sqrt[2])-3 (Sqrt[6])),pr2->(1/3) (2+2 (Sqrt[2])+(Sqrt[6])),pr3->(1/6) (4-5 (Sqrt[2])+(Sqrt[6]))};

Then the equations for the first data pair,

{e1,e2,e3}={eq1,eq2,eq3}/.dataA

and from the second data pair we consider only the first coordinates,

{e4}={eq1}/.dataB

To compute Q = (q0, q1, q2, q3) this multivariate polynomial system with 4 unknowns should be solved. Let us employ numerical Groebner basis

AbsoluteTiming[sol=NSolve[{e1,e2,e3,e4},{q0,q1,q2,q3}];]

The solutions are

sol

Considering the positive real solutions we get for the quaternion components

Q={q0,q1,q2,q3}/.sol[[2]]

Let us check the result

Q-{Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))}

The rotation angle and the coordinates of the rotation axis can be easily computed from these components with Eqs. (4) and (5).   Although the standard rotation matrix can be expressed by the element of the quaternion, too,

RQ = (({{((q0)^2)+((q1)^2)-((q2)^2)-((q3)^2), 2 (q1 q2-q0 q3), 2 (q0 q2+q1 q3)}, {2 (q1 q2+q0 q3), ((q0)^2)-((q1)^2)+((q2)^2)-((q3)^2), 2 (-q0 q1+q2 q3)}, {2 (-q0 q2+q1 q3), 2 (q0 q1+q2 q3), ((q0)^2)-((q1)^2)-((q2)^2)+((q3)^2)}}))

Let us define the corresponding Mathematica  function,

QuaternionToRotationMatrix[{q0_,q1_,q2_,q3_}]:={{(q0^2)+(q1^2)-(q2^2)-(q3^2),2(q1 q2-q0 q3),2(q1 q3+q0 q2)},{2(q1 q2+q0 q3),(q0^2)-(q1^2)+(q2^2)-(q3^2),2(q2 q3-q0 q1)},{2(q1 q3-q0 q2),2(q2 q3+q0 q1),(q0^2)-(q1^2)-(q2^2)+(q3^2)}}

Then the rotation matrix is

RQ=QuaternionToRotationMatrix[Q];MatrixForm[RQ]

To check this result, let us consider the rotation transformation resulted by the built in function of Mathematica. We have seen that the rotation transformation function is

R

Extracting the rotation matrix

RM=Map[Drop[#,-1]&,Drop[R[[1]],-1]];MatrixForm[RM]

and indeed, the dual quaternion solution provide the same result,

MatrixForm[RM-RQ]

3- Dual quaternions

3- 1- Dual number and dual quaternion

Dual numbers were invented by Clifford in 1873. Dual numbers are akin to complex numbers.

z = r + ϵ d   with  (ϵ^2)= 0   but ϵ ≠ 0

where ϵ is known as the dual operator, r is the real part and d is the dual part of a dual number. Now, let us consider r and d as two standard quaternions, Q and (Subscript[Q, ϵ]), then

(Subscript[Q, d])= Q + ϵ (Subscript[Q, ϵ])

where (Subscript[Q, d]) is called as dual quaternion and it can be considered as an extension of the dual numbers. This formalism allows the unification of rotation and translation.

3- 2- Dual Quaternion to rigid  transformation

Let p = ((Subscript[p, 1]),(Subscript[p, 2]),(Subscript[p, 3])) is a position vector of a source point, t = ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) is a translation vector and Q = (cos ((θ/2)), n sin((θ/2))) is a unit quaternion for rotation. Then we can express the image of the point (Subscript[p, (Subscript[Q, d])])= ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) after this translation and rotation as the dual quaternion of the image point is,

(Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

where (Subscript[P, d]) is a unit dual quaternion

(Subscript[P, d]) = 1 + ϵ (i (Subscript[p, 1])  + J (Subscript[p, 2])+ K (Subscript[p, 3]))

and let 𝒯 is a pure quaternion as

𝒯=0 +(i (Subscript[t, 1])  + J (Subscript[t, 2])+ K (Subscript[t, 3]))

Now the dual quaternion (Subscript[Q, d]) has its real part Q and its dual part (1/2) 𝒯 Q, namely

(Subscript[Q, d]) = Q + ϵ (Subscript[Q, ϵ]) = Q + ϵ (1/2) 𝒯 Q

and (Subscript[(Subscript[Q, d]), c]) is the conjugate of  (Subscript[Q, d]). The resulted dual quaternion (Subscript[P, (Subscript[Q, d])]) is a unit dual quaternion

(Subscript[P, (Subscript[Q, d])])=1 + ϵ ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

Now let us develop some Mathematical  functions to handle dual quaternions, since Mathematica does not support operations with dual quaternion data type.

• First, we  define a function, which computes dual quaternion from the quaternion of the real part and from the dual part,

DualQuaternion[Q1_,Q2_]:=Quaternion[Q1[[1]],Q1[[2]],Q1[[3]],Q1[[4]]]+e Quaternion[Q2[[1]],Q2[[2]],Q2[[3]],Q2[[4]]]

• We require to define the product of two dual quaternions. Let (Subscript[Q, (Subscript[d, 1])]) and (Subscript[Q, (Subscript[d, 2])]) two dual quaternions, where (Subscript[Q, d])= Q + ϵ (Subscript[Q, ϵ]), then

(Subscript[Q, (Subscript[d, 1])]) (Subscript[Q, (Subscript[d, 2])]) =(Subscript[Q, 1]) (Subscript[Q, 2])+ϵ ((Subscript[Q, 1])(Subscript[Q, (Subscript[ϵ, 2])]) + (Subscript[Q, (Subscript[ϵ, 1])])(Subscript[Q, 2]))

The corresponding Mathematica function is

ProductDual[Q1_,Q2_]:=Coefficient[Q1,e,0]**Coefficient[Q2,e,0]+e(Coefficient[Q1,e,0]**Coefficient[Q2,e,1]+Coefficient[Q1,e,1]**Coefficient[Q2,e,0])

• We also need a function, which computes the dual conjugate of the dual quaternion, (Subscript[(Subscript[Q, d]), c]). The conjugate of the quaternion Q is (Subscript[Q, c]), and the dual conjugate of the dual quaternion is

(Subscript[(Subscript[Q, d]), c])=(Subscript[Q, c])-  ϵ (Subscript[Q, (Subscript[ϵ, c])])

The corresponding Mathematica function is

ConjugateDual[Q_]:=(Conjugate[Coefficient[Q,e,0]]-e Conjugate[Coefficient[Q,e,1]])

• We may also require to extract the real and the dual part of a dual quaternion,

RealFromDualQuaternion[Q_]:=FromQuaternion[Coefficient[Q,e,0]]

DualFromDualQuaternion[Q_]:=FromQuaternion[Coefficient[Q,e,1]]

Example 6. First let us apply dual quaternion to only pure rotation employing data used in the examples above. Since there is no translation, now the pure zero quaternion, namely  𝒯 ≡0.

T=Quaternion[0,0,0,0]

Let the rotation represented by

Q=Quaternion[Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))];

the dual part of the quaternion, see Eq. (19),

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) itself  is

Qd=DualQuaternion[Q,Qe]

Similarly let us compute the unit dual quaternion of the p vector, namely for (Subscript[P, d])

Pd=DualQuaternion[Quaternion[1,0,0,0],P]

Let us compute the dual conjugate of this dual quaternion

Qdc=ConjugateDual[Qd]

Now, we  can compute the dual quaternion of the 3D rigid transformation, now only rotation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

The real part of the resulted dual quaternion is

wxyz=RealFromDualQuaternion[PQd]

and the dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the rotated vector is

{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//Simplify

Example 7. Now, we add a translation to the transformation with vector t = (1, 2, -1),

The pure quaternion for translation  𝒯  is

T=Quaternion[0,1,2,-1]

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) is

Qd=DualQuaternion[Q,Qe]

Its dual conjugate

Qdc=ConjugateDual[Qd]

Now, we  can compute the dual quaternion of the 3D rigid transformation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c]) , see Eq. (16)

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

and

wxyz=RealFromDualQuaternion[PQd]

The dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the image  vector, rotated and translated source vector is

{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//Simplify

Let us check this result via using built-in function of Mathematica The transformation function for this translation is

T=TranslationTransform[{1,2,-1}]

Then the translation after rotation

pT=T[R[p]]//Simplify

So we have the same result.

3- 3- Dual quaternion to matrix

Let us consider a dual quaternion as (Subscript[Q, d]) = Q + ϵ (Subscript[Q, ϵ]), where the real part is Q = (q0, q1, q2, q3) and the dual part

(Subscript[Q, ϵ])=(1/2)𝒯 Q=(1/2) (0,t1,t2,t3)**(q0,q1,q2,q3)

then the  matrix representation of  (Subscript[Q, d]) is a 8 × 8 matrix,

(Subscript[Q, d]) =(({{Q, (Subscript[Q, ϵ])}, {0, Q}}))

The matrix form of Q is

Q=QuaternionMatrixForm[{q0,q1,q2,q3}];MatrixForm[Q]

The dual part is

qe=(1/2)QuaternionMatrixForm[{0,t1,t2,t3}].{q0,q1,q2,q3};

In matrix form

Qe=QuaternionMatrixForm[qe];MatrixForm[Qe]

Let us put them together in a matrix

MD=Join[Join[Q,Qe,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},Q,2]];MatrixForm[MD]

Similarly, the matrix representation of the dual conjugate of the dual quaternion is

(Subscript[(Subscript[Q, d]), c])=(({{(Subscript[Q, c]), -(Subscript[Q, (Subscript[ϵ, c])])}, {0, (Subscript[Q, c])}}))

Qc=QuaternionMatrixForm[{q0,-q1,-q2,-q3}];MatrixForm[Qc]

Qec=QuaternionMatrixForm[{qe[[1]],-qe[[2]],-qe[[3]],-qe[[4]]}];MatrixForm[Qec]

Let us put them together

MQc=Join[Join[Qc,-Qec,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},Qc,2]];MatrixForm[MQc]

As we have seen, the dual quaternion of the position vector of the source  point to be transformed is a unit dual quaternion, see Eq. (17)

(Subscript[P, d]) = 1 + ϵ (i (Subscript[p, 1])  + J (Subscript[p, 2])+ K (Subscript[p, 3]))

Therefore the real part of  (Subscript[P, d]) is

PdR=QuaternionMatrixForm[{1,0,0,0}];MatrixForm[PdR]

and its dual part

PdD=QuaternionMatrixForm[{0,p1,p2,p3}];MatrixForm[PdD]

Now putting them together in a matrix

MPd=Join[Join[PdR,PdD,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},PdR,2]];MatrixForm[MPd]

Then the matrix form of the dual quaternion representing the image point is

PQd=MD.MPd.MQc;

This matrix can be simplified again considering  ((q0)^2)+((q1)^2)+((q2)^2)+((q3)^2)= 1. Indeed

dataQ={q0->Cos[(π/8)],q1->((Sin[(π/8)])/(Sqrt[3])),q2->((Sin[(π/8)])/(Sqrt[3])),q3->((Sin[(π/8)])/(Sqrt[3]))};

(((q0^2)+(q1^2)+(q2^2)+(q3^2))/.dataQ)//TrigReduce

Therefore  (Subscript[P, Qd]) can be simplified, too

PQd=(PQd//FullSimplify)/.{((q0^2)+(q1^2)+(q2^2)+(q3^2))->1};

The result is a unit dual quaternion in matrix form. Let the image point coordinates ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])), and we have seen that the result matrix is the matrix form of  the dual quaternion, see Eq. (20)

(Subscript[P, (Subscript[Q, d])])=1 + ϵ ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

In addition the matrix form of the dual part, which is a pure quaternion is

QuaternionMatrixForm[{0,pQd1,pQd2,pQd3}]//MatrixForm

Therefore the structure of our matrix is

(Subscript[P, Qd])=(({{1, 0, 0, 0, 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 3])])}, {0, 1, 0, 0, (Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 3])]),   (Subscript[p, (Subscript[(Subscript[Q, d]), 2])])}, {0, 0, 1, 0, (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]),   (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]), 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 1])])}, {0, 0, 0, 1, (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]),   (Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), 0}, {0, 0, 0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 0, 0, 1}}))

Indeed,

PQd[[All,1;;4]]//MatrixForm

PQd[[5;;8]]//MatrixForm

Then the coordinates of the position of the image  vector are

pQd1=-PQd[[1,6]]

pQd2=-PQd[[1,7]]

pQd3=-PQd[[1,8]]

Example 8. Let us compute the coordinates of the image position vector with data used in Example 7, using matrix representation.

The translation vector

datat={t1->1,t2->2,t3->-1};

The rotation quaternion

dataQ

then the coordinates are

{pQd1,pQd2,pQd3}/.dataQ/.datat/.{p1->2,p2->0,p3->1}//TrigReduce//FullSimplify

Let us check it with the result of the Example 7

pT

3- 4- Inverse problem of the rigid transformation

The prototype equations describing the relation between the source vector  p = ((Subscript[p, 1]),(Subscript[p, 2]), (Subscript[p, 3])) and the image vector (Subscript[p, (Subscript[Q, d])]) = ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) with the components of the unit quaternion of rotation Q = (q0, q1 , q2, q3)  and translation vector  t =  ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) according to the result of the last section, can be written as

Clear[pQd1,pQd2,pQd3];

eqPt1=pQd1-(2 p2 q1 q2-2 p2 q0 q3+2 p3 (q0 q2+q1 q3)-p1 (-(q0^2)-(q1^2)+(q2^2)+(q3^2))+ t1);

eqPt2=pQd2-(2 (-p3 q0 q1+p1 q1 q2+p1 q0 q3+p3 q2 q3)-p2 (-(q0^2)+(q1^2)-(q2^2)+(q3^2))+ t2);

eqPt3=pQd3-(2 (p2 q0 q1-p1 q0 q2+p1 q1 q3+p2 q2 q3)-p3 (-(q0^2)+(q1^2)+(q2^2)-(q3^2))+ t3);

Example 9. Do compute the parameters of the rigid transformation determined by Q and t! Since we need to compute the 4 + 3 components, we require 7 equations, which means we should use 3 corresponding points.

The three corresponding data pairs are in Table 2.

Table 2.

Coordinates of source and image points

({{Point , p1, p2, p3, pQd1, pQd2, pQd3}, {A, 2, 0, 1, 1+(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(Sqrt[(2/3)])}, {B, 1, 2, -1, (1/6) (4+(Sqrt[2])-3 (Sqrt[6])), (1/3) (2+2 (Sqrt[2])+(Sqrt[6])), (1/6) (4-5 (Sqrt[2])+(Sqrt[6]))}, {C, 0, 1, 2, 2-(1/(Sqrt[2]))+(1/(Sqrt[6])), 3-(Sqrt[(2/3)]), (1/(Sqrt[2]))+(1/(Sqrt[6]))}})

Then our equation system is

dataA={p1->2,p2->0,p3->1,pQd1->2+(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd2->3-(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd3->-(Sqrt[(2/3)])};

{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.dataA

dataB={p1->1,p2->2,p3->-1,pQd1->(1/6) (10+(Sqrt[2])-3 (Sqrt[6])),pQd2->(1/3) (8+2 (Sqrt[2])+(Sqrt[6])),pQd3->(1/6) (-2-5 (Sqrt[2])+(Sqrt[6]))};

{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.dataB

dataC={p1->0,p2->1,p3->2,pQd1->2-(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd2->3-(Sqrt[(2/3)]),pQd3->(1/(Sqrt[2]))+(1/(Sqrt[6]))};

From the third point, we use only the first corresponding coordinates,

{e7}={eqPt1}/.dataC

These 7 equations can be simplified by eliminating the components of the translation vector t.

E1=e1-e4

E2=e2-e5

E3=e3-e6

E4=e1-e7

AbsoluteTiming[sol=NSolve[{E1,E2,E3,E4},{q0,q1,q2,q3}];]

sol

We select quaternion having  all real and positive elements

solq=sol[[3]]

Indeed for rotation,

{Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))}//N

as well as for the translation vector we have got the proper result

𝒯=MapThread[Solve[#1==0,#2]/.solq&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten

4- Extension to similarity transformation

4- 1- Direct similarity transformation

We have seen that rigid transformation can be achieved by dual quaternions. But in geodesy we need to apply similarity datum transformation including scaling in addition. Namely the transformation is

(Subscript[p, ℐ]) = s ℛ p + t

where s is the scaling parameter, ℛ is the rotation matrix, t the translation vector, p and (Subscript[p, ℐ]) are the source vector and image vector, respectively. Let use assume that the scaling parameter is known, than the transformation can be written in the form of a rigid transformation, and can be expressed by dual quaternions

(Subscript[p, (Subscript[Q, d])])= ((Subscript[p, ℐ])/s) = ℛ p + (t/s) = (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

Then the direct transformation problem can be solved similarly to the case of the rigid transformation, see Example 7. In Section 3.2. Although one shall employ t = (1/s) ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) as translation vector, and the resulted image vector is (Subscript[p, ℐ]) = s(Subscript[p, (Subscript[Q, d])]) = s ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])).

Example 10. Compute the coordinates of the image vector with data used in Example 7, with  scaling s = 1.1

𝓈=1.1;𝓈=Rationalize[𝓈]

The pure quaternion  T  is

T=(1/𝓈)Quaternion[0,1,2,-1]

The quaternion of the rotation

Q=Quaternion[Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))];

then

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) is

Qd=DualQuaternion[Q,Qe]

Its dual conjugate

Qdc=ConjugateDual[Qd]

Now we  can compute the dual quaternion of the 3D  transformation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

where

Pd

and

wxyz=RealFromDualQuaternion[PQd]

The dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the rotated position vector is

𝓈{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//FullSimplify

Let us check this result via using built-in function

T=TranslationTransform[{1,2,-1}]

The translation after rotation

pT=T[𝓈 R[p]]//FullSimplify

4- 2- Inverse problem of the similarity transformation

In order employ the form of the rigid transformation to compute the transformation parameters from the measured corresponding data points, first the scaling parameter should be determined.

Let us ((Subscript[p, 1]),(Subscript[p, 2]), (Subscript[p, 3])) are three source vectors and  ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) are the three corresponding image vectors. Then we compute the gravitational center points of these two triplets, which are

(Subscript[p, 123]) = (1/3)  ((Subscript[p, 1])+(Subscript[p, 2])+(Subscript[p, 3]))

and

(Subscript[p, (Subscript[Q, (Subscript[d, 123])])]) = (1/3)  ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])])+(Subscript[p, (Subscript[(Subscript[Q, d]), 2])])+ (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

Then scaling parameter can be  computed as

s= ((‖(Subscript[p, (Subscript[(Subscript[Q, d]), i])])-(Subscript[p, (Subscript[Q, (Subscript[d, 123])])])‖)/(  ‖(Subscript[p, i])-(Subscript[p, 123]) ‖ )) ,  i = 1, 2, 3

or in more robust way,

s=(1/3)(∑_(i=1)^3)((‖(Subscript[p, (Subscript[(Subscript[Q, d]), i])])-(Subscript[p, (Subscript[Q, (Subscript[d, 123])])])‖)/(  ‖(Subscript[p, i])-(Subscript[p, 123]) ‖ ))

When the value of  s is known then the image vector can be modified as  (SuperStar[(Subscript[p, I])])= ((Subscript[p, I])/s), and employing

(SuperStar[(Subscript[p, I])]) = ℛ p + (SuperStar[t])

This means that the translation vector can be computed as

t = s (SuperStar[t])

Example 11. Compute the coordinates of the image position vector with data used in Example 7, using  scaling  s = 1.1.
Now our data pairs are, see Table 3.

Table 3.

Coordinates of source and image points

({{Point , p1, p2, p3, pQd1, pQd2, pQd3}, {A, 2, 0, 1, (1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])), (1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))), (1/30) (3-11 (Sqrt[6]))}, {B, 1, 2, -1, (1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])), (1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])), (1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))}, {C, 0, 1, 2, (1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))), (1/30) (93-11 (Sqrt[6])), (1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}})

Now let us see the actual computation. To compute the scaling factor we need the means, see Eq. (31)

p123=Mean[{{2,0,1},{1,2,-1},{0,1,2}}]

and Eq. (32)

pQd123=Mean[{{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}}]//N

Then the scaling factor is, Eq. (33) for i =1,

s=Norm[pQd123-{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))}]/Norm[p123-{2,0,1}]

or Eq. (33) for  i =2,

s=Norm[pQd123-{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))}]/Norm[p123-{1,2,-1}]

Let us employ s in rational form to ensure to avoid round off error,

s=Rationalize[s]

Once the scaling factor is known, the input data for the 3 points A, B and C  are

dataA={p1->2,p2->0,p3->1,pQd1->(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))]))(1/s),pQd2->(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2])))(1/s),pQd3->(1/30) (3-11 (Sqrt[6]))(1/s)};

dataB={p1->1,p2->2,p3->-1,pQd1->(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6]))(1/s),pQd2->(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6]))(1/s),pQd3->(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))(1/s)};

dataC={p1->0,p2->1,p3->2,pQd1->(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2])))(1/s),pQd2->(1/30) (93-11 (Sqrt[6]))(1/s),pQd3->(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))(1/s)};

The corresponding equations

{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.dataA

{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.dataB

{e7}={eqPt1}/.dataC

Here we considered again the first coordinate of the third point C.

Eliminating the translation vector

E1=e1-e4

E2=e2-e5

E3=e3-e6

E4=e1-e7

We have four multivariate polynomial equations with unknown  (q0, q1, q2, q3)

AbsoluteTiming[sol=NSolve[{E1,E2,E3,E4},{q0,q1,q2,q3},Reals];]

The result is

sol

The proper components should be real and positive

solq=Select[sol,(#[[1]][[2]]>0)∧(#[[2]][[2]]>0)∧(#[[3]][[2]]>0)∧(#[[4]][[2]]>0)&]//Flatten

The translation vector can be expressed directly as

𝒯= MapThread[Solve[#1==0,#2]/.solq&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten

Multiplying by the scaling factor

𝒯=s{t1,t2,t3}/.𝒯

The rotation matrix is

RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];MatrixForm[RN]

Then the image vectors are

Map[(11/10)(RN.#)+𝒯&,{{2,0,1},{1,2,-1},{0,1,2}}]

Indeed

{{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}}//N

5- Mathematica function

For further application, to integrate the dual quaternion solution as a core algorithm into the RANSAC method, let us summarize the steps of the procedure in a Mathematica function.

DualQHelmert[XYZin_,xyzin_]:=Module[
{XYZ,xyz,XYZavg,xyzavg,s,sol,solq,tt,RN,i,n,sel,imin},
(*scaling factor s *)
XYZ=SetPrecision[XYZin,20];xyz=SetPrecision[xyzin,20];
XYZavg=Mean[XYZ];xyzavg=Mean[xyz];
s=Mean[MapThread[Norm[#1-xyzavg]/Norm[#2-XYZavg]&,{xyz,XYZ}]];

(* egyenletek quaternion elemeire *)
eqPt1=pQd1-(2 p2 q1 q2-2 p2 q0 q3+2 p3 (q0 q2+q1 q3)-p1 (-(q0^2)-(q1^2)+(q2^2)+(q3^2))+ t1);
     eqPt2=pQd2-(2 (-p3 q0 q1+p1 q1 q2+p1 q0 q3+p3 q2 q3)-p2 (-(q0^2)+(q1^2)-(q2^2)+(q3^2))+ t2);
      eqPt3=pQd3-(2 (p2 q0 q1-p1 q0 q2+p1 q1 q3+p2 q2 q3)-p3 (-(q0^2)+(q1^2)+(q2^2)-(q3^2))+ t3);

data1={p1->XYZ[[1]][[1]],p2->XYZ[[1]][[2]],p3->XYZ[[1]][[3]],pQd1->(1/s)xyz[[1]][[1]],pQd2->(1/s)xyz[[1]][[2]],pQd3->(1/s)xyz[[1]][[3]]};
{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.data1;

data2={p1->XYZ[[2]][[1]],p2->XYZ[[2]][[2]],p3->XYZ[[2]][[3]],pQd1->(1/s)xyz[[2]][[1]],pQd2->(1/s)xyz[[2]][[2]],pQd3->(1/s)xyz[[2]][[3]]};
{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.data2;

data3={p1->XYZ[[3]][[1]],p2->XYZ[[3]][[2]],p3->XYZ[[3]][[3]],pQd1->(1/s)xyz[[3]][[1]],pQd2->(1/s)xyz[[3]][[2]],pQd3->(1/s)xyz[[3]][[3]]};
e7=eqPt1/.data3;

E1=e1-e4;
E2=e2-e5;
E3=e3-e6;
E4=e1-e7;
(* solution of the equations *)

sol=NSolve[SetPrecision[{E1,E2,E3,E4},30],{q0,q1,q2,q3},Reals];

(*Elimination of parasitic solutions*)
sel={};
n=Length[sol];
For[i=1,i<n+1,i++,
solq=sol[[i]];

(* translations *)

tt= MapThread[(Solve[#1==0,#2]/.solq)&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten;
tt=s{t1,t2,t3}/.tt;

(* elements of quaternion *)
RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];
AppendTo[sel,Norm[xyz-Map[s RN.#+tt&,XYZ]]];

imin=First[Position[sel,Min[sel]]//Flatten];
solq=sol[[imin]]];

(* end of For *)

tt= MapThread[(Solve[#1==0,#2]/.solq)&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten;
tt=s{t1,t2,t3}/.tt;

RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];

{s,tt,RN}]

Example 12. Let us test the module DualQHelmert with data employed in Example 11.

XYZ1={{2,0,1},{1,2,-1},{0,1,2}};
xyz1={{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}};

sol1=DualQHelmert[XYZ1,xyz1]//N//Quiet

Indeed

sol1[[3]]-RQ

6- Application of the algorithm in RANSAC

6- 1 RANSAC algorithm

RANSAC is an effective algorithm to eliminate outliers from the data set applied to determination of the parameters of a similarity transformation.

Let us try to apply the RANSAC method, given in Yaniv (2010), which is proved to be successful for detecting outliers. The basic RANSAC algorithm is as follows:

1) Pick up a model type (ℳ)
2) Input data as
     -  dataQ  - data corrupted with outliers (cardinality (dataQ) = n)
     -  s     - number of data elements required per subset
     -  𝒩           -  number of subsets to draw from the data
     -  τ            -  threshold which defines if  data element, (Subscript[d, i])∈ dataQ, agrees with the model ℳ

Remarks

In general s can be the minimal number of the data which results a determined system for the unknown parameters of the model.
The number of subsets to draw from the data, 𝒩 is chosen high enough to ensure that at least one of the subsets of the random examples does not include an outliers (with the probability p, which is usually set to 0.99). Let u represent the probability that any selected data point is an inlier and v = 1 - u the probability of observing an outlier. Then the iterations 𝒩 can be computed as

𝒩 = ((log(1-p))/(log(1-(((1-v))^s))))

3) maximalConsensusSet ← ∅
4) Iterate 𝒩 times:
            a) ConsensusSet ← ∅
            b) Randomly draw a subset containing s elements and estimate the parameters of the model ℳ
            c) For each data element,  (Subscript[d, i])∈ dataQ:
                if  agree ((Subscript[d, i]),ℳ, τ), ConsensusSet ← (Subscript[d, i])
            d) if cardinality (maximalConsensusSet) < cardinality(ConsensusSet),
                maximalConsensusSet ←   ConsensusSet
                
   5) Estimate model parameters using maximalConsensusSet

6- 2 The necessary number of iterations

In our case we have 3 parameters to determine, so we need minimum 3 equations, therefore

s=3;

In this case the parameters can be computed via numerical Gröbner solution. Let us a high probability value resulting more necessary iteration

p=0.99999;

In addition let us assume that the probability of observing outlier is

ν=1/2;

Then the number of the necessary iterations is

𝒩=Round[Log[1-p]/Log[1-(((1-ν))^s)]]

6- 3 Computation of the threshold

Let the threshold  τ = 0.3 m

τ=0.3;

7- N point real world problem

We will determine the parameters of the similarity transformation  of  the coordinates of the 81 first order Hungarian stations between  the local Hungarian system HD72 and the global system of  ETRS89.

Fig.1 Map of Hungary showing the 81 points in the local system

Let us read in the corresponding measured coordinates

xyz=ReadList["E:\\Data\\HD72xyzLocal.txt",{Number,Number,Number}];

XYZ=ReadList["E:\\Data\\ETR589XYZGlobal.txt",{Number,Number,Number}];

The numerical data are,

Short[xyz,5]

Short[XYZ,5]

where xyz are the coordinates for the  ETRS89 system, while XYZ are the coordinates for the local system. Now we have

n=Length[xyz]

Let us prepare the parallel computation via launching 12 kernels

(* LaunchKernels[2 $ProcessorCount]*)

We stack the input and the corresponding output data together for the DualQHelmert function

dataQ=MapThread[{#1,#2}&,{XYZ,xyz}];

Preparation of the sets of inliers for every iteration step - at the beginning they are empty sets:

inliers=Table[{},{𝒩}] (* the consensus sets, j = 1,2, ...𝒩*);

Then let us carry out the double  For cycle to find the maximum consensus set

For[j=1,j≤𝒩,j++,
(*get four random points*)
dataS=Transpose[RandomSample[dataQ,s]];
(*determine model parameters*)
sols=DualQHelmert[dataS[[1]],dataS[[2]]];
(* determine inliers*)
For[i=1,i≤n,i++,
If[Norm[sols[[1]] sols[[3]].XYZ[[i]]+sols[[2]]-xyz[[i]]]<τ,inliers[[j]]=Append[inliers[[j]],dataQ[[i]]]];
];];

trimmeddata=inliers[[Ordering[inliers,-1]]][[1]];

The size of the consensus set

m=Length[trimmeddata]

The number of the subsets

mn=Binomial[m,s]

This is not a big number, therefore it is reasonable to use combinatorial solution, especially since the solution of the 3-Point Problem is very fast.

These subset indices  are,

qs=Partition[Map[#&,Flatten[Subsets[Range[m],{s}]]],s];Short[qs,5]

The corresponding points from the consensus set

dataGJ=ParallelMap[Transpose[{trimmeddata[[#[[1]]]],trimmeddata[[#[[2]]]],trimmeddata[[#[[3]]]]}]&,qs];

The solution sets of the Gauss Jacobi method for the parameters

solGJ=ParallelMap[DualQHelmert[#[[1]],#[[2]]]&,dataGJ];

Now we compute simple average for the parameters. The scaling factor

r1=Mean[Map[#[[1]]&,solGJ]]

The translation vector

r2=Mean[Map[#[[2]]&,solGJ]]

The rotation matrix

r3=Mean[Map[#[[3]]&,solGJ]]

Let us compute the norm of the error vector of the coordinates (x, y, z) for all of the data including outliers

Δxyz=Map[#&,xyz-Map[r1 r3.#+r2&,XYZ]];

nΔxyz=Map[Norm[#]&,Δxyz];

The mean of the error norm

Mean[nΔxyz]

The standard deviation

StandardDeviation[nΔxyz]

Show[{Histogram[nΔxyz, Automatic, "PDF"], SmoothHistogram[nΔxyz, Automatic, "PDF"]}]

Fig. 1 Error distribution of  coordinate vectors in case of all data

Let us consider now the error statistics of  the inliers only

XYZxyz=Transpose[trimmeddata];

Dimensions[XYZxyz]

nΔxyz=Map[Norm[#]&,XYZxyz[[2]]-Map[r1 r3.#+r2&,XYZxyz[[1]]]];

Mean[nΔxyz]

StandardDeviation[nΔxyz]

Show[{Histogram[nΔxyz, Automatic, "PDF"], SmoothHistogram[nΔxyz, Automatic, "PDF"]}]

Fig.2 Error distribution of  coordinate vectors in case of inliers only

Conclusions

These results show that selecting  55 data points from the 81 ones could improve the results very considerably. In addition the hump in Fig.1 indicates a mixed distribution resulted by  the mixture of the outliers and inliers errors. The main advantage of this suggested technique comparing to the other non-iterative methods is that neither SVD nor tedious algebraic preprocessing is required. The efficiency of the method can be improved to parallelize the RANSAC computation, too.

References

Awange J L, Grafarend E W, Paláncz B and Zaletnyik P (2010) Algebraic Geodesy and Geoinformatics 2nd Edition, Springer, Heidelberg.
 
Kenwright B (2012) Dual - Quaternions, From Classical  Mechanics to Computer Graphics and Beyond, pp.1 - 11.

 Paláncz B and Zaletnyik P (2011)  A symbolic solution of 3D affine transformation problem, The Mathematica Journal, Vol.13, pp. 1-15.
 
Proskova J (2011) Application of dual quaternions algorithm for geodetic datum transformation, Journal of Applied Mathematics, Vol. IV, No. 2., pp. 225 - 235

Sjöberg L E (2013) Closed -form and iterative weighted least squares solutions of Helmert transformation parameters, J of Geodetic Science, pp. 7-11.

Wang Yo, Wang Yu, Wu K, Yang H and Zhang H (2014)  A dual quaternion based, closed form pairwise registration algorithm for point clouds, ISPRS Journal of Photogrammetry and Remote Sensing, 94, pp. 63 - 69

Yaniv Z (2010) Random Sample Consensus (RANSAC) Algorithm, A Generic Implementation, Georgetown University Medical Center, Washington, DC, USA, zivy@isis.georgetown.edu

Zuliani M (2012) RANSAC for Dummies, vision.ece.ucsb.edu/~zuliani

Quaternions can be considered as an extension of the complex numbers, namely

Q = (q0, q1, q2, q3) = q0 + i q1 + J q2 + K q3 = (q0, q)

where q0 is a scalar real part and  q = (q1, q2, q3) is an imaginary vector. In Mathematica we can work with quaternions as a special data type:

<<Quaternions`

Example 1. Let us define a quaternion with the following values for components of a quaternion,

q0=1;q1=2;q2=3;q3=4;

Then the quaternion type variable Q with theses data is

Q=Quaternion[q0,q1,q2,q3]

indeed Q is quaternion data type

Head[Q]

2- 2-  Quaternion of rotation

Given an angle θ and the axis of rotation represented by a unit-vector n = ((Subscript[n, x]), (Subscript[n, y]), (Subscript[n, z])). Then we can construct a quaternion as

Q = (cos ((θ/2)), n sin((θ/2)))

or

q0 = cos ((θ/2)), q1= (Subscript[n, x]) sin((θ/2)), q2= (Subscript[n, y]) sin((θ/2)), q3= (Subscript[n, z]) sin((θ/2))

Sometimes having the quaternion, we need to know the angle, then

θ =2 Arccos(q0)

and the vector of rotation axis is

(Subscript[n, x])= (q1/(sin((θ/2)))), (Subscript[n, y])= (q2/(sin((θ/2)))), (Subscript[n, z])= (q3/(sin((θ/2)))),

Example 2. Let us develop the quaternion of rotation when  θ = (π/4) and the axis of rotation is  n = ( (1/(Sqrt[3])),(1/(Sqrt[3])), (1/(Sqrt[3]))).

θ=(π/4);n={(1/(Sqrt[3])),(1/(Sqrt[3])),(1/(Sqrt[3]))};

where n is a unit vector. Indeed

Norm[n]

The corresponding quaternion is

Q=Quaternion[Cos[(θ/2)],Sin[(θ/2)]n[[1]],Sin[(θ/2)]n[[2]],Sin[(θ/2)]n[[3]]]

Using built-in function, we can get the elements of the quaternion, since

wxyz=FromQuaternion[Q]

Then the angel of the rotation is

θ=2ArcCos[Re[wxyz]/.{J->0,K->0}]

or more directly

2ArcCos[Re[Q]]

and the unit vector of the rotation axis

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}/Sin[(θ/2)]

Since rotation matrix is orthogonal, therefore

Norm[Q]//TrigReduce

2- 3-  Rotation transformation with Quaternion

Let p a position vector of a point in 3D. We can rotate this vector counter-clockwise with the quaternion of rotation Q as

(Subscript[P, Q])= Q P (Subscript[Q, c])

where P is a pure quaternion, P = (0, p) and (Subscript[Q, c]) is the conjugate quaternion of Q. (Subscript[P, Q]) is quaternion of the rotated vector, which is also a pure quaternion (Subscript[P, Q]) = (0, (Subscript[p, Q]) ) and (Subscript[p, Q]) is the position vector of the rotated point.

Example 3. Let us compute the conjugate of the quaternion of rotation Q . We can use built in function,

Qc=Conjugate[Q]

You can realize that, similarly to complex numbers, the conjugate can be expressed as

(Subscript[Q, c]) = (({{ q0}, {-q1}, {-q2}, {-q3}}))

We can consider the conjugate of a quaternion as the inverse of the quaternion,

FromQuaternion[Q**Qc]

Example 4. Let  a position  vector be p = {2, 0, 1}, which is rotated by  θ = (π/2) about the axis given by n . Let us compute the image of p.

p={2,0,1};

P=Quaternion[0,p[[1]],p[[2]],p[[3]]]

Then according to Eq.(6)

PQ=Q**P**Qc

The result is a pure quaternion

wxyz=FromQuaternion[PQ]//TrigReduce

which means its real part is zero. The rotated vector, the image point is

pQ={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}//Simplify

Let us check the result employing built in function of the Mathematica. The matrix of the rotation is

R=RotationTransform[θ,n]

Then the rotated vector

R[p]//Simplify

2- 4-  Quaternion to Matrix

In order to avoid using quaternion as a new data type, we can employ the matrix form of a quaternion. A quaternion can be expressed by a 4 × 4 matrix

Q =(q0, q1, q2, q3)= (({{q0, -q1, -q2,  -q3}, {q1,     q0, -q3,      q2}, {q2,    q3,     q0, -q1}, {q3, -q2,     q1,     q0}}))

Let us define a new function, which creates the matrix form of a quaternion from its components,

QuaternionMatrixForm[{q0_,q1_,q2_,q3_}]:={{q0,-q1,-q2,-q3},{q1,q0,-q3,q2},{q2,q3,q0,-q1},{q3,-q2,q1,q0}}

Example 5. Do carry out the multiplication (Subscript[P, Q])=Q**P**(Subscript[Q, c]) in matrix form! We should create the matrix form of the quaternions Q and P, namely (Subscript[M, Q]) and (Subscript[M, P]), and the vector form of  (Subscript[Q, c]). Then the vector form of the rotated vector  is a pure quaternion

(Subscript[p, Q])= (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c])= (Subscript[M, Q]) (Subscript[M, P]) (({{    q0}, {-q1}, {-q2}, {-q3}}))=(({{0}, {(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

Consequently the rotated vector is

(Subscript[p, r])=(({{(Subscript[p, r1])}, {(Subscript[P, r2])}, {(Subscript[P, r3])}}))= (({{(Subscript[p, Q1])}, {(Subscript[P, Q2])}, {(Subscript[P, Q3])}}))

Let us construct the matrix form of the rotation quaternion, (Subscript[M, Q]). Since

wxyz=FromQuaternion[Q]

and

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

Therefore the matrix form of Q is,

MQ=QuaternionMatrixForm[Join[{Re[wxyz/.{J->0,K->0}]},n]]

or

MatrixForm[MQ]

Similarly, the matrix form of the position vector (Subscript[M, P]) can be computed as

wxyz=FromQuaternion[P]

and

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

Then

MP=QuaternionMatrixForm[Join[{Re[wxyz/.{J->0,K->0}]},n]];

MatrixForm[MP]

or in more simple  way,

QuaternionMatrixForm[{0,2,0,1}]//MatrixForm

The matrix  (vector) form of the conjugate of the rotation quaternion, (Subscript[M, Q]) can be similarly computed

wxyz=FromQuaternion[Qc]

n={Im[wxyz/.{J->0,K->0}],Coefficient[wxyz,J],Coefficient[wxyz,K]}

MQc=Join[{Re[wxyz/.{J->0,K->0}]},n];MatrixForm[MQc]

Then the rotated vector

pQ=MQ.MP.MQc//TrigReduce//Expand

It is really a pure quaternion.

2- 5-   Inverse problem of rotation

Now, we assume that the coordinates of a point and the coordinates of its image are known and we should like to get the vector of the rotation axis and the angle of the rotation. We shall use here source point and image point , instead of original and rotated vector. This means we are looking for q0, q1, q2 and q3, the components of the quaternion of the rotation. We have seen that the image, the rotated point is a pure quaternion, see Eq.(9)

(Subscript[p, Q])= (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c])= (Subscript[M, Q]) (Subscript[M, P]) (({{    q0}, {-q1}, {-q2}, {-q3}}))=(({{0}, {(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

and the position vector of the rotated point is, see Eq. (10)

(Subscript[p, r])=(({{(Subscript[p, r1])}, {(Subscript[p, r2])}, {(Subscript[p, r3])}}))= (({{(Subscript[p, Q1])}, {(Subscript[p, Q2])}, {(Subscript[p, Q3])}}))

Consequently, these relations provide only  3 equations for the 4 unknowns. So we need 2 points to solve the inverse problem - more precisely, one more coordinate pair of  an additional second point is needed. Let us consider the term (Subscript[M, Q]) (Subscript[M, P])  (Subscript[Q, c]) which provides 4 equations,

Clear[q0,q1,q2,q3]

eqs=QuaternionMatrixForm[{q0,q1,q2,q3}].QuaternionMatrixForm[{0,p1,p2,p3}].{q0,-q1,-q2,-q3}

But the first one is zero as it is expected,

eqs[[1]]//Simplify

Therefore we have 3 prototype equations for the source point p = (p1, p2, p3) and its image (Subscript[p, r]) = ((Subscript[p, r1]), (Subscript[p, r2]), (Subscript[p, r3])), namely

eq1=eqs[[2]]-pr1

eq2=eqs[[3]]-pr2

eq3=eqs[[4]]-pr3

We have already one data pair, now we computed a new one, see Table 1.

Table 1.

Coordinates of source and image points

({{Point, p1, p2, p3, pr1, pr2, pr3}, {a, 2, 0, 1, 1+(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(Sqrt[(2/3)])}, {b, 1, 2, -1, (1/6) (4+(Sqrt[2])-3 (Sqrt[6])), (1/3) (2+2 (Sqrt[2])+(Sqrt[6])), (1/6) (4-5 (Sqrt[2])+(Sqrt[6]))}})

Let us develop the equations with these corresponding numerical data. The data pairs for the 2 corresponding pairs of points  are

data1={p1->2,p2->0,p3->1,pr1->1+(1/(Sqrt[2]))+(1/(Sqrt[6])),pr2->1-(1/(Sqrt[2]))+(1/(Sqrt[6])),pr3->1-(Sqrt[(2/3)])};
data2={p1->1,p2->2,p3->-1,pr1->(1/6) (4+(Sqrt[2])-3 (Sqrt[6])),pr2->(1/3) (2+2 (Sqrt[2])+(Sqrt[6])),pr3->(1/6) (4-5 (Sqrt[2])+(Sqrt[6]))};

Then the equations for the first data pair,

{e1,e2,e3}={eq1,eq2,eq3}/.data1

and from the second data pair we consider only the first coordinates,

{e4}={eq1}/.data2

To compute Q = (q0, q1, q2, q3) this multivariate polynomial system with 4 unknowns should be solved. let us employ numerical Groebner basis

AbsoluteTiming[sol=NSolve[{e1,e2,e3,e4},{q0,q1,q2,q3}];]

The solutions are

sol

Considering the positive real solutions

Q={q0,q1,q2,q3}/.sol[[2]]

Let us check the result

Q-{Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))}

Here we have got the components of the rotation quaternion, and the rotation angle and the coordinates of the rotation axis can be easily computed with Eqs. (4) and (5).   Although the standard rotation matrix can be expressed by the element of the quaternion, too,

RQ = (({{((q0)^2)+((q1)^2)-((q2)^2)-((q3)^2), 2 (q1 q2-q0 q3), 2 (q0 q2+q1 q3)}, {2 (q1 q2+q0 q3), ((q0)^2)-((q1)^2)+((q2)^2)-((q3)^2), 2 (-q0 q1+q2 q3)}, {2 (-q0 q2+q1 q3), 2 (q0 q1+q2 q3), ((q0)^2)-((q1)^2)-((q2)^2)+((q3)^2)}}))

Let us define the corresponding function,

QuaternionToRotationMatrix[{q0_,q1_,q2_,q3_}]:={{(q0^2)+(q1^2)-(q2^2)-(q3^2),2(q1 q2-q0 q3),2(q1 q3+q0 q2)},{2(q1 q2+q0 q3),(q0^2)-(q1^2)+(q2^2)-(q3^2),2(q2 q3-q0 q1)},{2(q1 q3-q0 q2),2(q2 q3+q0 q1),(q0^2)-(q1^2)-(q2^2)+(q3^2)}}

In our case

RQ=QuaternionToRotationMatrix[Q];MatrixForm[RQ]

To check this result, let us consider the rotation transformation has been given by the built in function of Mathematica

R

Extracting the rotation matrix

RM=Map[Drop[#,-1]&,Drop[R[[1]],-1]];MatrixForm[RM]

and indeed,

MatrixForm[RM-RQ]

3 - Dual quaternions

3- 1-   Dual number and dual quaternion

Dual numbers were invented by Clifford in 1873. Dual numbers are akin to complex numbers.

z = r + ϵ d   with  (ϵ^2)= 0   but ϵ ≠ 0

where ϵ is known as the dual operator, r is the real part and d is the dual part. Now, let us consider r and d as two standard quaternions, Q and (Subscript[Q, ϵ])

(Subscript[Q, d])= Q + ϵ (Subscript[Q, ϵ])

where (Subscript[Q, d]) is called as dual quaternion and it can be considered as an extension of the dual numbers. This formalism allows the unification of rotation and translation.

3- 2- Dual Quaternion to rigid  transformation

Let p = ((Subscript[p, 1]),(Subscript[p, 2]), (Subscript[p, 3])) is a position vector of a point, t =  ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) is a translation vector and Q = (cos ((θ/2)), n sin((θ/2))) is a unit quaternion for rotation. Then we can express the image of the point (Subscript[p, (Subscript[Q, d])]) =  ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) after this translation and rotation as

(Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

where (Subscript[P, d]) is a unit dual quaternion

(Subscript[P, d]) = 1 + ϵ (i (Subscript[p, 1])  + J (Subscript[p, 2])+ K (Subscript[p, 3]))

and let 𝒯 is a pure quaternion as

𝒯=0 +(i (Subscript[t, 1])  + J (Subscript[t, 2])+ K (Subscript[t, 3]))

Now the dual quaternion (Subscript[Q, d]) has its real part Q and its dual part (1/2) 𝒯 Q, namely

(Subscript[Q, d]) = Q + ϵ (Subscript[Q, ϵ]) = Q + ϵ (1/2) 𝒯 Q

and (Subscript[(Subscript[Q, d]), c]) is the conjugate of  (Subscript[Q, d]). The resulted dual quaternion (Subscript[P, (Subscript[Q, d])]) is a unit dual quaternion

(Subscript[P, (Subscript[Q, d])])=1 + ϵ ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

Now let us develop some functions to handle dual quaternions, since Mathematica does not support dual quaternion data type.

• First, we  define a function, which creates dual quaternion from the quaternion of the real part and from the dual part,

DualQuaternion[Q1_,Q2_]:=Quaternion[Q1[[1]],Q1[[2]],Q1[[3]],Q1[[4]]]+e Quaternion[Q2[[1]],Q2[[2]],Q2[[3]],Q2[[4]]]

• We require to define the product of two dual quaternions. Let (Subscript[Q, (Subscript[d, 1])]) and (Subscript[Q, (Subscript[d, 2])]) two dual quaternions, where (Subscript[Q, d])= Q + ϵ (Subscript[Q, ϵ]), then

(Subscript[Q, (Subscript[d, 1])]) (Subscript[Q, (Subscript[d, 2])]) =(Subscript[Q, 1]) (Subscript[Q, 2])+ϵ ((Subscript[Q, 1])(Subscript[Q, (Subscript[ϵ, 2])]) + (Subscript[Q, (Subscript[ϵ, 1])])(Subscript[Q, 2]))

The corresponding Mathematica function is

ProductDual[Q1_,Q2_]:=Coefficient[Q1,e,0]**Coefficient[Q2,e,0]+e(Coefficient[Q1,e,0]**Coefficient[Q2,e,1]+Coefficient[Q1,e,1]**Coefficient[Q2,e,0])

• We also need a function, which computes the dual conjugate of the dual quaternion, (Subscript[(Subscript[Q, d]), c]). The conjugate of  the quaternion  Q is (Subscript[Q, c]), and the dual conjugate of the dual quaternion is

(Subscript[(Subscript[Q, d]), c])=(Subscript[Q, c])-  ϵ (Subscript[Q, (Subscript[ϵ, c])])

The corresponding Mathematica function is

ConjugateDual[Q_]:=(Conjugate[Coefficient[Q,e,0]]-e Conjugate[Coefficient[Q,e,1]])

• We may also require to extract the real and the dual part of a dual quaternion,

RealFromDualQuaternion[Q_]:=FromQuaternion[Coefficient[Q,e,0]]

DualFromDualQuaternion[Q_]:=FromQuaternion[Coefficient[Q,e,1]]

Example 6. First let us apply dual quaternion to only pure rotation employing data used in the examples above. Since there is no translation, now the pure zero quaternion, namely  𝒯

T=Quaternion[0,0,0,0]

Let

Q=Quaternion[Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))];

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) is

Qd=DualQuaternion[Q,Qe]

Similarly let us create the unit dual quaternion of the p vector, (Subscript[P, d])

Pd=DualQuaternion[Quaternion[1,0,0,0],P]

Let us compute the dual conjugate of this dual quaternion

Qdc=ConjugateDual[Qd]

Now, we  can compute the dual quaternion of the 3D rigid transformation, now only rotation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

The real part of the resulted dual quaternion is

wxyz=RealFromDualQuaternion[PQd]

and the dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the rotated vector

{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//Simplify

Example 7. Now, we add a translation with vector t = (1, 2, -1),

The pure quaternion for translation  𝒯  is

T=Quaternion[0,1,2,-1]

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) is

Qd=DualQuaternion[Q,Qe]

Its dual conjugate

Qdc=ConjugateDual[Qd]

Now we  can compute the dual quaternion of the 3D rigid transformation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

wxyz=RealFromDualQuaternion[PQd]

and the dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the rotated and translated vector

{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//Simplify

Let us check this result via using built-in function. The transformation of the translation

T=TranslationTransform[{1,2,-1}]

The translation after rotation

pT=T[R[p]]//Simplify

3- 3-  Dual quaternion to Matrix

Let us consider a dual quaternion as (Subscript[Q, d]) = Q + ϵ (Subscript[Q, ϵ]), where the real part is Q = (q0, q1, q2, q3) and the dual part

(Subscript[Q, ϵ])=(1/2)𝒯 Q=(1/2) (0,t1,t2,t3)**(q0,q1,q2,q3)

then the  matrix representation of  (Subscript[Q, d]) is a 8 × 8 matrix,

(Subscript[Q, d]) =(({{Q, (Subscript[Q, ϵ])}, {0, Q}}))

The matrix form of Q

Q=QuaternionMatrixForm[{q0,q1,q2,q3}];MatrixForm[Q]

The dual part is

qe=(1/2)QuaternionMatrixForm[{0,t1,t2,t3}].{q0,q1,q2,q3};

In matrix form

Qe=QuaternionMatrixForm[qe];MatrixForm[Qe]

Let put them together in a matrix

MD=Join[Join[Q,Qe,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},Q,2]];MatrixForm[MD]

Similarly the matrix representation of the dual conjugate of the dual quaternion is

(Subscript[(Subscript[Q, d]), c])=(({{(Subscript[Q, c]), -(Subscript[Q, (Subscript[ϵ, c])])}, {0, (Subscript[Q, c])}}))

Qc=QuaternionMatrixForm[{q0,-q1,-q2,-q3}];MatrixForm[Qc]

Qec=QuaternionMatrixForm[{qe[[1]],-qe[[2]],-qe[[3]],-qe[[4]]}];MatrixForm[Qec]

Let us put them together

MQc=Join[Join[Qc,-Qec,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},Qc,2]];MatrixForm[MQc]

As we have seen, the dual quaternion of the position vector of the point to be transformed is a unit dual quaternion

(Subscript[P, d]) = 1 + ϵ (i (Subscript[p, 1])  + J (Subscript[p, 2])+ K (Subscript[p, 3]))

Therefore the real part of   (Subscript[P, d])

PdR=QuaternionMatrixForm[{1,0,0,0}];MatrixForm[PdR]

and its dual part

PdD=QuaternionMatrixForm[{0,p1,p2,p3}];MatrixForm[PdD]

Now putting them together in a matrix

MPd=Join[Join[PdR,PdD,2],Join[{{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}},PdR,2]];MatrixForm[MPd]

Then the matrix form of the dual quaternion representing the image point is

PQd=MD.MPd.MQc;

This matrix can be simplified considering  ((q0)^2)+((q1)^2)+((q2)^2)+((q3)^2)= 1. Indeed

dataQ={q0->Cos[(π/8)],q1->((Sin[(π/8)])/(Sqrt[3])),q2->((Sin[(π/8)])/(Sqrt[3])),q3->((Sin[(π/8)])/(Sqrt[3]))};

(((q0^2)+(q1^2)+(q2^2)+(q3^2))/.dataQ)//TrigReduce

Therefore  (Subscript[P, Qd]) can be simplified

PQd=(PQd//FullSimplify)/.{((q0^2)+(q1^2)+(q2^2)+(q3^2))->1};

The result is a unit dual quaternion in matrix form. Let the image point coordinates ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])), and we have seen that the result matrix is the matrix form of  the dual quaternion

(Subscript[P, (Subscript[Q, d])])=1 + ϵ ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

In addition the matrix form of the dual part, which is a pure quaternion is

QuaternionMatrixForm[{0,pQd1,pQd2,pQd3}]//MatrixForm

Therefore the structure of our matrix is

(Subscript[P, Qd])=(({{1, 0, 0, 0, 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 3])])}, {0, 1, 0, 0, (Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 3])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])])}, {0, 0, 1, 0, (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]), 0, -(Subscript[p, (Subscript[(Subscript[Q, d]), 1])])}, {0, 0, 0, 1, (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]), -(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), 0}, {0, 0, 0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 0, 0, 1}}))

Indeed,

PQd[[All,1;;4]]//MatrixForm

PQd[[5;;8]]//MatrixForm

Then the coordinates of the image position  vector are

pQd1=-PQd[[1,6]]

pQd2=-PQd[[1,7]]

pQd3=-PQd[[1,8]]

Example 8. Let us compute the coordinates of the image position vector with data used in Example 7, using matrix representation.

The translation vector

datat={t1->1,t2->2,t3->-1};

The rotation quaternion

dataQ

then the coordinates are

{pQd1,pQd2,pQd3}/.dataQ/.datat/.{p1->2,p2->0,p3->1}//TrigReduce//FullSimplify

Let us check it with the result of the Example 7

pT

3- 4-  Inverse problem of the rigid transformation

Similarly to the inverse problem of rotation, now the 7 parameters of  rigid transformation should be computed from the corresponding coordinate pairs. Here, we need 3 points or at least 7 coordinate pairs. The prototype equations with the known  p = ((Subscript[p, 1]),(Subscript[p, 2]), (Subscript[p, 3])) and (Subscript[p, (Subscript[Q, d])]) = ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]),(Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) position vectors,  and with the unknown components of the unit quaternion of rotation Q = (q0, q1 , q2, q3)  and translation vector  t =  ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) are

Clear[pQd1,pQd2,pQd3];

eqPt1=pQd1-(2 p2 q1 q2-2 p2 q0 q3+2 p3 (q0 q2+q1 q3)-p1 (-(q0^2)-(q1^2)+(q2^2)+(q3^2))+ t1);

eqPt2=pQd2-(2 (-p3 q0 q1+p1 q1 q2+p1 q0 q3+p3 q2 q3)-p2 (-(q0^2)+(q1^2)-(q2^2)+(q3^2))+ t2);

eqPt3=pQd3-(2 (p2 q0 q1-p1 q0 q2+p1 q1 q3+p2 q2 q3)-p3 (-(q0^2)+(q1^2)+(q2^2)-(q3^2))+ t3);

Example 9. Compute the parameters of the rigid transformation employed above!

We three data pairs, see Table 2.

Table 2.

Coordinates of source and image points

({{Point , p1, p2, p3, pQd1, pQd2, pQd3}, {a, 2, 0, 1, 1+(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(1/(Sqrt[2]))+(1/(Sqrt[6])), 1-(Sqrt[(2/3)])}, {b, 1, 2, -1, (1/6) (4+(Sqrt[2])-3 (Sqrt[6])), (1/3) (2+2 (Sqrt[2])+(Sqrt[6])), (1/6) (4-5 (Sqrt[2])+(Sqrt[6]))}, {c, 0, 1, 2, 2-(1/(Sqrt[2]))+(1/(Sqrt[6])), 3-(Sqrt[(2/3)]), (1/(Sqrt[2]))+(1/(Sqrt[6]))}})

Then our equation system is

data1={p1->2,p2->0,p3->1,pQd1->2+(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd2->3-(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd3->-(Sqrt[(2/3)])};

{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.data1

data2={p1->1,p2->2,p3->-1,pQd1->(1/6) (10+(Sqrt[2])-3 (Sqrt[6])),pQd2->(1/3) (8+2 (Sqrt[2])+(Sqrt[6])),pQd3->(1/6) (-2-5 (Sqrt[2])+(Sqrt[6]))};

{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.data2

data3={p1->0,p2->1,p3->2,pQd1->2-(1/(Sqrt[2]))+(1/(Sqrt[6])),pQd2->3-(Sqrt[(2/3)]),pQd3->(1/(Sqrt[2]))+(1/(Sqrt[6]))};

{e7}={eqPt1}/.data3

These 7 equations can be simplified by eliminating the components of the translation vector t.

E1=e1-e4

E2=e2-e5

E3=e3-e6

E4=e1-e7

AbsoluteTiming[sol=NSolve[{E1,E2,E3,E4},{q0,q1,q2,q3}];]

sol

We select quaternions having elements being all real and positive

solq=sol[[4]]

indeed

{Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))}//N

The translation vector can be expressed directly as

𝒯=MapThread[Solve[#1==0,#2]/.solq&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten

4- Extention to similarity transformation

4- 4-  Direct similarity transformation

We have seen that rigid transformation can be achieved by dual quaternions. But in geodesy we need to apply similarity datum transformation including scaling in addition. Namely the transformation is

(Subscript[p, ℐ]) = s ℛ p + t

where s is the scaling parameter, ℛ is the rotation matrix, t the translation vector, p and (Subscript[p, ℐ]) are the position vector and image vector, respectively. Let use assume that the scaling parameter is known, than the transformation can be written in the form of a rigid transformation,

(Subscript[p, (Subscript[Q, d])])= ((Subscript[p, ℐ])/s) = ℛ p + (t/s) = (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

Then the direct transformation problem can be solved similarly to the case of the rigid transformation, see Section 3.2, but we shall employ t = (1/s) ((Subscript[t, 1]), (Subscript[t, 2]), (Subscript[t, 3])) as translation vector, and the resulted image vector is (Subscript[p, ℐ]) = s(Subscript[p, (Subscript[Q, d])]) = s ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])).

Example 10. Compute the coordinates of the image position vector with data used in Example 7, using  scaling s = 1.1

𝓈=1.1;𝓈=Rationalize[𝓈]

The pure quaternion  T  is

T=(1/𝓈)Quaternion[0,1,2,-1]

The quaternion of the rotation

Q=Quaternion[Cos[(π/8)],((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3])),((Sin[(π/8)])/(Sqrt[3]))];

then

Qe=(1/2) T**Q

Then the dual quaternion  (Subscript[Q, d]) is

Qd=DualQuaternion[Q,Qe]

Its dual conjugate

Qdc=ConjugateDual[Qd]

Now we  can compute the dual quaternion of the 3D  transformation (Subscript[P, (Subscript[Q, d])])= (Subscript[Q, d]) (Subscript[P, d]) (Subscript[(Subscript[Q, d]), c])

PQd=ProductDual[ProductDual[Qd,Pd],Qdc]

Pd

wxyz=RealFromDualQuaternion[PQd]

and the dual part of the resulted dual quaternion is

ewxyz=DualFromDualQuaternion[PQd]//TrigReduce

Then the rotated position vector

𝓈{Im[ewxyz/.{J->0,K->0}],Coefficient[ewxyz,J],Coefficient[ewxyz,K]}//FullSimplify

Let us check this result via using built-in function

T=TranslationTransform[{1,2,-1}]

The translation after rotation

pT=T[𝓈 R[p]]//FullSimplify

4- 5-  Inverse problem of the similarity transformation

In order employ the form of the rigid transformation to compute the transformation parameters from the measured corresponding data points, first the scaling parameter should be determined.

Let us ((Subscript[p, 1]),(Subscript[p, 2]), (Subscript[p, 3])) are three position source vectors and  ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 2])]), (Subscript[p, (Subscript[(Subscript[Q, d]), 3])])) are the three corresponding image vectors, then we compute the gravitational center points of these two triplets are

(Subscript[p, 123]) = (1/3)  ((Subscript[p, 1])+(Subscript[p, 2])+(Subscript[p, 3]))

and

(Subscript[p, (Subscript[Q, (Subscript[d, 123])])]) = (1/3)  ((Subscript[p, (Subscript[(Subscript[Q, d]), 1])])+(Subscript[p, (Subscript[(Subscript[Q, d]), 2])])+ (Subscript[p, (Subscript[(Subscript[Q, d]), 3])]))

Then scaling parameter can be computed as

s= ((‖(Subscript[p, (Subscript[(Subscript[Q, d]), i])])-(Subscript[p, (Subscript[Q, (Subscript[d, 123])])])‖)/(  ‖(Subscript[p, i])-(Subscript[p, 123]) ‖ )) ,  i = 1, 2, 3

Since it is in practice rarely true, an average can be considered

s=(1/3)(∑_(i=1)^3)((‖(Subscript[p, (Subscript[(Subscript[Q, d]), i])])-(Subscript[p, (Subscript[Q, (Subscript[d, 123])])])‖)/(  ‖(Subscript[p, i])-(Subscript[p, 123]) ‖ ))

Whenever the s is known then the image data can be modified (SuperStar[(Subscript[p, I])])= ((Subscript[p, I])/s), and employing

(SuperStar[(Subscript[p, I])]) = ℛ p + (SuperStar[t])

means that the translation vector is

t = s (SuperStar[t])

Example 11. Compute the coordinates of the image position vector with data used in Example 7, using  scaling s = 1.1.
Now our data pairs are, see Table 3.

Table 3.

Coordinates of source and image points

({{Point , p1, p2, p3, pQd1, pQd2, pQd3}, {a, 2, 0, 1, (1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])), (1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))), (1/30) (3-11 (Sqrt[6]))}, {b, 1, 2, -1, (1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])), (1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])), (1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))}, {c, 0, 1, 2, (1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))), (1/30) (93-11 (Sqrt[6])), (1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}})

To compute the scaling factor we need the means

p123=Mean[{{2,0,1},{1,2,-1},{0,1,2}}]

pQd123=Mean[{{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}}]//N

Then the scaling factor is

s=Norm[pQd123-{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))}]/Norm[p123-{2,0,1}]

or

s=Norm[pQd123-{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))}]/Norm[p123-{1,2,-1}]

s=Rationalize[s]

Once the scaling factor is known, the input data are

data1={p1->2,p2->0,p3->1,pQd1->(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))]))(1/s),pQd2->(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2])))(1/s),pQd3->(1/30) (3-11 (Sqrt[6]))(1/s)};

data2={p1->1,p2->2,p3->-1,pQd1->(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6]))(1/s),pQd2->(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6]))(1/s),pQd3->(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))(1/s)};

data3={p1->0,p2->1,p3->2,pQd1->(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2])))(1/s),pQd2->(1/30) (93-11 (Sqrt[6]))(1/s),pQd3->(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))(1/s)};

The corresponding equations

{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.data1

{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.data2

{e7}={eqPt1}/.data3

Eliminating the translation vector

E1=e1-e4

E2=e2-e5

E3=e3-e6

E4=e1-e7

AbsoluteTiming[sol=NSolve[{E1,E2,E3,E4},{q0,q1,q2,q3},Reals];]

sol

All elements should be real and positive

solq=Select[sol,(#[[1]][[2]]>0)∧(#[[2]][[2]]>0)∧(#[[3]][[2]]>0)∧(#[[4]][[2]]>0)&]//Flatten

The translation vector can be expressed directly as

𝒯= MapThread[Solve[#1==0,#2]/.solq&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten

Multiplying by the scaling factor

𝒯=s{t1,t2,t3}/.𝒯

The rotation matrix is

RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];MatrixForm[RN]

Then the transformation, the image vectors are

Map[(11/10)(RN.#)+𝒯&,{{2,0,1},{1,2,-1},{0,1,2}}]

indeed

{{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}}//N

5- Application to real data

5- 1-  Mathematica function

DualQHelmert[XYZin_,xyzin_]:=Module[
{XYZ,xyz,XYZavg,xyzavg,s,sol,solq,tt,RN,i,n,sel,imin},
(*scaling factor s *)
XYZ=SetPrecision[XYZin,20];xyz=SetPrecision[xyzin,20];
XYZavg=Mean[XYZ];xyzavg=Mean[xyz];
s=Mean[MapThread[Norm[#1-xyzavg]/Norm[#2-XYZavg]&,{xyz,XYZ}]];

(* egyenletek quaternion elemeire *)
eqPt1=pQd1-(2 p2 q1 q2-2 p2 q0 q3+2 p3 (q0 q2+q1 q3)-p1 (-(q0^2)-(q1^2)+(q2^2)+(q3^2))+ t1);
     eqPt2=pQd2-(2 (-p3 q0 q1+p1 q1 q2+p1 q0 q3+p3 q2 q3)-p2 (-(q0^2)+(q1^2)-(q2^2)+(q3^2))+ t2);
      eqPt3=pQd3-(2 (p2 q0 q1-p1 q0 q2+p1 q1 q3+p2 q2 q3)-p3 (-(q0^2)+(q1^2)+(q2^2)-(q3^2))+ t3);

data1={p1->XYZ[[1]][[1]],p2->XYZ[[1]][[2]],p3->XYZ[[1]][[3]],pQd1->(1/s)xyz[[1]][[1]],pQd2->(1/s)xyz[[1]][[2]],pQd3->(1/s)xyz[[1]][[3]]};
{e1,e2,e3}={eqPt1,eqPt2,eqPt3}/.data1;

data2={p1->XYZ[[2]][[1]],p2->XYZ[[2]][[2]],p3->XYZ[[2]][[3]],pQd1->(1/s)xyz[[2]][[1]],pQd2->(1/s)xyz[[2]][[2]],pQd3->(1/s)xyz[[2]][[3]]};
{e4,e5,e6}={eqPt1,eqPt2,eqPt3}/.data2;

data3={p1->XYZ[[3]][[1]],p2->XYZ[[3]][[2]],p3->XYZ[[3]][[3]],pQd1->(1/s)xyz[[3]][[1]],pQd2->(1/s)xyz[[3]][[2]],pQd3->(1/s)xyz[[3]][[3]]};
e7=eqPt1/.data3;

E1=e1-e4;
E2=e2-e5;
E3=e3-e6;
E4=e1-e7;
(* solution of the equations *)

sol=NSolve[SetPrecision[{E1,E2,E3,E4},30],{q0,q1,q2,q3},Reals];

(*Elimination of parasitic solutions*)
sel={};
n=Length[sol];
For[i=1,i<n+1,i++,
solq=sol[[i]];

(* translations *)

tt= MapThread[(Solve[#1==0,#2]/.solq)&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten;
tt=s{t1,t2,t3}/.tt;

(* elements of quaternion *)
RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];
AppendTo[sel,Norm[xyz-Map[s RN.#+tt&,XYZ]]];

imin=First[Position[sel,Min[sel]]//Flatten];
solq=sol[[imin]]];

(* eltolás *)

tt= MapThread[(Solve[#1==0,#2]/.solq)&,{{e1,e2,e3},{t1,t2,t3}}]//Flatten;
tt=s{t1,t2,t3}/.tt;

(* quaternion elemei *)
RN=QuaternionToRotationMatrix[{q0,q1,q2,q3}/.solq];

{s,tt,RN}]

5- 2-  Three- point problem

Example 12. Let us test the modul DualQHelmert with data employed in Example 11.

XYZ1={{2,0,1},{1,2,-1},{0,1,2}};
xyz1={{(1/30) (63+11 (Sqrt[3 (2+(Sqrt[3]))])),(1/30) (93+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (3-11 (Sqrt[6]))},{(1/60) (104+11 (Sqrt[2])-33 (Sqrt[6])),(1/30) (82+22 (Sqrt[2])+11 (Sqrt[6])),(1/60) (-16-55 (Sqrt[2])+11 (Sqrt[6]))},{(1/30) (63+((11 (-3+(Sqrt[3])))/(Sqrt[2]))),(1/30) (93-11 (Sqrt[6])),(1/30) (3+11 (Sqrt[3 (2+(Sqrt[3]))]))}};

sol1=DualQHelmert[XYZ1,xyz1]//N

sol1[[3]]-RQ

Example 12. Let us test the modul DualQHelmert with data employed in Example 11.

xyz2=(({{183.00, 500.40, 800.02}, {181.53, 749.31, 402.84}, {223.76, 279.16, 393.28}}));XYZ2=(({{423.09, 500.42, 800.02}, {423.78, 766.61, 379.80}, {465.59, 267.41, 367.23}}));

AbsoluteTiming[sol2=DualQHelmert[XYZ2,xyz2];]

sol2[[1]]

sol2[[2]]

sol2[[3]]//MatrixForm

Map[sol2[[1]] sol2[[3]].#+sol2[[2]]&,XYZ2]-xyz2

ko=

5- 2-  N point problem

The numerical data are,

xyz=(({{4149691.049, 688865.785, 4779096.588}, {4173451.354, 690369.375, 4758594.075}, {4177796.064, 643026.700, 4761228.899}, {4139407.506, 702700.227, 4786016.645}}));

and

XYZ=(({{4149043.336, 688836.443, 4778632.188}, {4172803.511, 690340.078, 4758129.701}, {4177148.376, 642997.635, 4760764.800}, {4138759.902, 702670.738, 4785552.196}}));

where xyz are the coordinates for the WGS-84 system, while XYZ are the coordinates for the local system. In rule form,

Now, we have 7 points and any 3 of them form a subset,

n=3;m=4;

The number of the subsets

mn=Binomial[m,n]

This is not a big number, therefore it is reasonable to use combinatorial solution, especially since the solution of the 3-Point Problem is very fast.

These subsets  are,

qs=Partition[Map[#&,Flatten[Subsets[Range[m],{n}]]],n]

The data values for the subsets can be generated as it follows,

dataGJ=Map[Transpose[#]&,Table[{XYZ[[qs[[i,j]]]],xyz[[qs[[i,j]]]]},{i,1,mn},{j,1,n}]];

AbsoluteTiming[solGJ=ParallelMap[DualQHelmert[#[[1]],#[[2]]]&,dataGJ];]

obj[r_,xyz_,XYZ_]:=Norm[xyz-Map[r[[1]] r[[3]].#+r[[2]]&,XYZ]]

errs=Table[obj[solGJ[[i]],xyz,XYZ],{i,1,mn}]

Max[errs]

Min[errs]

err=Select[errs, ( Min[errs]<# <Max[errs]) &]

SmoothHistogram[errs]

s1=Map[Position[errs,#]&,err]//Flatten

Map[qs[[#]]&,s1]

Mean[Map[solGJ[[#,1]]&,s1]]

NumberForm[Mean[Map[solGJ[[#,2]]&,s1]],6]

Mean[Map[solGJ[[#,3]]&,s1]]//MatrixForm

References