run_ba_tests

#!/usr/bin/perl
#===============================================================================
#
#         FILE:  run_ba_tests
#
#        USAGE:  ./run_ba_tests [options] <test_plan_file>
#
#  DESCRIPTION:  Runs a set of bundle adjustment tests
#
#      OPTIONS:  --help (-?)
#                --man
#                --num-tests (-n)
#                --directory (-d)
#                --pct_low (-l)
#                --pct_high (-h)
#                --bundle_adjustment_types (-b)
#
# REQUIREMENTS:  Statistics::Descriptive
#                File::Path
#                Getopt::Long (Perl core)
#
#         BUGS:  ---
#        NOTES:  Run 'perldoc run_ba_tests' or 'run_ba_tests --man' for
#                complete documentation, or see the Perldoc section at the end
#                of this file.
#       AUTHOR:  Michael Styer <michael@styer.net>
#      COMPANY:
#      VERSION:  1.0
#      CREATED:  09/03/2009 02:24:05 PM PDT
#     REVISION:  ---
#===============================================================================

#use lib "./run_ba_tests_lib";
use lib "/opt/vw/lib";
use File::Path qw(mkpath);
use Getopt::Long;
use Pod::Usage;
use Statistics::Descriptive;
use strict;
use warnings;
no warnings qw(uninitialized);

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

## Default root directory for this test run; all synthetic data and results
## will be stored in subdirectories of this directory.
## Can override on the command line with -d or --directory.
my $tests_root         = 'ba_tests';

## Default number of tests to run.
## Can override on the command line via -n or --num_tests
my $num_tests          = 1;

## A directory with this prefix will be created in $tests_root for each test
## plan: <prefix>0, <prefix>1, <prefix>2, etc.
my $data_dir_prefix    = 'test_plan';

## The name of the control network file to read.
## Should be either 'control.cnet' or 'noisy_control.cnet'
my $control_net_file   = 'noisy_control.cnet';

## Prefix of the camera files to read.
## Should be either 'camera' or 'noisy_camera'
my $camera_prefix      = "noisy_camera";

# Don't change this
my $camera_ext         = "tsai";

## Accepts: ref, sparse, robust_ref, robust sparse, sparse_huber,
## sparse_cauchy, ref_control, sparse_control, robust_ref_control,
## robust_sparse_control
## Append '_no_outliers' to any of the above to do a second run after
## eliminating outliers (> 2 std dev) from the control network
my @ba_types = qw/sparse robust_sparse/;

## Low and high percentiles to report in result statistics
##
## NB: Set these with a view to the number of tests you're running.
## If they are set very low or high and you aren't running many tests,
## there may not be any results in that percentile and you'll get
## warnings about "uninitialized values" at line 326"
my $percentile_low  = 25;
my $percentile_high = 75;

## Huber parameter values
my @huber_params = ();

## Cauchy parameter values
my @cauchy_params = ();

#################################################
##
## CONFIG VARIABLES YOU SHOULD NOT CHANGE
##

## Set on the command line
my $verbose = 0;

## Name of the data creation executable
my $make_data          = 'make_ba_test_data';

## Name of the executable that runs bundle adjustment
my $ba_test            = 'ba_test';

## Name of the configuration file to read; don't change.
my $ba_test_cfg        = 'ba_test.cfg';

## Don't change these; hard-coded in make_ba_test_data and ba_test
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 $cnet_deleted_index_file = "cnet_deleted";
# must be >= 35 to generate the image errors file required for outlier removal
my $ba_report_level = 35;
# Default values; generally these should be set by your test plan file
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"  => 100,
    "number-of-cameras"        => 3,
    "lambda"                   => 1,
    "camera-position-sigma"    => 1,
    "camera-pose-sigma"        => 1,
    "max-iterations"           => 30,
    "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 mse {
    my $a1 = shift;
    my $a2 = shift;
    my $size = scalar @$a1;
    die "Error: different size arrays passed to mse()\n" unless scalar @$a2 == $size;
    my $stat = Statistics::Descriptive::Sparse->new();
    my @sqerr = map {($a1->[$_] - $a2->[$_])**2} (0 .. $size-1);
    $stat->add_data(@sqerr);
    return $stat->mean();
}

