Converting a point into another reference frame (Aruco)
I'm trying to convert the position of a standalone/loose ArUco marker into the reference frame of an ArUco board.
I'm using Python, so my matrices are created via NumPy and the ArUco methods are accessed via cv2.aruco. My camera has been properly calibrated with the checkerboard pattern and I have the parameters.
I have the pose of the ArUco board via cv2.aruco.estimatePoseBoard. This returns a 1x3 rvec (rotation compared to the camera in the Rodrigues format which I convert to a more standard 3x3 rotation matrix with cv2.Rodrigues(rvec) ) and a 1x3 tvec (position compared to the camera in meters, specifically one of the corners). I also have the pose of the loose ArUco marker via cv2.aruco.estimatePoseSingleMarkers. This returns an array of rvecs and tvecs corresponding to all visible ArUco markers (including the ones on the board) from which I pull the loose marker's position (specifically, the very center of the marker).
I'm trying to create a 4x4 transformation matrix that I can multiply the marker's position by to get the position of the marker in the board's reference frame. I only need rotation and translation as there is no need to skew (the board is a rectangle) or scale (both positions are in meters). I am currently taking the transpose of the board rotation matrix and combining it with the negative board position matrix to get the transformation matrix. This is then multiplied by the loose marker's position matrix (4x4 on the left, 4x1 vertical on the right) to get what should be a position in board space.
At the moment, it mostly works. I'm getting the correct position on the x axis, but y and z are completely wrong. Any chance I could get some help figuring out what I'm doing wrong?
camToBoard = np.identity(4)
camToBoard[:3,:3] = cv2.Rodrigues(boardRotation)[0]
camToBoard.transpose()
camToBoard[0][3] = -boardPosition[0]
camToBoard[1][3] = -boardPosition[1]
camToBoard[2][3] = -boardPosition[2]
markerPos4x1 = np.matrix([[markerPos[0]],[markerPos[1]],[markerPos[2]],[1]])
markerPosBoardFrame = np.matmul(camToBoard, markerPos4x1 )
print('Marker Pos: {}'.format(markerPosBoardFrame))