[37] | 1 | /* Euclidean bundle adjustment demo using the sba package */ |
---|
| 2 | |
---|
| 3 | #include <stdio.h> |
---|
| 4 | #include <stdlib.h> |
---|
| 5 | #include <string.h> |
---|
| 6 | #include <math.h> |
---|
| 7 | #include <time.h> |
---|
| 8 | |
---|
| 9 | #include <sba.h> |
---|
| 10 | #include "eucsbademo.h" |
---|
| 11 | |
---|
| 12 | #define CLOCKS_PER_MSEC (CLOCKS_PER_SEC/1000.0) |
---|
| 13 | |
---|
| 14 | |
---|
| 15 | /* pointers to additional data, used for computed image projections and their jacobians */ |
---|
| 16 | struct globs_{ |
---|
| 17 | double *intrcalib; /* intrinsic callibration matrix in row-major storage */ |
---|
| 18 | |
---|
| 19 | double *ptparams; /* needed only when bundle adjusting for camera parameters only */ |
---|
| 20 | double *camparams; /* needed only when bundle adjusting for structure parameters only */ |
---|
| 21 | } globs; |
---|
| 22 | |
---|
| 23 | |
---|
| 24 | /* Routines to estimate the estimated measurement vector (i.e. "func") and |
---|
| 25 | * its sparse jacobian (i.e. "fjac") needed in BA. Code below makes use of the |
---|
| 26 | * routines calcImgProj() and calcImgProjJacRTS() which compute the predicted |
---|
| 27 | * projection & jacobian of a SINGLE 3D point (see imgproj.c). |
---|
| 28 | * In the terminology of TR-340, these routines compute Q and its jacobians A=dQ/da, B=dQ/db. |
---|
| 29 | * Notice also that what follows is two pairs of "func" and corresponding "fjac" routines. |
---|
| 30 | * The first is to be used in full (i.e. motion + structure) BA, the second in |
---|
| 31 | * motion only BA. |
---|
| 32 | */ |
---|
| 33 | |
---|
| 34 | |
---|
| 35 | /**********************************************************************/ |
---|
| 36 | /* MEASUREMENT VECTOR AND JACOBIAN COMPUTATION FOR THE SIMPLE DRIVERS */ |
---|
| 37 | /**********************************************************************/ |
---|
| 38 | |
---|
| 39 | /* FULL BUNDLE ADJUSTMENT, I.E. SIMULTANEOUS ESTIMATION OF CAMERA AND STRUCTURE PARAMETERS */ |
---|
| 40 | |
---|
| 41 | /* Given the parameter vectors aj and bi of camera j and point i, computes in xij the |
---|
| 42 | * predicted projection of point i on image j |
---|
| 43 | */ |
---|
| 44 | static void img_projRTS(int j, int i, double *aj, double *bi, double *xij, void *adata) |
---|
| 45 | { |
---|
| 46 | double *Kcalib; |
---|
| 47 | struct globs_ *gl; |
---|
| 48 | |
---|
| 49 | gl=(struct globs_ *)adata; |
---|
| 50 | Kcalib=gl->intrcalib; |
---|
| 51 | |
---|
| 52 | calcImgProj(Kcalib, aj, aj+4, bi, xij); // 4 is the quaternion's length |
---|
| 53 | } |
---|
| 54 | |
---|
| 55 | /* Given the parameter vectors aj and bi of camera j and point i, computes in Aij, Bij the |
---|
| 56 | * jacobian of the predicted projection of point i on image j |
---|
| 57 | */ |
---|
| 58 | static void img_projRTS_jac(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata) |
---|
| 59 | { |
---|
| 60 | double *Kcalib; |
---|
| 61 | struct globs_ *gl; |
---|
| 62 | |
---|
| 63 | gl=(struct globs_ *)adata; |
---|
| 64 | Kcalib=gl->intrcalib; |
---|
| 65 | |
---|
| 66 | calcImgProjJacRTS(Kcalib, aj, aj+4, bi, (double (*)[7])Aij, (double (*)[3])Bij); // 4 is the quaternion's length |
---|
| 67 | } |
---|
| 68 | |
---|
| 69 | /* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */ |
---|
| 70 | |
---|
| 71 | /* Given the parameter vector aj of camera j, computes in xij the |
---|
| 72 | * predicted projection of point i on image j |
---|
| 73 | */ |
---|
| 74 | static void img_projRT(int j, int i, double *aj, double *xij, void *adata) |
---|
| 75 | { |
---|
| 76 | const int pnp=3; /* euclidean 3D points */ |
---|
| 77 | |
---|
| 78 | double *Kcalib, *ptparams; |
---|
| 79 | struct globs_ *gl; |
---|
| 80 | |
---|
| 81 | gl=(struct globs_ *)adata; |
---|
| 82 | Kcalib=gl->intrcalib; |
---|
| 83 | ptparams=gl->ptparams; |
---|
| 84 | |
---|
| 85 | calcImgProj(Kcalib, aj, aj+4, ptparams+i*pnp, xij); // 4 is the quaternion's length |
---|
| 86 | } |
---|
| 87 | |
---|
| 88 | /* Given the parameter vector aj of camera j, computes in Aij |
---|
| 89 | * the jacobian of the predicted projection of point i on image j |
---|
| 90 | */ |
---|
| 91 | static void img_projRT_jac(int j, int i, double *aj, double *Aij, void *adata) |
---|
| 92 | { |
---|
| 93 | const int pnp=3; /* euclidean 3D points */ |
---|
| 94 | |
---|
| 95 | double *Kcalib, *ptparams; |
---|
| 96 | struct globs_ *gl; |
---|
| 97 | |
---|
| 98 | gl=(struct globs_ *)adata; |
---|
| 99 | Kcalib=gl->intrcalib; |
---|
| 100 | ptparams=gl->ptparams; |
---|
| 101 | |
---|
| 102 | calcImgProjJacRTS(Kcalib, aj, aj+4, ptparams+i*pnp, (double (*)[7])Aij, NULL); // 4 is the quaternion's length |
---|
| 103 | } |
---|
| 104 | |
---|
| 105 | /* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */ |
---|
| 106 | |
---|
| 107 | /* Given the parameter vector bi of point i, computes in xij the |
---|
| 108 | * predicted projection of point i on image j |
---|
| 109 | */ |
---|
| 110 | static void img_projS(int j, int i, double *bi, double *xij, void *adata) |
---|
| 111 | { |
---|
| 112 | const int cnp=7; |
---|
| 113 | |
---|
| 114 | double *Kcalib, *camparams, *aj; |
---|
| 115 | struct globs_ *gl; |
---|
| 116 | |
---|
| 117 | gl=(struct globs_ *)adata; |
---|
| 118 | Kcalib=gl->intrcalib; |
---|
| 119 | camparams=gl->camparams; |
---|
| 120 | aj=camparams+j*cnp; |
---|
| 121 | |
---|
| 122 | calcImgProj(Kcalib, aj, aj+4, bi, xij); // 4 is the quaternion's length |
---|
| 123 | } |
---|
| 124 | |
---|
| 125 | /* Given the parameter vector bi of point i, computes in Bij |
---|
| 126 | * the jacobian of the predicted projection of point i on image j |
---|
| 127 | */ |
---|
| 128 | static void img_projS_jac(int j, int i, double *bi, double *Bij, void *adata) |
---|
| 129 | { |
---|
| 130 | const int cnp=7; |
---|
| 131 | |
---|
| 132 | double *Kcalib, *camparams, *aj; |
---|
| 133 | struct globs_ *gl; |
---|
| 134 | |
---|
| 135 | gl=(struct globs_ *)adata; |
---|
| 136 | Kcalib=gl->intrcalib; |
---|
| 137 | camparams=gl->camparams; |
---|
| 138 | aj=camparams+j*cnp; |
---|
| 139 | |
---|
| 140 | calcImgProjJacRTS(Kcalib, aj, aj+4, bi, NULL, (double (*)[3])Bij); // 4 is the quaternion's length |
---|
| 141 | } |
---|
| 142 | |
---|
| 143 | /**********************************************************************/ |
---|
| 144 | /* MEASUREMENT VECTOR AND JACOBIAN COMPUTATION FOR THE EXPERT DRIVERS */ |
---|
| 145 | /**********************************************************************/ |
---|
| 146 | |
---|
| 147 | /* FULL BUNDLE ADJUSTMENT, I.E. SIMULTANEOUS ESTIMATION OF CAMERA AND STRUCTURE PARAMETERS */ |
---|
| 148 | |
---|
| 149 | /* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in |
---|
| 150 | * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements |
---|
| 151 | * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted |
---|
| 152 | * projection of the i-th point on the j-th camera. |
---|
| 153 | * Notice that depending on idxij, some of the hx_ij might be missing |
---|
| 154 | * |
---|
| 155 | */ |
---|
| 156 | static void img_projsRTS_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) |
---|
| 157 | { |
---|
| 158 | register int i, j; |
---|
| 159 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 160 | pnp=3, /* euclidean 3D points */ |
---|
| 161 | mnp=2; /* img ponts are 2D */ |
---|
| 162 | double *pa, *pb, *pqr, *pt, *ppt, *pmeas, *Kcalib; |
---|
| 163 | //int n; |
---|
| 164 | int m, nnz; |
---|
| 165 | struct globs_ *gl; |
---|
| 166 | |
---|
| 167 | gl=(struct globs_ *)adata; |
---|
| 168 | Kcalib=gl->intrcalib; |
---|
| 169 | |
---|
| 170 | //n=idxij->nr; |
---|
| 171 | m=idxij->nc; |
---|
| 172 | pa=p; pb=p+m*cnp; |
---|
| 173 | |
---|
| 174 | for(j=0; j<m; ++j){ |
---|
| 175 | /* j-th camera parameters */ |
---|
| 176 | pqr=pa+j*cnp; |
---|
| 177 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 178 | |
---|
| 179 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 180 | |
---|
| 181 | for(i=0; i<nnz; ++i){ |
---|
| 182 | ppt=pb + rcsubs[i]*pnp; |
---|
| 183 | pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij |
---|
| 184 | |
---|
| 185 | calcImgProj(Kcalib, pqr, pt, ppt, pmeas); // evaluate Q in pmeas |
---|
| 186 | } |
---|
| 187 | } |
---|
| 188 | } |
---|
| 189 | |
---|
| 190 | /* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in |
---|
| 191 | * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. |
---|
| 192 | * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm, B_11, ..., B_1m, ..., B_n1, ..., B_nm), |
---|
| 193 | * where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ). |
---|
| 194 | * Notice that depending on idxij, some of the A_ij, B_ij might be missing |
---|
| 195 | * |
---|
| 196 | */ |
---|
| 197 | static void img_projsRTS_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) |
---|
| 198 | { |
---|
| 199 | register int i, j; |
---|
| 200 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 201 | pnp=3, /* euclidean 3D points */ |
---|
| 202 | mnp=2; /* img ponts are 2D */ |
---|
| 203 | double *pa, *pb, *pqr, *pt, *ppt, *jaca, *jacb, *pA, *pB, *Kcalib; |
---|
| 204 | //int n; |
---|
| 205 | int m, nnz, Asz, Bsz, idx; |
---|
| 206 | struct globs_ *gl; |
---|
| 207 | |
---|
| 208 | gl=(struct globs_ *)adata; |
---|
| 209 | Kcalib=gl->intrcalib; |
---|
| 210 | |
---|
| 211 | //n=idxij->nr; |
---|
| 212 | m=idxij->nc; |
---|
| 213 | pa=p; pb=p+m*cnp; |
---|
| 214 | Asz=mnp*cnp; Bsz=mnp*pnp; |
---|
| 215 | jaca=jac; jacb=jac+idxij->nnz*Asz; |
---|
| 216 | |
---|
| 217 | for(j=0; j<m; ++j){ |
---|
| 218 | /* j-th camera parameters */ |
---|
| 219 | pqr=pa+j*cnp; |
---|
| 220 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 221 | |
---|
| 222 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 223 | |
---|
| 224 | for(i=0; i<nnz; ++i){ |
---|
| 225 | ppt=pb + rcsubs[i]*pnp; |
---|
| 226 | idx=idxij->val[rcidxs[i]]; |
---|
| 227 | pA=jaca + idx*Asz; // set pA to point to A_ij |
---|
| 228 | pB=jacb + idx*Bsz; // set pB to point to B_ij |
---|
| 229 | |
---|
| 230 | calcImgProjJacRTS(Kcalib, pqr, pt, ppt, (double (*)[7])pA, (double (*)[3])pB); // evaluate dQ/da, dQ/db in pA, pB |
---|
| 231 | } |
---|
| 232 | } |
---|
| 233 | } |
---|
| 234 | |
---|
| 235 | /* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */ |
---|
| 236 | |
---|
| 237 | /* Given a parameter vector p made up of the parameters of m cameras, compute in |
---|
| 238 | * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. |
---|
| 239 | * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, |
---|
| 240 | * where hx_ij is the predicted projection of the i-th point on the j-th camera. |
---|
| 241 | * Notice that depending on idxij, some of the hx_ij might be missing |
---|
| 242 | * |
---|
| 243 | */ |
---|
| 244 | static void img_projsRT_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) |
---|
| 245 | { |
---|
| 246 | register int i, j; |
---|
| 247 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 248 | pnp=3, /* euclidean 3D points */ |
---|
| 249 | mnp=2; /* img ponts are 2D */ |
---|
| 250 | double *pqr, *pt, *ppt, *pmeas, *Kcalib, *ptparams; |
---|
| 251 | //int n; |
---|
| 252 | int m, nnz; |
---|
| 253 | struct globs_ *gl; |
---|
| 254 | |
---|
| 255 | gl=(struct globs_ *)adata; |
---|
| 256 | Kcalib=gl->intrcalib; |
---|
| 257 | ptparams=gl->ptparams; |
---|
| 258 | |
---|
| 259 | //n=idxij->nr; |
---|
| 260 | m=idxij->nc; |
---|
| 261 | |
---|
| 262 | for(j=0; j<m; ++j){ |
---|
| 263 | /* j-th camera parameters */ |
---|
| 264 | pqr=p+j*cnp; |
---|
| 265 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 266 | |
---|
| 267 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 268 | |
---|
| 269 | for(i=0; i<nnz; ++i){ |
---|
| 270 | ppt=ptparams + rcsubs[i]*pnp; |
---|
| 271 | pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij |
---|
| 272 | |
---|
| 273 | calcImgProj(Kcalib, pqr, pt, ppt, pmeas); // evaluate Q in pmeas |
---|
| 274 | } |
---|
| 275 | } |
---|
| 276 | } |
---|
| 277 | |
---|
| 278 | /* Given a parameter vector p made up of the parameters of m cameras, compute in jac |
---|
| 279 | * the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. |
---|
| 280 | * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm), |
---|
| 281 | * where A_ij=dx_ij/db_j (see HZ). |
---|
| 282 | * Notice that depending on idxij, some of the A_ij might be missing |
---|
| 283 | * |
---|
| 284 | */ |
---|
| 285 | static void img_projsRT_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) |
---|
| 286 | { |
---|
| 287 | register int i, j; |
---|
| 288 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 289 | pnp=3, /* euclidean 3D points */ |
---|
| 290 | mnp=2; /* img ponts are 2D */ |
---|
| 291 | double *pqr, *pt, *ppt, *pA, *Kcalib, *ptparams; |
---|
| 292 | //int n; |
---|
| 293 | int m, nnz, Asz, idx; |
---|
| 294 | struct globs_ *gl; |
---|
| 295 | |
---|
| 296 | gl=(struct globs_ *)adata; |
---|
| 297 | Kcalib=gl->intrcalib; |
---|
| 298 | ptparams=gl->ptparams; |
---|
| 299 | |
---|
| 300 | //n=idxij->nr; |
---|
| 301 | m=idxij->nc; |
---|
| 302 | Asz=mnp*cnp; |
---|
| 303 | |
---|
| 304 | for(j=0; j<m; ++j){ |
---|
| 305 | /* j-th camera parameters */ |
---|
| 306 | pqr=p+j*cnp; |
---|
| 307 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 308 | |
---|
| 309 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 310 | |
---|
| 311 | for(i=0; i<nnz; ++i){ |
---|
| 312 | ppt=ptparams + rcsubs[i]*pnp; |
---|
| 313 | idx=idxij->val[rcidxs[i]]; |
---|
| 314 | pA=jac + idx*Asz; // set pA to point to A_ij |
---|
| 315 | |
---|
| 316 | calcImgProjJacRTS(Kcalib, pqr, pt, ppt, (double (*)[7])pA, (double (*)[3])NULL); // evaluate dQ/da in pA |
---|
| 317 | } |
---|
| 318 | } |
---|
| 319 | } |
---|
| 320 | |
---|
| 321 | /* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */ |
---|
| 322 | |
---|
| 323 | /* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in |
---|
| 324 | * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements |
---|
| 325 | * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted |
---|
| 326 | * projection of the i-th point on the j-th camera. |
---|
| 327 | * Notice that depending on idxij, some of the hx_ij might be missing |
---|
| 328 | * |
---|
| 329 | */ |
---|
| 330 | static void img_projsS_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) |
---|
| 331 | { |
---|
| 332 | register int i, j; |
---|
| 333 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 334 | pnp=3, /* euclidean 3D points */ |
---|
| 335 | mnp=2; /* img ponts are 2D */ |
---|
| 336 | double *pqr, *pt, *ppt, *pmeas, *Kcalib, *camparams; |
---|
| 337 | //int n; |
---|
| 338 | int m, nnz; |
---|
| 339 | struct globs_ *gl; |
---|
| 340 | |
---|
| 341 | gl=(struct globs_ *)adata; |
---|
| 342 | Kcalib=gl->intrcalib; |
---|
| 343 | camparams=gl->camparams; |
---|
| 344 | |
---|
| 345 | //n=idxij->nr; |
---|
| 346 | m=idxij->nc; |
---|
| 347 | |
---|
| 348 | for(j=0; j<m; ++j){ |
---|
| 349 | /* j-th camera parameters */ |
---|
| 350 | pqr=camparams+j*cnp; |
---|
| 351 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 352 | |
---|
| 353 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 354 | |
---|
| 355 | for(i=0; i<nnz; ++i){ |
---|
| 356 | ppt=p + rcsubs[i]*pnp; |
---|
| 357 | pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij |
---|
| 358 | |
---|
| 359 | calcImgProj(Kcalib, pqr, pt, ppt, pmeas); // evaluate Q in pmeas |
---|
| 360 | } |
---|
| 361 | } |
---|
| 362 | } |
---|
| 363 | |
---|
| 364 | /* Given a parameter vector p made up of the 3D coordinates of n points, compute in |
---|
| 365 | * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. |
---|
| 366 | * The jacobian is returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), |
---|
| 367 | * where B_ij=dx_ij/db_i (see HZ). |
---|
| 368 | * Notice that depending on idxij, some of the B_ij might be missing |
---|
| 369 | * |
---|
| 370 | */ |
---|
| 371 | static void img_projsS_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) |
---|
| 372 | { |
---|
| 373 | register int i, j; |
---|
| 374 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 375 | pnp=3, /* euclidean 3D points */ |
---|
| 376 | mnp=2; /* img ponts are 2D */ |
---|
| 377 | double *pqr, *pt, *ppt, *pB, *Kcalib, *camparams; |
---|
| 378 | //int n; |
---|
| 379 | int m, nnz, Bsz, idx; |
---|
| 380 | struct globs_ *gl; |
---|
| 381 | |
---|
| 382 | gl=(struct globs_ *)adata; |
---|
| 383 | Kcalib=gl->intrcalib; |
---|
| 384 | camparams=gl->camparams; |
---|
| 385 | |
---|
| 386 | //n=idxij->nr; |
---|
| 387 | m=idxij->nc; |
---|
| 388 | Bsz=mnp*pnp; |
---|
| 389 | |
---|
| 390 | for(j=0; j<m; ++j){ |
---|
| 391 | /* j-th camera parameters */ |
---|
| 392 | pqr=camparams+j*cnp; |
---|
| 393 | pt=pqr+4; // rot. quaternion has 4 elements |
---|
| 394 | |
---|
| 395 | nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ |
---|
| 396 | |
---|
| 397 | for(i=0; i<nnz; ++i){ |
---|
| 398 | ppt=p + rcsubs[i]*pnp; |
---|
| 399 | idx=idxij->val[rcidxs[i]]; |
---|
| 400 | pB=jac + idx*Bsz; // set pB to point to B_ij |
---|
| 401 | |
---|
| 402 | calcImgProjJacRTS(Kcalib, pqr, pt, ppt, (double (*)[7])NULL, (double (*)[3])pB); // evaluate dQ/da in pB |
---|
| 403 | } |
---|
| 404 | } |
---|
| 405 | } |
---|
| 406 | |
---|
| 407 | |
---|
| 408 | /* Driver for sba_xxx_levmar */ |
---|
| 409 | void sba_driver(char *camsfname, char *ptsfname, char *calibfname) |
---|
| 410 | { |
---|
| 411 | double *motstruct, *motstruct_copy, *imgpts; |
---|
| 412 | double ical[9]; // intrinsic calibration params |
---|
| 413 | char *vmask; |
---|
| 414 | double opts[SBA_OPTSSZ], info[SBA_INFOSZ], phi; |
---|
| 415 | int howto, expert, analyticjac, n, verbose=0; |
---|
| 416 | int nframes, numpts3D, numprojs, nvars; |
---|
| 417 | const int cnp=7, pnp=3, mnp=2, nconstframes=1; |
---|
| 418 | |
---|
| 419 | static char *howtoname[]={"BA_MOTSTRUCT", "BA_MOT", "BA_STRUCT", "BA_MOT_MOTSTRUCT"}; |
---|
| 420 | |
---|
| 421 | clock_t start_time, end_time; |
---|
| 422 | |
---|
| 423 | |
---|
| 424 | readInitialSBAEstimate(camsfname, ptsfname, &nframes, &numpts3D, &numprojs, &motstruct, &imgpts, &vmask); |
---|
| 425 | |
---|
| 426 | //printSBAData(motstruct, nframes, numpts3D, imgpts, numprojs, vmask); |
---|
| 427 | |
---|
| 428 | /* set up globs structure */ |
---|
| 429 | readCalibParams(calibfname, ical); |
---|
| 430 | globs.intrcalib=ical; |
---|
| 431 | globs.ptparams=NULL; |
---|
| 432 | globs.camparams=NULL; |
---|
| 433 | |
---|
| 434 | /* call sparse LM routine */ |
---|
| 435 | opts[0]=SBA_INIT_MU; opts[1]=SBA_TERMINATION_THRESH; opts[2]=SBA_TERMINATION_THRESH; |
---|
| 436 | opts[3]=SBA_TERMINATION_THRESH; |
---|
| 437 | //opts[3]=0.05*numprojs; // uncomment to force termination if the average reprojection error drops below 0.05 |
---|
| 438 | |
---|
| 439 | /* Notice the various BA options demonstrated below */ |
---|
| 440 | |
---|
| 441 | /* minimize motion & structure, motion only, o |
---|
| 442 | * motion and possibly motion & structure in a 2nd pass? |
---|
| 443 | */ |
---|
| 444 | howto=BA_MOTSTRUCT; |
---|
| 445 | //howto=BA_MOT; |
---|
| 446 | //howto=BA_STRUCT; |
---|
| 447 | //howto=BA_MOT_MOTSTRUCT; |
---|
| 448 | |
---|
| 449 | /* simple or expert drivers? */ |
---|
| 450 | //expert=0; |
---|
| 451 | expert=1; |
---|
| 452 | |
---|
| 453 | /* analytic or approximate jacobian? */ |
---|
| 454 | //analyticjac=0; |
---|
| 455 | analyticjac=1; |
---|
| 456 | |
---|
| 457 | start_time=clock(); |
---|
| 458 | switch(howto){ |
---|
| 459 | case BA_MOTSTRUCT: /* BA for motion & structure */ |
---|
| 460 | nvars=nframes*cnp+numpts3D*pnp; |
---|
| 461 | if(expert) |
---|
| 462 | n=sba_motstr_levmar_x(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, mnp, |
---|
| 463 | img_projsRTS_x, (analyticjac)? img_projsRTS_jac_x : NULL, (void *)(&globs), 150, verbose, opts, info); |
---|
| 464 | else |
---|
| 465 | n=sba_motstr_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, mnp, |
---|
| 466 | img_projRTS, (analyticjac)? img_projRTS_jac : NULL, (void *)(&globs), 150, verbose, opts, info); |
---|
| 467 | break; |
---|
| 468 | |
---|
| 469 | case BA_MOT: /* BA for motion only */ |
---|
| 470 | globs.ptparams=motstruct+nframes*cnp; |
---|
| 471 | nvars=nframes*cnp; |
---|
| 472 | if(expert) |
---|
| 473 | n=sba_mot_levmar_x(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, imgpts, mnp, |
---|
| 474 | img_projsRT_x, (analyticjac)? img_projsRT_jac_x : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 475 | else |
---|
| 476 | n=sba_mot_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, imgpts, mnp, |
---|
| 477 | img_projRT, (analyticjac)? img_projRT_jac : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 478 | break; |
---|
| 479 | |
---|
| 480 | case BA_STRUCT: /* BA for structure only */ |
---|
| 481 | globs.camparams=motstruct; |
---|
| 482 | nvars=numpts3D*pnp; |
---|
| 483 | if(expert) |
---|
| 484 | n=sba_str_levmar_x(numpts3D, nframes, vmask, motstruct+nframes*cnp, pnp, imgpts, mnp, |
---|
| 485 | img_projsS_x, (analyticjac)? img_projsS_jac_x : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 486 | else |
---|
| 487 | n=sba_str_levmar(numpts3D, nframes, vmask, motstruct+nframes*cnp, pnp, imgpts, mnp, |
---|
| 488 | img_projS, (analyticjac)? img_projS_jac : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 489 | break; |
---|
| 490 | |
---|
| 491 | case BA_MOT_MOTSTRUCT: /* BA for motion only; if error too large, then BA for motion & structure */ |
---|
| 492 | if((motstruct_copy=(double *)malloc((nframes*cnp + numpts3D*pnp)*sizeof(double)))==NULL){ |
---|
| 493 | fprintf(stderr, "memory allocation failed in readInitialSBAEstimate()\n"); |
---|
| 494 | exit(1); |
---|
| 495 | } |
---|
| 496 | |
---|
| 497 | memcpy(motstruct_copy, motstruct, (nframes*cnp + numpts3D*pnp)*sizeof(double)); // save starting point for later use |
---|
| 498 | globs.ptparams=motstruct+nframes*cnp; |
---|
| 499 | nvars=nframes*cnp; |
---|
| 500 | |
---|
| 501 | if(expert) |
---|
| 502 | n=sba_mot_levmar_x(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, imgpts, mnp, |
---|
| 503 | img_projsRT_x, (analyticjac)? img_projsRT_jac_x : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 504 | else |
---|
| 505 | n=sba_mot_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, imgpts, mnp, |
---|
| 506 | img_projRT, (analyticjac)? img_projRT_jac : NULL, (void *)(&globs), 100, verbose, opts, info); |
---|
| 507 | |
---|
| 508 | if((phi=info[1]/numprojs)>SBA_MAX_REPROJ_ERROR){ |
---|
| 509 | fflush(stdout); fprintf(stdout, "Refining structure (motion only error %g)...\n", phi); fflush(stdout); |
---|
| 510 | memcpy(motstruct, motstruct_copy, (nframes*cnp + numpts3D*pnp)*sizeof(double)); // reset starting point |
---|
| 511 | |
---|
| 512 | if(expert) |
---|
| 513 | n=sba_motstr_levmar_x(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, mnp, |
---|
| 514 | img_projsRTS_x, (analyticjac)? img_projsRTS_jac_x : NULL, (void *)(&globs), 150, verbose, opts, info); |
---|
| 515 | else |
---|
| 516 | n=sba_motstr_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, mnp, |
---|
| 517 | img_projRTS, (analyticjac)? img_projRTS_jac : NULL, (void *)(&globs), 150, verbose, opts, info); |
---|
| 518 | } |
---|
| 519 | free(motstruct_copy); |
---|
| 520 | |
---|
| 521 | break; |
---|
| 522 | |
---|
| 523 | default: |
---|
| 524 | fprintf(stderr, "unknown BA method \"%d\" in sba_driver()!\n", howto); |
---|
| 525 | exit(1); |
---|
| 526 | } |
---|
| 527 | end_time=clock(); |
---|
| 528 | |
---|
| 529 | |
---|
| 530 | fflush(stdout); |
---|
| 531 | |
---|
| 532 | fprintf(stdout, "SBA using %d 3D pts, %d frames and %d image projections, %d variables\n", numpts3D, nframes, numprojs, nvars); |
---|
| 533 | fprintf(stdout, "\nMethod %s, %s driver, %s jacobian\n\n", howtoname[howto], |
---|
| 534 | expert? "expert" : "simple", |
---|
| 535 | analyticjac? "analytic" : "approximate"); |
---|
| 536 | fprintf(stdout, "SBA returned %d in %g iter, reason %g, error %g [initial %g], %d/%d func/fjac evals, %d lin. systems\n", n, info[5], info[6], info[1]/numprojs, info[0]/numprojs, (int)info[7], (int)info[8], (int)info[9]); |
---|
| 537 | fprintf(stdout, "Elapsed time: %.2lf seconds, %.2lf msecs\n", ((double) (end_time - start_time)) / CLOCKS_PER_SEC, |
---|
| 538 | ((double) (end_time - start_time)) / CLOCKS_PER_MSEC); |
---|
| 539 | fflush(stdout); |
---|
| 540 | |
---|
| 541 | /* refined motion and structure are now in motstruct */ |
---|
| 542 | |
---|
| 543 | printSBAData(motstruct, nframes, numpts3D, imgpts, numprojs, vmask); //Min modified |
---|
| 544 | |
---|
| 545 | /* just in case... */ |
---|
| 546 | globs.intrcalib=NULL; |
---|
| 547 | |
---|
| 548 | free(motstruct); |
---|
| 549 | free(imgpts); |
---|
| 550 | free(vmask); |
---|
| 551 | } |
---|
| 552 | |
---|
| 553 | int main(int argc, char *argv[]) |
---|
| 554 | { |
---|
| 555 | if(argc!=4){ |
---|
| 556 | fprintf(stderr, "Usage is %s <camera params> <point params> <intrinsic calibration params>\n", argv[0]); |
---|
| 557 | exit(1); |
---|
| 558 | } |
---|
| 559 | |
---|
| 560 | sba_driver(argv[1], argv[2], argv[3]); |
---|
| 561 | |
---|
| 562 | return 0; |
---|
| 563 | } |
---|