使用pcl transformcloud将三维点平移并旋转到原点

给定一个旋转矩阵和平移矩阵，该矩阵是从相机对charucoBoard的一个角的姿态估计中获得的，我想通过使用齐次变换将一个特定的角变换到原点。如果我理解正确，相机的位置在相机的世界中是(0,0,0(，旋转矩阵和平移向量也在相机的坐标系中解释。

平移向量告诉摄影机坐标系中的点的确切位置，而旋转矩阵告诉摄影机的旋转角度。

然后，我将齐次变换矩阵构造为：

float transformation_matrix[] = {rmatrix.at<float>(0,0),rmatrix.at<float>(0,1), rmatrix.at<float>(0,2), tvec[0],rmatrix.at<float>(1,0), rmatrix.at<float>(1,1), rmatrix.at<float>(1,2), tvec[1], rmatrix.at<float>(2,0), rmatrix.at<float>(2,1), rmatrix.at<float>(2,2), tvec[2],0.f ,0.f ,0.f, 1.f};

其中rmatrix是我的旋转矩阵，tvec是平移向量

transformation_ matrix的输出为

transformation matrix        = 
[-0.99144238, 0.12881841, 0.021162758, 0.18116128;
0.045751289, 0.49469706, -0.86786038, -0.037676446;
-0.12226554, -0.8594653, -0.49635723, 0.67637974;
0, 0, 0, 1]

旋转矩阵为：

rodrigues matrix              = 
[-0.99144238, 0.12881841, 0.021162758;
0.045751289, 0.49469706, -0.86786038;
-0.12226554, -0.8594653, -0.49635723]

翻译矢量：

[0.181161, -0.0376764, 0.67638]

然后我使用来自的transformpointcloudpcl::transformPointCloud (*cloud, *transformed_cloud, translation_matrix);

但是，我无法在转换后将点与原点对齐。有人知道我可能会错在哪里吗？

aruco::drawAxis(imageCopy, Camera_matrix, distortion, rvec, tvec, 0.1);
cv::Rodrigues(rvec, rmatrix);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
float transformation_matrix[] = {rmatrix.at<float>(0,0),rmatrix.at<float>(0,1), rmatrix.at<float>(0,2),         tvec[0],rmatrix.at<float>(1,0), rmatrix.at<float>(1,1), rmatrix.at<float>(1,2), tvec[1], rmatrix.at<float>(2,0), rmatrix.at<float>(2,1), rmatrix.at<float>(2,2), tvec[2],0.f ,0.f ,0.f, 1.f};
cv::Mat translation_m = cv::Mat(4,4,CV_32F, transformation_matrix);
Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> translation_matrix;
cv::cv2eigen(translation_m, translation_matrix);           
pcl::transformPointCloud (*cloud, *transformed_cloud, translation_matrix);

更新：

我测试了前后的变换，发现我用齐次矩阵的变换是错误的：

这是从板的侧面获取的深度数据如您所见，x、y、z轴分别显示为红色、绿色和蓝色，白色点云在前面，而红色点云在后面。我发现图像被正确地翻转到了我想要的方向，但原点(红、绿、蓝轴的交点(有时没有连接到板的一个角，x轴也没有与板的边界完全对齐。

可以肯定的是，我可以确认的一件事是，当相机与板平行时，x轴也与变换后的图像平行。从板顶部的垂直角度拍摄

我推测我缺少一个或两个旋转轴。

我通过使用R*p+T公式用以下代码标记另一个角来确保我的角的旋转矩阵和平移向量是正确的：

aruco::drawAxis(imageCopy, Camera_matrix, distortion, rvec, tvec, 0.1);
cv::Rodrigues(rvec, rmatrix);
Vec3f corner1_0(0.4, 0, 0);

Mat matcorner1_0;
matcorner1_0 = FloatMatFromVec3f(corner1_0);

auto mat_tvec = FloatMatFromVec3f(tvec);

Mat3f rcorner1_0 = rmatrix * matcorner1_0 + mat_tvec;
float cornerdata[3] = {rcorner1_0.at<float>(0,0),rcorner1_0.at<float>(0,1),rcorner1_0.at<float>(0,2)};
cv::Mat rcorner_10 = cv::Mat(3,1, CV_32F, cornerdata);
auto r1_0 = to_vec3f(rcorner_10);
aruco::drawAxis(imageCopy, Camera_matrix, distortion, rvec, r1_0, 0.1);

木板的长度是0.4米，所以我将corner1_0设置为(0.4，0，0(然后这是我为指示目的绘制的两个标记轴的图像

正如您所看到的，旋转矩阵和平移向量是正确的。非常感谢任何帮助/提示/猜测：(