Положение камеры в мировой координате не работает, но положение объекта в системе координат камеры работает правильно

Я работаю над оценкой позы камеры (iphone camera) для устройства крепления на голову (Hololens), используя светодиоды в качестве маркера, используя решает pnp. Я калибровал камеру ниже, это внутренние параметры камеры

/* approx model*/
double focal_length = image.cols;
Point2d center = cv::Point2d(image.cols/2,image.rows/2);
iphone_camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0,    center.x, 0 , focal_length, center.y, 0, 0, 1);
iphone_dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type);
/* caliberated(usng opencv) model */
iphone_camera_matrix = (cv::Mat_<double>(3,3) <<839.43920487140315, 0, 240, 0, 839.43920487140315, 424, 0, 0, 1);
iphone_dist_coeffs = (cv::Mat_<double>(5,1) <<4.6476561543838640e-02, -2.0580084834071521, 0, 0 ,2.0182662261396342e+01);

usng solvpnp я могу получить правильную позу объекта в системе координат камеры ниже код

cv::solvePnP(world_points, image_points, iphone_camera_matrix, iphone_dist_coeffs, rotation_vector, translation_vector, true, SOLVEPNP_ITERATIVE);

выход

rotation_vector :
[-65.41956646885059;
-52.49185328449133;
36.82917796058498]

translation_vector :
[94.1158604375937;
-164.2178023980637;
580.5666657301058]

с помощью этого вращения_vector и translation_vector визуализирую позу, проецируя тривектор, точки которого

points_to_project :
[0, 0, 0;
20, 0, 0;
0, 20, 0;
0, 0, 20]

projectPoints(points_to_project, rotation_vector, translation_vector, iphone_camera_matrix, iphone_dist_coeffs, projected_points);

вывод projectedPoints задан как

projected_points :
[376.88803, 185.15131;
383.05768, 195.77643;
406.46454, 175.12997;
372.67371, 155.56181]

что кажется правильным, как показано ниже

положение объекта в системе координат камеры

Я пытаюсь найти позу камеры в системе координат мира / объекта, используя преобразования вращение_вектор и перевод_вектор, заданные в качестве решения solvepnp как

cv::Rodrigues(rotation_vector, rotation_matrix);
rot_matrix_wld = rotation_matrix.t();
translation_vec_wld = -rot_matrix_wld * translation_vector;

Я использовал rot_matrix_wld, translation_vec_wld для визуализации позы (так же, как я визуализировал позу объекта в системе координат камеры, как сказано выше)

projectPoints(points_to_project, rot_matrix_wld, translation_vec_wld, iphone_camera_matrix, iphone_dist_coeffs, projected_points);

с

points_to_project :
[0, 0, 0;
20, 0, 0;
0, 20, 0;
0, 0, 20]

я получаю неправильный вектор перевода (ниже 2 projected_points для 2 разных кадров изображения видео)

projected_points :
[-795.11768, -975.85846;
-877.84937, -932.39697;
-868.5517, -1197.4443;

projected_points :
[589.42999, 3019.0732;
590.64789, 2665.5835;
479.49728, 2154.8057;
187.78407, 3333.3054]
-593.41058, -851.74432]

Я использовал приблизительную модель камеры и калиброванную модель камеры, и оба дают неправильный вектор перевода.

Я перешел по ссылке Вот и проверил мою процедуру калибровки, я сделал это правильно.

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

заранее спасибо.