I have a Cozmo robot that navigates in a flat surface. The robot has a camera that detects ArUco markers and I use openCV functions to estimate the pose of the center of these markers. In the end I obtain a rvec and a tvec vectors. One thing I want to do is to align the robot with the markers and for this I transform the rvec into a rotation matrix, then I invert the matrix and finally I obtain the euler angles. To align the robot with the marker I use the yaw angle. My first question is if I am doing this alignment well?
The other question is regarding with tvec. To move the robot to the center of the marker, do I just need to give the tvec coordinates to the robot or do I need to multiply tvec with the inverted rotation matrix? Also the camera is looking down with an angle of -15 degrees around the x axis, how can I considerate this factor as well? The x axis of the robot is facing forwards, the y axis goes to the left and the z axis goes up
Regarding the rvec I think it is working well since I am having good results but I am not sure if I am doing this correctly. About the tvec, if I plug in directly the tvec coordinates to the robot, the robot goes to the more and less to the middle of the marker. If I multiply tvec with a rotation matrix around the x axis of -15 degrees or 15 degrees the results seem to be similiar, so I don't know what is the correct way.
