c++ - 使用pcl transformcloud将3d点平移并旋转到原点

Question

给定从charucoBoard的一个角的相机姿态估计获得的旋转矩阵和平移矩阵，我想通过使用齐次变换将一个特定的角变换为原点。如果我理解正确的话，相机在相机世界中的位置为(0,0,0)，并且旋转矩阵和平移 vector 也会在相机的坐标系中进行解释。

平移 vector 会告诉您相机坐标系中的确切位置，而旋转矩阵会告诉您相对于相机的旋转 Angular 。

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

    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是平移 vector

并且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]

翻译 vector :

    [0.181161, -0.0376764, 0.67638]

然后，我使用transformpointcloud从
pcl::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);

更新:

我在前后测试转换，发现用均匀矩阵进行转换是不正确的:

这是从the side of the board 获取的深度数据
如您所见，x，y，z轴分别显示为红色，绿色和蓝色，白色点云在前面，而红色点云在后面。我发现图像已正确翻转到所需的方向，但原点(红色，绿色和蓝色轴的交点)有时未连接到板的一个角上，并且x轴与董事会的边界。

可以肯定的是，我可以确认的是，当相机与板子平行时，x轴也与变换后的图像平行。
taken from the upright angle at the top of the board

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

通过使用以下代码使用R * P + T公式标记另一个拐角，确保了拐角的旋转矩阵和平移 vector 正确:

    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)
然后这是图像of the two marker axis I draw for indication purpose

如您所见，旋转矩阵和平移 vector 是正确的。
任何帮助/提示/猜测都将不胜感激:)

Answer 1

我解决了问题。
问题在于，齐次变换矩阵正在从相机的坐标系转换为现实世界的坐标系。但是，我想要从现实世界坐标系到相机坐标系的转换矩阵。

您可以通过变换矩阵的逆来实现。

所以在之间添加translation_m= translation_m.inv();

    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);

会给您正确的答案。