Внешняя калибровка с cv :: SolvePnP

Question

В настоящее время я пытаюсь реализовать альтернативный метод для AR на основе веб-камеры, используя внешнюю систему отслеживания. У меня есть все, что было в моей среде, за исключением внешней калибровки. Я решил использовать cv::solvePnP(), поскольку он, якобы, делает практически то, что я хочу, но через две недели я вытаскиваю свои волосы, пытаясь заставить его работать. На приведенной ниже диаграмме показана моя конфигурация. c1 - моя камера, c2 - оптический трекер, который я использую, M - отслеживаемый маркер, прикрепленный к камере, а ch - шахматная доска.

Как она стоит я прохожу в моем пикселя изображения координаты получены с cv::findChessboardCorners(). Мировые точки приобретаются со ссылкой на отслеживаемый маркер M, прикрепленный к камере c1 (Таким образом, внешнее является преобразованием из кадра этого маркера в начало камеры). Я тестировал это с наборами данных до 50 пунктов, чтобы уменьшить вероятность локальных минимумов, но пока я тестирую только четыре пары 2D/3D точек. Получающаяся внешняя я получаю от rvec и tvec, возвращенных с cv::solvePnP(), массово отключается как по переводу, так и по отношению к как оригиналу, создаваемому вручную, так и базовому визуальному анализу (перевод подразумевает расстояние 1100 мм, в то время как камера находится на расстоянии не более 10 мм).

Первоначально я думал, что проблема в том, что у меня были некоторые двусмысленности в том, как моя поза была определена, но теперь я уверен, что это не так. Математика кажется довольно простой, и после всей моей работы по настройке системы, увлечение тем, что по сути является одним лайнером, - огромное разочарование. У меня, честно говоря, нехватка опций, поэтому, если кто-то может помочь, я буду чрезвычайно в вашем долге. Мой тестовый код размещен ниже и совпадает с моей реализацией минус некоторые вызовы рендеринга. Внешняя земля правда у меня есть, что работает с моей программой выглядит следующим образом (в основном чистое вращение вокруг одной оси и небольшой перевод):

1  0  0  29 
0 .77 -.64 32.06 
0 .64 .77 -39.86 
0  0  0  1 

Спасибо!

#include <opencv2\opencv.hpp> 
#include <opencv2\highgui\highgui.hpp> 

