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

Question

给定从 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]

然后我使用 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 公式用以下代码标记另一个角来确保我的旋转矩阵和平移向量是正确的：

    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

如您所见，旋转矩阵和平移向量是正确的。非常感谢任何 help/hint/speculation:)

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

会给你正确答案。

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

Translate and rotate a 3d point to origin with pcl transformcloud

c++

robotics

opencv

homogenous-transformation

matrix-transform