Transformation of 3d points and camera poses to a new world frame

edit

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

1. 3D points
2. Camera poses (Rotation, centers)

to a new world frame.

I went through the exercise to try figure what is going on using generated data.

1. Generate some data
2. Transform it and calculate the new camera poses
3. 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 ...