1 // Copyright 2003, 2004, 2005 David Hilvert <dhilvert@auricle.dyndns.org>,
2 // <dhilvert@ugcs.caltech.edu>
4 /* This file is part of the Anti-Lamenessing Engine.
6 The Anti-Lamenessing Engine is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
11 The Anti-Lamenessing Engine is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
16 You should have received a copy of the GNU General Public License
17 along with the Anti-Lamenessing Engine; if not, write to the Free Software
18 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 * d3/cpf.h: Control point file interface.
31 #define CPF_VERSION_MAX 0
36 static int load_version
;
38 static const char *load_n
;
39 static const char *save_n
;
40 static int save_version
;
42 static ale_pos cpp_upper
;
43 static ale_pos va_upper
;
44 static ale_pos cpp_lower
;
46 static ale_pos stereo_threshold
;
48 static int total_error_mean
;
56 struct control_point
{
62 static struct control_point
*cp_array
;
63 static unsigned int cp_array_max
;
64 static unsigned int cp_index
;
66 static unsigned int systems_solved
;
68 static void error(const char *message
) {
69 fprintf(stderr
, "cpf: Error: %s", message
);
73 static void get_integer(int *i
) {
74 if(fscanf(load_f
, " %d", i
) != 1)
75 error("Could not get integer.");
78 static void get_double(double *d
) {
79 if (fscanf(load_f
, " %lf", d
) != 1)
80 error("Could not get double.");
83 static void get_new_line() {
84 int next_character
= 0;
86 while (next_character
!= EOF
87 && next_character
!= '\n')
88 next_character
= fgetc(load_f
);
91 static void check_version(int v
) {
92 if (v
> CPF_VERSION_MAX
)
93 error("Incompatible version number.");
97 * Type A control point record
99 * A <frame 0 coord 1> <frame 0 coord 0> ... <frame n coord 0>
101 static void get_type_a() {
104 * Get the number of frames.
106 int n
= d2::image_rw::count();
109 * Allocate storage for N frames.
111 d2::point
*coords
= new d2::point
[n
];
113 for (int i
= 0; i
< n
; i
++)
114 for (int j
= 0; j
< 2; j
++) {
117 coords
[i
][1 - j
] = dp
;
120 cp_array
[cp_array_max
- 1].d2
= coords
;
121 cp_array
[cp_array_max
- 1].d3
= point::undefined();
122 cp_array
[cp_array_max
- 1].type
= 0;
126 * Type B control point record
128 * B <coord 1> <coord 0> <coord 2>
130 static void get_type_b() {
137 cp_array
[cp_array_max
- 1].d3
= point(d
[0], d
[1], d
[2]);
138 cp_array
[cp_array_max
- 1].type
= 1;
142 * Type C control point record
144 * C <type A data> <type B data>
146 static void get_type_c() {
149 cp_array
[cp_array_max
- 1].type
= 2;
153 * Read a control point file.
155 static void read_file() {
156 while (load_f
&& !feof(load_f
)) {
159 command_char
= fgetc(load_f
);
161 switch (command_char
) {
173 get_integer(&load_version
);
174 check_version(load_version
);
178 cp_array
= (control_point
*) realloc(cp_array
, ++cp_array_max
* sizeof(control_point
));
184 cp_array
= (control_point
*) realloc(cp_array
, ++cp_array_max
* sizeof(control_point
));
190 cp_array
= (control_point
*) realloc(cp_array
, ++cp_array_max
* sizeof(control_point
));
196 error("Unrecognized command");
202 * Calculate the centroid of a set of points.
204 static point
calculate_centroid(point
*points
, int n
) {
208 for (int i
= 0; i
< n
; i
++) {
209 if (!points
[i
].defined())
219 * Measure the error between a projected system and a solved coordinate.
221 static ale_accum
measure_projected_error(point solved
, const d2::point coords
[], int n
) {
223 ale_accum divisor
= 0;
225 for (int i
= 0; i
< n
; i
++) {
226 if (!coords
[i
].defined())
229 pt t
= align::projective(i
);
231 point sp
= t
.wp_unscaled(solved
);
233 error
+= (sp
.xy() - coords
[i
]).normsq();
237 return sqrt(error
/ divisor
);
240 static ale_accum
measure_total_error() {
242 if (cp_array_max
== 0)
245 if (total_error_mean
) {
250 ale_accum result
= 0;
251 ale_accum divisor
= 0;
253 for (unsigned int i
= 0; i
< cp_array_max
; i
++) {
254 ale_accum e
= measure_projected_error(cp_array
[i
].d3
, cp_array
[i
].d2
, d2::image_rw::count());
262 return sqrt(result
/ divisor
);
268 std::vector
<ale_accum
> v
;
270 for (unsigned int i
= 0; i
< cp_array_max
; i
++) {
271 ale_accum e
= measure_projected_error(cp_array
[i
].d3
, cp_array
[i
].d2
, d2::image_rw::count());
275 v
.push_back(fabs(e
));
278 std::sort(v
.begin(), v
.end());
281 return v
[v
.size() / 2];
283 return 0.5 * (v
[v
.size() / 2 - 1]
290 * Solve for a 3D point from a system of projected coordinates.
292 * The algorithm is this:
294 * First, convert all 2D points to 3D points by projecting them onto a
295 * plane perpendicular to the view axis of the corresponding camera,
296 * and passing through the origin of the world coordinate system.
297 * Then, for each point, move it along the view ray to the point
298 * closest to the centroid of all of the points. Repeat this last loop
299 * until the largest adjustment is smaller than some specified lower
302 static point
solve_projected_system(const d2::point points
[], int n
) {
305 * Copy the passed array.
307 point
*points_copy
= new point
[n
];
308 for (int i
= 0; i
< n
; i
++)
309 points_copy
[i
] = point(points
[i
][0], points
[i
][1], 0);
312 * Set an initial depth for each point, and convert it to world
316 for (int i
= 0; i
< n
; i
++) {
317 pt t
= align::projective(i
);
319 point p
= t
.wc(point(0, 0, 0));
320 ale_pos plane_depth
= p
[2];
322 points_copy
[i
][2] = plane_depth
;
324 points_copy
[i
] = t
.pw_unscaled(points_copy
[i
]);
328 * For each point, adjust the depth along the view ray to
329 * minimize the distance from the centroid of the points_copy.
332 ale_pos max_diff
= 0;
333 ale_pos prev_max_diff
= 0;
334 ale_pos diff_bound
= 0.99999;
336 while (!(max_diff
/ prev_max_diff
> diff_bound
) || !finite(max_diff
/ prev_max_diff
)) {
339 * Calculate the centroid of all points_copy.
342 point centroid
= calculate_centroid(points_copy
, n
);
344 // fprintf(stderr, "md %f pmd %f ratio %f ", max_diff, prev_max_diff, max_diff / prev_max_diff);
345 // fprintf(stderr, "centroid %f %f %f ", centroid[0], centroid[1], centroid[2]);
346 // fprintf(stderr, "%f ", measure_projected_error(centroid, points, n));
348 prev_max_diff
= max_diff
;
351 for (int i
= 0; i
< n
; i
++) {
352 // fprintf(stderr, "points_copy[%d] %f %f %f ", i, points_copy[i][0], points_copy[i][1], points_copy[i][2]);
354 if (!points_copy
[i
].defined())
357 pt t
= align::projective(i
);
358 point camera_origin
= t
.cw(point(0, 0, 0));
360 point v
= points_copy
[i
] - camera_origin
;
361 ale_pos l
= (centroid
- camera_origin
).norm();
362 ale_pos alpha
= camera_origin
.anglebetw(points_copy
[i
], centroid
);
364 point new_point
= camera_origin
+ v
/ v
.norm() * l
* cos(alpha
);
366 ale_pos diff
= points_copy
[i
].lengthto(new_point
);
371 points_copy
[i
] = new_point
;
375 // fprintf(stderr, "%f\n", measure_projected_error(calculate_centroid(points_copy, n), points, n));
376 // fprintf(stderr, "md %f pmd %f ratio %f ", max_diff, prev_max_diff, max_diff / prev_max_diff);
378 point result
= calculate_centroid(points_copy
, n
);
380 delete[] points_copy
;
385 static void solve_total_system() {
386 for (unsigned i
= 0; i
< cp_array_max
; i
++) {
387 if (cp_array
[i
].type
== 0)
388 cp_array
[i
].d3
= solve_projected_system(cp_array
[i
].d2
, d2::image_rw::count());
394 static void err_median() {
395 total_error_mean
= 0;
398 static void err_mean() {
399 total_error_mean
= 1;
402 static void set_cpp_upper(ale_pos cu
) {
406 static void set_cpp_lower(ale_pos cl
) {
410 static void set_va_upper(ale_pos vau_degrees
) {
411 va_upper
= vau_degrees
* M_PI
/ 180;
414 static void init_loadfile(const char *filename
) {
416 load_f
= fopen(load_n
, "r");
419 fprintf(stderr
, "cpf: Error: could not open control point file '%s'.", load_n
);
424 static void init_savefile(const char *filename
) {
426 save_f
= fopen(save_n
, "w");
429 fprintf(stderr
, "cpf: Error: could not open control point file '%s'.", save_n
);
433 fprintf(save_f
, "# created by ALE control point file handler version %d\n", CPF_VERSION
);
438 static void adjust_cameras() {
439 ale_accum current_error
= measure_total_error();
440 unsigned int n
= d2::image_rw::count();
443 * Perturbation measured in pixels or degrees
445 * XXX: should probably be pixel arclength instead of degrees.
448 ale_pos max_perturbation
= pow(2, floor(log(cpp_upper
) / log(2)));
449 ale_pos min_perturbation
= cpp_lower
;
450 ale_pos perturbation
= max_perturbation
;
453 * XXX: Does this ever execute?
455 if (cp_array_max
== 0) {
456 fprintf(stderr
, " (no points specified)");
460 if (perturbation
< min_perturbation
|| cp_array_max
== 0) {
461 // fprintf(stderr, " (skipping adjustment)");
465 fprintf(stderr
, "Adjusting cameras to match control points");
467 while (perturbation
>= min_perturbation
) {
469 ale_accum previous_error
;
470 ale_pos angular_p
= perturbation
/ 180 * M_PI
;
472 // fprintf(stderr, "P %f AP %f ", perturbation, angular_p);
477 * Minimum frame to adjust
481 previous_error
= current_error
;
483 ale_accum test_error
;
486 * Try adjusting camera positions
489 for (unsigned int i
= M
; i
< n
; i
++)
490 for (unsigned int d
= 0; d
< 3; d
++)
491 for ( int s
= -1; s
<= 1; s
+=2) {
492 align::adjust_translation(i
, d
, s
* perturbation
);
493 solve_total_system();
494 test_error
= measure_total_error();
495 if (test_error
< current_error
) {
496 current_error
= test_error
;
498 align::adjust_translation(i
, d
, -s
* perturbation
);
503 * Try adjusting camera orientations
506 for (unsigned int i
= M
; i
< n
; i
++)
507 for (unsigned int d
= 0; d
< 3; d
++)
508 for ( int s
= -1; s
<= 1; s
+=2) {
509 align::adjust_rotation(i
, d
, s
* angular_p
);
510 solve_total_system();
511 test_error
= measure_total_error();
512 if (test_error
< current_error
) {
513 current_error
= test_error
;
515 align::adjust_rotation(i
, d
, -s
* angular_p
);
520 * Try adjusting view angle
522 if (angular_p
<= va_upper
)
523 for (int s
= -1; s
<= 1; s
+=2) {
524 align::adjust_view_angle(s
* angular_p
);
525 solve_total_system();
526 test_error
= measure_total_error();
527 if (test_error
< current_error
) {
528 current_error
= test_error
;
530 align::adjust_view_angle(-s
* angular_p
);
533 } while (current_error
< previous_error
);
535 // fprintf(stderr, "E %f\n", current_error);
537 fprintf(stderr
, ".");
542 solve_total_system();
544 fprintf(stderr
, "\n");
545 fprintf(stderr
, "Final view angle: %f\n", d3::align::angle_of(0) / M_PI
* 180);
548 static void st(ale_pos _st
) {
549 stereo_threshold
= _st
;
552 static void solve_3d() {
556 solve_total_system();
561 static unsigned int count() {
563 if (cp_array_max
== 0)
569 static const d2::point
*get_2d(unsigned int index
) {
570 assert (index
< cp_array_max
);
571 assert (cp_array
!= NULL
);
573 if (cp_array
[index
].type
== 1)
576 return cp_array
[index
].d2
;
579 static point
get(unsigned int index
) {
581 assert (index
< cp_array_max
);
582 assert (cp_array
!= NULL
);
587 if (stereo_threshold
< measure_projected_error(cp_array
[index
].d3
,
589 d2::image_rw::count()))
590 return point::undefined();
593 return cp_array
[index
].d3
;
596 static void set(point p
) {
600 static void finish_loadfile() {
603 static void finish_savefile() {