OpenCV solvePnP получить положение начала шаблона относительно камеры

В настоящее время я пытаюсь получить относительное положение двух Kinect v2, получая положение шаблона отслеживания, которое могут видеть обе камеры. К сожалению, я не могу получить правильную позицию происхождения паттернов.

Это мой текущий код, чтобы получить положение шаблона относительно камеры:

std::vector<cv::Point2f> centers;
cv::findCirclesGrid( registeredColor, m_patternSize, centers, cv::CALIB_CB_ASYMMETRIC_GRID );
cv::solvePnPRansac( m_corners, centers, m_camMat, m_distCoeffs, m_rvec, m_tvec, true );

// calculate the rotation matrix
cv::Matx33d rotMat;
cv::Rodrigues( m_rvec, rotMat );

// and put it in the 4x4 transformation matrix
transformMat = matx3ToMatx4(rotMat);

for( int i = 0; i < 3; ++i )
transformMat(i,3) = m_tvec.at<double>(i);

transformMat = transformMat.inv();

cv::Vec3f originPosition( transformMat(0,3), transformMat(1,3), transformMat(2,3) );

К сожалению, когда я сравниваю originPosition до точки в pointcloud, которая соответствует началу координат, найденному в экранном пространстве (сохранено в centers.at(0) выше) я получаю совсем другой результат.
На приведенном ниже снимке экрана показано облако точек от кинекта с точкой в ​​положении на экране исходного изображения узора красным цветом в красном круге и точкой на originPosition в голубом в голубом круге. Скриншот был взят прямо перед шаблоном. originPosition также немного больше впереди.

Как вы можете видеть, красная точка идеально находится в первом круге рисунка, в то время как синяя точка, соответствующая originPosition это даже не близко. Особенно это определенно не просто проблема масштабирования вектора от камеры до источника. Так же findCirclesGrid выполняется на зарегистрированном цветном изображении, а внутренние параметры берутся из самой камеры, чтобы гарантировать отсутствие различий между изображениями и вычислением облака точек.