Неправильная перепроекция 3D точек в 2D в MATLAB
Я получил правильное положение и ориентацию камеры, используя алгоритм PNP. Теперь алгоритмы кажутся работающими при визуализации положения камеры в облаке точек. Я аннотировал камеру, представляющую собой гомогенную матрицу (Rt T; 0 0 0 1).
Я использовал эту функцию во время визуализации:
plotCamera('Size',50,'Orientation',Rp,'Location', Tp);
Кажется, все в порядке. Когда я пытаюсь вычислить перепроецированные точки в 2D, я получаю неправильные точки.
В приведенном ниже сценарии я рассчитываю среднюю ошибку между выходными Urep и выбранными вручную точками. Это около 870, что слишком велико. Что я пропускаю, что вызывает такую большую ошибку?
Xw=worldPts;
n=size(Xw,1);
P=intrinsics_rgb*[Rp,Tp];
Xw_h=[Xw,ones(n,1)];
Urep_=(P*Xw_h')';
Urep=zeros(n,2);
Urep(:,1)=Urep_(:,1)./Urep_(:,3);
Urep(:,2)=Urep_(:,2)./Urep_(:,3);
U=unimgPts_h;
err_=sqrt((U(:,1)-Urep(:,1)).^2+(U(:,2)-Urep(:,2)).^2);
err=sum(err_)/n;
У меня есть следующая внутренняя матрица:
542.549995292180 0 1342.89857154447
0 541.976228890787 949.936703164229
0 0 1
Следующие камеры представляют как (Rp,Tp;0,0,0,1)
-0.995549036123955 -0.0899756549267030 0.0280445751113267 27087.6028132003
-0.0909854204644286 0.995161307306489 -0.0370894284527536 11054.2753192400
-0.0245717304147492 -0.0394759922047775 -0.998918353071898 2744.16886936102
0 0 0 1
В то время как, например, проектирование этих мировых координат
26037.5399780273 12335.9003906250 740
27759.0900878906 12515.3701171875 740
26310.5100097656 9655.70898437500 740
28032.8400878906 9829.47460937500 740
Я получаю эти координаты 2D-изображения
1390.33084886566 13869.0934284976
260.671832750537 14653.2564039941
1368.53210370380 11066.2174125842
358.417883286246 11611.7366977161
Однако я ожидаю что-то вроде этого:
1131 635
1604 623
1128 1349
1627 1354