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