Error in getrotT for det(R)<0

[seiferth]

Hi,

I found that efficient_pnp in some cases returned wrong poses and traced the error down to the function [R, T]=getrotT(wpts,cpts).
In efficient_pnp.m, line 202:
if det(R)<0
R=-R;
end

This leads to wrong poses when det(R)<0. The following fixes this issue:
if det(R)<0
R = U * diag([1, 1, -1]) * V';
end

furthermore, this function uses some slow for loops for things that can be done using faster matrix functions, so here'd be the fully fixed function:

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [R, T]=getrotT(wpts,cpts)

% This routine solves the exterior orientation problem for a point cloud
% given in both camera and world coordinates.

% wpts = 3D points in arbitrary reference frame
% cpts = 3D points in camera reference frame

ccent=mean(cpts);
wcent=mean(wpts);

cpts = cpts - ccent;
wpts = wpts - wcent;

M = cpts' * wpts;

[U S V]=svd(M);
R=UV';
if det(R)<0
R = U * diag([1, 1, -1]) * V';
end
T=ccent'-Rwcent';
%

I don't know if anyone is still reading this, but it might save future users a lot of hassle if this gets fixed in the code. I also haven't checked if the same error exists in the C++ version (but it likely does).

Best,
Hagen