sub median_euclidean_error {
    my $a1 = shift;
    my $a2 = shift;
    my $size = scalar @$a1;
    die "Error: different size arrays passed to mean_euclidean_error()\n" unless scalar @$a2 == $size;
    die "Error: don't have 3 coordinates for each point in mean_euclidean_error()\n" unless $size % 3 == 0;
    my $stat = Statistics::Descriptive::Full->new();
    my $num_wp = $size/3;
    foreach my $i (0 .. $num_wp-1) {
        my @p1 = @$a1[$i*3 .. ($i*3)+2];
        my @p2 = @$a2[$i*3 .. ($i*3)+2];
        my $err = 0;
        for (0 .. 2) { $err += ($p1[$_] - $p2[$_])**2; }
        $err = sqrt($err);
        $stat->add_data($err);
    }
    return $stat->median();
}

sub read_test_plans {
    my ($test_plan_file, $num_test_plans) = @_;
    die "Error: must provide a test plan\n" unless defined $test_plan_file;
    my %test_plans = ();

    open (TEST_PLAN, "<$test_plan_file")
        or die "Error: failed opening $test_plan_file: $!\n";
    my @test_params = split(/\t/, <TEST_PLAN>);
    map { chomp; s/[^\w]$//; $test_plans{$_} = []; } @test_params;
    while (<TEST_PLAN>) {
        chomp;
        my @vals = split /\t/;
        for (my $i = 0; $i <= $#vals; $i++) {
            push(@{$test_plans{$test_params[$i]}}, $vals[$i]);
        }
        $$num_test_plans++;
    }
    close TEST_PLAN;

    return \%test_plans;
}

sub run_tests {
    my ($num_test_plans, $test_plans, $test_config) = @_;

    for (my $t = 0; $t < $num_test_plans; $t++) {
        map {$test_config{$_} = @{$test_plans->{$_}}[$t]} keys %$test_plans;
        my $plan_dir = $tests_root."/".$data_dir_prefix.$t;

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

        # create a test configuration file
        my $test_cfg_file = "$plan_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;

        # For each test
        for (my $i = 0; $i < $num_tests; $i++) {
            print STDOUT "Plan $t, Test $i\n";
            # create a test directory
            my $test_dir = "$plan_dir/$i";
            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 set of test data
            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, "$test_dir/$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 = ('-d',
                    '-R',$results_dir,
                    '-D',$test_dir,
                    '-r',$ba_report_level,
                    '-f',$test_cfg_file);
                my $ba_type_arg;
                if ($ba_type =~ /^(sparse_(huber|cauchy))_([\d\.]+)$/) {
                    $ba_type_arg = $1;
                    push @ba_test_opts, ("--$2-param",$3);
                    push @ba_test_opts, ("--control", 1);
                } elsif ($ba_type =~ /^([\w]+)_control$/) {
                    $ba_type_arg = $1;
                    push @ba_test_opts, ("--control", 1);
                } else {
                    $ba_type_arg = $ba_type;
                }
                # add option for outlier removal if required (defaults to
                # removing outliers beyond 2 standard deviations)
                if ($ba_type =~ /no_outliers$/) {
                    push(@ba_test_opts, '-M');
                    $ba_type_arg =~ s/_no_outliers$//;
                }
                push(@ba_test_opts,'-b',$ba_type_arg);
                my @ba_test = ($ba_test, @ba_test_opts, @camera_files);
                print STDERR join(" ", @ba_test), "\n" if $verbose;
                system(@ba_test) == 0 or die "@ba_test failed: $!";
            }
        }
    }
}

