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.
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.