| 1 |
#!/usr/bin/env python3 |
| 2 |
|
| 3 |
# Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. |
| 4 |
# Government sponsorship acknowledged. All rights reserved. |
| 5 |
# |
| 6 |
# Licensed under the Apache License, Version 2.0 (the "License"); |
| 7 |
# You may obtain a copy of the License at |
| 8 |
# |
| 9 |
# http://www.apache.org/licenses/LICENSE-2.0 |
| 10 |
|
| 11 |
r'''Converts a kalibr-formatted camera model to the .cameramodel file format |
| 12 |
|
| 13 |
SYNOPSIS |
| 14 |
|
| 15 |
$ cat model.yaml |
| 16 |
|
| 17 |
cam0: |
| 18 |
camera_model: pinhole |
| 19 |
intrinsics: [461.629, 460.152, 362.680, 246.049] |
| 20 |
distortion_model: radtan |
| 21 |
distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556] |
| 22 |
T_cam_imu: |
| 23 |
- [0.01779318, 0.99967549,-0.01822936, 0.07008565] |
| 24 |
- [-0.9998017, 0.01795239, 0.00860714,-0.01771023] |
| 25 |
- [0.00893160, 0.01807260, 0.99979678, 0.00399246] |
| 26 |
- [0.0, 0.0, 0.0, 1.0] |
| 27 |
timeshift_cam_imu: -8.121e-05 |
| 28 |
rostopic: /cam0/image_raw |
| 29 |
resolution: [752, 480] |
| 30 |
|
| 31 |
|
| 32 |
$ mrcal-from-kalibr model.yaml |
| 33 |
|
| 34 |
Wrote model-cam0.cameramodel |
| 35 |
|
| 36 |
|
| 37 |
$ cat model-cam0.cameramodel |
| 38 |
|
| 39 |
{ |
| 40 |
'lensmodel': 'LENSMODEL_OPENCV4', |
| 41 |
|
| 42 |
# intrinsics are fx,fy,cx,cy,distortion0,distortion1,.... |
| 43 |
'intrinsics': [ 461.629, 460.152, 362.68, 246.049, -0.27695497, 0.06712482, 0.00087538, 0.00011556,], |
| 44 |
|
| 45 |
'rt_cam_ref': [ 0, 0, 0, 0, 0, 0,], |
| 46 |
'extrinsics': [ 0, 0, 0, 0, 0, 0,], # for compatibility with mrcal < 2.5 |
| 47 |
|
| 48 |
'imagersize': [ 752, 480,], |
| 49 |
} |
| 50 |
|
| 51 |
File formats supported by mrcal are described at |
| 52 |
https://mrcal.secretsauce.net/cameramodels.html#cameramodel-file-formats |
| 53 |
|
| 54 |
This tool converts the given model(s) to the mrcal-native .cameramodel file |
| 55 |
format. No changes to the content are made; this is purely a format converter |
| 56 |
(the mrcal-convert-lensmodel tool fits different lens models instead). Model |
| 57 |
filenames are given on the commandline. Output is written to the same directory, |
| 58 |
with the same filename, but annotated with the camera name from the data file, |
| 59 |
and with a .cameramodel extension. |
| 60 |
|
| 61 |
If the model is omitted or given as "-", the input is read from standard input, |
| 62 |
and the output is written to standard output |
| 63 |
|
| 64 |
Unlike mrcal .cameramodel files where one camera is described by one file, the |
| 65 |
.yaml files used by kalibr are intended to describe multiple cameras. This |
| 66 |
format conversion tool will write out multiple .cameramodel file to represent |
| 67 |
the given data. To select a single camera, pass --camera CAMERA. |
| 68 |
|
| 69 |
By default we set the extrinsics of camera-0 as they appear in the input: the |
| 70 |
"previous" camera from camera-0 is used as the reference coordinate system. If |
| 71 |
we want to force camera-0 to have an identity transform, pass |
| 72 |
--cam0-at-reference. Usually this will be the case anyway in the input data, but |
| 73 |
this option makes sure. |
| 74 |
|
| 75 |
The kalibr format is described at |
| 76 |
https://github.com/ethz-asl/kalibr/wiki/Yaml-formats |
| 77 |
|
| 78 |
At this time we only support |
| 79 |
|
| 80 |
- LENSMODEL_PINHOLE |
| 81 |
- LENSMODEL_OPENCV4 |
| 82 |
- LENSMODEL_CAHVOR (experimental) |
| 83 |
- LENSMODEL_CAHVORE (experimental) |
| 84 |
|
| 85 |
which corresponds to |
| 86 |
|
| 87 |
camera_model == 'pinhole' and distortion_model in ('radtan','none') |
| 88 |
|
| 89 |
''' |
| 90 |
|
| 91 |
import sys |
| 92 |
import argparse |
| 93 |
import re |
| 94 |
import os |
| 95 |
|
| 96 |
def parse_args(): |
| 97 |
|
| 98 |
parser = \ |
| 99 |
argparse.ArgumentParser(description = __doc__, |
| 100 |
formatter_class=argparse.RawDescriptionHelpFormatter) |
| 101 |
|
| 102 |
parser.add_argument('--cam0-at-reference', |
| 103 |
action='store_true', |
| 104 |
default=False, |
| 105 |
help='''By default we set the extrinsics of camera-0 as |
| 106 |
they appear in the input. If we want to force camera-0 |
| 107 |
to have an identity transform, pass --cam0-at-reference. |
| 108 |
Usually this will be the case anyway in the input data, |
| 109 |
but this option makes sure''') |
| 110 |
parser.add_argument('--camera', |
| 111 |
help='''Kalibr model files may contain multiple cameras, |
| 112 |
selected by this argument. If omitted, all available |
| 113 |
cameras will be read. If given, we read JUST the one |
| 114 |
requested camera''') |
| 115 |
parser.add_argument('--force', '-f', |
| 116 |
action='store_true', |
| 117 |
default=False, |
| 118 |
help='''By default existing files are not overwritten. Pass --force to overwrite them |
| 119 |
without complaint''') |
| 120 |
parser.add_argument('--outdir', |
| 121 |
required=False, |
| 122 |
type=lambda d: d if os.path.isdir(d) else \ |
| 123 |
parser.error("--outdir requires an existing directory as the arg, but got '{}'".format(d)), |
| 124 |
help='''Directory to write the output models into. If omitted, we write the output |
| 125 |
models to the same directory as the input models''') |
| 126 |
parser.add_argument('model', |
| 127 |
default=['-'], |
| 128 |
nargs='*', |
| 129 |
type=str, |
| 130 |
help='''Input camera model; '-' to read standard input''') |
| 131 |
|
| 132 |
args = parser.parse_args() |
| 133 |
|
| 134 |
if len(args.model) > 1: |
| 135 |
print("ERROR: only one input file may be processed", file=sys.stderr) |
| 136 |
sys.exit(1) |
| 137 |
args.model = args.model[0] |
| 138 |
return args |
| 139 |
|
| 140 |
args = parse_args() |
| 141 |
|
| 142 |
# arg-parsing is done before the imports so that --help works without building |
| 143 |
# stuff, so that I can generate the manpages and README |
| 144 |
|
| 145 |
|
| 146 |
import mrcal |
| 147 |
import mrcal.cahvor |
| 148 |
import yaml |
| 149 |
import numpy as np |
| 150 |
import numpysane as nps |
| 151 |
|
| 152 |
|
| 153 |
|
| 154 |
Rt_camprev_ref = None |
| 155 |
|
| 156 |
# stolen from mrcal-to-kalibr. Please consolidate |
| 157 |
def Rt_is_identity(Rt): |
| 158 |
cos_th = (np.trace(Rt[:3,:]) - 1.) / 2. |
| 159 |
# cos_th ~ 1 - x^2/2 |
| 160 |
th_sq = (1 - cos_th)*2. |
| 161 |
norm2_t = nps.norm2(Rt[3,:]) |
| 162 |
return norm2_t < 1e-12 and th_sq < 1e-12 |
| 163 |
|
| 164 |
def convert_one(d,name): |
| 165 |
|
| 166 |
global Rt_camprev_ref |
| 167 |
|
| 168 |
|
| 169 |
# I expect at least this structure: |
| 170 |
# |
| 171 |
# camera_model: pinhole |
| 172 |
# intrinsics: [461.629, 460.152, 362.680, 246.049] |
| 173 |
# distortion_model: radtan |
| 174 |
# distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556] |
| 175 |
# resolution: [752, 480] |
| 176 |
# T_cn_cnm1: |
| 177 |
# - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785] |
| 178 |
# - [-0.00221074, 0.99992702, 0.01187697, 0.00045792] |
| 179 |
# - [-0.00424598,-0.01188627, 0.99992034,-0.00064487] |
| 180 |
# - [0.0, 0.0, 0.0, 1.0] |
| 181 |
# |
| 182 |
# T_cn_cnm1 may be omitted: I use an identity transform in that case |
| 183 |
camera_model_known = set(('pinhole', |
| 184 |
'cahvor','cahvore'),) |
| 185 |
if not d['camera_model'] in camera_model_known: |
| 186 |
print(f"ERROR: at this time I only support kalibr camera_model: {camera_model_known}; got camera_model={d['camera_model']}", |
| 187 |
file=sys.stderr) |
| 188 |
sys.exit(1) |
| 189 |
|
| 190 |
if not re.match('cahvor',d['camera_model']): |
| 191 |
|
| 192 |
# The "usual" path |
| 193 |
distortion_model_known = set(('radtan','none'),) |
| 194 |
if not d['distortion_model'] in distortion_model_known: |
| 195 |
print(f"ERROR: at this time I only support kalibr distortion_model: {distortion_model_known}; got distortion_model={d['distortion_model']}", |
| 196 |
file=sys.stderr) |
| 197 |
sys.exit(1) |
| 198 |
|
| 199 |
if d['distortion_model'] == 'radtan': |
| 200 |
intrinsics = ('LENSMODEL_OPENCV4', |
| 201 |
np.array(d['intrinsics'] + |
| 202 |
d['distortion_coeffs'], |
| 203 |
dtype=float)) |
| 204 |
else: |
| 205 |
intrinsics = ('LENSMODEL_PINHOLE', |
| 206 |
np.array(d['intrinsics'], |
| 207 |
dtype=float)) |
| 208 |
|
| 209 |
m = mrcal.cameramodel( intrinsics = intrinsics, |
| 210 |
imagersize = np.array(d['resolution'], |
| 211 |
dtype=int) ) |
| 212 |
|
| 213 |
else: |
| 214 |
# cahvor or cahvore. This can only come from the hacked kalibr at jpl |
| 215 |
if 'distortion_model' in d: |
| 216 |
print(f"ERROR: camera_model={d['camera_model']}, so no distortion_model should have been given, but we have distortion_model={d['distortion_model']}", |
| 217 |
file=sys.stderr) |
| 218 |
sys.exit(1) |
| 219 |
|
| 220 |
intrinsics = np.array(d['intrinsics']) |
| 221 |
|
| 222 |
if d['camera_model'] == 'cahvor': |
| 223 |
if intrinsics.size != 6*3: |
| 224 |
print(f"ERROR: camera_model='cahvor' should have {6*3} intrinsics values, but got {intrinsics.size}", |
| 225 |
file=sys.stderr) |
| 226 |
sys.exit(1) |
| 227 |
|
| 228 |
C,A,H,V,O,R = intrinsics.reshape(6,3) |
| 229 |
E = None |
| 230 |
|
| 231 |
else: |
| 232 |
# cahvore |
| 233 |
if intrinsics.size != 7*3: |
| 234 |
print(f"ERROR: camera_model='cahvor' should have {7*3} intrinsics values, but got {intrinsics.size}", |
| 235 |
file=sys.stderr) |
| 236 |
sys.exit(1) |
| 237 |
|
| 238 |
C,A,H,V,O,R,E = intrinsics.reshape(7,3) |
| 239 |
|
| 240 |
try: |
| 241 |
m = mrcal.cahvor._construct_model(C,A,H,V,O,R,E, |
| 242 |
is_cahvor_or_cahvore = True, |
| 243 |
is_cahvore = E is not None, |
| 244 |
cahvore_linearity = d.get('cahvore_linearity',0.37), |
| 245 |
Dimensions = np.array(d['resolution'],dtype=np.int32), |
| 246 |
name = name) |
| 247 |
except Exception as e: |
| 248 |
print(f"ERROR: couldn't parse cahvor(e) model: {e}", |
| 249 |
file=sys.stderr) |
| 250 |
sys.exit(1) |
| 251 |
|
| 252 |
if not Rt_is_identity(m.Rt_cam_ref()): |
| 253 |
print(f"ERROR: given cahvor(e) model has non-identity extrinsics. These may conflict with the extrinsics in the rest of the kalibr file", |
| 254 |
file=sys.stderr) |
| 255 |
sys.exit(1) |
| 256 |
|
| 257 |
|
| 258 |
if not 'T_cn_cnm1' in d: |
| 259 |
Rt_cam_camprev = mrcal.identity_Rt() |
| 260 |
|
| 261 |
else: |
| 262 |
T_cn_cnm1 = np.array(d['T_cn_cnm1']) |
| 263 |
if T_cn_cnm1.shape != (4,4): |
| 264 |
print(f"ERROR: T_cn_cnm1 must be a 4x4 array; got T_cn_cnm1.shape={T_cn_cnm1.shape}", |
| 265 |
file=sys.stderr) |
| 266 |
sys.exit() |
| 267 |
if np.any(T_cn_cnm1[3,:] - np.array((0,0,0,1))): |
| 268 |
print(f"ERROR: T_cn_cnm1 must have a last row of (0,0,0,1); got {T_cn_cnm1[3,:]}", |
| 269 |
file=sys.stderr) |
| 270 |
sys.exit() |
| 271 |
|
| 272 |
# The documentation is here: |
| 273 |
# |
| 274 |
# https://github.com/ethz-asl/kalibr/wiki/Yaml-formats |
| 275 |
# |
| 276 |
# It says: |
| 277 |
# |
| 278 |
# T_cn_cnm1 camera extrinsic transformation, always with respect to the |
| 279 |
# last camera in the chain (e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to |
| 280 |
# cam1 coordinates) |
| 281 |
# |
| 282 |
# This isn't clear. I assume "last" means "previous" |
| 283 |
R_cam_camprev = T_cn_cnm1[:3,:3] |
| 284 |
if nps.norm2((nps.matmult(R_cam_camprev, R_cam_camprev.T) - np.eye(3)).ravel()) > 1e-12: |
| 285 |
print(f"ERROR: T_cn_cnm1[:3,:3] must be a valid rotation. Instead it is {T_cn_cnm1[:3,:3]}", |
| 286 |
file=sys.stderr) |
| 287 |
sys.exit(1) |
| 288 |
t_cam_camprev = T_cn_cnm1[:3, 3] |
| 289 |
Rt_cam_camprev = nps.glue(R_cam_camprev,t_cam_camprev, axis=-2) |
| 290 |
|
| 291 |
if Rt_camprev_ref is not None: |
| 292 |
Rt_cam_ref = mrcal.compose_Rt(Rt_cam_camprev, Rt_camprev_ref) |
| 293 |
else: |
| 294 |
# This is the first camera. What do we use as our reference? |
| 295 |
if args.cam0_at_reference: |
| 296 |
Rt_cam_ref = mrcal.identity_Rt() |
| 297 |
else: |
| 298 |
# By default the "prev cam" from cam0 is the reference |
| 299 |
Rt_cam_ref = Rt_cam_camprev |
| 300 |
|
| 301 |
|
| 302 |
Rt_camprev_ref = Rt_cam_ref # for the next one |
| 303 |
|
| 304 |
m.Rt_cam_ref(Rt_cam_ref) |
| 305 |
return m |
| 306 |
|
| 307 |
|
| 308 |
def convert_all(f, which): |
| 309 |
|
| 310 |
# which is None = "all cameras" |
| 311 |
|
| 312 |
try: |
| 313 |
D = yaml.safe_load(f) |
| 314 |
except yaml.scanner.ScannerError as e: |
| 315 |
print("Error parsing YAML:\n\n") |
| 316 |
print(e) |
| 317 |
sys.exit(1) |
| 318 |
|
| 319 |
if len(D) == 0: |
| 320 |
print("Zero models read from input", |
| 321 |
file=sys.stderr) |
| 322 |
sys.exit(1) |
| 323 |
|
| 324 |
if 'camera_model' in D: |
| 325 |
# one camera; stored inline |
| 326 |
d = D |
| 327 |
if which is not None: |
| 328 |
print(f"Error: a single in-line model found in the file, but a specific camera is requested: '{which}'. Omit --camera", |
| 329 |
file=sys.stderr) |
| 330 |
sys.exit(1) |
| 331 |
try: |
| 332 |
return ( \ |
| 333 |
(None, convert_one(d,'camera')), |
| 334 |
) |
| 335 |
except KeyError as e: |
| 336 |
print(f"Error parsing input; missing key: {e}", file=sys.stderr) |
| 337 |
sys.exit(1) |
| 338 |
|
| 339 |
if which is not None and \ |
| 340 |
not which in D: |
| 341 |
print(f"Error: asked for camera '{which}', but the given file contains only these cameras: {tuple(D.keys())}", |
| 342 |
file=sys.stderr) |
| 343 |
sys.exit(1) |
| 344 |
|
| 345 |
|
| 346 |
# The extrinsics are specified as a serial chain. So I need to parse all the |
| 347 |
# cameras, even if I only need to save one |
| 348 |
models = dict() |
| 349 |
for name,d in D.items(): |
| 350 |
try: |
| 351 |
models[name] = convert_one(d,name) |
| 352 |
except KeyError as e: |
| 353 |
print(f"Error parsing input; missing key: {e}", file=sys.stderr) |
| 354 |
sys.exit(1) |
| 355 |
|
| 356 |
if which is None: |
| 357 |
# all cameras requested |
| 358 |
return [ (name, models[name]) \ |
| 359 |
for name in models.keys() ] |
| 360 |
|
| 361 |
return ( \ |
| 362 |
(which, models[which]), |
| 363 |
) |
| 364 |
|
| 365 |
|
| 366 |
|
| 367 |
if args.model == '-': |
| 368 |
if sys.stdin.isatty(): |
| 369 |
# This isn't an error per-se. But most likely the user ran this |
| 370 |
# without redirecting any data into it. Without this check the |
| 371 |
# program will sit there, waiting for input. Which will look strange |
| 372 |
# to an unsuspecting user |
| 373 |
print("Trying to read a model from standard input, but no file is being redirected into it", |
| 374 |
file=sys.stderr) |
| 375 |
sys.exit(1) |
| 376 |
|
| 377 |
names_models = convert_all(sys.stdin, which = args.camera) |
| 378 |
if len(names_models) > 1: |
| 379 |
print(f"Reading from standard input may only produce ONE model on stdout, but got more here. Pass --camera to select the camera you want; available cameras: {list(n for n,m in names_models)}", |
| 380 |
file=sys.stderr) |
| 381 |
sys.exit(1) |
| 382 |
|
| 383 |
names_models[0][1].write(sys.stdout) |
| 384 |
|
| 385 |
else: |
| 386 |
base,extension = os.path.splitext(args.model) |
| 387 |
if extension.lower() == '.cameramodel': |
| 388 |
print("Input file is already in the cameramodel format (judging from the filename). Doing nothing", |
| 389 |
file=sys.stderr) |
| 390 |
sys.exit(0) |
| 391 |
|
| 392 |
if args.outdir is not None: |
| 393 |
base = args.outdir + '/' + os.path.split(base)[1] |
| 394 |
|
| 395 |
with open(args.model, "r") as f: |
| 396 |
names_models = convert_all(f, which = args.camera) |
| 397 |
|
| 398 |
for name,model in names_models: |
| 399 |
|
| 400 |
if name is None: name = '' |
| 401 |
else: name = f"-{name}" |
| 402 |
filename_out = f"{base}{name}.cameramodel" |
| 403 |
if not args.force and os.path.exists(filename_out): |
| 404 |
print(f"Target model '{filename_out}' already exists. Doing nothing with this model. Pass -f to overwrite", |
| 405 |
file=sys.stderr) |
| 406 |
continue |
| 407 |
|
| 408 |
model.write(filename_out) |
| 409 |
print("Wrote " + filename_out) |