OpencvsolvePnP в Python и в ros C++ возвращает разные rvecs ⇐ C++

[Anonymous]

python 3.10
opencv-python 4.10.0.84
Сейчас я калибрую камеру на роботе, собираю данные о положении робота и положении камеры относительно шахматной доски, затем с помощью алгоритма qr24 нахожу матрицу поворота и транспонирования от точки TCP к точке камеры. когда я считаю ошибку "обучения" этой матрицы, то получаю огромные значения, если собираю координаты камеры через python и маленькие, если использую c++
Этот код возвращает ему правильное, но rvec неверен
Если сравнивать его с реализацией на C++

Код: Выделить всё

SIZE_OF_CHESSBOARD_SQUARES_MM = 25
CHECKERBOARD_WIDTH = 9
CHECKERBOARD_HEIGHT = 6

pattern_size = (CHECKERBOARD_WIDTH, CHECKERBOARD_HEIGHT)
objp = np.zeros((np.prod(pattern_size), 3), np.float32)
objp[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
objp = objp * SIZE_OF_CHESSBOARD_SQUARES_MM

success, img = camera.get_frame()
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

ret, corners = cv.findChessboardCorners(gray, (CHECKERBOARD_WIDTH, CHECKERBOARD_HEIGHT))
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners2 = cv.cornerSubPix(gray, corners, (CHECKERBOARD_WIDTH, CHECKERBOARD_WIDTH), (-1, -1), criteria)

ret, rvec, tvec = cv.solvePnP(objp, corners2, camera.intrinsic_matrix, None)
rot = Rotation.from_euler('xyz', rvec.T.reshape((1, 3)))

-+-+-+-+-+-+-+-+-+-+-+--+-
-9.7176079739920895e-01 1.2718198267005383e-03 -2.3596469039325010e-01 7.0983453038087008e-02
-3.4017052293320985e-02 -9.9029513612821629e-01 1.3475304639996588e-01 5.1239971587134996e-02
-2.3350330359831620e-01 1.3897455103408790e-01 9.6237325470606605e-01 5.3722382914334610e-01
0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00

Реализация C++

Код: Выделить всё

object_corners_.clear();
for ( int i = 0; i < config_.checkerboard_rows; i++ ) {
for ( int j = 0; j < config_.checkerboard_columns; j++ ) {
object_corners_.push_back ( Point3f ( float ( i * config_.checkerboard_square_size ), float ( j * config_.checkerboard_square_size ), 0.f ) );
}
}

int flags = cv::CALIB_CB_ADAPTIVE_THRESH;
findChessboardCorners ( image_grey_, patternsize, image_corners_, flags );
cornerSubPix ( image_grey_, image_corners_, Size ( winSize, winSize ), Size ( -1, -1 ), TermCriteria ( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ) );
solvePnP ( object_corners_, image_corners_, cv::Mat ( intrinsic_matrix_, cv::Rect ( 0, 0, 3, 3 ) ) , dist_coeff, rotation_vec, translation_vec );

-+-+-+-+-+-+-+-+-+-+-+--+-
-1.560543739123828634e-02 -9.955283001334500614e-01 9.316584115023952839e-02 7.107269043826081045e-02
-9.875806266509048470e-01 2.991518096500692461e-02 1.542387364174025444e-01 5.123935745406452136e-02
-1.563361000781144927e-01 -8.960181684114672251e-02 -9.836313019780996703e-01 5.386745927479859031e-01
0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00

Я подозреваю, что реализация SolPnP на Python не работает.
Я пробовал много флагов и много раз переписывал код, но реализация на C++ все равно возвращает один из углов, больший, чем у Python. (примерно на 90 градусов)

Подробнее здесь: https://stackoverflow.com/questions/787 ... rent-rvecs