1
$\begingroup$

I am trying to undo some simple rotation errors of my sensor by tracking a reference plane. I am using the following accepted solution: Calculate Rotation Matrix to align Vector A to Vector B in 3d? I simply find the matrix that aligns the observed plane vector to the known reference plane vector, and I get the rotation matrix that would undo my rotation.

Although I do not know the rotation around this vector, I observed there is usually none present. When my reference plane is conveniently placed perpendicularly to a main reference system axis, the obtained rotation matrix tends to be elementary:

def unit_vector(vector):
    """ Returns the unit vector of the vector.  """
    return vector / np.linalg.norm(vector)

def rot_matrix(A,B):
    A = unit_vector(A)
    B = unit_vector(B)
    AxB = np.cross(A,B)
    ssc = np.array([[0,-AxB[2],AxB[1]], [AxB[2],0.0,-AxB[0]], [-AxB[1],AxB[0],0.0]])
    return np.eye(3) + ssc + np.matmul(ssc,ssc)*(1-np.dot(A,B))/(np.linalg.norm(AxB)**2)

# Transition matrix
A = rot_matrix([-0.1,0,0.99498743710662],[0,0,1])
At = rot_matrix([0,0,1],[-0.1,0,0.99498743710662])
print(A)

'''[[ 0.99498744  0.          0.1       ]
 [ 0.          1.          0.        ]
 [-0.1         0.          0.99498744]]'''

Moving the reference plain to a tilted position gives a more complicated transformation matrix, even though it is still the same elementary rotation as shown below:

mv = np.array([[0,-0.15,0.9775**0.5]]).T
f_new = np.matmul(At,mv)
print(f_new,mv)

B = rot_matrix(f_new[:,0],mv[:,0])
print(B)
'''[[ 9.95100497e-01 -1.48667118e-02  9.77444742e-02]
 [ 1.47938681e-02  9.99889484e-01  1.46998815e-03]
 [-9.77555258e-02 -1.67670737e-05  9.95210459e-01]]'''

Is there some way I can select a "simplified" form out of all the non-unique solutions? Or am I missing something obvious?

EDIT: After some thinking, the point is I want to avoid rotations around Z-axis altogether. Since there are only two degrees of freedom for each vector, is there a way I can design a rotation matrix from only Rx and Ry? Having your target vector on the Z-axis does that automatically, and I was wondering if I can replicate that directly.

$\endgroup$
  • $\begingroup$ What do you consider “simplified?’ All of the possible rotation axes lie on the angle bisector of your reference plane’s and measured plane’s normals. You can certainly always pick one that lies one one of the coordinate *planes°, but in general there won’t be any that parallel a coordinate axes. $\endgroup$ – amd Aug 7 '18 at 23:48
  • $\begingroup$ Thanks for the answer. I consider simplified when having as little elementary rotation as possible. Can you elaborate further? Why can't I obtain the one that I used to map one on the other, with rotation around Y-axis? I have shown it exists and it works. $\endgroup$ – Cindy Almighty Aug 8 '18 at 10:01
  • $\begingroup$ I edited my question, and I think this is your point of choosing the rotation axis in the XY-plane. Can you explain how to choose and execute this rotation? $\endgroup$ – Cindy Almighty Aug 8 '18 at 11:53
  • $\begingroup$ Simple example: reference normal $(0,0,1)$, measured normal $(1,1,1)$. Angle bisector plane has normal $(1,1,1-\sqrt3)$. None of the coordinate axes lie on this plane, so there’s no single elementary rotation that will align the measured and reference planes. $\endgroup$ – amd Aug 8 '18 at 22:04
  • $\begingroup$ If I understand your update correctly, you’re looking for a way to compute an aligning rotation that’s a composition of a rotation around the $x$-axis and one around the $y$-axis (or vice-versa). Does that capture your intent? $\endgroup$ – amd Aug 8 '18 at 22:49
0
$\begingroup$

It’s a bit difficult to turn your “no $z$-axis rotation” requirement into something precise. Let’s say that the aligning rotation must be a composition of a single basic rotations about each of the $x$- and $y$-axes, in either order. Here’s one way to find such a rotation.

If we first rotate about the $x$-axis, the resulting matrix looks like $$R_y(\eta) R_x(\xi) = \begin{bmatrix} \cos\eta & \sin\xi\sin\eta & \cos\xi\sin\eta \\ 0 & \cos\xi & -\sin\xi \\ \sin\eta & \sin\xi\cos\eta & \cos\xi\cos\eta \end{bmatrix}.$$ The corresponding axis of rotation can be extracted using well-documented methods. For this rotation, the axis is spanned by $\mathbf v_{yx} = \left((1+\cos\eta)\sin\xi, (1+\cos\xi)\sin\eta, -\sin\xi\sin\eta\right)$. Similarly, $$R_x(\xi) R_y(\eta) = \begin{bmatrix} \cos\eta & 0 & \sin\eta \\ \sin\xi\sin\eta & \cos\xi & - \sin\xi\cos\eta \\ -\cos\xi\sin\eta & \sin\xi & \cos\xi\cos\eta \end{bmatrix}$$ with axis $\mathbf v_{xy} = \left((1+\cos\eta)\sin\xi, (1+\cos\xi)\sin\eta, \sin\xi\sin\eta \right)$.

