OpenCV projectPoints jacobian matrix is not correct

asked 2016-01-28 07:23:20 -0500

updated 2016-01-28 09:49:34 -0500

Eduardo gravatar image

We compared the Jacobian matrix coming from projectPoints in OpenCV 2.4.11 against the one we have provided with matlab code (we have written the projection formula exactly based on the OpenCV source code). We found out that when the Tangential distortion coefficients (p1 and p2) are not zero the output of MATLAB code and the OpenCV projectPoints are not identical for the Jacobian of rotation and translation. It seems that the OpenCV source code is not correct for the computing the Jacobian for rotation and translation. we are using Ubuntu 14.04 64 bit with gcc version 4.8.4. for your convenience you can find our MATLAB code in the following:

% convert rotation vector to rotation matrix (world_in_camera)
syms rx ry rz real;
r=[rx; ry; rz];
theta=norm(r);
r=r/theta;
R=cos(theta)*eye(3)+(1-cos(theta))*(r*r')+sin(theta)*[0,-r(3), r(2);r(3),0,-r(1);-r(2),r(1),0];

% camera Position world_in_camera
syms tx ty tz real;
T = [tx; ty; tz];

syms fx fy cx cy real ; 


% 3D points in camera coordinate system
syms X Y Z real;   % position of 3D Points in world coordinate system
P = R*[X; Y; Z] + [tx; ty; tz];

% distortion
syms k1 k2 p1 p2 k3
x_perim = P(1)/P(3);
y_perim = P(2)/P(3);
r2 = (x_perim * x_perim) + (y_perim * y_perim);
r4 = r2 * r2;
r6 = r4 * r2;

cdist = 1 + k1*r2 + k2*r4 + k3*r6;
a1 = 2*x_perim*y_perim;
a2 = r2 + 2*x_perim*x_perim;
a3 = r2 + 2*y_perim*y_perim;

x_zegond = x_perim * cdist + p1 * a1 + p2 * a2;
y_zegond = y_perim * cdist + p1 * a3 + p2 * a1;

 % 3D point projection equation
xp(X, Y, Z) = x_zegond * fx + cx;
yp(X, Y, Z) = y_zegond * fy + cy;

syms X3 Y3 Z3 real;
x = xp(X3, Y3, Z3);
y = yp(X3, Y3, Z3);

imagePoints=[x;y];

% disp('imagePoints = ');
% pretty(imagePoints);

dp_dfx    = diff(imagePoints, fx);
dp_dfy    = diff(imagePoints, fy);
dp_dcx    = diff(imagePoints, cx);
dp_dcy    = diff(imagePoints, cy);

dp_dk1    = diff(imagePoints, k1);
dp_dk2    = diff(imagePoints, k2);
dp_dp1    = diff(imagePoints, p1);
dp_dp2    = diff(imagePoints, p2);
dp_dk3    = diff(imagePoints, k3);

dp_drx    = diff(imagePoints, rx);
dp_dry    = diff(imagePoints, ry);
dp_drz    = diff(imagePoints, rz);

dp_dtx    = diff(imagePoints, tx);
dp_dty    = diff(imagePoints, ty);
dp_dtz    = diff(imagePoints, tz);

jacobianMat = [dp_drx, dp_dry, dp_drz ,...
            dp_dtx, dp_dty, dp_dtz ,...
            dp_dfx, dp_dfy, dp_dcx, dp_dcy, ...
            dp_dk1, dp_dk2, dp_dp1, dp_dp2, dp_dk3];
fprintf('generating c code for jacobian \n');
ccode(jacobianMat,'file','jacobian.cpp');
edit retag flag offensive close merge delete