решитеPNP для плоского треугольника

У меня довольно простая задача: получить углы Эйлера от плоского треугольника. Шаблон выглядит этот

Итак, алгоритм:

1) получить изображение с веб-камеры — готово

2) конвертировать в гряскале, фильтровать и т. Д. — сделано

3) получить центры масс всех подключенных компонентов и отфильтровать их — готово. Похоже этот. Красные круги описывают центры масс вершин треугольников.

Код довольно прост, но вот он:

QMap<int, QVector<double> > massCenters(const cv::Mat& image)
{
cv::Mat output(IMAGE_WIDTH, IMAGE_HEIGHT, CV_32S);
cv::connectedComponents(image, output, 8);

QMap<int, QVector<double> > result;

for (int y = 0; y < IMAGE_HEIGHT; ++y)
{
for (int x = 0; x < IMAGE_WIDTH; ++x)
{
int label = output.at<int>(y, x);

if (label)
{
QVector<double> vec = result.value(label, QVector<double>());

if (vec.isEmpty())
{
vec.resize(3);
vec.fill(0);
}

vec[0] += x;
vec[1] += y;
vec[2] += 1;
result[label] = vec;
}
}
}

return result;
}

4) затем я вызываю решениеPNP, чтобы получить ротацию & векторы перевода

 cv::solvePnP(m_origin, m_imagePoints, m_cameraMatrix, m_distMatrix, m_rvec, m_tvec);

//this code is for drawing rvec & tvec on a screen
std::vector<cv::Point3f> axis;
vector<cv::Point2f> axis2D;

axis.push_back(Point3f(0.0f, 0.0f, 0.0f));
axis.push_back(Point3f(30.0f, 0.0f, 0.0f));
axis.push_back(Point3f(0.0f, 30.0f, 0.0f));
axis.push_back(Point3f(0.0f, 0.0f, 30.0f));

cv::projectPoints(axis, m_rvec, m_tvec, m_cameraMatrix, m_distMatrix, axis2D);

cv::line(m_orig, axis2D[0], axis2D[1], cv::Scalar(255, 0, 0), 2);
cv::line(m_orig, axis2D[0], axis2D[2], cv::Scalar(0, 255, 0), 2);
cv::line(m_orig, axis2D[0], axis2D[3], cv::Scalar(0, 0, 255), 2);

m_origin объявлен как std::vector<cv::Point3f> m_origin; и заполняется значениями (измеряется в мм)

m_origin.push_back(cv::Point3f( 0.0f,   51.0f, 0.0f));
m_origin.push_back(cv::Point3f(-56.0f, -26.0f, 0.0f));
m_origin.push_back(cv::Point3f( 56.0f, -26.0f, 0.0f));

m_imagePoints объявлен как std::vector<cv::Point2f> m_imagePoints; и содержит пиксельные координаты центров масс (красные кружки на втором экране).

И я получаю довольно странные результаты: от этот в тот

Что я пробовал, и это не помогло мне:

1) использовали типы double и float в m_cameraMatrix, m_distMatrix, m_rvec, m_tvec

2) переставить точки в м_оригине

3) поиграл с solvePnPRansac и его входными параметрами

4) играется с помощью методов pnp: итеративный и epnp

5) useExtrinsicGuess = true — это помогает, но иногда решение «застревает» и дает совершенно неправильные значения (тысяча градусов в векторе вращения)

И у меня есть несколько вопросов:

1) имеет ли значение пункт происхождения и точки изображения? Как уже упоминалось Вот, иногда это происходит, но это было год назад.

2) может ли моя задача быть решена не так, как с помощью solvePnP?

Благодарю. Буду признателен за любую помощь!