[926] | 1 | function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha) |
---|
| 2 | |
---|
| 3 | %project_points2.m |
---|
| 4 | % |
---|
| 5 | %[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha) |
---|
| 6 | % |
---|
| 7 | %Projects a 3D structure onto the image plane. |
---|
| 8 | % |
---|
| 9 | %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) |
---|
| 10 | % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame |
---|
| 11 | % om: rotation vector (3x1 vector); T: translation vector (3x1 vector) |
---|
| 12 | % f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) |
---|
| 13 | % c: principal point location in pixel units (2x1 vector) |
---|
| 14 | % k: Distortion coefficients (radial and tangential) (4x1 vector) |
---|
| 15 | % alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) |
---|
| 16 | % |
---|
| 17 | %OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) |
---|
| 18 | % dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) |
---|
| 19 | % dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) |
---|
| 20 | % dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) |
---|
| 21 | % dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) |
---|
| 22 | % dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) |
---|
| 23 | % |
---|
| 24 | %Definitions: |
---|
| 25 | %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) |
---|
| 26 | %The coordinate vector of P in the camera reference frame is: Xc = R*X + T |
---|
| 27 | %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); |
---|
| 28 | %call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); |
---|
| 29 | %The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. |
---|
| 30 | %call r^2 = a^2 + b^2. |
---|
| 31 | %The distorted point coordinates are: xd = [xx;yy] where: |
---|
| 32 | % |
---|
| 33 | %xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); |
---|
| 34 | %yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; |
---|
| 35 | % |
---|
| 36 | %The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion |
---|
| 37 | % |
---|
| 38 | %Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: |
---|
| 39 | % |
---|
| 40 | %xxp = f(1)*(xx + alpha*yy) + c(1) |
---|
| 41 | %yyp = f(2)*yy + c(2) |
---|
| 42 | % |
---|
| 43 | % |
---|
| 44 | %NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices |
---|
| 45 | % |
---|
| 46 | % |
---|
| 47 | %Important function called within that program: |
---|
| 48 | % |
---|
| 49 | %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector |
---|
| 50 | % |
---|
| 51 | %rigid_motion.m: Computes the rigid motion transformation of a given structure |
---|
| 52 | |
---|
| 53 | |
---|
| 54 | if nargin < 7, |
---|
| 55 | alpha = 0; |
---|
| 56 | if nargin < 6, |
---|
| 57 | k = zeros(5,1); |
---|
| 58 | if nargin < 5, |
---|
| 59 | c = zeros(2,1); |
---|
| 60 | if nargin < 4, |
---|
| 61 | f = ones(2,1); |
---|
| 62 | if nargin < 3, |
---|
| 63 | T = zeros(3,1); |
---|
| 64 | if nargin < 2, |
---|
| 65 | om = zeros(3,1); |
---|
| 66 | if nargin < 1, |
---|
| 67 | error('Need at least a 3D structure to project (in project_points.m)'); |
---|
| 68 | return; |
---|
| 69 | end; |
---|
| 70 | end; |
---|
| 71 | end; |
---|
| 72 | end; |
---|
| 73 | end; |
---|
| 74 | end; |
---|
| 75 | end; |
---|
| 76 | |
---|
| 77 | |
---|
| 78 | [m,n] = size(X); |
---|
| 79 | |
---|
| 80 | if nargout > 1, |
---|
| 81 | [Y,dYdom,dYdT] = rigid_motion(X,om,T); |
---|
| 82 | else |
---|
| 83 | Y = rigid_motion(X,om,T); |
---|
| 84 | end; |
---|
| 85 | |
---|
| 86 | |
---|
| 87 | inv_Z = 1./Y(3,:); |
---|
| 88 | |
---|
| 89 | x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; |
---|
| 90 | |
---|
| 91 | |
---|
| 92 | bb = (-x(1,:) .* inv_Z)'*ones(1,3); |
---|
| 93 | cc = (-x(2,:) .* inv_Z)'*ones(1,3); |
---|
| 94 | |
---|
| 95 | if nargout > 1, |
---|
| 96 | dxdom = zeros(2*n,3); |
---|
| 97 | dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); |
---|
| 98 | dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); |
---|
| 99 | |
---|
| 100 | dxdT = zeros(2*n,3); |
---|
| 101 | dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); |
---|
| 102 | dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); |
---|
| 103 | end; |
---|
| 104 | |
---|
| 105 | |
---|
| 106 | % Add distortion: |
---|
| 107 | |
---|
| 108 | r2 = x(1,:).^2 + x(2,:).^2; |
---|
| 109 | |
---|
| 110 | if nargout > 1, |
---|
| 111 | dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); |
---|
| 112 | dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); |
---|
| 113 | end; |
---|
| 114 | |
---|
| 115 | |
---|
| 116 | r4 = r2.^2; |
---|
| 117 | |
---|
| 118 | if nargout > 1, |
---|
| 119 | dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; |
---|
| 120 | dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; |
---|
| 121 | end |
---|
| 122 | |
---|
| 123 | r6 = r2.^3; |
---|
| 124 | |
---|
| 125 | if nargout > 1, |
---|
| 126 | dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; |
---|
| 127 | dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; |
---|
| 128 | end; |
---|
| 129 | |
---|
| 130 | % Radial distortion: |
---|
| 131 | |
---|
| 132 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; |
---|
| 133 | |
---|
| 134 | if nargout > 1, |
---|
| 135 | dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; |
---|
| 136 | dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; |
---|
| 137 | dcdistdk = [ r2' r4' zeros(n,2) r6']; |
---|
| 138 | end; |
---|
| 139 | |
---|
| 140 | xd1 = x .* (ones(2,1)*cdist); |
---|
| 141 | |
---|
| 142 | if nargout > 1, |
---|
| 143 | dxd1dom = zeros(2*n,3); |
---|
| 144 | dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; |
---|
| 145 | dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; |
---|
| 146 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); |
---|
| 147 | dxd1dom = dxd1dom + coeff.* dxdom; |
---|
| 148 | |
---|
| 149 | dxd1dT = zeros(2*n,3); |
---|
| 150 | dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; |
---|
| 151 | dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; |
---|
| 152 | dxd1dT = dxd1dT + coeff.* dxdT; |
---|
| 153 | |
---|
| 154 | dxd1dk = zeros(2*n,5); |
---|
| 155 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; |
---|
| 156 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; |
---|
| 157 | end; |
---|
| 158 | |
---|
| 159 | |
---|
| 160 | % tangential distortion: |
---|
| 161 | |
---|
| 162 | a1 = 2.*x(1,:).*x(2,:); |
---|
| 163 | a2 = r2 + 2*x(1,:).^2; |
---|
| 164 | a3 = r2 + 2*x(2,:).^2; |
---|
| 165 | |
---|
| 166 | delta_x = [k(3)*a1 + k(4)*a2 ; |
---|
| 167 | k(3) * a3 + k(4)*a1]; |
---|
| 168 | |
---|
| 169 | |
---|
| 170 | %ddelta_xdx = zeros(2*n,2*n); |
---|
| 171 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); |
---|
| 172 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); |
---|
| 173 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); |
---|
| 174 | |
---|
| 175 | if nargout > 1, |
---|
| 176 | ddelta_xdom = zeros(2*n,3); |
---|
| 177 | ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); |
---|
| 178 | ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); |
---|
| 179 | |
---|
| 180 | ddelta_xdT = zeros(2*n,3); |
---|
| 181 | ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); |
---|
| 182 | ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); |
---|
| 183 | |
---|
| 184 | ddelta_xdk = zeros(2*n,5); |
---|
| 185 | ddelta_xdk(1:2:end,3) = a1'; |
---|
| 186 | ddelta_xdk(1:2:end,4) = a2'; |
---|
| 187 | ddelta_xdk(2:2:end,3) = a3'; |
---|
| 188 | ddelta_xdk(2:2:end,4) = a1'; |
---|
| 189 | end; |
---|
| 190 | |
---|
| 191 | |
---|
| 192 | xd2 = xd1 + delta_x; |
---|
| 193 | |
---|
| 194 | if nargout > 1, |
---|
| 195 | dxd2dom = dxd1dom + ddelta_xdom ; |
---|
| 196 | dxd2dT = dxd1dT + ddelta_xdT; |
---|
| 197 | dxd2dk = dxd1dk + ddelta_xdk ; |
---|
| 198 | end; |
---|
| 199 | |
---|
| 200 | |
---|
| 201 | % Add Skew: |
---|
| 202 | |
---|
| 203 | xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; |
---|
| 204 | |
---|
| 205 | % Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha |
---|
| 206 | if nargout > 1, |
---|
| 207 | dxd3dom = zeros(2*n,3); |
---|
| 208 | dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); |
---|
| 209 | dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); |
---|
| 210 | dxd3dT = zeros(2*n,3); |
---|
| 211 | dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); |
---|
| 212 | dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); |
---|
| 213 | dxd3dk = zeros(2*n,5); |
---|
| 214 | dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); |
---|
| 215 | dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); |
---|
| 216 | dxd3dalpha = zeros(2*n,1); |
---|
| 217 | dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; |
---|
| 218 | end; |
---|
| 219 | |
---|
| 220 | |
---|
| 221 | |
---|
| 222 | % Pixel coordinates: |
---|
| 223 | if length(f)>1, |
---|
| 224 | xp = xd3 .* (f(:) * ones(1,n)) + c(:)*ones(1,n); |
---|
| 225 | if nargout > 1, |
---|
| 226 | coeff = reshape(f(:)*ones(1,n),2*n,1); |
---|
| 227 | dxpdom = (coeff*ones(1,3)) .* dxd3dom; |
---|
| 228 | dxpdT = (coeff*ones(1,3)) .* dxd3dT; |
---|
| 229 | dxpdk = (coeff*ones(1,5)) .* dxd3dk; |
---|
| 230 | dxpdalpha = (coeff) .* dxd3dalpha; |
---|
| 231 | dxpdf = zeros(2*n,2); |
---|
| 232 | dxpdf(1:2:end,1) = xd3(1,:)'; |
---|
| 233 | dxpdf(2:2:end,2) = xd3(2,:)'; |
---|
| 234 | end; |
---|
| 235 | else |
---|
| 236 | xp = f * xd3 + c*ones(1,n); |
---|
| 237 | if nargout > 1, |
---|
| 238 | dxpdom = f * dxd3dom; |
---|
| 239 | dxpdT = f * dxd3dT; |
---|
| 240 | dxpdk = f * dxd3dk; |
---|
| 241 | dxpdalpha = f .* dxd3dalpha; |
---|
| 242 | dxpdf = xd3(:); |
---|
| 243 | end; |
---|
| 244 | end; |
---|
| 245 | |
---|
| 246 | if nargout > 1, |
---|
| 247 | dxpdc = zeros(2*n,2); |
---|
| 248 | dxpdc(1:2:end,1) = ones(n,1); |
---|
| 249 | dxpdc(2:2:end,2) = ones(n,1); |
---|
| 250 | end; |
---|
| 251 | |
---|
| 252 | |
---|
| 253 | return; |
---|
| 254 | |
---|
| 255 | % Test of the Jacobians: |
---|
| 256 | |
---|
| 257 | n = 10; |
---|
| 258 | |
---|
| 259 | X = 10*randn(3,n); |
---|
| 260 | om = randn(3,1); |
---|
| 261 | T = [10*randn(2,1);40]; |
---|
| 262 | f = 1000*rand(2,1); |
---|
| 263 | c = 1000*randn(2,1); |
---|
| 264 | k = 0.5*randn(5,1); |
---|
| 265 | alpha = 0.01*randn(1,1); |
---|
| 266 | |
---|
| 267 | [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha); |
---|
| 268 | |
---|
| 269 | |
---|
| 270 | % Test on om: OK |
---|
| 271 | |
---|
| 272 | dom = 0.000000001 * norm(om)*randn(3,1); |
---|
| 273 | om2 = om + dom; |
---|
| 274 | |
---|
| 275 | [x2] = project_points2(X,om2,T,f,c,k,alpha); |
---|
| 276 | |
---|
| 277 | x_pred = x + reshape(dxdom * dom,2,n); |
---|
| 278 | |
---|
| 279 | |
---|
| 280 | norm(x2-x)/norm(x2 - x_pred) |
---|
| 281 | |
---|
| 282 | |
---|
| 283 | % Test on T: OK!! |
---|
| 284 | |
---|
| 285 | dT = 0.0001 * norm(T)*randn(3,1); |
---|
| 286 | T2 = T + dT; |
---|
| 287 | |
---|
| 288 | [x2] = project_points2(X,om,T2,f,c,k,alpha); |
---|
| 289 | |
---|
| 290 | x_pred = x + reshape(dxdT * dT,2,n); |
---|
| 291 | |
---|
| 292 | |
---|
| 293 | norm(x2-x)/norm(x2 - x_pred) |
---|
| 294 | |
---|
| 295 | |
---|
| 296 | |
---|
| 297 | % Test on f: OK!! |
---|
| 298 | |
---|
| 299 | df = 0.001 * norm(f)*randn(2,1); |
---|
| 300 | f2 = f + df; |
---|
| 301 | |
---|
| 302 | [x2] = project_points2(X,om,T,f2,c,k,alpha); |
---|
| 303 | |
---|
| 304 | x_pred = x + reshape(dxdf * df,2,n); |
---|
| 305 | |
---|
| 306 | |
---|
| 307 | norm(x2-x)/norm(x2 - x_pred) |
---|
| 308 | |
---|
| 309 | |
---|
| 310 | % Test on c: OK!! |
---|
| 311 | |
---|
| 312 | dc = 0.01 * norm(c)*randn(2,1); |
---|
| 313 | c2 = c + dc; |
---|
| 314 | |
---|
| 315 | [x2] = project_points2(X,om,T,f,c2,k,alpha); |
---|
| 316 | |
---|
| 317 | x_pred = x + reshape(dxdc * dc,2,n); |
---|
| 318 | |
---|
| 319 | norm(x2-x)/norm(x2 - x_pred) |
---|
| 320 | |
---|
| 321 | % Test on k: OK!! |
---|
| 322 | |
---|
| 323 | dk = 0.001 * norm(k)*randn(5,1); |
---|
| 324 | k2 = k + dk; |
---|
| 325 | |
---|
| 326 | [x2] = project_points2(X,om,T,f,c,k2,alpha); |
---|
| 327 | |
---|
| 328 | x_pred = x + reshape(dxdk * dk,2,n); |
---|
| 329 | |
---|
| 330 | norm(x2-x)/norm(x2 - x_pred) |
---|
| 331 | |
---|
| 332 | |
---|
| 333 | % Test on alpha: OK!! |
---|
| 334 | |
---|
| 335 | dalpha = 0.001 * norm(k)*randn(1,1); |
---|
| 336 | alpha2 = alpha + dalpha; |
---|
| 337 | |
---|
| 338 | [x2] = project_points2(X,om,T,f,c,k,alpha2); |
---|
| 339 | |
---|
| 340 | x_pred = x + reshape(dxdalpha * dalpha,2,n); |
---|
| 341 | |
---|
| 342 | norm(x2-x)/norm(x2 - x_pred) |
---|