Debian Codegraph @

analyses/mrcal-pick-features

raw
1 #!/usr/bin/python3
2
3 r'''Facilitate interactive human-in-the-loop corresponding-feature picking
4
5 And use features to solve for extrinsics in a stereo pair. This needs a clearer,
6 more general purpose. Needs to be documented, and made to work reliably and
7 tested
8
9
10 talk about extrinsics. I leave cam0 where it is, and move cam1.
11
12 default geometry selectable. baseline stable or fixed at 1.0?
13
14 We always leave the extrinsics of camera0, and move camera1 instead. By default,
15 we move camera1 as dictated by the solve, leaving the baseline the same if the
16 solve is unique up-to-scale only.
17
18 '''
19
20 import sys
21 import argparse
22 import re
23 import os
24
25 def parse_args():
26
27 def positive_int(string):
28 try:
29 value = int(string)
30 except:
31 raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
32 if value <= 0 or abs(value-float(string)) > 1e-6:
33 raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
34 return value
35
36
37 parser = \
38 argparse.ArgumentParser(description = __doc__,
39 formatter_class=argparse.RawDescriptionHelpFormatter)
40
41 parser.add_argument('--valid-intrinsics-region',
42 action='store_true',
43 help='''If given, annotate the image with its
44 valid-intrinsics region''')
45 parser.add_argument('--range-estimate',
46 type=float,
47 default=10.,
48 help='''Initial guess for the range of all picked
49 points. Defaults to 10m''')
50 parser.add_argument('--template-size',
51 type=positive_int,
52 nargs=2,
53 default = (13,13),
54 help='''The size of the template used for feature
55 matching, in pixel coordinates of the second image. Two
56 arguments are required: width height. This is passed
57 directly to mrcal.match_feature(). We default to
58 13x13''')
59 parser.add_argument('--search-radius',
60 type=positive_int,
61 default = 20,
62 help='''How far the feature-matching routine should
63 search, in pixel coordinates of the second image. This
64 should be larger if the nominal range estimate is poor,
65 especially, at near ranges. This is passed directly to
66 mrcal.match_feature(). We default to 20 pixels''')
67 parser.add_argument('--initial-correspondences', '--correspondences',
68 help='''Start with the given correspondences vnl. If
69 omitted, we don't start with any features at all. The
70 given vnl must have the columns: x0 y0 x1 y1. Exclusive
71 with --initial-features''')
72 parser.add_argument('--initial-features',
73 help='''Start with the given features vnl. If omitted,
74 we don't start with any features at all. At startup we
75 try to find matches in image1 for each of these
76 features; any failed matches are discarded. The given
77 vnl must have the columns: x0 y0. Exclusive with
78 --initial-correspondences''')
79 parser.add_argument('--rt01',
80 help='''We always leave the extrinsics of camera0, and
81 move camera1, as dictated by the solve; we leave the
82 baseline the same if the solve is unique up-to-scale
83 only. By default we start with the geometry given by the
84 models. If --rt01 is given, we use this relative initial
85 geometry instead. This is a comma-separated list of 6
86 numbers''')
87 ######## image pre-filtering; same as in mrcal-stereo; please consolidate
88 parser.add_argument('--equalization',
89 choices=('clahe', 'fieldscale','stretch'),
90 help='''The equalization method to use for the input
91 images. "fieldscale" requires mrcam to be installed, and
92 can only operate on uint16 images''')
93
94 parser.add_argument('models',
95 type=str,
96 nargs = 2,
97 help='''Camera models representing cameras used to
98 capture the images. Intrinsics only are used. A nominal
99 stereo geometry with unit baseline is assumed. Given
100 models are the left, right cameras''')
101 parser.add_argument('images',
102 type=str,
103 nargs=2,
104 help='''The images to use for the matching''')
105
106 args = parser.parse_args()
107
108 if args.initial_correspondences is not None and \
109 args.initial_features is not None:
110 print("--initial-correspondences and --initial-features are mutually exclusive",
111 file=sys.stderr)
112 sys.exit(1)
113 return args
114
115 args = parse_args()
116
117 # arg-parsing is done before the imports so that --help works without building
118 # stuff, so that I can generate the manpages and README
119
120
121
122
123 from fltk import *
124 from Fl_Gl_Image_Widget import *
125
126 import numpy as np
127 import numpysane as nps
128 import mrcal
129 import pyopengv
130 import vnlog
131
132 if args.equalization == 'fieldscale':
133 try:
134 import mrcam
135 except:
136 print("ERROR: the 'fieldscale' equalization method requires mrcam, but it could not be imported", file=sys.stderr)
137 sys.exit(1)
138 if args.equalization == 'clahe':
139 import cv2
140
141
142 def get_q01_estimate(*,
143 q,
144 index):
145 i_from = index
146 i_to = 1-index
147
148
149 if index == 0: Rt_to_from = mrcal.invert_Rt(context['Rt01'])
150 else: Rt_to_from = context['Rt01']
151
152 v_from = mrcal.unproject(q, *models[i_from].intrinsics(),
153 normalize = True)
154 p_from = v_from * args.range_estimate
155 p_to = mrcal.transform_point_Rt(Rt_to_from, p_from)
156 q_to = mrcal.project(p_to, *models[i_to].intrinsics())
157
158 if index == 0:
159 return nps.cat(q, q_to)
160 else:
161 return nps.cat(q_to, q)
162
163
164 def solve_R01__kneip(v0,v1,
165 **cookie):
166 r'''Rotation from corresponding-feature observations: Kneip's eigesolver
167
168 Kneip describes a method to compute the rotation:
169
170 L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
171 Images Independently of the Translation", Proc. of The European Conference on
172 Computer Vision (ECCV), Florence, Italy. October 2012.
173
174 L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
175 The International Conference on Computer Vision (ICCV), Sydney, Australia.
176 December 2013.
177 '''
178 if cookie:
179 raise Exception("This function does not use the cookie")
180 return \
181 pyopengv.relative_pose_eigensolver(v0,v1,
182 # seed
183 mrcal.identity_R())
184
185
186 def solve_R01__assume_infinity(v0,v1,
187 **cookie):
188 r'''Rotation from corresponding-feature observations: assume everything is infinitely-far away
189
190 For testing or maybe seeding
191 '''
192
193 # if cookie:
194 # raise Exception("This function does not use the cookie")
195 return \
196 mrcal.align_procrustes_vectors_R01(v0,v1)
197
198
199 def solve_R01__optimize(v0,v1,
200 *,
201 # cookie
202 t01_fixed = None):
203 r'''Rotation from corresponding-feature observations: direct optimization
204
205 This is for experimenting. solve_R01__kneip() should work well.
206 '''
207 import scipy.optimize
208
209 def cost__coplanarity_normal_vectors(r01):
210 n = np.cross(v0,
211 mrcal.rotate_point_r(r01,v1))
212 l,v = mrcal.sorted_eig(nps.matmult(n.T,n))
213 return l[0]
214
215 def cost__triangulation_geometric(r01):
216
217 R01 = mrcal.R_from_r(r01)
218 t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,
219 R01 = R01)
220 return Epredicted
221
222 def cost__triangulation_have_t01(r01):
223
224 R01 = mrcal.R_from_r(r01)
225 p0 = \
226 mrcal.triangulate_leecivera_wmid2(v0, v1,
227 v_are_local = True,
228 Rt01 = nps.glue(R01,
229 t01_fixed,
230 axis = -2))
231
232 # Have triangulated points p0. Divergent or at-infinity triangulations
233 # are reported as p0=(0,0,0).
234 #
235 # I want to minimize sum(th_err). It's analogous to maximize
236 # sum(cos(th_err)) ~ sum(inner(v0, p0/mag(p0))). I minimize
237 # -sum(inner(...)) instead.
238 #
239 # For the divergent points I assume we're at infinity, and use inner(v0,
240 # R01 v1)/2. The /2 is because the convergent cost is from v0 to the
241 # midpoint, and I want to keep the same scaling for the divergent cost.
242 # This inner(v0,v1) logic will treat noise incorrectly (just before
243 # infinity I correctly only respond to yaw noise, but past infinity I
244 # would respond to all noise). This is close-enough for now.
245
246 mask = nps.norm2(p0) > 0
247
248 if np.any(mask):
249 cost_convergent = \
250 -np.sum( nps.inner(p0[mask]/nps.dummy(nps.mag(p0[mask]),-1), v0[mask]) )
251 else:
252 cost_convergent = 0
253
254 if np.any(~mask):
255 cost_divergent = \
256 -np.sum( nps.inner(v0[~mask], mrcal.rotate_point_R(R01, v1[~mask])) ) / 2
257 else:
258 cost_divergent = 0
259
260 return cost_convergent + cost_divergent
261
262 if t01_fixed is None:
263 res = \
264 scipy.optimize.minimize( cost__triangulation_geometric,
265 x0 = mrcal.identity_r() )
266 else:
267 res = \
268 scipy.optimize.minimize( cost__triangulation_have_t01,
269 x0 = mrcal.identity_r() )
270
271 R01 = mrcal.R_from_r(res.x)
272
273 return R01
274
275
276
277 # debugging stuff
278 if True:
279
280 t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,
281 R01 = R01)
282 # e = p - k0 v0
283 # E = sum(norm2(e_i))
284
285 Rt01 = nps.glue(R01, t01, axis = -2)
286 Rt01neg = nps.glue(R01, -t01, axis = -2)
287 p = mrcal.triangulate_geometric(v0, v1,
288 v_are_local = True,
289 Rt01 = Rt01 )
290 E = np.sum(nps.norm2(p - nps.dummy(nps.inner(p,v0),-1) * v0))
291 print(f"E_relerr={(E-Epredicted) / ((np.abs(E)+np.abs(Epredicted))/2.)} {E=} {Epredicted=}")
292
293 R01_in = context['Rt01'][:3,:]
294 R01_procrustes = mrcal.align_procrustes_vectors_R01(v0,v1)
295 R01_kneip = solve_R01__kneip(v0,v1)
296
297 r01_in = mrcal.r_from_R(R01_in)
298 r01_procrustes = mrcal.r_from_R(R01_procrustes)
299 r01_kneip = mrcal.r_from_R(R01_kneip)
300
301 if False:
302 print(f"{r01=} {r01_in=} {r01_procrustes=} {r01_kneip=}")
303
304 return R01
305
306
307 def t01_from_R01__geometric(v0,v1,R01):
308 r'''Translation from corresponding-feature observations: Dima's method
309
310 Given corresponding feature observations from two cameras we compute the
311 transform between the cameras. This is unique up-to-scale, so the reported
312 translation has length = 1.
313
314 This method assumes a rotation is already available. Given this rotation this
315 method computes the translation. This may or may not be any better than
316 solve_Rt01_given_R01__epipolar_plane_normality(). That needs evaluation.
317
318 The derivation of this method follows. I use the geometric triangulation
319 expression derived here:
320
321 https://github.com/dkogan/mrcal/blob/8be76fc28278f8396c0d3b07dcaada2928f1aae0/triangulation.cc#L112
322
323 I assume that I'm triangulating normalized v0,v1 both expressed in cam-0
324 coordinates. And I have a t01 translation that I call "t" from here on. This is
325 unique only up-to-scale, so I assume that norm2(t) = 1. The geometric
326 triangulation from the above link says that
327
328 [k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1 v0.v1][ t01.v0]
329 [k1] [ v0.v1 v0.v0][-t01.v1]
330
331 The midpoint p is
332
333 p = (k0 v0 + t01 + k1 v1)/2
334
335 I assume that v are normalized and I represent k as a vector. I also define
336
337 c = inner(v0,v1)
338
339 So
340
341 k = 1/(1 - c^2) [1 c] [ v0t] t
342 [c 1] [-v1t]
343
344 I define
345
346 A = 1/(1 - c^2) [1 c] This is a 2x2 array
347 [c 1]
348
349 B = [ v0t] This is a 2x3 array
350 [-v1t]
351
352 V = [v0 v1] This is a 3x2 array
353
354 Note that none of A,B,V depend on t.
355
356 So
357
358 k = A B t
359
360 Then
361
362 p = (k0 v0 + t01 + k1 v1)/2
363 = (V k + t)/2
364 = (V A B t + t)/2
365 = (I + V A B) t/2
366
367 Each triangulated error is
368
369 e = mag(p - k0 v0)
370
371 I split A into its rows
372
373 A = [ a0t ]
374 [ a1t ]
375
376 Then
377
378 e = p - k0 v0
379 = (I + V A B) t/2 - v0 a0t B t
380 = I t/2 + V A B t/2 - v0 a0t B t
381 = I t/2 + v0 a0t B t/2 + v1 a1t B t/2 - v0 a0t B t
382 = I t/2 - v0 a0t B t/2 + v1 a1t B t/2
383 = ((I - v0 a0t B + v1 a1t B) t) / 2
384 = ((I + (- v0 a0t + v1 a1t) B) t) / 2
385 = ((I - Bt A B) t) / 2
386
387 I define a joint error function I'm optimizing as the sum of all the individual
388 triangulation errors:
389
390 E = sum(norm2(e_i))
391
392 Each component is
393
394 norm2(e) = 1/4 tt (I - Bt A B)t (I - Bt A B) t
395 = 1/4 tt (I - 2 Bt A B + Bt A B Bt A B ) t
396
397 B Bt = [1 -c]
398 [-c 1]
399
400 B Bt A = 1/(1 - c^2) [1 -c] [1 c]
401 [-c 1] [c 1]
402 = 1/(1 - c^2) [1-c^2 0 ]
403 [0 1-c^2 ]
404 = I
405
406 -> norm2(e) = 1/4 tt (I - 2 Bt A B + Bt A B) t
407 = 1/4 tt (I - Bt A B) t
408 = 1/4 - 1/4 tt Bt A B t
409
410 So
411
412 E = N/4 - 1/4 tt sum(Bt A B) t
413
414 I let
415
416 M = sum(Bt A B)
417 M = sum(Mi)
418 Mi = Bt A B
419
420 So
421
422 E = N/4 - 1/4 tt M t
423 = N/4 - 1/4 lambda
424
425 So to minimize E I find t that is the eigenvector of M that corresponds to its
426 largest eigenvalue lambda. Furthermore, lambda depends on the rotation. If I
427 couldn't estimate the rotation from far-away features I can solve the
428 eigenvalue-optimization problem to maximize lambda.
429
430 More simplification:
431
432 Mi = Bt A B = [ v0 -v1 ] A [ v0t]
433 [-v1t]
434 = 1/(1-c^2) [ v0 - v1 c v0 c - v1] [ v0t]
435 [-v1t]
436 = 1/(1-c^2) ((v0 - v1 c) v0t - (v0 c - v1) v1t)
437
438 c = v0t v1 ->
439 F0 = v0 v0t
440 F1 = v1 v1t
441
442 -> Mi = 1/(1-c^2) (v0 v0t - v1 v1t v0 v0t + v1 v1t - v0 v0t v1 v1t)
443 = 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
444 = (F0 - F1)^2 / (1 - c^2)
445 = ((F0 - F1)/s)^2
446
447 where s = mag(cross(v0,v1))
448
449
450 tt M t = sum( norm2((F0i - F1i)/si t) )
451
452 Let Di = (F0i - F1i)/si
453
454 I want to maximize sum( norm2(Di t) )
455
456
457 (F0 - F1)/s = (v0 v0t - v1 v1t) / mag(cross(v0,v1))
458 ~ (v0 v0t - R v1 v1t Rt) / mag(cross(v0,R v1))
459
460 experiments:
461
462 = 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
463 = 1/(1-c^2) (v0 v0t + v1 v1t - (c v0 v1t + c v1 v0t))
464
465 '''
466 v1 = mrcal.rotate_point_R(R01,v1)
467
468 # shape (N,)
469 c = nps.inner(v0,v1)
470
471 N = len(c)
472
473 # shape (N,2,2)
474 A = np.ones((N,2,2), dtype=float)
475 A[:,0,1] = c
476 A[:,1,0] = c
477 A /= nps.mv(1. - c*c, -1,-3)
478
479 # shape (N,2,3)
480 B = np.empty((N,2,3), dtype=float)
481 B[:,0,:] = v0
482 B[:,1,:] = -v1
483
484 # shape (3,3)
485 M = np.sum( nps.matmult(nps.transpose(B), A, B), axis = 0 )
486
487 l,v = mrcal.sorted_eig(M)
488
489 # The answer is the eigenvector corresponding to the biggest eigenvalue
490 t01 = v[:,-1]
491
492 E = N/4 - l[-1]/4
493
494 return t01,E,A,B
495
496
497 def solve_Rt01_given_R01__geometric(v0,v1,R01):
498 r'''Estimate pose from rotation and feature observations
499
500 See the docstring for t01_from_R01__geometric()
501 '''
502 t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,R01)
503
504 # Almost done. I want either t or -t. The wrong one will produce
505 # mostly triangulations behind me
506 k = nps.matmult(A,B, nps.transpose(t01))[..., 0]
507
508 mask_divergent_t = (k[:,0] <= 0) + (k[:,1] <= 0)
509 mask_divergent_negt = (k[:,0] >= 0) + (k[:,1] >= 0)
510 N_divergent_t = np.count_nonzero( mask_divergent_t )
511 N_divergent_negt = np.count_nonzero( mask_divergent_negt )
512
513 if False:
514 # Epipolar plane normals. Should be planar, if R01 is right
515 n = np.cross(v0,mrcal.rotate_point_R(R01,v1))
516 l,vv = mrcal.sorted_eig(nps.matmult(n.T,n))
517 print(f"epipolar normal t,mine: {vv[:,0]=} {t01=}")
518
519 if N_divergent_t < N_divergent_negt:
520 return ( nps.glue(R01, t01, axis=-2),
521 mask_divergent_t,
522 N_divergent_t )
523 else:
524 return ( nps.glue(R01, -t01, axis=-2),
525 mask_divergent_negt,
526 N_divergent_negt )
527
528
529 def solve_Rt01_given_R01__epipolar_plane_normality(v0,v1,R01):
530 r'''Translation from corresponding-feature observations: Kneip's method
531
532 Given corresponding feature observations from two cameras we compute the
533 transform between the cameras. This is unique up-to-scale, so the reported
534 translation has length = 1.
535
536 This method assumes a rotation is already available (from Kneip's eigensolver,
537 for instance). Given this rotation this method computes the translation. This
538 may or may not be any better than solve_Rt01_given_R01__geometric(). That needs
539 evaluation.
540
541 Kneip describes a method to compute the ROTATION:
542
543 L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
544 Images Independently of the Translation", Proc. of The European Conference on
545 Computer Vision (ECCV), Florence, Italy. October 2012.
546
547 L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
548 The International Conference on Computer Vision (ICCV), Sydney, Australia.
549 December 2013.
550
551 The rotation is the hard part, but we do still need a translation. opengv should
552 do this for us, and I think its C++ API does, but its Python bindings are
553 lacking. So I compute t myself for now
554
555 '''
556
557 Rt01 = np.zeros((4,3), dtype=float)
558 Rt01[:3,:] = R01
559
560 # shape (N,3)
561 c = np.cross(v0, mrcal.rotate_point_R(R01, v1))
562 l,v = mrcal.sorted_eig(np.sum(nps.outer(c,c), axis=0))
563 # t is the eigenvector corresponding to the smallest eigenvalue
564 t01 = v[:,0]
565
566 Rt01[3,:] = t01
567
568 # Almost done. I want either t or -t. The wrong one will produce
569 # mostly triangulations behind me
570 p_t = mrcal.triangulate_geometric(v0, v1,
571 v_are_local = True,
572 Rt01 = Rt01 )
573 mask_divergent_t = (nps.norm2(p_t) == 0)
574 N_divergent_t = np.count_nonzero( mask_divergent_t )
575
576 Rt01_negt = Rt01 * nps.transpose(np.array((1,1,1,-1),))
577 p_negt = mrcal.triangulate_geometric(v0, v1,
578 v_are_local = True,
579 Rt01 = Rt01_negt )
580 mask_divergent_negt = (nps.norm2(p_negt) == 0)
581 N_divergent_negt = np.count_nonzero( mask_divergent_negt )
582
583 if N_divergent_t == 0 and N_divergent_negt == 0:
584 raise Exception("Cannot ambiguously determine t: neither has divergent observations")
585
586 # We have divergences even in the best case. I pick the fewer-divergence
587 # case, and report the diverging observations as outliers. All converging
588 # observations are treated as inlier here, regardless of reprojection error
589 if N_divergent_t < N_divergent_negt:
590 return (Rt01,
591 mask_divergent_t,
592 N_divergent_t)
593 else:
594 return (Rt01_negt,
595 mask_divergent_negt,
596 N_divergent_negt)
597
598
599 def solve_Rt01__baseline_noz(v0,v1,R01,
600 *,
601 t01_fixed = None,
602 baseline_want = None,
603 th0 = None,
604 dth = None,
605 Nth = None):
606 r'''Translation keeping a constant z offset, sampling the xy plane
607 '''
608 if t01_fixed is not None:
609 raise Exception("This function uses baseline_want, and NOT t01_fixed")
610 if baseline_want is None:
611 raise Exception("This function requires baseline_want")
612
613 # I do this discretely
614 if not ( th0 is not None \
615 and dth is not None \
616 and Nth is not None):
617 # default: look at the full 360 span
618 Nth = 1000
619 thdeg = np.linspace(0, 360.,
620 num = Nth,
621 endpoint = False)
622 else:
623 thdeg = np.linspace(th0 - dth,
624 th0 + dth,
625 num = Nth)
626 th = thdeg * np.pi/180.
627
628
629 c = np.cos(th)
630 s = np.sin(th)
631
632 Rt01 = np.zeros((Nth,4,3), dtype=float)
633 Rt01[:,:3,:] += R01
634 Rt01[:, 3,:] = nps.transpose( nps.cat(c, s, np.zeros(c.shape)) ) * baseline_want
635
636 # shape (Nth,Npoints,3)
637 p = mrcal.triangulate_geometric(v0, v1,
638 v_are_local = True,
639 Rt01 = nps.dummy(Rt01, -3) )
640
641 Npoints = p.shape[-2]
642
643 # shape (Nth,Npoints)
644 magp = nps.mag(p)
645 # shape (Nth,Npoints)
646 mask_divergent = (magp == 0)
647 # shape (Nth)
648 N_divergent = np.count_nonzero( mask_divergent,
649 axis = -1)
650
651 mean_thsq_err = np.zeros( (Nth,), dtype=float)
652 mean_thsq_err_accept_divergences = np.zeros( (Nth,), dtype=float)
653 for ith in range(Nth):
654 m = ~mask_divergent[ith]
655 if not np.any(m):
656 mean_thsq_err[ith] = 1e6
657 else:
658 # inner ~ cos ~ 1 - th^2/2
659 sum_cos = np.sum(nps.inner(p[ith][m], v0[m]) / magp[ith][m])
660 mean_cos = sum_cos / np.count_nonzero(m)
661
662 sum_cos_accept_divergences = sum_cos + np.count_nonzero(~m)
663 mean_cos_accept_divergences = sum_cos_accept_divergences/Npoints
664
665 mean_thsq_err[ith] = 2.*(1. - mean_cos)
666 mean_thsq_err_accept_divergences[ith] = 2.*(1. - mean_cos_accept_divergences)
667
668 # diagnostic code to visualize the scores and divergence counts
669 if False:
670 import gnuplotlib as gp
671 gp.plot( (th/np.pi*180., nps.cat(mean_thsq_err,
672 mean_thsq_err_accept_divergences), ),
673 (th/np.pi*180., N_divergent,
674 dict(y2=True) ),
675 ymax=1e-6)
676 import IPython
677 IPython.embed()
678 sys.exit()
679
680 ith = np.argmin(mean_thsq_err)
681 if False:
682 # be truthful about divergences
683 return (Rt01[ith],
684 mask_divergent[ith],
685 N_divergent[ith])
686 else:
687 # lie, and say that there were no divergences
688 return (Rt01[ith],
689 np.zeros( (Npoints,), dtype=bool),
690 0)
691
692
693 def solve_Rt10(# shape (N,Nleftright=2,Nxy=2)
694 q01,
695 intrinsics0,
696 intrinsics1,
697 *,
698 solve_R10 = solve_R01__kneip,
699 solve_Rt10_from_R01 = solve_Rt01_given_R01__geometric,
700 # cookie
701 t01_fixed = None,
702 baseline_want = None):
703 r'''Full-transform optimization
704
705 Given corresponding feature observations from two cameras we compute the
706 transform between the cameras. This is unique up-to-scale, so the reported
707 translation has length = 1.
708
709 The methods to compute the rotation and the translation are given in solve_R10
710 and solve_Rt10_from_R01.
711
712 '''
713
714 # shape (N,3)
715 # These are in their LOCAL coord system
716 v0 = mrcal.unproject(q01[...,0,:], *intrinsics0, normalize = True)
717 v1 = mrcal.unproject(q01[...,1,:], *intrinsics1, normalize = True)
718
719
720 # Keep all all points initially
721 mask_inliers = np.ones( (q01.shape[0],), dtype=bool )
722
723 while True:
724
725 if np.count_nonzero(mask_inliers) == 0:
726 raise Exception("All points thrown out as outliers")
727
728 R01 = solve_R10(v0[mask_inliers],
729 v1[mask_inliers],
730 t01_fixed = t01_fixed)
731
732 if t01_fixed is not None:
733 Rt01 = nps.glue(R01, t01_fixed, axis=-2)
734 return Rt01, mask_inliers
735
736 Rt01, mask_outlier, Noutliers = \
737 solve_Rt10_from_R01(v0[mask_inliers],
738 v1[mask_inliers],
739 R01 = R01,
740 t01_fixed = t01_fixed,
741 baseline_want = baseline_want)
742 if Noutliers == 0:
743 break
744
745 mask_inliers[np.nonzero(mask_inliers)[0][mask_outlier]] = False
746
747 # This hopefully was already enforced above
748 # if baseline_want is not None:
749 # baseline_have = nps.mag(Rt01[3,:])
750 # Rt01[3,:] *= baseline_want/baseline_have
751
752 return \
753 Rt01, \
754 mask_inliers
755
756 def update_p0_triangulated_stored():
757 global context
758
759 q0 = context['q01_stored'][...,0,:]
760 q1 = context['q01_stored'][...,1,:]
761 v0 = mrcal.unproject(q0, *models[0].intrinsics(), normalize = True)
762 v1 = mrcal.unproject(q1, *models[1].intrinsics(), normalize = True)
763 context['p0_triangulated_stored'] = \
764 mrcal.triangulate_geometric(v0, v1,
765 v_are_local = True,
766 Rt01 = context['Rt01'] )
767
768 def update_q01_stored(_q01_stored):
769 global context
770
771 context['q01_stored'] = _q01_stored
772 update_p0_triangulated_stored()
773 try:
774 widget_table.rows(len(context['q01_stored']))
775 except NameError:
776 # widget_table might not exist yet
777 pass
778
779
780 def line_segments_squares(# shape (..., 2)
781 q,
782 radii):
783
784 Nradii = len(radii)
785
786 # shape (..., Nradii, Nsegments_in_square=4, Npoints_in_line_segment=2, xy=2)
787 p = np.zeros(q.shape[:-1] + (Nradii,4,2,2), dtype=np.float32)
788 p += nps.dummy(q,-2,-2,-2)
789
790 rx = np.array((1,0), dtype=np.float32)
791 ry = np.array((0,1), dtype=np.float32)
792
793 for i,radius in enumerate(radii):
794
795 # line segment 0
796 p[..., i,0,0,:] += (-rx-ry)*radius
797 p[..., i,0,1,:] += (-rx+ry)*radius
798 # line segment 1
799 p[..., i,1,0,:] += (+rx-ry)*radius
800 p[..., i,1,1,:] += (+rx+ry)*radius
801 # line segment 2
802 p[..., i,2,0,:] += (-rx-ry)*radius
803 p[..., i,2,1,:] += (+rx-ry)*radius
804 # line segment 3
805 p[..., i,3,0,:] += (-rx+ry)*radius
806 p[..., i,3,1,:] += (+rx+ry)*radius
807
808 # flatten to a list of line segments
809 # shape (N, 2,2)
810 return nps.clump(p, n=p.ndim-2)
811
812 def line_segments_crosshairs(# shape (..., 2)
813 q,
814 radius):
815
816 # shape (..., Nsegments_in_crosshair=2, Npoints_in_line_segment=2, xy=2)
817 p = np.zeros(q.shape[:-1] + (4,2,2), dtype=np.float32)
818 p += nps.dummy(q,-2,-2)
819
820 rx = np.array((1,0), dtype=np.float32)
821 ry = np.array((0,1), dtype=np.float32)
822
823 # line segment 0
824 p[..., 0,0,:] += (-rx-ry)*radius
825 p[..., 0,1,:] += (+rx+ry)*radius
826 # line segment 1
827 p[..., 1,0,:] += (+rx-ry)*radius
828 p[..., 1,1,:] += (-rx+ry)*radius
829
830 # flatten to a list of line segments
831 # shape (..., 2,2)
832 return nps.clump(p, n=p.ndim-2)
833
834
835 def set_all_overlay_lines_and_redraw():
836
837 radius_stored = 10
838 crosshair_radius = 1
839
840 color_stored = np.array((0,1,0), dtype=np.float32)
841
842 radius_selected = 11
843 color_selected = np.array((0,0,1), dtype=np.float32)
844
845 color_searchbox = np.array((1,1,0), dtype=np.float32)
846
847
848
849 Nstored = len(context['q01_stored'])
850
851 Rt01 = context['Rt01']
852 baseline = nps.mag(Rt01[3,:])
853
854
855 # for projected rays, I start at a small ratio of the baseline, and increase
856 # the range geometrically. I add another point at infinity at the end
857 k = 1.1
858 N = 25
859 r = baseline * 1. * (k ** np.arange(N))
860
861 iselected = tuple(i for i in range(Nstored) if widget_table.row_selected(i))
862
863 for w in widgets_image:
864
865 ####### The squares and crosshair for all the stored features
866 lines_stored = \
867 [ dict(points = nps.glue( line_segments_squares(context['q01_stored'][:, w.index,:],
868 (radius_stored,)),
869 line_segments_crosshairs(context['q01_stored'][:, w.index,:],
870 crosshair_radius),
871 axis = -3 ),
872 color_rgb = color_stored ) ]
873
874
875 ####### The search box
876 if context['search_center__q01_indexfrom_showfrom_searchradius'] is not None:
877 q01_search_center,indexfrom,showfrom,search_radius = context['search_center__q01_indexfrom_showfrom_searchradius']
878 if indexfrom == w.index and showfrom:
879 lines_stored.append( dict(points = line_segments_crosshairs(q01_search_center[w.index],
880 crosshair_radius),
881 color_rgb = color_searchbox ) )
882 elif indexfrom != w.index:
883 lines_stored.append( dict(points = line_segments_squares(q01_search_center[w.index],
884 (search_radius,)),
885 color_rgb = color_searchbox ) )
886
887
888 if len(iselected):
889
890 ####### The epipolar CURVE corresponding to THIS point in the OTHER
891 ####### camera
892
893 # shape (Nselected, 2)
894 q_stored = context['q01_stored'][iselected, w.index,:]
895 q_stored_other = context['q01_stored'][iselected, 1-w.index,:]
896
897 # shape (Nranges, Nselected, 3)
898 p = mrcal.unproject(q_stored_other, *models[1-w.index].intrinsics(),
899 normalize = True) * \
900 nps.mv(r,-1,-3)
901 if w.index == 0:
902 pinf = mrcal.rotate_point_R(Rt01[:3,:], p[0])
903 p = mrcal.transform_point_Rt(Rt01, p)
904 else:
905 pinf = mrcal.rotate_point_R(mrcal.invert_R(Rt01[:3,:]), p[0])
906 p = mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01), p)
907
908 # shape (Nsegments, Nsegmentpoint=2, Nxy=2)
909 qsegments = np.zeros( (0,2,2), dtype=np.float32)
910
911 # shape (Nranges+1, Nselected, Nxyz=3)
912 p = nps.glue(p,pinf, axis=-3)
913
914 for pselected in nps.mv(p,-2,-3):
915 # pselected.shape is (Nranges+1,Nxyz=3)
916
917 # cut off points where the ray is behind the other camera
918 pselected = pselected[pselected[:,2] > 0,:]
919
920 if len(pselected)>=2:
921 # shape (N, Nxy=2)
922 q = mrcal.project(pselected,
923 *models[w.index].intrinsics()).astype(np.float32)
924 # shape (N-1, Nsegmentpoint=2, Nxy=2)
925 qsegments = \
926 nps.glue(qsegments,
927 nps.mv( nps.cat(q[0:-1,...],q[1:,...]),
928 0,1 ),
929 axis=-3)
930
931 lines_stored.append( dict(points = nps.glue( line_segments_squares(q_stored,
932 (radius_selected,),),
933 qsegments,
934 axis = -3),
935 color_rgb = color_selected ) )
936
937 w.set_lines(*lines_stored)
938
939
940 def clear_q_search_center():
941 global context
942 context['search_center__q01_indexfrom_showfrom_searchradius'] = None
943
944
945
946 def widget_table_callback(*args):
947 ctx = widget_table.callback_context()
948 event = Fl.event()
949 if ctx == Fl_Table.CONTEXT_CELL and \
950 event == FL_RELEASE:
951
952 # I want the selected feature to be in view. I do that for image0
953 # (image1 will be automatically panned). If it's already in view or
954 # close to it, then I don't touch it
955 row = widget_table.callback_row()
956 i_image = 0
957 q = context['q01_stored'][row,i_image,:]
958
959 viewport_w = widgets_image[i_image].w()
960 viewport_h = widgets_image[i_image].h()
961 qviewport = widgets_image[i_image].map_pixel_viewport_from_image(q)
962
963 # The margin is 1/10 or 100 pixels, whichever is smaller. If we're within
964 # the margin, we pan
965 margin_w = min(viewport_w//10,100)
966 margin_h = min(viewport_h//10,100)
967 if qviewport[0] < margin_w or viewport_w-1-qviewport[0] < margin_w or \
968 qviewport[1] < margin_h or viewport_h-1-qviewport[1] < margin_h:
969
970 widgets_image[i_image].set_panzoom(*q,
971 visible_width_pixels = np.nan, # leave the same
972 notify_other_widgets = True)
973
974
975 clear_q_search_center()
976 set_all_overlay_lines_and_redraw()
977
978 def widget_solve_callback(*args):
979 global context
980
981 try:
982 Rt01, mask_inliers = \
983 solve_Rt10(context['q01_stored'],
984 *[m.intrinsics() for m in models],
985 solve_R10 = solve_R01__optimize,
986 t01_fixed = context['Rt01'][3,:])
987 except Exception as e:
988 widget_info.value(UI_usage_message + "\n" + \
989 f"Solve failed: {e}")
990 return
991
992 # Store the new transform. The model0 extrinsics are untouched, but I move
993 # model1
994 context['Rt01'] = Rt01
995 Rtr0 = models[0].Rt_ref_cam()
996 Rtr1 = mrcal.compose_Rt(Rtr0, Rt01)
997 models[1].Rt_ref_cam(Rtr1)
998
999 update_p0_triangulated_stored()
1000
1001 widget_table.redraw()
1002 clear_q_search_center()
1003 set_all_overlay_lines_and_redraw()
1004
1005 N = len(context['q01_stored'])
1006 Noutliers = N-np.count_nonzero(mask_inliers)
1007 widget_info.value(UI_usage_message + "\n" + \
1008 f"Extrinsics of model1 updated: {Noutliers}/{N} outliers\n" + \
1009 f"Outlier points: {np.nonzero(~mask_inliers)[0]}")
1010
1011
1012
1013 # Debugging code to write the models and correspondences to disk after each
1014 # solve
1015 if False:
1016 for icam in (0,1):
1017 filename = f"/tmp/camera-{icam}.cameramodel"
1018 models[icam].write(filename)
1019 print(f"Wrote '{filename}")
1020
1021 p = context['p0_triangulated_stored']
1022 r = nps.mag(p)
1023 idx = nps.norm2(p) > 0
1024 r[~idx] = np.nan
1025
1026 filename = '/tmp/correspondences.vnl'
1027 np.savetxt(filename,
1028 nps.glue( nps.clump(context['q01_stored'][:,:4],
1029 n = -2),
1030 nps.dummy(r,-1),
1031 axis = -1),
1032 fmt = '%.2f',
1033 header = "x0 y0 x1 y1 range")
1034 print(f"Wrote '{filename}")
1035
1036
1037
1038
1039 def widget_write_callback(*args):
1040 p = context['p0_triangulated_stored']
1041 r = nps.mag(p)
1042 idx = nps.norm2(p) > 0
1043 r[~idx] = np.nan
1044
1045 np.savetxt(sys.stdout,
1046 nps.glue( nps.clump(context['q01_stored'][:,:4],
1047 n = -2),
1048 nps.dummy(r,-1),
1049 axis = -1),
1050 fmt = '%.2f',
1051 header = "x0 y0 x1 y1 range")
1052
1053 for m in models: m.write(sys.stdout)
1054
1055
1056 def find_corresponding_feature(q, index,
1057 *,
1058 qother_estimate = None,
1059 search_radius):
1060 if not (q[0] >= -0.5 and q[0] <= W-0.5 and \
1061 q[1] >= -0.5 and q[1] <= H-0.5):
1062 return f"Out of bounds: {q=}", None, None
1063
1064 if qother_estimate is None:
1065 # shape (2,2): (leftright, qxy)
1066 q01_estimate = get_q01_estimate(q = q,
1067 index = index)
1068 else:
1069 if index == 0: q01_estimate = nps.cat(q, qother_estimate)
1070 else: q01_estimate = nps.cat(qother_estimate,q)
1071
1072 index_other = 1 - index
1073 try:
1074 match_feature_out = \
1075 mrcal.match_feature(images[index], images[index_other],
1076 q0 = q01_estimate[index],
1077 q1_estimate = q01_estimate[index_other],
1078 search_radius1 = search_radius,
1079 template_size1 = args.template_size)
1080 q_other, match_feature_diagnostics = match_feature_out[:2]
1081 except:
1082 q_other = None
1083
1084 if q_other is None:
1085 return "Error matching feature", q01_estimate, None
1086
1087 q01 = np.array(q01_estimate)
1088 q01[index_other] = q_other
1089
1090 return "Feature match successful", q01_estimate, q01
1091
1092
1093 class Fl_Gl_Image_Widget_Derived(Fl_Gl_Image_Widget):
1094
1095 def __init__(self,
1096 *args,
1097 index,
1098 **kwargs):
1099 self.index = index
1100 return super().__init__(*args, **kwargs)
1101
1102 def set_panzoom(self,
1103 x_centerpixel, y_centerpixel,
1104 visible_width_pixels,
1105 notify_other_widgets = True):
1106 r'''Pan/zoom the image
1107
1108 This is an override of the function to do this: any request to
1109 pan/zoom the widget will come here first. I dispatch any
1110 pan/zoom commands to all the widgets, so that they all work in
1111 unison. visible_width_pixels < 0 means: this is the redirected
1112 call; just call the base class
1113
1114 '''
1115 if not notify_other_widgets:
1116 return super().set_panzoom(x_centerpixel, y_centerpixel,
1117 visible_width_pixels)
1118
1119 # All the widgets should pan/zoom together
1120 result = \
1121 all( w.set_panzoom(x_centerpixel, y_centerpixel,
1122 visible_width_pixels,
1123 notify_other_widgets = False) \
1124 for w in widgets_image )
1125
1126 # Switch back to THIS widget
1127 self.make_current()
1128 return result
1129
1130
1131 def handle_right_mouse_button(self):
1132
1133 search_center__q01_indexfrom_showfrom_searchradius_copy = context['search_center__q01_indexfrom_showfrom_searchradius']
1134 clear_q_search_center()
1135
1136
1137 try:
1138 q = \
1139 np.array( self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), ),
1140 dtype=float )
1141 except:
1142 q = None
1143 if q is None:
1144 message = "Error converting pixel coordinates"
1145 widget_info.value(UI_usage_message + "\n" + \
1146 message)
1147 return 1
1148
1149
1150 if Fl.event_state() & FL_CTRL:
1151 if search_center__q01_indexfrom_showfrom_searchradius_copy is None:
1152 # Ctrl-right-click but we didn't just do a search. Do nothing
1153 return 1
1154
1155 # We just did a search. It may have succeeded, with the match
1156 # selected, in the last row of the table. Or it may have failed,
1157 # with nothing selected
1158 q01_search_center, indexfrom, showfrom, _ = search_center__q01_indexfrom_showfrom_searchradius_copy
1159 if self.index == indexfrom:
1160 # Ctrl-right-click but we just did a search, from the same image. Do nothing
1161 return 1
1162
1163 # We can rerun the search with human seeding. If the search we just
1164 # did succeeded, then I need to clear out that feature; I'm about to
1165 # redo it
1166 iselected = tuple(i for i in range(widget_table.rows()) if widget_table.row_selected(i))
1167 if len(iselected) == 0:
1168 # Nothing selected; the previous search failed, and we don't
1169 # need to clear it out
1170 pass
1171 elif len(iselected) > 1:
1172 print("WARNING: We're trying to rerun a search, but more than one feature is selected in the UI. This shouldn't be able to happen, and it's a bug. Doing nothing")
1173 return 1
1174
1175 else:
1176 # Have one feature. Delete it
1177 mask = np.ones((widget_table.rows(),), dtype=bool)
1178 mask[iselected] = 0
1179 update_q01_stored(context['q01_stored'][mask, ...])
1180 widget_table.rows(len(context['q01_stored']))
1181 widget_table.select_all_rows(0) # deselect all
1182
1183
1184 # Set up the new search. I want to search FROM the other widget: I'm
1185 # repeating the previous search
1186 qother_estimate = q
1187 q = q01_search_center[indexfrom]
1188 search_radius = 5 # tighter bounds
1189
1190 else:
1191 # normal path
1192 indexfrom = self.index
1193 qother_estimate = None
1194 search_radius = args.search_radius
1195
1196
1197 message,q01_estimate,q01 = find_corresponding_feature(q, indexfrom,
1198 qother_estimate = qother_estimate,
1199 search_radius = search_radius)
1200
1201 widget_info.value(UI_usage_message + "\n" + \
1202 message)
1203
1204 if q01 is not None:
1205 update_q01_stored(nps.glue( context['q01_stored'],
1206 q01,
1207 axis=-3))
1208
1209 Nstored = len(context['q01_stored'])
1210 widget_table.rows( Nstored )
1211 widget_table.select_all_rows(0) # deselect all
1212 widget_table.select_row(Nstored-1)
1213 else:
1214 # match failed
1215 widget_table.select_all_rows(0) # deselect all
1216
1217 if q01_estimate is not None:
1218 context['search_center__q01_indexfrom_showfrom_searchradius'] = (q01_estimate, indexfrom, q01 is None, search_radius)
1219
1220 return 1
1221
1222 def handle(self, event):
1223 if event == FL_ENTER:
1224 return 1
1225 if event == FL_LEAVE:
1226 widget_status.value("")
1227 return 1
1228 if event == FL_MOVE:
1229 try:
1230 q = self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), )
1231 this = f"Image {self.index}"
1232 widget_status.value(f"{this}: {q[0]:.2f},{q[1]:.2f}")
1233 except:
1234 widget_status.value("")
1235 return 1
1236
1237 if event == FL_PUSH and Fl.event_button() == FL_LEFT_MOUSE:
1238 self.dragged = False
1239 return super().handle(event)
1240
1241 if event == FL_DRAG and Fl.event_state() & FL_BUTTON1:
1242 self.dragged = True
1243 return super().handle(event)
1244
1245 if event == FL_RELEASE:
1246 if Fl.event_button() == FL_RIGHT_MOUSE:
1247 result = self.handle_right_mouse_button()
1248 set_all_overlay_lines_and_redraw()
1249 return result
1250
1251 if Fl.event_button() == FL_LEFT_MOUSE:
1252 if not self.dragged:
1253 clear_q_search_center()
1254
1255 # Clicked. Select the nearest feature
1256 if len(context['q01_stored']) > 0:
1257 qwidget = np.array((Fl.event_x(),Fl.event_y()),
1258 dtype=int)
1259 qwidget_stored = \
1260 np.array([self.map_pixel_viewport_from_image(q) for q in context['q01_stored'][:,self.index,:]])
1261 # within 10 pixels in the UI
1262 i = np.nonzero(nps.norm2(qwidget - qwidget_stored) < 10*10)[0]
1263 if len(i) < 1:
1264 widget_info.value(UI_usage_message + "\n" + \
1265 "No feature near click")
1266 widget_table.select_all_rows(0) # deselect all
1267 set_all_overlay_lines_and_redraw()
1268 elif len(i) > 1:
1269 widget_info.value(UI_usage_message + "\n" + \
1270 "Too many features near click")
1271 widget_table.select_all_rows(0) # deselect all
1272 set_all_overlay_lines_and_redraw()
1273 else:
1274 # Just one feature. Select it
1275 i = i[0]
1276 widget_table.select_all_rows(0) # deselect all
1277 widget_table.select_row(int(i))
1278 set_all_overlay_lines_and_redraw()
1279 return super().handle(event)
1280
1281
1282 # Must be key UP, not key DOWN. Because of https://github.com/fltk/fltk/issues/1044
1283 if event == FL_KEYUP:
1284 if Fl.event_key() == fltk.FL_Delete:
1285 i_keep = tuple(i for i in range(widget_table.rows()) \
1286 if not widget_table.row_selected(i))
1287 update_q01_stored(context['q01_stored'][i_keep, ...])
1288 widget_table.rows(len(context['q01_stored']))
1289 widget_table.select_all_rows(0) # deselect all
1290 clear_q_search_center()
1291 set_all_overlay_lines_and_redraw()
1292 widget_info.value(UI_usage_message + "\n" + \
1293 "Feature(s) deleted")
1294
1295 return 1
1296
1297 return super().handle(event)
1298
1299
1300 class Fl_Table_Derived(Fl_Table_Row):
1301
1302 def __init__(self, x, y, w, h, *args):
1303 Fl_Table_Row.__init__(self, x, y, w, h, *args)
1304
1305 self.col_labels = \
1306 [ "x0",
1307 "y0",
1308 "x1",
1309 "y1",
1310 "Triangulated range",
1311 "Cam0 triangulated reprojection error", ]
1312 len_col_labels = [len(x) for x in self.col_labels]
1313 min_len_col_labels = min(len_col_labels)
1314 max_ratio_len_col_labels = 3 # limit max ratio
1315 normalized_len_col_labels = [min(x/min_len_col_labels, max_ratio_len_col_labels) \
1316 for x in len_col_labels]
1317 sum_normalized_len_col_labels = sum(normalized_len_col_labels)
1318 self.ratio_col_width = [w/sum_normalized_len_col_labels for w in normalized_len_col_labels]
1319
1320
1321 self.type(fltk.Fl_Table_Row.SELECT_MULTI)
1322 self.rows(len(context['q01_stored']))
1323 self.cols(len(self.col_labels))
1324 self.col_header(1)
1325 self.col_resize(0)
1326
1327 self.when(FL_WHEN_RELEASE)
1328 self.callback(widget_table_callback)
1329
1330 self.end()
1331
1332 def draw_cell(self, context_table, row, col, x, y, w, h):
1333
1334 if context_table == self.CONTEXT_STARTPAGE:
1335 fl_font(FL_HELVETICA, 12)
1336 return
1337
1338 if context_table == self.CONTEXT_COL_HEADER:
1339 text = self.col_labels[col]
1340
1341 fl_push_clip(x, y, w, h)
1342 fl_draw_box(FL_THIN_UP_BOX, x, y, w, h, self.row_header_color())
1343 fl_color(FL_BLACK)
1344 fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)
1345 fl_pop_clip()
1346 return
1347
1348 if context_table == self.CONTEXT_CELL:
1349 if col < 4:
1350 iimage = col // 2
1351 ixy = col % 2
1352 text = f"{context['q01_stored'][row,iimage,ixy]:.2f}"
1353 else:
1354 p = context['p0_triangulated_stored'][row]
1355 if nps.norm2(p) == 0:
1356 # Divergent feature. No triangulated anything available
1357 text = '-'
1358 else:
1359 if col == 4:
1360 # range
1361 text = f"{nps.mag(p):.2f}"
1362 else:
1363 # reprojection error
1364 icam = 0
1365 q = mrcal.project(p, *models[icam].intrinsics())
1366 text = f"{nps.mag(context['q01_stored'][row,icam] - q):.1f}"
1367
1368 fl_push_clip(x, y, w, h)
1369 # background color
1370 fl_color(self.selection_color() if self.row_selected(row) else FL_WHITE)
1371 fl_rectf(x, y, w, h)
1372
1373 # text
1374 fl_color(FL_BLACK)
1375 fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)
1376
1377 # border
1378 fl_color(FL_LIGHT2)
1379 fl_rect(x, y, w, h)
1380 fl_pop_clip()
1381
1382 return
1383
1384 return
1385
1386 def resize(self, x,y,w,h):
1387 Fl_Table_Row.resize(self, x,y,w,h)
1388 Ncols = self.cols()
1389 width = self.w()
1390 width_margin = 2 # for some reason, I need to cut this many pixels to
1391 # avoid creating a scrollbar
1392 x0 = 0
1393 for icol in range(Ncols-1):
1394 w_here = int(self.ratio_col_width[icol] * width)
1395 self.col_width(icol, w_here)
1396 x0 += w_here
1397 # last col
1398 self.col_width(Ncols-1, width-x0 - width_margin)
1399
1400
1401
1402 models = [mrcal.cameramodel(m) for m in args.models]
1403
1404
1405 if args.rt01 is not None:
1406 try:
1407 rt01 = np.array([float(d) for d in args.rt01.split(',')])
1408 except:
1409 print("--rt01 must be a comma-separated list of 6 numbers; couldn't parse as numbers into an array", file=sys.stderr)
1410 sys.exit(1)
1411 if rt01.shape != (6,):
1412 print("--rt01 must be a comma-separated list of 6 numbers; incorrect number of values given", file=sys.stderr)
1413 sys.exit(1)
1414
1415 models[1].rt_ref_cam( mrcal.compose_rt(models[0].rt_ref_cam(),
1416 rt01) )
1417
1418 # To make it possible to concisely print out the models
1419 for m in models:
1420 m.optimization_inputs_reset()
1421
1422 W,H = models[0].imagersize()
1423
1424 if args.equalization == 'clahe':
1425 clahe = cv2.createCLAHE()
1426 clahe.setClipLimit(8)
1427 images = [mrcal.load_image(f) for f in args.images]
1428 for i in range(2):
1429 if images[i].ndim == 3:
1430 images[i] = np.mean(images[i], axis=-1, dtype=images[i].dtype)
1431 if args.equalization == 'clahe':
1432 images = [ clahe.apply(image) for image in images ]
1433 elif args.equalization == 'fieldscale':
1434 images = [ mrcam.equalize_fieldscale(image) for image in images ]
1435 elif args.equalization == 'stretch':
1436 images = [ ((image - np.min(image))/(np.max(image)-np.min(image))*255.).astype(np.uint8) for image in images ]
1437
1438 if args.valid_intrinsics_region:
1439 for i in range(2):
1440 mrcal.annotate_image__valid_intrinsics_region(images[i], models[i])
1441
1442 # To make it possible to concisely print out the models
1443 for m in models:
1444 m.valid_intrinsics_region( (), )
1445
1446 UI_usage_message = r'''Usage:
1447
1448 Left mouse button click/drag: pan
1449 Mouse wheel up/down/left/right: pan
1450 Ctrl-mouse wheel up/down: zoom
1451 'u': reset view: zoom out, pan to the center
1452
1453 Right-click: Find matching feature
1454
1455 Ctrl-right-click immediately after a right-click in the other widget: re-run
1456 feature search starting with the new click position as a seed, and with a much
1457 smaller search radius
1458
1459 Click in table: select feature(s)
1460
1461 Delete: delete selected feature(s)
1462 '''
1463
1464
1465
1466 ## these two arrays correspond to each other. If you update one, do update the
1467 ## other
1468 context = dict( # shape (N, Nimages = 2,xy=2)
1469 q01_stored = np.zeros((0,2,2), dtype=float),
1470
1471 # If non-None, describes the feature search that just occurred
1472 search_center__q01_indexfrom_showfrom_searchradius = None,
1473
1474 # shape (N, xy=2)
1475 p0_triangulated_stored = np.zeros((0,2), dtype=float),
1476
1477 Rt01 = mrcal.compose_Rt( models[0].Rt_cam_ref(),
1478 models[1].Rt_ref_cam() ) )
1479
1480 if args.initial_correspondences is not None:
1481 try:
1482 context['q01_stored'] = vnlog.slurp(args.initial_correspondences,
1483 dtype = np.dtype([('x0 y0 x1 y1', float, (4,))]))
1484 except:
1485 print(f"--initial-correspondences expects a text table with columns 'x0 y0 x1 y1'; couldn't load text table from '{args.initial_correspondences}'", file=sys.stderr)
1486 sys.exit(1)
1487
1488 N = len(context['q01_stored'])
1489
1490 update_q01_stored(context['q01_stored']['x0 y0 x1 y1'].reshape((N,2,2),))
1491
1492
1493
1494
1495
1496 WINDOW_W = 800
1497 IMAGES_H = 300
1498 TABLE_H = 300
1499 BUTTON_H = 30
1500 STATUS_H = 20
1501
1502 WINDOW_H = IMAGES_H + TABLE_H + BUTTON_H + STATUS_H
1503
1504 window = Fl_Window(WINDOW_W, WINDOW_H, "mrcal feature picker")
1505 body = Fl_Group(0,0,
1506 WINDOW_W, IMAGES_H + TABLE_H)
1507
1508 y = 0
1509 widgets_image = (Fl_Gl_Image_Widget_Derived(0, y,
1510 WINDOW_W//2,IMAGES_H,
1511 index = 0),
1512 Fl_Gl_Image_Widget_Derived(WINDOW_W//2, y,
1513 WINDOW_W//2,IMAGES_H,
1514 index = 1))
1515 y += IMAGES_H
1516
1517
1518 widget_table = Fl_Table_Derived( 0, y,
1519 WINDOW_W//2,TABLE_H)
1520 widget_info = Fl_Multiline_Output(WINDOW_W//2,y,
1521 WINDOW_W//2,TABLE_H)
1522 y += TABLE_H
1523 body.end()
1524
1525 Nbuttons = 2
1526 BUTTON_W = WINDOW_W//Nbuttons
1527 ibutton = 0
1528
1529 widget_solve_button = Fl_Button(ibutton*BUTTON_W, y,
1530 BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
1531 BUTTON_H, "Solve for the extrinsics")
1532 widget_solve_button.callback(widget_solve_callback)
1533 ibutton+=1
1534
1535 widget_write_button = Fl_Button(ibutton*BUTTON_W, y,
1536 BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
1537 BUTTON_H, "Write output to stdout")
1538 widget_write_button.callback(widget_write_callback)
1539 ibutton+=1
1540
1541 y += BUTTON_H
1542
1543 widget_status = Fl_Output(0,y,
1544 WINDOW_W,STATUS_H)
1545 widget_info.value(UI_usage_message)
1546 y += STATUS_H
1547
1548
1549 window.resizable(body)
1550 window.end()
1551 window.show()
1552
1553 for i in range(2):
1554 widgets_image[i]. \
1555 update_image(decimation_level = 0,
1556 image_data = images[i])
1557
1558 if args.initial_features is not None:
1559
1560 try:
1561 q0 = vnlog.slurp(args.initial_features,
1562 dtype = np.dtype([('x0 y0', float, (2,))]))
1563 except:
1564 print(f"--initial-features expects a text table with columns 'x0 y0'; couldn't load text table from '{args.initial_features}'", file=sys.stderr)
1565 sys.exit(1)
1566
1567 q0 = q0['x0 y0']
1568 N = len(q0)
1569 mask_good = np.zeros((N,), dtype=bool)
1570 q01 = np.zeros( (N,2,2), dtype=float)
1571 for i in range(N):
1572 message,q01_estimate,_q01 = \
1573 find_corresponding_feature(q0[i,:], 0,
1574 search_radius = args.search_radius)
1575 if _q01 is not None:
1576 q01[i] = _q01
1577 mask_good[i] = True
1578
1579 update_q01_stored(q01[mask_good])
1580
1581
1582 if len(context['q01_stored']):
1583 # Drawing lines at startup requires gl-image-display 0.19, so I only do it
1584 # if I need to. Most of the time we will start out with no features, so
1585 # older gl-image-display would be fine
1586 set_all_overlay_lines_and_redraw()
1587
1588 Fl.run()
1589
1590 sys.exit(0)
1591
1592
1593
1594
1595
1596 r'''
1597 todo notes:
1598
1599 'u' should pan to the center
1600
1601 ctrl-drag should lock panning
1602
1603 arrow-keys in the table should work
1604
1605 better UI messages
1606
1607
1608 solve diagnostics. residuals. UI for ranged fiducial/at-infinity
1609 '''