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 | } |
---|