creating a rotation matrix in OpenCV

edit

I am trying to recreate a rotation matrix in OpenCV using one of the direction vectors along the z-axes but have not been having any success with it. I am also not sure if this can be done in a unique fashion but I was hoping to get some help from the forum.

So, initially, I try and create the rotation matrix that I want to recover. I do this as following:

import cv2
import numpy
def generate_fake_opencv_calibrations(num_calibs=10):
    calibs = list()
    cm = np.asarray([[300.0, 0.0, 400], [0, 300.0, 700.0], [0., 0., 1.]])
    for i in range(num_calibs):
        image_c = np.random.rand(6, 2) * 100.0
        world_c = np.random.rand(6, 3)
        _, r, t = cv2.solvePnP(world_c, image_c, cm, None, flags=cv2.SOLVEPNP_ITERATIVE)
        # Create the rotation matrix
        r, _ = cv2.Rodrigues(r)
        calibs.append(r)
    return calibs

So, the rotation matrix has all the properties that the rows and columns are orthonormal wrt to each other. So, what I was hoping to do is recreate this rotation matrix when I have the direction vector for the z-axes.

So, I have:

z = c[:, 2]

Now I wrote a function to create the other two axes as:

def create_orthonormal_basis(v):
    v = v / np.linalg.norm(v)
    if v[0] > 0.9:
        b1 = np.asarray([0.0, 1.0, 0.0])
    else:
        b1 = np.asarray([1.0, 0.0, 0.0])

    b1 -= v * np.dot(b1, v)
    b1 *= np.reciprocal(np.linalg.norm(b1))
    b2 = np.cross(v, b1)
    return b1, b2, v

I can then create the matrix as:

x, y, z = create_orthonormal_basis(z)
mat = np.asarray([[x[0], y[0], z[0]],
                  [x[1], y[1], z[1]],
                  [x[2], y[2], z[2]]])

I was expecting this matrix to map a given point to approximately the same location, however this was not the case. So for a random case, I am getting the following:

For the input matrix, given by:

[-0.5917787  -0.69902414  0.40145141]
[ 0.76717701 -0.64127655  0.01427625]
[ 0.24746193  0.31643267  0.91576905]]

The output is:

[ 0.91588032  0.          0.40145141]
[-0.00625761  0.99987851  0.01427625]
[-0.40140263 -0.01558747  0.91576905]]

I take a random input like:

[0.33385406 0.91243684 0.33755828]

and map it using the original and reconstructed matrices and the output is quite different:

Original: [-0.69986985 -0.32418012  0.68046643]
Reconstructed: [0.44128362 0.91505592 0.16089295]