Note, by the way, that having the reference normal aligned with the $z$-axis doesn’t automatically result in a rotation with no $z$-axis component, as defined at top. The algorithm you’re using generates a minimal-angle rotation by rotating in the plane defined by the two vectors. For your first example you chose a vector in the $x$-$z$ plane, so it’s really just coincidence that the rotation is a pure $y$-axis rotation. Examining the rotation matrices above, a necessary condition for there being no $z$-axis component is that the $(1,2)$ or $(2,1)$ element vanish. Using Rodrigues’ rotation formula with an axis of $(x,y,0)$ and angle $\theta$, we can calculate that both of these rotation matrix elements are equal to $xy(1+\cos\theta)$. For arbitrary rotation angles, this vanishes when either $x$ or $y$ is zero, i.e., only when the measured normal lies on either the $x$-$z$ plane or $y$-$z$ plane.

Getting back to the problem at hand, all of the rotations $R$ that align the measured plane normal $\mathbf n_{\text{meas}}$ to the reference plane normal $\mathbf n_{\text{ref}}$ have axes that lie on the angle bisector of these vectors. This plane has normal $$\mathbf n = \|\mathbf n_{\text{ref}}\|\mathbf n_{\text{meas}} - \|\mathbf n_{\text{meas}}\|\mathbf n_{\text{ref}}.$$ If these are unit vectors, the normalizing factors can of course be dropped. The conditions that $\mathbf v_{yx}$ and $\mathbf v_{xy}$ lie on this plane can be expressed as $\mathbf n\cdot\mathbf v_{yx} = \mathbf n\cdot\mathbf v_{xy} = 0$. Additional constraints on $\xi$ and $\eta$ come from the goal of rotating $\mathbf n_{\text{meas}}$ onto $\mathbf n_{\text{ref}}$: $\mathbf n_{\text{ref}} \times R\mathbf n_{\text{meas}} = 0$ and $\mathbf n_{\text{ref}} \cdot R\mathbf n_{\text{meas}} \gt 0$. If the two plane normals to be aligned are unit vectors, the latter two conditions can be replaced by the single equality $\mathbf n_{\text{ref}} = R\mathbf n_{\text{meas}}$.

Applying these ideas to your first example, we have $\mathbf n = [-0.1,0,-0.00501256]$, so $$\mathbf n\cdot\mathbf v_{yx} = -0.1(1+\cos\eta-0.0501256\sin\eta)\sin\xi = 0.\tag{*}$$ One solution is obviously $\xi=0$, which is a pure $y$-axis rotation like the algorithm in your code found. To find $\eta$, we compute $$R_y(\eta)\mathbf n_{\text{meas}} = (-0.1 \cos\eta + 0.994987 \sin\eta, 0., 0.994987 \cos\eta + 0.1 \sin\eta)$$ and set it equal to $(0,0,1)$. Solving the resulting equations gives $\eta = 0.100167$, and so $$R = \begin{bmatrix} 0.994987 & 0 & 0.1 \\ 0 & 1 & 0 \\ -0.1 & 0 & 0.994987 \end{bmatrix},$$ just as your code computed. Other solutions of (*) generate other rotations, as do the solutions to $\mathbf n\cdot\mathbf v_{xy}=0$.

Working through my example of $\mathbf n_{\text{meas}}=(1,1,1)$ and $\mathbf n_{\text{ref}}=(0,0,1)$, one possible solution is $$ \begin{bmatrix} 0.816497 & -0.408248 & -0.408248 \\ 0 & 0.707107 & -0.707107 \\ 0.57735 & 0.57735 & 0.57735 \\ \end{bmatrix}.$$ By contrast, the algorithm in your code produces $$\begin{bmatrix} 0.788675 & -0.211325 & -0.57735 \\ -0.211325 & 0.788675 & -0.57735 \\ 0.57735 & 0.57735 & 0.57735 \\ \end{bmatrix},$$ which is not the product of a pair of basic rotations about the $x$- and $y$-axes.

Instead of the above, one might require that the rotation axis lie on the $x$-$y$ plane. This is always possible—this axis is the intersection of the coordinate plane with the bisecting plane. Computing its direction is a matter of a cross product, and your existing algorithm can be adapted by having it use the orthogonal rejections of the plane normals from this axis instead of the normals themselves. However, I would argue that what you already have is the simplest rotation in the following sense: It simply pivots the measured plane about its intersection line with the reference plane. Every other aligning rotation introduces a secondary rotation about the reference normal in addition to this tilt. I might also argue that, because of the degree of freedom in selecting a rotation, this calibration procedure is incomplete and needs a second reference direction or point not on the reference normal.

$\endgroup$
0
$\begingroup$

Thanks for your help, you helped me formulate the problem. Rhis is my solution:

def XY_rot(v1,v2):

    v1 = unit_vector(v1)
    v2 = unit_vector(v2)

    # Get k and n of line
    if (v2[0]-v1[0]) == 0:

        xi = v2[0]
        yi = 0
        p = [1,0,0]

    elif (v2[1]-v1[1]) == 0:

        xi = 0
        yi = v2[1]
        p = [0,1,0]

    else:

        k = (v2[1]-v1[1])/(v2[0]-v1[0])
        n = v1[1] - k*v1[0]

        # Get perpendicular and intersection
        kp = -1/k
        xi = n/(kp-k)
        yi = kp*xi
        kp2 = kp**2+1
        p = [(1/kp2)**0.5,kp*(1/kp2)**0.5,0] # We could use unit_vector([xi,yi,0]) but they can both be zero

    no = np.array([xi,yi,0]) # new origin
    vn1 = v1 - no
    vn2 = v2 - no
    # Signed angle calculation
    dot = np.dot(vn1,vn2)
    det = np.dot(p,np.cross(vn1,vn2))
    ang = math.atan2(det, dot)


    # Apply Rodriguez formula
    K = np.array([[0,0,p[1]],[0,0,-p[0]],[-p[1],p[0],0]])
    R = np.eye(3) + math.sin(ang)*K + (1-math.cos(ang))*np.matmul(K,K)

    return R
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.