606 lines
13 KiB
C++
606 lines
13 KiB
C++
// Copyright 2003, 2004, 2005 David Hilvert <dhilvert@auricle.dyndns.org>,
|
|
// <dhilvert@ugcs.caltech.edu>
|
|
|
|
/* This file is part of the Anti-Lamenessing Engine.
|
|
|
|
The Anti-Lamenessing Engine is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
The Anti-Lamenessing Engine is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with the Anti-Lamenessing Engine; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
/*
|
|
* d3/cpf.h: Control point file interface.
|
|
*/
|
|
|
|
#ifndef __cpf_h__
|
|
#define __cpf_h__
|
|
|
|
#include "point.h"
|
|
|
|
#define CPF_VERSION 0
|
|
#define CPF_VERSION_MAX 0
|
|
|
|
class cpf {
|
|
static FILE *load_f;
|
|
static FILE *save_f;
|
|
static int load_version;
|
|
|
|
static const char *load_n;
|
|
static const char *save_n;
|
|
static int save_version;
|
|
|
|
static ale_pos cpp_upper;
|
|
static ale_pos va_upper;
|
|
static ale_pos cpp_lower;
|
|
|
|
static ale_pos stereo_threshold;
|
|
|
|
static int total_error_mean;
|
|
|
|
/*
|
|
* TYPE is:
|
|
* 0 type A
|
|
* 1 type B
|
|
* 2 type C
|
|
*/
|
|
struct control_point {
|
|
int type;
|
|
d2::point *d2;
|
|
point d3;
|
|
};
|
|
|
|
static struct control_point *cp_array;
|
|
static unsigned int cp_array_max;
|
|
static unsigned int cp_index;
|
|
|
|
static unsigned int systems_solved;
|
|
|
|
static void error(const char *message) {
|
|
fprintf(stderr, "cpf: Error: %s", message);
|
|
exit(1);
|
|
}
|
|
|
|
static void get_integer(int *i) {
|
|
if(fscanf(load_f, " %d", i) != 1)
|
|
error("Could not get integer.");
|
|
}
|
|
|
|
static void get_double(double *d) {
|
|
if (fscanf(load_f, " %lf", d) != 1)
|
|
error("Could not get double.");
|
|
}
|
|
|
|
static void get_new_line() {
|
|
int next_character = 0;
|
|
|
|
while (next_character != EOF
|
|
&& next_character != '\n')
|
|
next_character = fgetc(load_f);
|
|
}
|
|
|
|
static void check_version(int v) {
|
|
if (v > CPF_VERSION_MAX)
|
|
error("Incompatible version number.");
|
|
}
|
|
|
|
/*
|
|
* Type A control point record
|
|
*
|
|
* A <frame 0 coord 1> <frame 0 coord 0> ... <frame n coord 0>
|
|
*/
|
|
static void get_type_a() {
|
|
|
|
/*
|
|
* Get the number of frames.
|
|
*/
|
|
int n = d2::image_rw::count();
|
|
|
|
/*
|
|
* Allocate storage for N frames.
|
|
*/
|
|
d2::point *coords = new d2::point[n];
|
|
|
|
for (int i = 0; i < n; i++)
|
|
for (int j = 0; j < 2; j++) {
|
|
double dp;
|
|
get_double(&dp);
|
|
coords[i][1 - j] = dp;
|
|
}
|
|
|
|
cp_array[cp_array_max - 1].d2 = coords;
|
|
cp_array[cp_array_max - 1].d3 = point::undefined();
|
|
cp_array[cp_array_max - 1].type = 0;
|
|
}
|
|
|
|
/*
|
|
* Type B control point record
|
|
*
|
|
* B <coord 1> <coord 0> <coord 2>
|
|
*/
|
|
static void get_type_b() {
|
|
double d[3];
|
|
|
|
get_double(&d[1]);
|
|
get_double(&d[0]);
|
|
get_double(&d[2]);
|
|
|
|
cp_array[cp_array_max - 1].d3 = point(d[0], d[1], d[2]);
|
|
cp_array[cp_array_max - 1].type = 1;
|
|
}
|
|
|
|
/*
|
|
* Type C control point record
|
|
*
|
|
* C <type A data> <type B data>
|
|
*/
|
|
static void get_type_c() {
|
|
get_type_a();
|
|
get_type_b();
|
|
cp_array[cp_array_max - 1].type = 2;
|
|
}
|
|
|
|
/*
|
|
* Read a control point file.
|
|
*/
|
|
static void read_file() {
|
|
while (load_f && !feof(load_f)) {
|
|
int command_char;
|
|
|
|
command_char = fgetc(load_f);
|
|
|
|
switch (command_char) {
|
|
case EOF:
|
|
return;
|
|
case '#':
|
|
case ' ':
|
|
case '\t':
|
|
get_new_line();
|
|
break;
|
|
case '\r':
|
|
case '\n':
|
|
break;
|
|
case 'V':
|
|
get_integer(&load_version);
|
|
check_version(load_version);
|
|
get_new_line();
|
|
break;
|
|
case 'A':
|
|
cp_array = (control_point *) realloc(cp_array, ++cp_array_max * sizeof(control_point));
|
|
assert(cp_array);
|
|
get_type_a();
|
|
get_new_line();
|
|
break;
|
|
case 'B':
|
|
cp_array = (control_point *) realloc(cp_array, ++cp_array_max * sizeof(control_point));
|
|
assert(cp_array);
|
|
get_type_b();
|
|
get_new_line();
|
|
break;
|
|
case 'C':
|
|
cp_array = (control_point *) realloc(cp_array, ++cp_array_max * sizeof(control_point));
|
|
assert(cp_array);
|
|
get_type_c();
|
|
get_new_line();
|
|
break;
|
|
default:
|
|
error("Unrecognized command");
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Calculate the centroid of a set of points.
|
|
*/
|
|
static point calculate_centroid(point *points, int n) {
|
|
point sum(0, 0, 0);
|
|
int count = 0;
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
if (!points[i].defined())
|
|
continue;
|
|
sum += points[i];
|
|
count++;
|
|
}
|
|
|
|
return sum / count;
|
|
}
|
|
|
|
/*
|
|
* Measure the error between a projected system and a solved coordinate.
|
|
*/
|
|
static ale_real measure_projected_error(point solved, const d2::point coords[], int n) {
|
|
ale_accum error = 0;
|
|
ale_accum divisor = 0;
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
if (!coords[i].defined())
|
|
continue;
|
|
|
|
pt t = align::projective(i);
|
|
|
|
point sp = t.wp_unscaled(solved);
|
|
|
|
error += (sp.xy() - coords[i]).normsq();
|
|
divisor += 1;
|
|
}
|
|
|
|
return sqrt(error / divisor);
|
|
}
|
|
|
|
static ale_accum measure_total_error() {
|
|
|
|
if (cp_array_max == 0)
|
|
return 0;
|
|
|
|
if (total_error_mean) {
|
|
/*
|
|
* Use mean error.
|
|
*/
|
|
|
|
ale_accum result = 0;
|
|
ale_accum divisor = 0;
|
|
|
|
for (unsigned int i = 0; i < cp_array_max; i++) {
|
|
ale_accum e = measure_projected_error(cp_array[i].d3, cp_array[i].d2, d2::image_rw::count());
|
|
if (!finite(e))
|
|
continue;
|
|
|
|
result += e * e;
|
|
divisor += 1;
|
|
}
|
|
|
|
return sqrt(result / divisor);
|
|
} else {
|
|
/*
|
|
* Use median error
|
|
*/
|
|
|
|
std::vector<ale_accum> v;
|
|
|
|
for (unsigned int i = 0; i < cp_array_max; i++) {
|
|
ale_accum e = measure_projected_error(cp_array[i].d3, cp_array[i].d2, d2::image_rw::count());
|
|
if (!finite(e))
|
|
continue;
|
|
|
|
v.push_back(fabs(e));
|
|
}
|
|
|
|
std::sort(v.begin(), v.end());
|
|
|
|
if (v.size() % 2) {
|
|
return v[v.size() / 2];
|
|
} else {
|
|
return 0.5 * (v[v.size() / 2 - 1]
|
|
+ v[v.size() / 2]);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Solve for a 3D point from a system of projected coordinates.
|
|
*
|
|
* The algorithm is this:
|
|
*
|
|
* First, convert all 2D points to 3D points by projecting them onto a
|
|
* plane perpendicular to the view axis of the corresponding camera,
|
|
* and passing through the origin of the world coordinate system.
|
|
* Then, for each point, move it along the view ray to the point
|
|
* closest to the centroid of all of the points. Repeat this last loop
|
|
* until the largest adjustment is smaller than some specified lower
|
|
* bound.
|
|
*/
|
|
static point solve_projected_system(const d2::point points[], int n) {
|
|
|
|
/*
|
|
* Copy the passed array.
|
|
*/
|
|
point *points_copy = new point[n];
|
|
for (int i = 0; i < n; i++)
|
|
points_copy[i] = point(points[i][0], points[i][1], 0);
|
|
|
|
/*
|
|
* Set an initial depth for each point, and convert it to world
|
|
* coordinates.
|
|
*/
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
pt t = align::projective(i);
|
|
|
|
point p = t.wc(point(0, 0, 0));
|
|
ale_pos plane_depth = p[2];
|
|
|
|
points_copy[i][2] = plane_depth;
|
|
|
|
points_copy[i] = t.pw_unscaled(points_copy[i]);
|
|
}
|
|
|
|
/*
|
|
* For each point, adjust the depth along the view ray to
|
|
* minimize the distance from the centroid of the points_copy.
|
|
*/
|
|
|
|
ale_pos max_diff = 0;
|
|
ale_pos prev_max_diff = 0;
|
|
ale_pos diff_bound = 0.99999;
|
|
|
|
while (!(max_diff / prev_max_diff > diff_bound) || !finite(max_diff / prev_max_diff)) {
|
|
|
|
/*
|
|
* Calculate the centroid of all points_copy.
|
|
*/
|
|
|
|
point centroid = calculate_centroid(points_copy, n);
|
|
|
|
// fprintf(stderr, "md %f pmd %f ratio %f ", max_diff, prev_max_diff, max_diff / prev_max_diff);
|
|
// fprintf(stderr, "centroid %f %f %f ", centroid[0], centroid[1], centroid[2]);
|
|
// fprintf(stderr, "%f ", measure_projected_error(centroid, points, n));
|
|
|
|
prev_max_diff = max_diff;
|
|
max_diff = 0;
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
// fprintf(stderr, "points_copy[%d] %f %f %f ", i, points_copy[i][0], points_copy[i][1], points_copy[i][2]);
|
|
|
|
if (!points_copy[i].defined())
|
|
continue;
|
|
|
|
pt t = align::projective(i);
|
|
point camera_origin = t.cw(point(0, 0, 0));
|
|
|
|
point v = points_copy[i] - camera_origin;
|
|
ale_pos l = (centroid - camera_origin).norm();
|
|
ale_pos alpha = camera_origin.anglebetw(points_copy[i], centroid);
|
|
|
|
point new_point = camera_origin + v / v.norm() * l * cos(alpha);
|
|
|
|
ale_pos diff = points_copy[i].lengthto(new_point);
|
|
|
|
if (diff > max_diff)
|
|
max_diff = diff;
|
|
|
|
points_copy[i] = new_point;
|
|
}
|
|
|
|
}
|
|
// fprintf(stderr, "%f\n", measure_projected_error(calculate_centroid(points_copy, n), points, n));
|
|
// fprintf(stderr, "md %f pmd %f ratio %f ", max_diff, prev_max_diff, max_diff / prev_max_diff);
|
|
|
|
point result = calculate_centroid(points_copy, n);
|
|
|
|
delete[] points_copy;
|
|
|
|
return result;
|
|
}
|
|
|
|
static void solve_total_system() {
|
|
for (unsigned i = 0; i < cp_array_max; i++) {
|
|
if (cp_array[i].type == 0)
|
|
cp_array[i].d3 = solve_projected_system(cp_array[i].d2, d2::image_rw::count());
|
|
}
|
|
}
|
|
|
|
public:
|
|
|
|
static void err_median() {
|
|
total_error_mean = 0;
|
|
}
|
|
|
|
static void err_mean() {
|
|
total_error_mean = 1;
|
|
}
|
|
|
|
static void set_cpp_upper(ale_pos cu) {
|
|
cpp_upper = cu;
|
|
}
|
|
|
|
static void set_cpp_lower(ale_pos cl) {
|
|
cpp_lower = cl;
|
|
}
|
|
|
|
static void set_va_upper(ale_pos vau_degrees) {
|
|
va_upper = (double) vau_degrees * M_PI / 180;
|
|
}
|
|
|
|
static void init_loadfile(const char *filename) {
|
|
load_n = filename;
|
|
load_f = fopen(load_n, "r");
|
|
|
|
if (!load_f) {
|
|
fprintf(stderr, "cpf: Error: could not open control point file '%s'.", load_n);
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
static void init_savefile(const char *filename) {
|
|
save_n = filename;
|
|
save_f = fopen(save_n, "w");
|
|
|
|
if (!save_f) {
|
|
fprintf(stderr, "cpf: Error: could not open control point file '%s'.", save_n);
|
|
exit(1);
|
|
}
|
|
|
|
fprintf(save_f, "# created by ALE control point file handler version %d\n", CPF_VERSION);
|
|
|
|
fclose(save_f);
|
|
}
|
|
|
|
static void adjust_cameras() {
|
|
ale_accum current_error = measure_total_error();
|
|
unsigned int n = d2::image_rw::count();
|
|
|
|
/*
|
|
* Perturbation measured in pixels or degrees
|
|
*
|
|
* XXX: should probably be pixel arclength instead of degrees.
|
|
*/
|
|
|
|
ale_pos max_perturbation = pow(2, floor(log(cpp_upper) / log(2)));
|
|
ale_pos min_perturbation = cpp_lower;
|
|
ale_pos perturbation = max_perturbation;
|
|
|
|
ui::get()->d3_control_point_data(max_perturbation, min_perturbation, perturbation, current_error);
|
|
|
|
/*
|
|
* XXX: Does this ever execute?
|
|
*/
|
|
if (cp_array_max == 0) {
|
|
fprintf(stderr, " (no points specified)");
|
|
return;
|
|
}
|
|
|
|
if (perturbation < min_perturbation || cp_array_max == 0) {
|
|
// fprintf(stderr, " (skipping adjustment)");
|
|
return;
|
|
}
|
|
|
|
while (perturbation >= min_perturbation) {
|
|
|
|
ale_accum previous_error;
|
|
ale_pos angular_p = (double) perturbation / 180 * M_PI;
|
|
|
|
// fprintf(stderr, "P %f AP %f ", perturbation, angular_p);
|
|
|
|
do {
|
|
|
|
/*
|
|
* Minimum frame to adjust
|
|
*/
|
|
int M = 1;
|
|
|
|
previous_error = current_error;
|
|
|
|
ale_accum test_error;
|
|
|
|
/*
|
|
* Try adjusting camera positions
|
|
*/
|
|
|
|
for (unsigned int i = M; i < n; i++)
|
|
for (unsigned int d = 0; d < 3; d++)
|
|
for ( int s = -1; s <= 1; s+=2) {
|
|
align::adjust_translation(i, d, s * perturbation);
|
|
solve_total_system();
|
|
test_error = measure_total_error();
|
|
if (test_error < current_error) {
|
|
current_error = test_error;
|
|
} else {
|
|
align::adjust_translation(i, d, -s * perturbation);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Try adjusting camera orientations
|
|
*/
|
|
|
|
for (unsigned int i = M; i < n; i++)
|
|
for (unsigned int d = 0; d < 3; d++)
|
|
for ( int s = -1; s <= 1; s+=2) {
|
|
align::adjust_rotation(i, d, s * angular_p);
|
|
solve_total_system();
|
|
test_error = measure_total_error();
|
|
if (test_error < current_error) {
|
|
current_error = test_error;
|
|
} else {
|
|
align::adjust_rotation(i, d, -s * angular_p);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Try adjusting view angle
|
|
*/
|
|
if (angular_p <= va_upper)
|
|
for (int s = -1; s <= 1; s+=2) {
|
|
align::adjust_view_angle(s * angular_p);
|
|
solve_total_system();
|
|
test_error = measure_total_error();
|
|
if (test_error < current_error) {
|
|
current_error = test_error;
|
|
} else {
|
|
align::adjust_view_angle(-s * angular_p);
|
|
}
|
|
}
|
|
} while (current_error < previous_error);
|
|
|
|
// fprintf(stderr, "E %f\n", current_error);
|
|
|
|
perturbation /= 2;
|
|
|
|
ui::get()->d3_control_point_data(max_perturbation, min_perturbation, perturbation, current_error);
|
|
ui::get()->d3_control_point_step();
|
|
}
|
|
|
|
solve_total_system();
|
|
}
|
|
|
|
static void st(ale_pos _st) {
|
|
stereo_threshold = _st;
|
|
}
|
|
|
|
static void solve_3d() {
|
|
if (systems_solved)
|
|
return;
|
|
|
|
solve_total_system();
|
|
adjust_cameras();
|
|
systems_solved = 1;
|
|
}
|
|
|
|
static unsigned int count() {
|
|
|
|
if (cp_array_max == 0)
|
|
read_file();
|
|
|
|
return cp_array_max;
|
|
}
|
|
|
|
static const d2::point *get_2d(unsigned int index) {
|
|
assert (index < cp_array_max);
|
|
assert (cp_array != NULL);
|
|
|
|
if (cp_array[index].type == 1)
|
|
return NULL;
|
|
|
|
return cp_array[index].d2;
|
|
}
|
|
|
|
static point get(unsigned int index) {
|
|
|
|
assert (index < cp_array_max);
|
|
assert (cp_array != NULL);
|
|
|
|
if (!systems_solved)
|
|
solve_3d();
|
|
|
|
if (stereo_threshold < (ale_pos) measure_projected_error(cp_array[index].d3,
|
|
cp_array[index].d2,
|
|
d2::image_rw::count()))
|
|
return point::undefined();
|
|
|
|
|
|
return cp_array[index].d3;
|
|
}
|
|
|
|
static void set(point p) {
|
|
assert(0);
|
|
}
|
|
|
|
static void finish_loadfile() {
|
|
}
|
|
|
|
static void finish_savefile() {
|
|
}
|
|
};
|
|
|
|
#endif
|