int main() 
{ 
    int imageSize  = 4; 
    int markupsSize = 4; 
    std::vector<cv::Point2d> imagePoints; 
    std::vector<cv::Point3d> markupsPoints; 
    double tempImage[3], tempMarkups[3]; // Temp variables or iterative data construction 

    cv::Mat CamMat = (cv::Mat_<double>(3,3) << (566.07469648019332), 0, 318.20416967732666, 
     0, (565.68051204299513), -254.95231997403764, 0, 0, 1); 

    cv::Mat DistMat = (cv::Mat_<double>(5,1) << -1.1310542849120900e-001, 4.5797249991542077e-001, 
    7.8356355644908070e-003, 3.7617039978623504e-003, -1.2734302146228518e+000); 

    cv::Mat rvec = cv::Mat::zeros(3,1, cv::DataType<double>::type); 
    cv::Mat tvec = cv::Mat::zeros(3,1,cv::DataType<double>::type); 
    cv::Mat R; 
    cv::Mat extrinsic = cv::Mat::eye(4, 4, CV_64F); 

    // Escape if markup lists aren't equally sized 
    if(imageSize != markupsSize) 
    { 
    //TODO: Replace with try, throw error code, and catch in qSlicerLocalizationModuleWidget 
    return 0; 
    } 

    // Four principal chessboard corners only 
    imagePoints.push_back(cv::Point2d(368.906, 248.123)); 
    imagePoints.push_back(cv::Point2d(156.583, 252.414)); 
    imagePoints.push_back(cv::Point2d(364.808, 132.384)); 
    imagePoints.push_back(cv::Point2d(156.692, 128.289)); 

    markupsPoints.push_back(cv::Point3d(495.115, 39.106, 93.79)); 
    markupsPoints.push_back(cv::Point3d(463.143, -86.286, -51.178)); 
    markupsPoints.push_back(cv::Point3d(500.236, 121.988, 24.056)); 
    markupsPoints.push_back(cv::Point3d(471.276, -3.23, -127.809));  

    // Larger data set 
    /*imagePoints.push_back(cv::Point2d(482.066, 233.778)); 
    imagePoints.push_back(cv::Point2d(448.024, 232.038)); 
    imagePoints.push_back(cv::Point2d(413.895, 230.785)); 
    imagePoints.push_back(cv::Point2d(380.653, 229.242)); 
    imagePoints.push_back(cv::Point2d(347.983, 227.785)); 
    imagePoints.push_back(cv::Point2d(316.103, 225.977)); 
    imagePoints.push_back(cv::Point2d(284.02, 224.905)); 
    imagePoints.push_back(cv::Point2d(252.929, 223.611)); 
    imagePoints.push_back(cv::Point2d(483.41, 200.527)); 
    imagePoints.push_back(cv::Point2d(449.456, 199.406)); 
    imagePoints.push_back(cv::Point2d(415.843, 197.849)); 
    imagePoints.push_back(cv::Point2d(382.59, 196.763)); 
    imagePoints.push_back(cv::Point2d(350.094, 195.616)); 
    imagePoints.push_back(cv::Point2d(317.922, 194.027)); 
    imagePoints.push_back(cv::Point2d(286.922, 192.814)); 
    imagePoints.push_back(cv::Point2d(256.006, 192.022)); 
    imagePoints.push_back(cv::Point2d(484.292, 167.816)); 
    imagePoints.push_back(cv::Point2d(450.678, 166.982)); 
    imagePoints.push_back(cv::Point2d(417.377, 165.961)); 

    markupsPoints.push_back(cv::Point3d(457.132, 59.822, 89.247)); 
    markupsPoints.push_back(cv::Point3d(451.634, 42.015, 69.719)); 
    markupsPoints.push_back(cv::Point3d(447.06, 22.927, 48.635)); 
    markupsPoints.push_back(cv::Point3d(442.424, 4.454, 28.659)); 
    markupsPoints.push_back(cv::Point3d(437.621, -14.395, 7.495)); 
    markupsPoints.push_back(cv::Point3d(433.386, -33.034, -12.009)); 
    markupsPoints.push_back(cv::Point3d(429.227, -51.001, -32.269)); 
    markupsPoints.push_back(cv::Point3d(424.291, -70.266, -52.667)); 
    markupsPoints.push_back(cv::Point3d(460.300, 79.769, 69.948)); 
    markupsPoints.push_back(cv::Point3d(455.020, 61.379, 49.306)); 
    markupsPoints.push_back(cv::Point3d(450.501, 43.288, 30.250)); 
    markupsPoints.push_back(cv::Point3d(446.062, 24.572, 8.713)); 
    markupsPoints.push_back(cv::Point3d(441.346, 5.823, -10.997)); 
    markupsPoints.push_back(cv::Point3d(436.670, -13.135, -31.428)); 
    markupsPoints.push_back(cv::Point3d(432.367, -31.428, -51.785)); 
    markupsPoints.push_back(cv::Point3d(427.745, -50.016, -72.519)); 
    markupsPoints.push_back(cv::Point3d(464.824, 101.129, 52.251)); 
    markupsPoints.push_back(cv::Point3d(458.628, 81.864, 30.810)); 
    markupsPoints.push_back(cv::Point3d(454.120, 63.546, 10.458)); */ 


    // Calculate camera pose 
    cv::solvePnP(cv::Mat(markupsPoints), cv::Mat(imagePoints), CamMat, DistMat, rvec, tvec); 
    cv::Rodrigues(rvec, R); 

    // Invert results of Rodrigues by transposing rotation matrix and calculating inverted tvec 
    R = R.t(); 
    tvec = -R * tvec; // translation of inverse 

    std::cout << "Tvec = " << std::endl << tvec << std::endl; 

    // Copy R and tvec into the extrinsic matrix 
    extrinsic(cv::Range(0,3), cv::Range(0,3)) = R * 1; 
    extrinsic(cv::Range(0,3), cv::Range(3,4)) = tvec * 1; 

    // Fill last row of homogeneous transform (0,0,0,1) 
    double *p = extrinsic.ptr<double>(3); 
    p[0] = p[1] = p[2] = 0; p[3] = 1; 

    std::cout << "Extrinsic = " << std::endl << extrinsic << std::endl << std::endl; 
    std::cout << "Extrinsic (inv) = " << std::endl << extrinsic.inv() << std::endl; 
    std::cin >> tempImage[0]; 
    return 0; 
} 

