ba_unit_test

#!/usr/bin/perl

use File::Path qw(mkpath);
use Getopt::Long;
use strict;
use warnings;

################################################
##
## CONFIGURATION VARIABLES YOU CAN CHANGE
##

my $tests_root         = 'ba_unit_test_data';
my $num_tests          = 1;
my $control_net_file   = 'noisy_control.cnet';
my $camera_prefix      = "noisy_camera";
my $camera_ext         = "tsai";
my @ba_types =         qw/sparse robust_sparse ref robust_ref/;
my $make_data          = './make_ba_test_data';
my $ba_test            = './ba_test';
my $ba_test_cfg        = 'ba_test.cfg';
my $wp_true_file       = "wp_ground_truth.txt";
my $wp_init_file       = "wp_initial.txt";
my $wp_final_file      = "wp_final.txt";
my $cam_true_file      = "cam_ground_truth.txt";
my $cam_init_file      = "cam_initial.txt";
my $cam_final_file     = "cam_final.txt";
my $ba_report_level    = 0;
my %test_config = (
    "pixel-inlier-noise-type"  => "normal",
    "pixel-inlier-df"          => 1,
    "pixel-inlier-sigma"       => 1,
    "pixel-outlier-noise-type" => "normal",
    "pixel-outlier-df"         => 1,
    "pixel-outlier-sigma"      => 1,
    "pixel-outlier-freq"       => 0.0,
    "xyz-inlier-noise-type"    => "normal",
    "xyz-inlier-df"            => 1,
    "xyz-inlier-sigma"         => 100,
    "xyz-outlier-noise-type"   => "normal",
    "xyz-outlier-df"           => 1,
    "xyz-outlier-sigma"        => 1,
    "xyz-outlier-freq"         => 0.0,
    "euler-inlier-noise-type"  => "normal",
    "euler-inlier-df"          => 1,
    "euler-inlier-sigma"       => 0.1,
    "euler-outlier-noise-type" => "normal",
    "euler-outlier-df"         => 1,
    "euler-outlier-sigma"      => 1,
    "euler-outlier-freq"       => 0.0,
    "min-tiepoints-per-image"  => 50,
    "number-of-cameras"        => 4,
    "lambda"                   => 1,
    "camera-position-sigma"    => 1,
    "camera-pose-sigma"        => 1,
    "max-iterations"           => 1,
    "cnet"                     => $control_net_file
);

###################################################
##
## SUBROUTINES
##

sub read_wp_results {
    my $fname = shift;
    my $ret = [];
    open (FILE, $fname) or die "Error: failed opening $fname: $!\n";
    while (<FILE>) {
        chomp;
        push(@$ret, split /\t/);
    }
    return $ret;
}

sub read_cam_results {
    my $fname = shift;
    my $xyz_ret = [];
    my $euler_ret = [];
    open (FILE, $fname) or die "Error: failed opening $fname: $!\n";
    while (<FILE>) {
        chomp;
        my ($x, $y, $z, @euler) = split /\t/;
        push(@$xyz_ret, ($x, $y, $z));
        push(@$euler_ret, @euler);
    }
    return ($xyz_ret, $euler_ret);
}

sub run_test {
    my ($test_config) = @_;

    my $test_dir = $tests_root;

    if (!-e $test_dir) {
        mkpath($test_dir) or die "Error: failed to create $test_dir: $!\n";
    } elsif (!-d $test_dir) {
        die "Error: $test_dir exists and is not a directory\n";
    }

    # create a test configuration file
    my $test_cfg_file = "$test_dir/$ba_test_cfg";

    open(CONFIG, ">$test_cfg_file")
        or die "Error: open $test_cfg_file failed: $!\n";
    foreach (sort keys %test_config) {
        print CONFIG "$_=$test_config{$_}\n";
    }
    close CONFIG;

    my @make_data = ($make_data,
                     '-f', "$test_cfg_file",
                     '--data-dir', "$test_dir");
    system(@make_data) == 0 or die "@make_data failed: $!";

    my @camera_files = ();
    my $num_cameras = $test_config{"number-of-cameras"};
    foreach (0 .. $num_cameras-1) {
        push(@camera_files, "$camera_prefix$_.$camera_ext");
    }

    # For each bundle adjustment type
    foreach my $ba_type (@ba_types) {
        my $results_dir = "$test_dir/$ba_type";
        if (!-e $results_dir) {
            mkpath($results_dir) or die "Error: failed to create $results_dir: $!\n";
        } elsif (!-d $results_dir) {
            die "Error: $results_dir exists and is not a directory\n";
        }

        # run bundle adjustment
        my @ba_test_opts = (
            '-R',$results_dir,
            '-D',$test_dir,
            '-r',$ba_report_level,
            '-f',$test_cfg_file,
            '-b',$ba_type);
        my @ba_test = ($ba_test, @ba_test_opts, @camera_files);
        system(@ba_test) == 0 or die "@ba_test failed: $!";
    }
}

sub read_results {
    my %results = ();
    my $test_dir = $tests_root;
    foreach my $ba_type (@ba_types) {
        my @wp_mse = ();
        my @xyz_mse = ();
        my @euler_mse = ();

        my $results_dir = "$test_dir/$ba_type";
        my $wp_final = read_wp_results("$results_dir/$wp_final_file");
        my ($xyz_final, $euler_final) = read_cam_results("$results_dir/$cam_final_file");
        $results{$ba_type} = {wp    => $wp_final,
                              xyz   => $xyz_final,
                              euler => $euler_final};
    }
    return \%results;
}

sub check_results {
    my ($results) = @_;
    # check ref vs sparse
    my @ba_pairs = ([qw(ref sparse)],
                    [qw(robust_ref robust_sparse)]);
    foreach my $pair (@ba_pairs) {
        my ($type_1, $type_2) = @$pair;
        foreach my $data_type (qw(xyz euler wp)) {
            my $t1vals = $results->{$type_1}{$data_type};
            my $t2vals = $results->{$type_2}{$data_type};
            my $t1_len = scalar @$t1vals;
            my $t2_len = scalar @$t2vals;
            if ($t1_len != $t2_len) {
                print STDERR
                    join(" and ", map {ucfirst($_)} @$pair),
                    " returned different length ".
                    "vectors for $data_type parameters\n";
                exit;
            }
            for (my $i = 0; $i < $t1_len; $i++) {
                if (abs($t1vals->[$i] - $t2vals->[$i]) > 1e-3) {
                    print STDERR
                        join(" and ", map {ucfirst($_)} @$pair),
                        " returned different values for ".
                        "$data_type parameters (at $i)\n";
                    local $, = "\n";
                    print STDERR (map {join("\t",$_,$t1vals->[$_],$t2vals->[$_])}
                                  (0 .. $t1_len-1)),"\n";
                    exit;
                }
            }
        }
    }
    print STDERR "Success: All data values match\n";
}

#################################################
##
## BEGIN EXECUTION
##

run_test(\%test_config);

## Read results
my $results = read_results();

check_results($results);