d2/align: Migrate code (indicated as broken by GCC) to Libale.

[Ale.git] / d3 / cpf.h

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