РЕДАКТИРОВАТЬ 1: Я пытался нормализовать значения пикселей с использованием метода Chi Сюй (х = (х-Сх)/F, уп = (у-су)/ж). Не повезло :(

EDIT 2: Кажется, что почти каждый, кто использует solvePnP, использует метод, в котором они отмечают углы шахматной доски в качестве векторов для их мирового фрейма и происхождения, я собираюсь попытаться зарегистрировать мой трекер на Если это работает так, как ожидается, первая координата I будет равна приблизительно < 0,0>. Полученное преобразование из solvePnP будет затем умножено на обратное преобразование маркера от мира к камере, в результате чего (надеюсь) внешняя матрица.

EDIT 3: Я выполнил шаги, описанные на шаге 2. После установки системы координат на шахматной доске я вычислил преобразование cTw из пространства шахматной доски в мировое пространство и проверил его в моей среде рендеринга . Это вычисленный внешний mTc, представляющий преобразование из пространства камеры в пространство шахматной доски. Преобразование маркера камеры wTr было получено из системы слежения. Наконец, я принял все преобразования и умножают их следующим образом, чтобы переместить мое преобразование весь путь от камеры происхождения маркера камеры:

mTc*cTw*wTr

И вот он дал точно такой же результат. Я умираю здесь за любые признаки того, что я делаю неправильно. Если кто-нибудь подскажет, я прошу вас помочь.

EDIT 4: Сегодня я понял, что настроил оси шахматной доски таким образом, чтобы они были в левой системе координат. Поскольку OpenCV работает с использованием стандартной правой формы, я решил повторить тесты с моими осями рамы шахматной доски, настроенными в правой руке. В то время как результаты были примерно одинаковыми, я заметил, что преобразование «мир-шахматная доска» теперь стало нестандартным форматом преобразования, т. Е. Матрица вращения 3x3 уже не была примерно симметричной по диагонали, как и должно быть. Это указывает на то, что моя система отслеживания не использует правую систему координат (было бы здорово, если бы они задокументировали это. Или что-нибудь, если на то пошло). Хотя я не уверен, как это решить, я уверен, что это часть проблемы. Я буду продолжать бить его и надеяться, что кто-то здесь знает, что делать. Я также добавил диаграмму, представленную мне на платах OpenCV, на Eduardo. Спасибо Эдуардо!

Answer 1

Итак, я понял вопрос. Неудивительно, что это было в реализации, а не в теории. Когда проект был первоначально разработан, мы использовали веб-камеру для отслеживания. Этот трекер имел ось z, выходящую из плоскости маркера. Когда мы перешли на оптический трекер, код был портирован в основном как есть. К сожалению для нас (RIP 2 месяца в моей жизни), мы никогда не проверяли, осталась ли ось z из плоскости маркера (это не так). Таким образом, конвейер визуализации был назначен неверный просмотр и просмотр векторов на камеру сцены. В основном это работает сейчас, за исключением того, что переводы почему-то отключены. Совершенно другая проблема, хотя!