I have a question very similar to this post (that I found quite helpful), but slightly different.
Calculate Rotation Matrix to align Vector A to Vector B in 3d?
I would like to accomplish the same thing as in this post: I want to find a rotation matrix that maps an arbitrary vector v onto an arbitrary vector u.
What is important about my question is that I want to find a specific rotation matrix to do this. I am controlling a 2DOF gimbal with a camera, and so I can only control the yaw and the pitch of the gimbal to steer the camera. However, the coordinate frame of the camera may be misaligned with the rotation axes of the gimbal, i.e., the x axis of the camera (pointing to the right of the focal plane array of the camera) may not align with the pitch axis of the gimbal, and the y axis of the camera (pointing down on the focal plane array of the camera) may not align with the yaw axis of the gimbal.
In essence, this means that although there are infinite different rotation matrices that map the camera's line of sight to the desired vector, not all of them are achievable with my gimbal. Assume that the target does not fall inside the unachievable cone above the gimbal (due to the camera's misalignment in the gimbal's yaw direction). The method in the linked post works if there is no misalignment with my camera, but fails otherwise.
So far, these are the steps I have taken to try to solves this problem:
First, I determined my two coordinate frames, calling them $R_{g}$ for the Gimbal Frame and $R_c(yaw, pitch)$ for the camera frame. Note that $R_c$ is a function of the yaw and pitch angles of the gimbal. Also, I treat the Gimbal Frame as reference, so I assign it the identity.
Then, for any point in time, I have the change of basis formula $$R_gv_g=v_g=R_c(yaw, pitch)v_c$$
Next, I define my camera's "pointing direction" to be straight out the z direction in the camera's coordinate frame, i.e., $$v_c=\begin{bmatrix}0\\0\\1\end{bmatrix}$$
This lets me do the "forward" transform, meaning that given a set of yaw and pitch angles, I can determine where in space the camera is pointing by calculating $R_c$ and multipling by $v_c$ to get $v_g$, the pointing vector in reference coordinates.
My problem lies in doing the "inverse" transform: given a pointing vector $v_g$, I want to find a set of $(yaw,pitch)$ angles that yield the correct gimbal rotation such that the camera's z axis is aligned with that pointing vector.
So now, my givens are $v_g$ and $v_c$, and I need to solve for $yaw$ and $pitch$ such that $R_c(yaw, pitch)$ yields the correct solution.
The way I see this problem, the matrix I want to solve for will solve the equation $$R_c^T(yaw,pitch)v_g=\begin{bmatrix}0\\0\\1\end{bmatrix},$$ with $R_c$ constrained to be of the form $$\begin{bmatrix}cos(yaw) & -sin(yaw) & 0 \\ sin(yaw) & cos(yaw) & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}cos(pitch) & 0 & sin(pitch) \\ 0 & 1 & 0 \\ -sin(pitch) & 0 & cos(pitch) \end{bmatrix}$$
I realize this is a system of 3 equations with 2 unknowns, but I have no idea how to find an analytical solution for it. Also, I am not 100% certain that my problem formulation is correct.
I would love any help with this! If my post is unclear, let me know and I will edit it for more clarity, but I tried to simplify the problem as much as possible to its core elements.
EDIT: I have been doing (a lot) more thinking about this, and I think I can "simplify" the problem even more. In trying to generalize my problem formulation and keep everything in one reference frame, I came up with the following.
I have a vector, the z axis, that I want to align with a random target vector, $v_t$. To do this, I am allowed rotation around two fixed axes. Let's call these axes $v_y$ and $v_p$ for the yaw and pitch axes of the gimbal. Nominally, these would be the x and y axes, and it would be trivial to solve for the answer. Assume all vectors are of unit length.
To visualize this problem in my head, I am picturing two cones: one cone of the z axis rotated around $v_p$ and another of the target vector, $v_t$, rotated around $v_y$.
If these cones do not intersect, then the target vector is unachievable, and I can give up.
If these cones do intersect, then I want to perform the following:
- Solve for the angle between $v_t$ and $v_y$. This is simply $$\theta = cos^-1(v_t^Tv_y)$$
- Determine the amount to rotate the z axis around the pitch axis so that its angle with the yaw axis is $\theta$. This will rotate the z axis onto the intersection of the two cones. $$R_p\begin{bmatrix}0\\0\\1\end{bmatrix}=v_1,$$$$v_1^Tv_y=v_t^Tv_y$$
- Determine the amount to rotate $v_1$ so that it aligns with the target vector. To do this I am thinking I would project both $v_1$ and $v_t$ onto the plane perpendicular to $v_p$ and then find the angle between those two projected vectors.
Right now my problem lies in step 2, where I do not know how to solve for $R_p$ such that $v_1$ has angle $\theta$ with $v_y$. I know about the axis-angle formulation, but I am not comfortable enough with it to use it here without help.
Again, thanks to anyone who has read all this and thinks they can help!
EDIT 2:
So now I realize that if I can solve for the intersection of two circles in 3d space, I will be done with the problem! But I haven't yet figured it out...
Progress so far:
I have to find three things for each circle: the normal vector, the radius, and the center. This is pretty trivial:
- The normal vector for the circles are simply $v_y$ and $v_p$
- The centers of the circle are $v_c$ and $v_t$ projected onto $v_p$ and $v_y$, respectively: $$c_p = (v_c^Tv_p)v_p$$$$c_y = (v_t^Tv_y)v_y$$
- The radius is the norm of the vector rejection, namely, $$r_p = \lVert v_c-c_p\rVert$$$$r_y = \lVert v_t-c_y \rVert$$
Now, I looked to define the circles as follows. First, I found the spheres of the appropriate radii centered at the appropriate centers: $$(x-c_{px})^2 + (y-c_{py})^2 + (z-c_{pz})^2 = r_p^2$$ $$(x-c_{yx})^2 + (y-c_{yy})^2 + (z-c_{yz})^2 = r_y^2$$ expanding yields: $$(x^2 - 2c_{px} + c_{px}^2 + y^2 - 2c_{py} + c_{py}^2 + z^2 - 2c_{pz} + c_{pz}^2 = r_p^2$$ $$(x^2 - 2c_{yx} + c_{yx}^2 + y^2 - 2c_{yy} + c_{yy}^2 + z^2 - 2c_{yz} + c_{yz}^2 = r_y^2$$ Subtracting and rearranging yields the following plane of intersection: $$x(c_{yx}-c_{px}) + y(c_{yy}-c_{py}) + z(c_{yz}-c_{pz})=(r_p^2-r_y^2)/2 - c_{px}^2-c_{py}^2-c_{pz}^2-c_{yx}^2-c_{yy}^2-c_{yz}^2$$ Which can be rewritten in vector form as $$(c_y-c_p)^T\begin{bmatrix}x \\ y \\ z\end{bmatrix}=(r_p^2-r_y^2)/2-c_p^Tc_p-c_y^Tc_y$$
Now, for the specific circles on the sphere, they must satisfy the following: $$v_{px}(x-c_{px}) + v_{py}(y-c_{py}) + v_{pz}(z-c_{pz})=0$$ $$v_{yx}(x-c_{yx}) + v_{yy}(y-c_{yy}) + v_{yz}(z-c_{yz})=0$$ These can be rewritten in vector form as $$v_p^T\begin{bmatrix}x \\ y \\ z\end{bmatrix}=v_p^Tc_p$$ $$v_y^T\begin{bmatrix}x \\ y \\ z\end{bmatrix}=v_y^Tc_y$$
Now I have three equations with three unknowns, I believe that the solution is non-trivial unless the two circles happen to intersect at exactly one point.
Otherwise, the circles either do not intersect or they intersect at 2 points, and I am not sure how to go about solving that.
Again, thank you to anyone who has read through this wall of text and followed me on my journey!
EDIT: I figured it out!