sub read_results {
    my ($num_test_plans) = @_;

    print STDERR "Reading results\n" if $verbose;
    my @results = ();
    for (my $t = 0; $t < $num_test_plans; $t++) {
        print STDERR "  Test plan $t\n" if $verbose;
        my $plan_dir = $tests_root."/".$data_dir_prefix.$t;
        push @results, {};
        foreach my $ba_type (@ba_types) {
            print STDERR "    $ba_type\n" if $verbose;
            my @wp_initial_mee = ();
            my @wp_final_mee   = ();
            my @xyz_mee        = ();
            my @euler_mse      = ();

            for (my $i = 0; $i < $num_tests; $i++) {
                print STDERR "      Test $i\n" if $verbose;
                my $test_dir = "$plan_dir/$i";
                my $results_dir = "$test_dir/$ba_type";
                my $wp_true  = read_wp_results("$test_dir/$wp_true_file");
                print STDERR "        Read ".scalar(@$wp_true)." ground truth values from $test_dir/$wp_true_file\n" if $verbose;
                my $wp_init  = read_wp_results("$results_dir/$wp_init_file");
                my $wp_final = read_wp_results("$results_dir/$wp_final_file");
                print STDERR "        Read ".scalar(@$wp_final)." final values from $results_dir/$wp_final_file\n" if $verbose;

                # If we removed outliers, we have to delete the corresponding
                # world points from the world point files.
                if ($ba_type =~ /_no_outliers/) {
                  print STDERR "        Reading deleted indices from $results_dir/$cnet_deleted_index_file\n" if $verbose;
                  open CNET_DELETED, "<$results_dir/$cnet_deleted_index_file"
                    or die "Failed opening $results_dir/$cnet_deleted_index_file: $!\n";
                  my $deleted = <CNET_DELETED>;
                  chomp $deleted;
                  my @deleted = split / /, $deleted;
                  close CNET_DELETED;
                  if (3*scalar(@deleted) != scalar(@$wp_true)) {
                    die "Different number of points from cnet_deleted (".scalar(@deleted).
                        ") and wp_ground_truth (".scalar(@$wp_true)/3 . ")\n";
                  }
                  print STDERR "        Delete array has ".scalar(@deleted)." points\n" if $verbose;
                  my $valid_cnt = 0;
                  my $wp_init_del = [];
                  my $wp_true_del = [];
                  for (my $i = 0; $i <= $#deleted; $i++) {
                    if (not $deleted[$i]) {
                      push @$wp_init_del, @$wp_init[$i*3 .. ($i*3)+2];
                      push @$wp_true_del, @$wp_true[$i*3 .. ($i*3)+2];
                      $valid_cnt++
                    }
                  }
                  $wp_init = $wp_init_del;
                  $wp_true = $wp_true_del;
                  print STDERR "        Now have ".(scalar(@$wp_true)/3)." ground truth points\n" if $verbose;
                  open TEMP, ">$test_dir/wp_ground_truth_del.txt";
                  my $i = 0;
                  while ($i < scalar(@$wp_true)) {
                    print TEMP join(",",$wp_true->[$i++],$wp_true->[$i++],$wp_true->[$i++]),"\n";
                  }
                  close TEMP;
                }

                my ($xyz_true, $euler_true)   = read_cam_results("$test_dir/$cam_true_file");
                my ($xyz_init, $euler_init)   = read_cam_results("$results_dir/$cam_init_file");
                my ($xyz_final, $euler_final) = read_cam_results("$results_dir/$cam_final_file");
                print STDERR "        Calculating Errors\n" if $verbose;
                my $wp_initial_mee = median_euclidean_error($wp_init, $wp_true);
                my $wp_final_mee   = median_euclidean_error($wp_final, $wp_true);
                push @wp_initial_mee, $wp_initial_mee;
                push @wp_final_mee,   $wp_final_mee;
                push @xyz_mee,        median_euclidean_error($xyz_final, $xyz_true);
                push @euler_mse,      mse($euler_final, $euler_true);
            }
            $results[$t]{$ba_type} = {wp_initial => \@wp_initial_mee,
                                      wp_final   => \@wp_final_mee,
                                      xyz        => \@xyz_mee,
                                      euler      => \@euler_mse};
        }
    }
    return \@results;
}

