This post might be long but I am providing full code to replicate what I am seeing in hope of receiving help. In short, all I am trying to do is use 4x4 homogeneous matrices to transform current data to a new world frame. This should be as easy as multiplying 4x4 matrices with other 4x4 (homogeneous matrix of rotation, center of camera) or 4x1 (homogeneous points).
- Pt' = T * Pt, where Pt is made homogeneous
- C1' = T * C1 where C1 = [R | C] homogeneous
However the rotation results for C1' are not correct as I show below. Assuming I have a matrix T that transforms points from W to W', should not T also transform C1 to C1' ? Cameras are just another reference frame. That is how transformation matrices are combined I thought. So I am probably not understanding or missing something.
So the questions is: How to transform, using a 4x4 homogeneous transformation matrix, a set of
- 3D points
- Camera poses (Rotation, centers)
to a new world frame.
I went through the exercise to try figure what is going on using generated data.
- Generate some data
- Transform it and calculate the new camera poses
- Try to figure out how to transform the initial data to the new world frame
So here is the generation of the data at the initial World frame, nothing special...
cv::Matx33d cameraMatrix(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
cv::Mat distCoeffs;
// 3D points.
std::vector<cv::Point3d> points3d;
points3d.push_back(cv::Point3d(-0.42322458571123384, -0.40092165898617510, 0.58623004852443006));
points3d.push_back(cv::Point3d(-0.065169225135044417, -0.45710013122959070, -0.59439680166020692));
points3d.push_back(cv::Point3d(-0.58930631427961055, -0.14654377880184336, 0.037995544297616486));
points3d.push_back(cv::Point3d( 0.085421308023316114, 0.12211676381725516, 0.12859889522995691));
// Image 1
cv::Matx33d rotation1(-1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
cv::Matx31d c1(0, 0, 5);
Matx31d tvec1 = -rotation1 * c1;
cv::Matx31d rvec1;
cv::Rodrigues(rotation1, rvec1);
// Image 2
cv::Matx33d rotation2(0.49999999999999983, -0.00000000000000000, 0.86602540378443871,
0.00000000000000000, 1.0000000000000000, 0.00000000000000000,
-0.86602540378443871, -0.00000000000000000, 0.49999999999999983);
cv::Matx31d c2(4.3301270189221936, 0, -2.4999999999999991);
Matx31d tvec2 = -rotation2 * c2;
cv::Matx31d rvec2;
cv::Rodrigues(rotation2, rvec2);
// project 3D points to images
std::vector<cv::Point2d> image1_points;
cv::projectPoints(points3d, rvec1, tvec1, cameraMatrix, distCoeffs, image1_points);
std::vector<cv::Point2d> image2_points;
cv::projectPoints(points3d, rvec2, tvec2, cameraMatrix, distCoeffs, image2_points);
// Solve for pose just to validate that rvec == rvec_calc and tvec == tvec_calc
cv::Matx31d tvec1_calc, rvec1_calc, tvec2_calc, rvec2_calc;
bool b1 = cv::solvePnP(points3d, image1_points, cameraMatrix, distCoeffs, rvec1_calc, tvec1_calc, false, SOLVEPNP_AP3P);
bool b2 = cv::solvePnP(points3d, image2_points, cameraMatrix, distCoeffs, rvec2_calc, tvec2_calc, false, SOLVEPNP_AP3P);
Second part is transforming the data to the new world frame W' using T
// random transformation homogeneous 4x4 matrix including scale
Matx33d trans_rot(0.12549613112731250, 0.73504921120164424, -0.66253649350437904,
0.33726020650278216, -0.65962968356110196, -0.66794121052375732,
-0.93032655622793992, -0.13997344290286745, -0.33151344030090363);
Matx44d trans(trans_rot(0,0), trans_rot(0,1), trans_rot(0,2), 0.17001861629078041,
trans_rot(1,0), trans_rot(1,1), trans_rot(1,2), -0.040253913998840263,
trans_rot(2,0), trans_rot(2,1), trans_rot(2,2), -0.29941709646900849,
0, 0, 0, 1);
// remove scale from rotation just so we can compare
double scale = sqrt(trans_rot(0, 0) * trans_rot(0, 0) + trans_rot(1, 0) * trans_rot(1, 0) + trans_rot(2, 0) * trans_rot(2, 0));
trans_rot *= (1.0 / scale);
// transform 3d points
std::vector<cv::Vec4d> points3d_h;
cv::convertPointsToHomogeneous(points3d, points3d_h);
std::vector<cv::Vec4d> trans_points3d_h;
for (int i = 0; i < 4; ++i)
trans_points3d_h.emplace_back(trans * points3d_h[i]);
std::vector<cv::Point3d> trans_points3d;
cv::convertPointsFromHomogeneous(trans_points3d_h, trans_points3d);
Third is recalculating the poses and new centers
// solve for new pose using the transformed 3d points
cv::Matx31d tvec1_calc2, rvec1_calc2, tvec2_calc2, rvec2_calc2;
b1 = cv::solvePnP(trans_points3d, image1_points, cameraMatrix, distCoeffs, rvec1_calc2, tvec1_calc2, false, SOLVEPNP_AP3P);
b2 = cv::solvePnP(trans_points3d, image2_points, cameraMatrix, distCoeffs, rvec2_calc2, tvec2_calc2, false, SOLVEPNP_AP3P);
cv::Matx33d rotation1_calc2;
cv::Rodrigues(rvec1_calc2, rotation1_calc2);
cv::Matx33d rotation2_calc2;
cv::Rodrigues(rvec2_calc2, rotation2_calc2);
// calculate new centers to compare with the transformation
Matx31d c1_calc2 = -rotation1_calc2.t() * tvec1_calc2;
Matx31d c2_calc2 = -rotation2_calc2.t() * tvec2_calc2;
So the last part is how to perform this transformation directly to the data in the initial world frame
// the camera center is easy and is equal to c1_calc2 and c2_calc2
cv::Vec4d trans_c1 = trans * cv::Vec4d(c1(0, 0), c1(1, 0), c1(2, 0), 1);
cv::Vec4d trans_c2 = trans * cv::Vec4d(c2(0, 0), c2(1, 0), c2(2, 0), 1);
Check results of camera2 (just cause camera 1 is deceivingly close to actual results by a transpose)
Matx44d cam2(rotation2(0, 0), rotation2(0, 1), rotation2(0, 2), c2(0, 0),
rotation2(1, 0), rotation2(1, 1), rotation2(1, 2), c2(1, 0),
rotation2(2, 0), rotation2(2, 1), rotation2(2, 2), c2(2, 0),
0,0,0,1);
Matx44d trans_cam2 = trans * cam2;
scale = sqrt(trans_cam2(0, 0) * trans_cam2(0, 0) + trans_cam2(1, 0) * trans_cam2(1, 0) + trans_cam2(2, 0) * trans_cam2(2, 0));
Checking the results and removing scaling from the top left 3x3 matrix it seems that they are not correct and instead to get the results to match what solvePnP returns I have to instead do this for the rotational part. But that means I cannot perform one 4x4 multiplication and instead have to split it.
// to match the pose rotation calculated it seems that this is required
cv::Matx33d trans_rot1 = rotation1 * trans_rot.t();
cv::Matx33d trans_rot2 = rotation2 * trans_rot.t();
I thought that 4x4 transformation matrices could be combined somehow!