[37] | 1 | #!/usr/bin/perl |
---|
| 2 | |
---|
| 3 | ################################################################################# |
---|
| 4 | ## |
---|
| 5 | ## Perl script for computing the reprojection error corresponding to a |
---|
| 6 | ## given reconstruction. Currently, projective and quaternion-based euclidean |
---|
| 7 | ## reconstructions are supported. More reconstruction types can be added by |
---|
| 8 | ## supplying appropriate camera matrix generation routines (i.e. CamMat_Generate) |
---|
| 9 | ## Copyright (C) 2005 Manolis Lourakis (lourakis@ics.forth.gr) |
---|
| 10 | ## Institute of Computer Science, Foundation for Research & Technology - Hellas |
---|
| 11 | ## Heraklion, Crete, Greece. |
---|
| 12 | ## |
---|
| 13 | ## This program is free software; you can redistribute it and/or modify |
---|
| 14 | ## it under the terms of the GNU General Public License as published by |
---|
| 15 | ## the Free Software Foundation; either version 2 of the License, or |
---|
| 16 | ## (at your option) any later version. |
---|
| 17 | ## |
---|
| 18 | ## This program is distributed in the hope that it will be useful, |
---|
| 19 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
| 20 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
| 21 | ## GNU General Public License for more details. |
---|
| 22 | ## |
---|
| 23 | ################################################################################# |
---|
| 24 | |
---|
| 25 | ################################################################################# |
---|
| 26 | # Initializations |
---|
| 27 | |
---|
| 28 | $usage="Usage is $0 -e|-p [-s,-h] <cams file> <pts file> [<calib file>]"; |
---|
| 29 | $help="-e specifies a quaternion-based Euclidean reconstruction and -p a projective one.\n" |
---|
| 30 | ."-s computes the average *squared* reprojection error."; |
---|
| 31 | use constant EUCBA => 0; |
---|
| 32 | use constant PROJBA => 1; |
---|
| 33 | $cnp=$pnp=0; |
---|
| 34 | $camsfile=$ptsfile=$calfile=""; |
---|
| 35 | $CamMat_Generate=\&dont_know; |
---|
| 36 | |
---|
| 37 | ################################################################################# |
---|
| 38 | # Basic arguments parsing |
---|
| 39 | |
---|
| 40 | use Getopt::Std; |
---|
| 41 | getopts("epsh", \%opt) or die "$usage\n"; |
---|
| 42 | die "$0 help: Compute the average reprojection error for some reconstruction.\n$usage\n$help\n" if($opt{h}); |
---|
| 43 | |
---|
| 44 | if($opt{e} && $opt{p}){ |
---|
| 45 | die "$0: Only one of -e, -p can be specified!\n"; |
---|
| 46 | } elsif($opt{e}){ |
---|
| 47 | $batype=EUCBA; |
---|
| 48 | } elsif($opt{p}){ |
---|
| 49 | $batype=PROJBA; |
---|
| 50 | } |
---|
| 51 | $squared=$opt{s}? 1 : 0; |
---|
| 52 | print "@squared\n\n"; #Min added |
---|
| 53 | ################################################################################# |
---|
| 54 | # Initializations depending on reconstruction type |
---|
| 55 | if($batype==EUCBA){ |
---|
| 56 | $cnp=7; $pnp=3; |
---|
| 57 | die "$0: Cameras, points, or calibration file is missing!\n$usage" if(@ARGV<3); |
---|
| 58 | die "$0: Too many arguments!\n$usage" if(@ARGV>3); |
---|
| 59 | $camsfile=$ARGV[0]; |
---|
| 60 | $ptsfile=$ARGV[1]; |
---|
| 61 | $calfile=$ARGV[2]; |
---|
| 62 | $CamMat_Generate=\&PfromKRt; |
---|
| 63 | } |
---|
| 64 | elsif($batype==PROJBA){ |
---|
| 65 | $cnp=12; $pnp=4; |
---|
| 66 | die "$0: Cameras or points file is missing!\n$usage" if(@ARGV<2); |
---|
| 67 | die "$0: Too many arguments!\n$usage" if(@ARGV>2); |
---|
| 68 | $camsfile=$ARGV[0]; |
---|
| 69 | $ptsfile=$ARGV[1]; |
---|
| 70 | $CamMat_Generate=\&nop; |
---|
| 71 | } |
---|
| 72 | else{ |
---|
| 73 | die "Unknown BA type \"$batype\" specified!\n"; |
---|
| 74 | } |
---|
| 75 | |
---|
| 76 | die "$0: Do not know how to handle $pnp parameters per point!\n" if($pnp!=3 && $pnp!=4); |
---|
| 77 | |
---|
| 78 | |
---|
| 79 | ################################################################################# |
---|
| 80 | # Main code for computing the reprojection error. |
---|
| 81 | |
---|
| 82 | # NOTE: all 2D arrays are stored in row-major order as vectors |
---|
| 83 | @camPoses=(); # array of arrays storing each camera's pose; each element is of size $cnp |
---|
| 84 | |
---|
| 85 | @threeDpts=(); # array of arrays storing the reconstructed 3D points; each element is of size $pnp |
---|
| 86 | @twoDtrajs=(); # array of hashes storing the 2D trajectory correponding to reconstructed 3D points. |
---|
| 87 | # The hash key is the frame number |
---|
| 88 | @trajsFrames=(); # array of arrays storing the frame numbers corresponding to each trajectory. |
---|
| 89 | # The first number is the total number of frames, then follow the individual frame |
---|
| 90 | # numbers: [nframes, fr_i, fr_j, ..., fr_k] |
---|
| 91 | @camCal=(); # 3x3 array for storing the camera intrinsic calibration |
---|
| 92 | |
---|
| 93 | |
---|
| 94 | # read calibration file, if there is one |
---|
| 95 | if(length($calfile)>0){ |
---|
| 96 | if(not open(CAL, $calfile)){ |
---|
| 97 | print STDERR "cannot open file $calfile: $!\n"; |
---|
| 98 | exit(1); |
---|
| 99 | } |
---|
| 100 | for($i=0; $i<3; $i++){ |
---|
| 101 | $line=<CAL>; |
---|
| 102 | if($line=~/\r\n$/){ # CR+LF |
---|
| 103 | chop($line); chop($line); |
---|
| 104 | } |
---|
| 105 | else{ |
---|
| 106 | chomp($line); |
---|
| 107 | } |
---|
| 108 | @columns=split(" ", $line); |
---|
| 109 | die "line \"$line\" in $calfile does not contain exactly 3 numbers [$#columns+1]!\n" if($#columns+1!=3); |
---|
| 110 | $camCal[$i*3]=$columns[0]; $camCal[$i*3+1]=$columns[1]; $camCal[$i*3+2]=$columns[2]; |
---|
| 111 | } |
---|
| 112 | close(CAL); |
---|
| 113 | } |
---|
| 114 | |
---|
| 115 | # read cameras file |
---|
| 116 | if(not open(CAMS, $camsfile)){ |
---|
| 117 | print STDERR "cannot open file $camsfile: $!\n"; |
---|
| 118 | exit(1); |
---|
| 119 | } |
---|
| 120 | $ncams=0; |
---|
| 121 | while($line=<CAMS>){ |
---|
| 122 | if($line=~/\r\n$/){ # CR+LF |
---|
| 123 | chop($line); chop($line); |
---|
| 124 | } |
---|
| 125 | else{ |
---|
| 126 | chomp($line); |
---|
| 127 | } |
---|
| 128 | |
---|
| 129 | next if($line=~/^#.+/); # skip comments |
---|
| 130 | |
---|
| 131 | @columns=split(" ", $line); |
---|
| 132 | #next if($#columns==-1); # skip empty lines |
---|
| 133 | |
---|
| 134 | die "line \"$line\" in $camsfile does not contain exactly $cnp numbers [$#columns+1]!\n" if($cnp!=$#columns+1); |
---|
| 135 | @pose=(); |
---|
| 136 | for($i=0; $i<$cnp; $i++){ |
---|
| 137 | $pose[$i]=$columns[$i]; |
---|
| 138 | } |
---|
| 139 | $camPoses[$ncams]=$CamMat_Generate->($ncams, [@pose], [@camCal]); |
---|
| 140 | $ncams++; |
---|
| 141 | } |
---|
| 142 | close(CAMS); |
---|
| 143 | |
---|
| 144 | printf "Read %d cameras\n", scalar(@camPoses); |
---|
| 145 | |
---|
| 146 | # read points file |
---|
| 147 | if(not open(PTS, $ptsfile)){ |
---|
| 148 | print STDERR "cannot open file $ptsfile: $!\n"; |
---|
| 149 | exit(1); |
---|
| 150 | } |
---|
| 151 | |
---|
| 152 | $npts=0; |
---|
| 153 | $trajno=0; |
---|
| 154 | while($line=<PTS>){ |
---|
| 155 | $npts++; |
---|
| 156 | if($line=~/\r\n$/){ # CR+LF |
---|
| 157 | chop($line); chop($line); |
---|
| 158 | } |
---|
| 159 | else{ |
---|
| 160 | chomp($line); |
---|
| 161 | } |
---|
| 162 | |
---|
| 163 | next if($line=~/^#.+/); # skip comments |
---|
| 164 | @columns=split(" ", $line); |
---|
| 165 | |
---|
| 166 | die "line \"$line\" in $ptsfile contains less than $pnp numbers [$#columns+1]!\n" if($pnp>$#columns+1); |
---|
| 167 | @recpt=(); |
---|
| 168 | for($i=0; $i<$pnp; $i++){ |
---|
| 169 | $recpt[$i]=$columns[$i]; |
---|
| 170 | } |
---|
| 171 | |
---|
| 172 | $nframes=$columns[$pnp]; |
---|
| 173 | $i=$pnp+1+$nframes*3; # 3 numbers per image projection: (i.e. imgid, x, y) |
---|
| 174 | if($i!=$#columns+1){ |
---|
| 175 | die "line \"$line\" in $ptsfile does not contain exactly the $i numbers required for a 3D point with $nframes 2D projections [$#columns+1]!\n"; |
---|
| 176 | } |
---|
| 177 | |
---|
| 178 | %traj=(); |
---|
| 179 | @theframes=($nframes); |
---|
| 180 | for($i=0, $j=$pnp+1; $i<$nframes; $i++, $j+=3){ |
---|
| 181 | $traj{$columns[$j]}=[$columns[$j+1], $columns[$j+2]]; |
---|
| 182 | push @theframes, $columns[$j]; |
---|
| 183 | |
---|
| 184 | # printf "%d: %d %.6g %.6g\n", $j, $columns[$j], $columns[$j+1], $columns[$j+2]; |
---|
| 185 | } |
---|
| 186 | $threeDpts[$trajno]=[@recpt]; |
---|
| 187 | $twoDtrajs[$trajno]={%traj}; |
---|
| 188 | $trajsFrames[$trajno++]=[@theframes]; |
---|
| 189 | } |
---|
| 190 | close(PTS); |
---|
| 191 | |
---|
| 192 | printf "Read %d 3D points \& trajectories\n", scalar(@threeDpts); |
---|
| 193 | |
---|
| 194 | # Data file has now been read. Following fragment shows how it can be printed |
---|
| 195 | if(0){ |
---|
| 196 | for($i=0; $i<scalar(@threeDpts); $i++){ |
---|
| 197 | for($j=0; $j<$pnp; $j++){ |
---|
| 198 | printf "%.6g ", $threeDpts[$i][$j]; |
---|
| 199 | } |
---|
| 200 | |
---|
| 201 | printf "%d ", $trajsFrames[$i][0]; |
---|
| 202 | for($j=0; $j<$trajsFrames[$i][0]; $j++){ |
---|
| 203 | $fr=$trajsFrames[$i][$j+1]; |
---|
| 204 | if(defined($twoDtrajs[$i]{$fr})){ |
---|
| 205 | printf "%d %.6g %.6g ", $fr, $twoDtrajs[$i]{$fr}[0], $twoDtrajs[$i]{$fr}[1]; |
---|
| 206 | } |
---|
| 207 | } |
---|
| 208 | print "\n"; |
---|
| 209 | } |
---|
| 210 | } |
---|
| 211 | |
---|
| 212 | # compute reprojection error |
---|
| 213 | unless ($batype==EUCBA || $batype==PROJBA){ |
---|
| 214 | die "current implementation of reprError() cannot handle supplied reconstruction data!\n"; |
---|
| 215 | } |
---|
| 216 | |
---|
| 217 | $toterr=0.0; |
---|
| 218 | $totprojs=0.0; |
---|
| 219 | @error=(); |
---|
| 220 | for($fr=0; $fr<$ncams; $fr++){ |
---|
| 221 | $error[$fr]=0.0; |
---|
| 222 | for($i=$j=0; $i<scalar(@threeDpts); $i++){ |
---|
| 223 | if(defined($twoDtrajs[$i]{$fr})){ |
---|
| 224 | $theerr=&reprError($twoDtrajs[$i]{$fr}, @camPoses[$fr], @threeDpts[$i], $pnp); |
---|
| 225 | $theerr=sqrt($theerr) if(!$squared); |
---|
| 226 | $error[$fr]+=$theerr; |
---|
| 227 | # printf "@@@ point %d, camera %d: %g\n", $i, $fr, $theerr; |
---|
| 228 | $j++; |
---|
| 229 | } |
---|
| 230 | } |
---|
| 231 | printf "Mean error for camera %d [%d projections] is %g\n", $fr, $j, $error[$fr]/$j; |
---|
| 232 | $toterr+=$error[$fr]; |
---|
| 233 | $totprojs+=$j; |
---|
| 234 | } |
---|
| 235 | printf "\nMean error for the whole sequence [%d projections] is %g\n", $totprojs, $toterr/$totprojs; |
---|
| 236 | |
---|
| 237 | |
---|
| 238 | |
---|
| 239 | ################################################################################# |
---|
| 240 | # Misc routines |
---|
| 241 | |
---|
| 242 | # compute the SQUARED reprojection error |x-xx|^2 with xx=P*X |
---|
| 243 | sub reprError{ |
---|
| 244 | |
---|
| 245 | my ($x, $P, $X, $pnp)=@_; |
---|
| 246 | |
---|
| 247 | # error checking |
---|
| 248 | unless (@_==4 && ref($x) eq 'ARRAY' && ref($P) eq 'ARRAY' && ref($X) eq 'ARRAY'){ |
---|
| 249 | die "usage: reprError ARRAYREF1 ARRAYREF2 ARRAYREF3 pnp"; |
---|
| 250 | } |
---|
| 251 | |
---|
| 252 | my @xx=(); |
---|
| 253 | my $k; |
---|
| 254 | |
---|
| 255 | # compute the projection in xx |
---|
| 256 | for($k=0; $k<3; $k++){ |
---|
| 257 | $xx[$k]=$P->[$k*4]*$X->[0] + $P->[$k*4+1]*$X->[1] + $P->[$k*4+2]*$X->[2] + $P->[$k*4+3]*(($pnp==4)? $X->[3] : 1.0); |
---|
| 258 | } |
---|
| 259 | $xx[0]/=$xx[2]; |
---|
| 260 | $xx[1]/=$xx[2]; |
---|
| 261 | |
---|
| 262 | # printf "[%g %g -- %g %g] ", $x->[0], $x->[1], $xx[0], $xx[1]; |
---|
| 263 | |
---|
| 264 | return ($x->[0]-$xx[0])*($x->[0]-$xx[0]) + ($x->[1]-$xx[1])*($x->[1]-$xx[1]); |
---|
| 265 | } |
---|
| 266 | |
---|
| 267 | ################################################################################# |
---|
| 268 | # Camera matrix generation routines |
---|
| 269 | |
---|
| 270 | sub dont_know { |
---|
| 271 | my ($camid, $camparms)=@_; |
---|
| 272 | |
---|
| 273 | die "Don't know how to generate a projection matrix for camera $camid from the supplied camera parameters!\n"; |
---|
| 274 | return $camparms; |
---|
| 275 | } |
---|
| 276 | |
---|
| 277 | # Return as is |
---|
| 278 | sub nop { |
---|
| 279 | my ($camid, $camparms)=@_; |
---|
| 280 | |
---|
| 281 | return $camparms; |
---|
| 282 | } |
---|
| 283 | |
---|
| 284 | # Compute P as K[R|t]. R is specified by the first 4 elements of $camparms, while t corresponds to the last 3 ones |
---|
| 285 | sub PfromKRt { |
---|
| 286 | my ($camid, $camparms, $calparams)=@_; |
---|
| 287 | |
---|
| 288 | my $x, $y, $z, $w, $xx, $xy, $xz, $xw, $yy, $yz, $yw, $zz, $zw, $ww, $i, $j, $k; |
---|
| 289 | my @R=(), @P=(); # 3x3 & 3x4 resp. |
---|
| 290 | |
---|
| 291 | # compute the rotation matrix for q=(x, y, z, w); |
---|
| 292 | # see also http://www.gamedev.net/reference/articles/article1095.asp (but note that q=(w, x, y, z) there!) |
---|
| 293 | |
---|
| 294 | $x=$camparms->[0]; $y=$camparms->[1]; |
---|
| 295 | $z=$camparms->[2]; $w=$camparms->[3]; |
---|
| 296 | $xx=$x*$x; $xy=$x*$y; $xz=$x*$z; $xw=$x*$w; |
---|
| 297 | $yy=$y*$y; $yz=$y*$z; $yw=$y*$w; |
---|
| 298 | $zz=$z*$z; $zw=$z*$w; $ww=$w*$w; |
---|
| 299 | $R[0]=$xx+$yy - ($zz+$ww); $R[1]=2.0*($yz-$xw); $R[2]=2.0*($yw+$xz); |
---|
| 300 | $R[3]=2.0*($yz+$xw); $R[4]=$xx+$zz - ($yy+$ww); $R[5]=2.0*($zw-$xy); |
---|
| 301 | $R[6]=2.0*($yw-$xz); $R[7]=2.0*($zw+$xy); $R[8]=$xx+$ww - ($yy+$zz); |
---|
| 302 | |
---|
| 303 | print "@R\n\n"; #Min modified |
---|
| 304 | # compute the matrix-matrix & matrix-vector products |
---|
| 305 | for($i=0; $i<3; $i++){ |
---|
| 306 | for($j=0; $j<3; $j++){ |
---|
| 307 | for($k=0, $sum=0.0; $k<3; $k++){ |
---|
| 308 | $sum+=$calparams->[$i*3+$k]*$R[$k*3+$j]; |
---|
| 309 | } |
---|
| 310 | $P[$i*4+$j]=$sum; |
---|
| 311 | } |
---|
| 312 | for($j=0, $sum=0.0; $j<3; $j++){ |
---|
| 313 | $sum+=$calparams->[$i*3+$j]*$camparms->[4+$j]; |
---|
| 314 | } |
---|
| 315 | $P[$i*4+3]=$sum; |
---|
| 316 | } |
---|
| 317 | |
---|
| 318 | return [@P]; |
---|
| 319 | } |
---|