Skip to content

Commit ba09861

Browse files
veichtaveichta
authored
Add support for shared intrinsics with arbitrary distortions. (#27)
1 parent 9e3a341 commit ba09861

File tree

4 files changed

+66
-38
lines changed

4 files changed

+66
-38
lines changed

‎geocalib/camera.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -772,7 +772,7 @@ def J_up_projection_offset(self, p2d: torch.Tensor, wrt: str = "uv") -> torch.Te
772772
if wrt == "uv":
773773
uv_uvT = torch.einsum("...i,...j->...ij", p2d, p2d) # (..., 2, 2)
774774
I = torch.eye(2, device=p2d.device, dtype=p2d.dtype).expand(p2d.shape[:-1] + (2, 2))
775-
return 8 * k2 * uv_uvT + (2 * k1 + 4 * k2 * r2)[..., None] * I
775+
return 8 * k2[..., None] * uv_uvT + (2 * k1 + 4 * k2 * r2)[..., None] * I
776776
elif wrt == "dist":
777777
return torch.stack([2 * p2d, 4 * r2 * p2d], -1) # (..., 2, K)
778778

‎geocalib/lm_optimizer.py‎

Lines changed: 32 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -201,11 +201,6 @@ def setup_optimization_and_priors(
201201
data = {}
202202
self.shared_intrinsics = shared_intrinsics
203203

204-
if shared_intrinsics: # si => must use pinhole
205-
assert (
206-
self.camera_model == camera_models["pinhole"]
207-
), f"Shared intrinsics only supported with pinhole camera model: {self.camera_model}"
208-
209204
self.estimate_gravity = True
210205
if "prior_gravity" in data:
211206
self.estimate_gravity = False
@@ -239,6 +234,10 @@ def setup_optimization_and_priors(
239234
)
240235
)
241236

237+
self.n_intrinsic_params = self.estimate_focal + (
238+
self.camera_model.num_dist_params() if self.camera_has_distortion else 0
239+
)
240+
242241
logger.debug(f"Camera Model: {self.camera_model}")
243242
logger.debug(f"Optimizing gravity: {self.estimate_gravity} ({self.gravity_delta_dims})")
244243
logger.debug(f"Optimizing focal: {self.estimate_focal} ({self.focal_delta_dims})")
@@ -352,23 +351,35 @@ def calculate_gradient_and_hessian(
352351
# reshape to (1, B * (N_params-1) + 1)
353352
Grad_g = Grad[..., :2].reshape(1, -1)
354353
Grad_f = Grad[..., 2].reshape(1, -1).sum(-1, keepdim=True)
355-
Grad = torch.cat([Grad_g, Grad_f], dim=-1)
354+
Grad_dist = Grad[..., 3:].sum(-2).reshape(1, -1)
355+
Grad = torch.cat([Grad_g, Grad_f, Grad_dist], dim=-1)
356356

357357
Hess = torch.einsum("...Njk,...Njl->...Nkl", J, J)
358358
Hess = weights[..., None, None] * Hess
359359
Hess = Hess.sum(-3)
360360

361361
if shared_intrinsics:
362+
"""
363+
Hess =
364+
[
365+
diag(H_G ) J_g_intrinsic
366+
367+
J_g_intrinsic^T H_intrinsic
368+
]
369+
"""
370+
B = Hess.shape[0]
362371
H_g = torch.block_diag(*list(Hess[..., :2, :2]))
363-
J_fg = Hess[..., :2, 2].flatten()
364-
J_gf = Hess[..., 2, :2].flatten()
365-
J_f = Hess[..., 2, 2].sum()
366-
dims = H_g.shape[-1] + 1
372+
J_intrinsics_g = Hess[..., :2, 2:].reshape(B * 2, -1)
373+
J_g_intrinsics = Hess[..., 2:, :2].permute(0, 2, 1).reshape(B * 2, -1).T
374+
375+
H_intrinsics = Hess[..., 2:, 2:].sum(-3)
376+
377+
dims = H_g.shape[-1] + self.n_intrinsic_params
367378
Hess = Hess.new_zeros((dims, dims), dtype=torch.float32)
368-
Hess[:-1, :-1] = H_g
369-
Hess[-1, :-1] = J_gf
370-
Hess[:-1, -1] = J_fg
371-
Hess[-1, -1] = J_f
379+
Hess[: -self.n_intrinsic_params, : -self.n_intrinsic_params] = H_g
380+
Hess[-self.n_intrinsic_params :, : -self.n_intrinsic_params] = J_g_intrinsics
381+
Hess[: -self.n_intrinsic_params, -self.n_intrinsic_params :] = J_intrinsics_g
382+
Hess[-self.n_intrinsic_params :, -self.n_intrinsic_params :] = H_intrinsics
372383
Hess = Hess.unsqueeze(0)
373384

374385
return Grad, Hess
@@ -416,7 +427,9 @@ def setup_system(
416427
Hess = J_up.new_zeros(J_up.shape[0], n_params, n_params)
417428

418429
if shared_intrinsics:
419-
N_params = Grad.shape[0] * (n_params - 1) + 1
430+
N_params = (
431+
Grad.shape[0] * (n_params - self.n_intrinsic_params) + self.n_intrinsic_params
432+
)
420433
Grad = Grad.new_zeros(1, N_params)
421434
Hess = Hess.new_zeros(1, N_params, N_params)
422435

@@ -584,9 +597,10 @@ def optimize(
584597
delta = optimizer_step(Grad, Hess, lamb) # (B, N_params)
585598

586599
if self.shared_intrinsics:
587-
delta_g = delta[..., :-1].reshape(B, 2)
588-
delta_f = delta[..., -1].expand(B, 1)
589-
delta = torch.cat([delta_g, delta_f], dim=-1)
600+
delta_g = delta[..., : -self.n_intrinsic_params].reshape(B, 2)
601+
delta_f = delta[..., -self.n_intrinsic_params].expand(B, 1)
602+
delta_dist = delta[..., -self.n_intrinsic_params + 1 :].expand(B, -1)
603+
delta = torch.cat([delta_g, delta_f, delta_dist], dim=-1)
590604

591605
# calculate new cost
592606
camera_opt, gravity_opt = self.update_estimate(camera_opt, gravity_opt, delta)

‎siclib/geometry/camera.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -267,7 +267,7 @@ def J_up_projection_offset(self, p2d: torch.Tensor, wrt: str = "uv") -> torch.Te
267267
if wrt == "uv":
268268
uv_uvT = torch.einsum("...i,...j->...ij", p2d, p2d) # (..., 2, 2)
269269
I = torch.eye(2, device=p2d.device, dtype=p2d.dtype).expand(p2d.shape[:-1] + (2, 2))
270-
return 8 * k2 * uv_uvT + (2 * k1 + 4 * k2 * r2)[..., None] * I
270+
return 8 * k2[..., None] * uv_uvT + (2 * k1 + 4 * k2 * r2)[..., None] * I
271271
elif wrt == "dist":
272272
return torch.stack([2 * p2d, 4 * r2 * p2d], -1) # (..., 2, K)
273273

‎siclib/models/optimization/lm_optimizer.py‎

Lines changed: 32 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -101,11 +101,6 @@ def setup_optimization_and_priors(
101101
data = {}
102102
self.shared_intrinsics = shared_intrinsics
103103

104-
if shared_intrinsics: # si => must use pinhole
105-
assert (
106-
self.camera_model == camera_models["pinhole"]
107-
), f"Shared intrinsics only supported with pinhole camera model: {self.camera_model}"
108-
109104
self.estimate_gravity = True
110105
if "prior_gravity" in data:
111106
self.estimate_gravity = False
@@ -139,6 +134,10 @@ def setup_optimization_and_priors(
139134
)
140135
)
141136

137+
self.n_intrinsic_params = self.estimate_focal + (
138+
self.camera_model.num_dist_params() if self.camera_has_distortion else 0
139+
)
140+
142141
logger.debug(f"Camera Model: {self.camera_model}")
143142
logger.debug(f"Optimizing gravity: {self.estimate_gravity} ({self.gravity_delta_dims})")
144143
logger.debug(f"Optimizing focal: {self.estimate_focal} ({self.focal_delta_dims})")
@@ -256,23 +255,35 @@ def calculate_gradient_and_hessian(
256255
# reshape to (1, B * (N_params-1) + 1)
257256
Grad_g = Grad[..., :2].reshape(1, -1)
258257
Grad_f = Grad[..., 2].reshape(1, -1).sum(-1, keepdim=True)
259-
Grad = torch.cat([Grad_g, Grad_f], dim=-1)
258+
Grad_dist = Grad[..., 3:].sum(-2).reshape(1, -1)
259+
Grad = torch.cat([Grad_g, Grad_f, Grad_dist], dim=-1)
260260

261261
Hess = torch.einsum("...Njk,...Njl->...Nkl", J, J)
262262
Hess = weights[..., None, None] * Hess
263263
Hess = Hess.sum(-3)
264264

265265
if shared_intrinsics:
266+
"""
267+
Hess =
268+
[
269+
diag(H_G ) J_g_intrinsic
270+
271+
J_g_intrinsic^T H_intrinsic
272+
]
273+
"""
274+
B = Hess.shape[0]
266275
H_g = torch.block_diag(*list(Hess[..., :2, :2]))
267-
J_fg = Hess[..., :2, 2].flatten()
268-
J_gf = Hess[..., 2, :2].flatten()
269-
J_f = Hess[..., 2, 2].sum()
270-
dims = H_g.shape[-1] + 1
276+
J_intrinsics_g = Hess[..., :2, 2:].reshape(B * 2, -1)
277+
J_g_intrinsics = Hess[..., 2:, :2].permute(0, 2, 1).reshape(B * 2, -1).T
278+
279+
H_intrinsics = Hess[..., 2:, 2:].sum(-3)
280+
281+
dims = H_g.shape[-1] + self.n_intrinsic_params
271282
Hess = Hess.new_zeros((dims, dims), dtype=torch.float32)
272-
Hess[:-1, :-1] = H_g
273-
Hess[-1, :-1] = J_gf
274-
Hess[:-1, -1] = J_fg
275-
Hess[-1, -1] = J_f
283+
Hess[: -self.n_intrinsic_params, : -self.n_intrinsic_params] = H_g
284+
Hess[-self.n_intrinsic_params :, : -self.n_intrinsic_params] = J_g_intrinsics
285+
Hess[: -self.n_intrinsic_params, -self.n_intrinsic_params :] = J_intrinsics_g
286+
Hess[-self.n_intrinsic_params :, -self.n_intrinsic_params :] = H_intrinsics
276287
Hess = Hess.unsqueeze(0)
277288

278289
return Grad, Hess
@@ -320,7 +331,9 @@ def setup_system(
320331
Hess = J_up.new_zeros(J_up.shape[0], n_params, n_params)
321332

322333
if shared_intrinsics:
323-
N_params = Grad.shape[0] * (n_params - 1) + 1
334+
N_params = (
335+
Grad.shape[0] * (n_params - self.n_intrinsic_params) + self.n_intrinsic_params
336+
)
324337
Grad = Grad.new_zeros(1, N_params)
325338
Hess = Hess.new_zeros(1, N_params, N_params)
326339

@@ -488,9 +501,10 @@ def optimize(
488501
delta = optimizer_step(Grad, Hess, lamb) # (B, N_params)
489502

490503
if self.shared_intrinsics:
491-
delta_g = delta[..., :-1].reshape(B, 2)
492-
delta_f = delta[..., -1].expand(B, 1)
493-
delta = torch.cat([delta_g, delta_f], dim=-1)
504+
delta_g = delta[..., : -self.n_intrinsic_params].reshape(B, 2)
505+
delta_f = delta[..., -self.n_intrinsic_params].expand(B, 1)
506+
delta_dist = delta[..., -self.n_intrinsic_params + 1 :].expand(B, -1)
507+
delta = torch.cat([delta_g, delta_f, delta_dist], dim=-1)
494508

495509
# calculate new cost
496510
camera_opt, gravity_opt = self.update_estimate(camera_opt, gravity_opt, delta)

0 commit comments

Comments
 (0)