sub print_raw_results {
    my ($results) = @_;
    my $num_plans = scalar @$results;

    my @headers = ();
    my @ba_types = keys %{$results->[0]};
    my @data_types = keys %{$results->[0]{$ba_types[0]}};
    foreach my $ba_type (@ba_types) {
        foreach my $data_type (@data_types) {
            push (@headers, $ba_type."_".$data_type);
        }
    }

    for (my $plan = 0; $plan < $num_plans; $plan++) {
        open OUT, ">$tests_root/plan${plan}_raw.txt"
            or die "Error: Could not open raw results file: $!\n";
        print OUT join("\t",@headers),"\n";
        my @values = ();
        for (my $test = 0; $test < $num_tests; $test++) {
            foreach my $ba_type (@ba_types) {
                foreach my $data_type (@data_types) {
                    my $val = $results->[$plan]{$ba_type}{$data_type}[$test];
                    push(@values, $val);
                }
            }
            print OUT join("\t",@values),"\n";
            @values = ();
        }
        close OUT;
    }
}


sub print_result_stats {
    my ($results) = @_;
    my $num_test_plans = scalar @$results;
    my $stat = Statistics::Descriptive::Full->new();

    open(RESULTS, ">$tests_root/results.txt") or die "Could not open results file: $!\n";

    my @headers = ('test_plan','num_tests');
    my @ba_types = keys %{$results->[0]};
    my @data_types = qw(wp_initial wp_final xyz euler);
    my @stats = ("mean", "stddev",
                 $percentile_low."pct",
                 $percentile_high."pct");
    foreach my $ba_type (@ba_types) {
        foreach my $data_type (@data_types) {
            foreach my $stat (@stats) {
                push (@headers, $ba_type."_".$data_type."_".$stat);
            }
        }
    }
    print RESULTS join("\t",@headers), "\n";
    for (my $t = 0; $t < $num_test_plans; $t++) {
        my @values = ($t, $num_tests);
        foreach my $ba_type (@ba_types) {
            foreach my $data_type (@data_types) {
                my @data = @{$results->[$t]{$ba_type}{$data_type}};
                $stat->add_data(@data);
                my $mean = $stat->mean();
                my $sd = $stat->standard_deviation();
                my $pct_low = $stat->percentile($percentile_low);
                my $pct_high = $stat->percentile($percentile_high);
                my @new_vals = ($mean,$sd,$pct_low,$pct_high);
                push (@values, @new_vals);
                $stat->clear();
            }
        }
        print RESULTS join("\t",@values),"\n";
    }
}

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

my $help = 0;
my $man = 0;
my $ba_type_str = '';
my $huber_param_str = '';
my $cauchy_param_str = '';
GetOptions('help|?'        => \$help,
           'man'           => \$man,
           'verbose|v'     => \$verbose,
           'directory|d=s' => \$tests_root,
           'num_tests|n=i' => \$num_tests,
           'pct_low|l=i'   => \$percentile_low,
           'pct_high|h=i'  => \$percentile_high,
           'bundle_adjustment_types|b=s' => \$ba_type_str,
           'huber_param=s' => \$huber_param_str,
           'cauchy_params=s' => \$cauchy_param_str);

pod2usage(-verbose => 99, -sections => "USAGE") if $help;
pod2usage(-verbose => 2) if $man;

if ($ba_type_str ne '') {
    $ba_type_str =~ s/ //;
    @ba_types = split(",",$ba_type_str);
}

if ($huber_param_str ne '') {
    $huber_param_str =~ s/ //;
    @huber_params = split(",",$huber_param_str);
}

if ($cauchy_param_str ne '') {
    $cauchy_param_str =~ s/ //;
    @cauchy_params = split(",",$cauchy_param_str);
}

my @new_ba_types = ();
foreach my $t (@ba_types) {
    if ($t eq 'sparse_huber' and scalar(@huber_params) > 0) {
        foreach my $p (@huber_params) {
            push @new_ba_types, "sparse_huber_$p";
        }
    } elsif ($t eq 'sparse_cauchy' and scalar(@cauchy_params) > 0) {
        foreach my $p (@cauchy_params) {
            push @new_ba_types, "sparse_cauchy_$p";
        }
    } else {
        push @new_ba_types, $t;
    }
}
@ba_types = @new_ba_types;

