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