Переведите и поверните трехмерную точку в начало координат с помощью pcl transformcloud. - PullRequest
Новая
       99

Переведите и поверните трехмерную точку в начало координат с помощью pcl transformcloud.

0 голосов
/ 04 ноября 2019

Учитывая матрицу вращения и матрицу перевода, полученную из оценки положения камеры угла 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 - вектор перевода

, а output_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);

ОБНОВЛЕНИЕ:

Я проверил преобразование до и после и обнаружил, что мое преобразование с однородной матрицей было неверным:

Вот данные о глубине, взятые с стороны платы Как вы можете видеть, оси 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), а затем это изображение оси двух маркеров, которую я нарисовал для целей индикации

Как вы можете видеть поворотматрица и вектор перевода верны. Любая помощь / подсказка / предположение высоко ценится:)

1 Ответ

0 голосов
/ 05 ноября 2019

Я решил проблему. Проблема в том, что матрица однородного преобразования трансформируется из системы координат камеры в реальную систему координат. Тем не менее, я хочу преобразовать матрицу из реальной системы координат обратно в систему координат камеры.

Этого можно добиться, взяв обратную матрицу преобразования.

, добавив 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);

даст вам правильный ответ.

...