Some pose estimations in openCV such as marker tracking gives back two vectors tvec and rvec that describe the position and orientation of the tracked object. These can then be fed into cv::drawFrameAxes() to draw the XYZ axes of the tracked object onto the tracking image.
given that, tvec together with rvec must describe a mapping between object space and camera space - but how exactly? The documentation is really lacking.
My assumption is that if rotM is the matrix retrieved from Rodrigues, then
rotM rotM rotM tvecX
rotM rotM rotM tvecY
rotM rotM rotM tvecZ
0 0 0 1
should describe either the cameraToObject or the objectToCamera matrix. I tried to verify this, but unsuccessfully:
// Draw the frame axes to the camera image - this works!
cv::drawFrameAxes(image, cameraMatrix, distCoeffs, rvec, tvec, 100.0f);
cv::Mat rotationMatrix(3, 3, CV_64F);
cv::Rodrigues(rvec, rotationMatrix);
cv::Mat rotationTransposed = rotationMatrix.t();
cv::Mat zAxis(cv::Vec3d(0,0,1));
cv::Vec3d tvecInv = -cv::Vec3d(cv::Mat(rotationTransposed * cv::Mat(tvec)));
std::vector<cv::Point3d> axesPoints;
std::vector<cv::Point2d> imagePoints;
// draw 3x3 magenta points close to the origin of the tracker to the camera image - this works!
for(int y = 0; y < 3; y++) {
for(int x = 0; x < 3; x++) {
axesPoints.push_back(cv::Vec3d(x * 10, y * 10, 0));
}
}
projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
for(int i= 0; i<imagePoints.size(); i++) {
cv::circle(image, imagePoints[i], 5, cv::Scalar( 255, 0, 255 ), cv::FILLED, cv::LINE_8);
}
axesPoints.clear();
imagePoints.clear();
// Now trying to exactly understand the mapping described by rvec and tvec
// let "Object space" be the space that is visualized in the camera image by drawFrameAxes and
// let "Camera space" be the space that has the camera at it's center, with x pointing right,
// y downwards and z forward.
// First assumption: tvec is the position of the camera in Object space and rvec describes the
// camera orientation in object space.
// then point_in_object_space = rotationMatrix * point_in_camera_space + tvec
// in this case, the following code should result in a yellow dot in the center of th
// camera image:
cv::Vec3d p_positive_z(0,0,5000); // point on positive z axis. I also tried (0,0,-5000)
cv::Vec3d point_in_object_space = cv::Vec3d(cv::Mat(rotationMatrix * cv::Mat(p_positive_z))) + tvec;
axesPoints.push_back(point_in_object_space);
projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
cv::circle(image, imagePoints[0], 5, cv::Scalar( 255, 255, 0 ), cv::FILLED, cv::LINE_8);
axesPoints.clear();
imagePoints.clear();
// Second assumption: tvec is the position of the tracked object in camera space and rvec describes
// the object orientation in camera space.
// then point_in_object_space = rotationMatrixTransposed * point_in_camera_space + tvecInv
// in this case, the following code should result in a green dot in the center of the camera image:
point_in_object_space = cv::Vec3d(cv::Mat(rotationTransposed * cv::Mat(p_positive_z))) + tvec;
axesPoints.push_back(point_in_object_space);
projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
cv::circle(image, imagePoints[0], 5, cv::Scalar( 0, 0, 255 ), cv::FILLED, cv::LINE_8);
axesPoints.clear();
imagePoints.clear();
// There is neither a yellow nor a green point in the output image however
Can someone explain me how exactly tvec and rvec map between camera and object space?