my $num_test_plans = 0;
my $test_plans = read_test_plans($ARGV[0], \$num_test_plans);

run_tests($num_test_plans, $test_plans, \%test_config);

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

print_raw_results($results);

## Calculate result statistics
print_result_stats($results);


__END__

=head1 NAME run_ba_tests - Bundle adjustment test harness

=head1 USAGE

run_ba_tests [options] <test plan file>

 Options:
   --help | -?        brief help message

   --man              full documentation

   --directory | -d   Root directory for tests

   --num_tests | -n   Number of tests to run for each test plan

   --pct_low | -l     Lower percentile to report in results

   --pct_high | -h    Upper percentile to report in results

   --bundle_adjustment_types | -b
                      Bundle adjustment types to run

=head1 SUMMARY

This program is part of the bundle adjustment test harness written in Summer
2009. The other components are: make_ba_test_data and ba_test.

run_ba_tests runs multiple tests of bundle adjustment as specified in the test
plan file. The test plan file should be a TAB-SEPARATED file, with the first
row specifying the names of the configuration values to set, and the remaining
rows specifying distinct test plans. The configuration values can be any of
those accepted by either make_ba_test_data. The single exception is
B<bundle-adjustment-type>, which is controlled by this program.

For each test plan, the program will run the number of tests specified on the
command line by --num_tests or -n. For each test, a new set of synthetic data
will be generated, and bundle adjustment will be run once on that set of data
for each of the specified bundle adjustment implementation types.

=head1 DATA AND RESULTS

=head2 DATA

Data files and results are stored in subdirectories of the specified root
directory, as outlined below, where ./data is the root directory.

=over 4

=item B<./data>

Contains final results file (results.txt) and raw MSE files
for each test plan.

=item B<./data/test_plan#>

One directory for each test plan. Contains the configuration file
generated for the test plan, and the test directories.

=item B<./data/test_plan#/#>

One directory for each test. Contains the synthetic data generated for each
test by make_ba_test_data, and one results directory for each bundle
adjustment type.

=item B<./data/test_plan#/#/[type]>

One directory for each bundle adjustment type. Contains the initial and final
parameter files generated by ba_test.

=back

=head2 RESULTS

The primary statistic generated is the mean squared error for each bundle
adjustment type, parameter type and test plan. This is calculated for each test by
subtracting the final value of each calculated parameter from the ground truth
value, squaring the result, and taking the average. This gives a MSE for each
test.

The final result file reports the average MSE over all tests for each test
plan, each bundle adjustment type, and each parameter type, along with the
standard deviation and the low and high percentile values.

=head1 OPTIONS

=over 8

=item B<--directory | -d>

I<Default:> B<.>

Set the root directory for this test plan. All synthetic data and test
results will be created in subdirectories of the specified root directory.

=item B<--num_tests | -n>

I<Default:> B<1>

Set the number of tests to run for each test plan. Note that for each test,
the harness will run the bundle adjustment types specified in the
corresponding configuration variable in this program.

=item B<--pct_low | -l>

I<Default:> B<25>

=item B<--pct_high | -h>

I<Default:> B<75>

Set the low and high percentile ranks to return in final result statistics.
NB: These should be set bearing in mind the number of tests you are running.
If you run 5 tests and set pct_low=5 and pct_high=95, you probably will not
get any results for these statistics.

=item B<--bundle_adjustment_types | -b>

I<Default:> B<sparse, robust_sparse, sparse_no_outliers>

Set the bundle adjustment types to run for each test. This should be a
comma-separated list. The base types accepted are: I<ref>, I<sparse>,
I<robust_ref>, and I<robust_sparse>. Each of these can have I<_no_outliers>
appended to it to run the outlier removal heuristic in ba_test. Outlier
removal will be done using the default value of 2 standard deviations.

=back

=cut