# -----------------------------------------------------------------------------
# This file was autogenerated by symforce from template:
# cam_package/ops/CLASS/camera_ops.py.jinja
# Do NOT modify by hand.
# -----------------------------------------------------------------------------
# ruff: noqa: PLR0915, F401, PLW0211, PLR0914
import math
import typing as T
import numpy
import sym
[docs]class CameraOps(object):
"""
Python CameraOps implementation for :py:class:`symforce.cam.double_sphere_camera_cal.DoubleSphereCameraCal`.
"""
[docs] @staticmethod
def focal_length(self):
# type: (sym.DoubleSphereCameraCal) -> numpy.ndarray
"""
Return the focal length.
"""
# Total ops: 0
# Input arrays
_self = self.data
# Intermediate terms (0)
# Output terms
_focal_length = numpy.zeros(2)
_focal_length[0] = _self[0]
_focal_length[1] = _self[1]
return _focal_length
[docs] @staticmethod
def principal_point(self):
# type: (sym.DoubleSphereCameraCal) -> numpy.ndarray
"""
Return the principal point.
"""
# Total ops: 0
# Input arrays
_self = self.data
# Intermediate terms (0)
# Output terms
_principal_point = numpy.zeros(2)
_principal_point[0] = _self[2]
_principal_point[1] = _self[3]
return _principal_point
[docs] @staticmethod
def pixel_from_camera_point(self, point, epsilon):
# type: (sym.DoubleSphereCameraCal, numpy.ndarray, float) -> T.Tuple[numpy.ndarray, float]
"""
Project a 3D point in the camera frame into 2D pixel coordinates.
Returns:
pixel: (x, y) coordinate in pixels if valid
is_valid: 1 if the operation is within bounds else 0
"""
# Total ops: 73
# Input arrays
_self = self.data
if point.shape == (3,):
point = point.reshape((3, 1))
elif point.shape != (3, 1):
raise IndexError(
"point is expected to have shape (3, 1) or (3,); instead had shape {}".format(
point.shape
)
)
# Intermediate terms (13)
_tmp0 = epsilon**2 + point[0, 0] ** 2 + point[1, 0] ** 2
_tmp1 = math.sqrt(_tmp0 + point[2, 0] ** 2)
_tmp2 = _self[4] * _tmp1 + point[2, 0]
_tmp3 = min(0, (0.0 if _self[5] - 0.5 == 0 else math.copysign(1, _self[5] - 0.5)))
_tmp4 = 2 * _tmp3
_tmp5 = _self[5] - epsilon * (_tmp4 + 1)
_tmp6 = -_tmp5
_tmp7 = 1 / max(epsilon, _tmp2 * (_tmp6 + 1) + _tmp5 * math.sqrt(_tmp0 + _tmp2**2))
_tmp8 = _tmp3 + _tmp5
_tmp9 = (1.0 / 2.0) * _tmp4 + _tmp6 + 1
_tmp10 = _self[4] ** 2
_tmp11 = _tmp9**2 / _tmp8**2
_tmp12 = _tmp10 * _tmp11 - _tmp10 + 1
# Output terms
_pixel = numpy.zeros(2)
_pixel[0] = _self[0] * _tmp7 * point[0, 0] + _self[2]
_pixel[1] = _self[1] * _tmp7 * point[1, 0] + _self[3]
_is_valid = max(
0,
min(
max(
-(0.0 if _self[4] - 1 == 0 else math.copysign(1, _self[4] - 1)),
1
- max(
0,
-(
0.0
if _self[4] * point[2, 0] + _tmp1 == 0
else math.copysign(1, _self[4] * point[2, 0] + _tmp1)
),
),
),
max(
-(0.0 if _tmp12 == 0 else math.copysign(1, _tmp12)),
1
- max(
0,
-(
0.0
if -_tmp1
* (
_self[4] * _tmp11
- _self[4]
- _tmp9 * math.sqrt(max(_tmp12, math.sqrt(epsilon))) / _tmp8
)
+ point[2, 0]
== 0
else math.copysign(
1,
-_tmp1
* (
_self[4] * _tmp11
- _self[4]
- _tmp9 * math.sqrt(max(_tmp12, math.sqrt(epsilon))) / _tmp8
)
+ point[2, 0],
)
),
),
),
),
)
return _pixel, _is_valid
[docs] @staticmethod
def pixel_from_camera_point_with_jacobians(self, point, epsilon):
# type: (sym.DoubleSphereCameraCal, numpy.ndarray, float) -> T.Tuple[numpy.ndarray, float, numpy.ndarray, numpy.ndarray]
"""
Project a 3D point in the camera frame into 2D pixel coordinates.
Returns:
pixel: (x, y) coordinate in pixels if valid
is_valid: 1 if the operation is within bounds else 0
pixel_D_cal: Derivative of pixel with respect to intrinsic calibration parameters
pixel_D_point: Derivative of pixel with respect to point
"""
# Total ops: 134
# Input arrays
_self = self.data
if point.shape == (3,):
point = point.reshape((3, 1))
elif point.shape != (3, 1):
raise IndexError(
"point is expected to have shape (3, 1) or (3,); instead had shape {}".format(
point.shape
)
)
# Intermediate terms (40)
_tmp0 = epsilon**2 + point[0, 0] ** 2 + point[1, 0] ** 2
_tmp1 = math.sqrt(_tmp0 + point[2, 0] ** 2)
_tmp2 = _self[4] * _tmp1 + point[2, 0]
_tmp3 = min(0, (0.0 if _self[5] - 0.5 == 0 else math.copysign(1, _self[5] - 0.5)))
_tmp4 = 2 * _tmp3
_tmp5 = _self[5] - epsilon * (_tmp4 + 1)
_tmp6 = -_tmp5
_tmp7 = _tmp6 + 1
_tmp8 = math.sqrt(_tmp0 + _tmp2**2)
_tmp9 = _tmp2 * _tmp7 + _tmp5 * _tmp8
_tmp10 = max(_tmp9, epsilon)
_tmp11 = 1 / _tmp10
_tmp12 = _self[0] * _tmp11
_tmp13 = _self[1] * _tmp11
_tmp14 = _self[4] * point[2, 0]
_tmp15 = _tmp3 + _tmp5
_tmp16 = (1.0 / 2.0) * _tmp4 + _tmp6 + 1
_tmp17 = _self[4] ** 2
_tmp18 = _tmp16**2 / _tmp15**2
_tmp19 = _tmp17 * _tmp18 - _tmp17 + 1
_tmp20 = _tmp5 / _tmp8
_tmp21 = _tmp2 * _tmp20
_tmp22 = _tmp1 * _tmp21 + _tmp1 * _tmp7
_tmp23 = (
(1.0 / 2.0)
* ((0.0 if _tmp9 - epsilon == 0 else math.copysign(1, _tmp9 - epsilon)) + 1)
/ _tmp10**2
)
_tmp24 = _self[0] * point[0, 0]
_tmp25 = _tmp23 * _tmp24
_tmp26 = _self[1] * point[1, 0]
_tmp27 = _tmp23 * _tmp26
_tmp28 = -_tmp2 + _tmp8
_tmp29 = 1 / _tmp1
_tmp30 = _self[4] * _tmp29
_tmp31 = _tmp30 * _tmp7
_tmp32 = 2 * point[0, 0]
_tmp33 = _tmp2 * _tmp30
_tmp34 = (1.0 / 2.0) * _tmp20
_tmp35 = _tmp23 * (_tmp31 * point[0, 0] + _tmp34 * (_tmp32 * _tmp33 + _tmp32))
_tmp36 = 2 * point[1, 0]
_tmp37 = _tmp31 * point[1, 0] + _tmp34 * (_tmp33 * _tmp36 + _tmp36)
_tmp38 = _tmp14 * _tmp29 + 1
_tmp39 = _tmp21 * _tmp38 + _tmp38 * _tmp7
# Output terms
_pixel = numpy.zeros(2)
_pixel[0] = _self[2] + _tmp12 * point[0, 0]
_pixel[1] = _self[3] + _tmp13 * point[1, 0]
_is_valid = max(
0,
min(
max(
-(0.0 if _self[4] - 1 == 0 else math.copysign(1, _self[4] - 1)),
1 - max(0, -(0.0 if _tmp1 + _tmp14 == 0 else math.copysign(1, _tmp1 + _tmp14))),
),
max(
-(0.0 if _tmp19 == 0 else math.copysign(1, _tmp19)),
1
- max(
0,
-(
0.0
if -_tmp1
* (
_self[4] * _tmp18
- _self[4]
- _tmp16 * math.sqrt(max(_tmp19, math.sqrt(epsilon))) / _tmp15
)
+ point[2, 0]
== 0
else math.copysign(
1,
-_tmp1
* (
_self[4] * _tmp18
- _self[4]
- _tmp16 * math.sqrt(max(_tmp19, math.sqrt(epsilon))) / _tmp15
)
+ point[2, 0],
)
),
),
),
),
)
_pixel_D_cal = numpy.zeros((2, 6))
_pixel_D_cal[0, 0] = _tmp11 * point[0, 0]
_pixel_D_cal[1, 0] = 0
_pixel_D_cal[0, 1] = 0
_pixel_D_cal[1, 1] = _tmp11 * point[1, 0]
_pixel_D_cal[0, 2] = 1
_pixel_D_cal[1, 2] = 0
_pixel_D_cal[0, 3] = 0
_pixel_D_cal[1, 3] = 1
_pixel_D_cal[0, 4] = -_tmp22 * _tmp25
_pixel_D_cal[1, 4] = -_tmp22 * _tmp27
_pixel_D_cal[0, 5] = -_tmp25 * _tmp28
_pixel_D_cal[1, 5] = -_tmp27 * _tmp28
_pixel_D_point = numpy.zeros((2, 3))
_pixel_D_point[0, 0] = _tmp12 - _tmp24 * _tmp35
_pixel_D_point[1, 0] = -_tmp26 * _tmp35
_pixel_D_point[0, 1] = -_tmp25 * _tmp37
_pixel_D_point[1, 1] = _tmp13 - _tmp27 * _tmp37
_pixel_D_point[0, 2] = -_tmp25 * _tmp39
_pixel_D_point[1, 2] = -_tmp27 * _tmp39
return _pixel, _is_valid, _pixel_D_cal, _pixel_D_point
[docs] @staticmethod
def camera_ray_from_pixel(self, pixel, epsilon):
# type: (sym.DoubleSphereCameraCal, numpy.ndarray, float) -> T.Tuple[numpy.ndarray, float]
"""
Backproject a 2D pixel coordinate into a 3D ray in the camera frame.
Returns:
camera_ray: The ray in the camera frame (NOT normalized)
is_valid: 1 if the operation is within bounds else 0
"""
# Total ops: 62
# Input arrays
_self = self.data
if pixel.shape == (2,):
pixel = pixel.reshape((2, 1))
elif pixel.shape != (2, 1):
raise IndexError(
"pixel is expected to have shape (2, 1) or (2,); instead had shape {}".format(
pixel.shape
)
)
# Intermediate terms (12)
_tmp0 = -_self[2] + pixel[0, 0]
_tmp1 = -_self[3] + pixel[1, 0]
_tmp2 = _tmp1**2 / _self[1] ** 2 + _tmp0**2 / _self[0] ** 2
_tmp3 = -_tmp2 * (2 * _self[5] - 1) + 1
_tmp4 = _self[5] * math.sqrt(max(_tmp3, epsilon)) - _self[5] + 1
_tmp5 = _tmp4 + epsilon * (2 * min(0, (0.0 if _tmp4 == 0 else math.copysign(1, _tmp4))) + 1)
_tmp6 = -(_self[5] ** 2) * _tmp2 + 1
_tmp7 = _tmp6 / _tmp5
_tmp8 = _tmp6**2 / _tmp5**2
_tmp9 = _tmp2 * (1 - _self[4] ** 2) + _tmp8
_tmp10 = _tmp2 + _tmp8
_tmp11 = (_self[4] * _tmp7 + math.sqrt(max(_tmp9, epsilon))) / (
_tmp10 + epsilon * (2 * min(0, (0.0 if _tmp10 == 0 else math.copysign(1, _tmp10))) + 1)
)
# Output terms
_camera_ray = numpy.zeros(3)
_camera_ray[0] = _tmp0 * _tmp11 / _self[0]
_camera_ray[1] = _tmp1 * _tmp11 / _self[1]
_camera_ray[2] = -_self[4] + _tmp11 * _tmp7
_is_valid = min(
1 - max(0, -(0.0 if _tmp3 == 0 else math.copysign(1, _tmp3))),
1 - max(0, -(0.0 if _tmp9 == 0 else math.copysign(1, _tmp9))),
)
return _camera_ray, _is_valid
[docs] @staticmethod
def camera_ray_from_pixel_with_jacobians(self, pixel, epsilon):
# type: (sym.DoubleSphereCameraCal, numpy.ndarray, float) -> T.Tuple[numpy.ndarray, float, numpy.ndarray, numpy.ndarray]
"""
Backproject a 2D pixel coordinate into a 3D ray in the camera frame.
Returns:
camera_ray: The ray in the camera frame (NOT normalized)
is_valid: 1 if the operation is within bounds else 0
point_D_cal: Derivative of point with respect to intrinsic calibration parameters
point_D_pixel: Derivation of point with respect to pixel
"""
# Total ops: 298
# Input arrays
_self = self.data
if pixel.shape == (2,):
pixel = pixel.reshape((2, 1))
elif pixel.shape != (2, 1):
raise IndexError(
"pixel is expected to have shape (2, 1) or (2,); instead had shape {}".format(
pixel.shape
)
)
# Intermediate terms (117)
_tmp0 = -_self[2] + pixel[0, 0]
_tmp1 = 1 / _self[0]
_tmp2 = _tmp0**2
_tmp3 = _self[0] ** (-2)
_tmp4 = -_self[3] + pixel[1, 0]
_tmp5 = _tmp4**2
_tmp6 = _self[1] ** (-2)
_tmp7 = _tmp2 * _tmp3 + _tmp5 * _tmp6
_tmp8 = 2 * _self[5]
_tmp9 = _tmp8 - 1
_tmp10 = -_tmp7 * _tmp9 + 1
_tmp11 = math.sqrt(max(_tmp10, epsilon))
_tmp12 = _self[5] * _tmp11 - _self[5] + 1
_tmp13 = _tmp12 + epsilon * (
2 * min(0, (0.0 if _tmp12 == 0 else math.copysign(1, _tmp12))) + 1
)
_tmp14 = 1 / _tmp13
_tmp15 = _self[5] ** 2
_tmp16 = -_tmp15 * _tmp7 + 1
_tmp17 = _tmp14 * _tmp16
_tmp18 = _tmp13 ** (-2)
_tmp19 = _tmp16**2
_tmp20 = _tmp18 * _tmp19
_tmp21 = 1 - _self[4] ** 2
_tmp22 = _tmp20 + _tmp21 * _tmp7
_tmp23 = math.sqrt(max(_tmp22, epsilon))
_tmp24 = _self[4] * _tmp17 + _tmp23
_tmp25 = _tmp20 + _tmp7
_tmp26 = _tmp25 + epsilon * (
2 * min(0, (0.0 if _tmp25 == 0 else math.copysign(1, _tmp25))) + 1
)
_tmp27 = 1 / _tmp26
_tmp28 = _tmp24 * _tmp27
_tmp29 = _tmp1 * _tmp28
_tmp30 = 1 / _self[1]
_tmp31 = _tmp28 * _tmp30
_tmp32 = _tmp2 / _self[0] ** 3
_tmp33 = 2 * _tmp32
_tmp34 = _self[5] * _tmp9
_tmp35 = _tmp19 / _tmp13**3
_tmp36 = -epsilon
_tmp37 = ((0.0 if _tmp10 + _tmp36 == 0 else math.copysign(1, _tmp10 + _tmp36)) + 1) / _tmp11
_tmp38 = _tmp35 * _tmp37
_tmp39 = _tmp34 * _tmp38
_tmp40 = _tmp16 * _tmp18
_tmp41 = 4 * _tmp40
_tmp42 = _tmp15 * _tmp41
_tmp43 = -_tmp32 * _tmp39 + _tmp32 * _tmp42
_tmp44 = ((0.0 if _tmp22 + _tmp36 == 0 else math.copysign(1, _tmp22 + _tmp36)) + 1) / _tmp23
_tmp45 = (1.0 / 4.0) * _tmp44
_tmp46 = (1.0 / 2.0) * _tmp37
_tmp47 = _tmp40 * _tmp46
_tmp48 = _self[4] * _tmp47
_tmp49 = _tmp34 * _tmp48
_tmp50 = _tmp14 * _tmp15
_tmp51 = _self[4] * _tmp50
_tmp52 = _tmp27 * (
-_tmp32 * _tmp49 + _tmp33 * _tmp51 + _tmp45 * (-_tmp21 * _tmp33 + _tmp43)
)
_tmp53 = _tmp0 * _tmp1
_tmp54 = -_tmp33 + _tmp43
_tmp55 = _tmp24 / _tmp26**2
_tmp56 = _tmp53 * _tmp55
_tmp57 = _tmp0 * _tmp3
_tmp58 = _tmp28 * _tmp57
_tmp59 = _tmp30 * _tmp4
_tmp60 = _tmp55 * _tmp59
_tmp61 = _tmp28 * _tmp50
_tmp62 = _tmp17 * _tmp55
_tmp63 = _tmp34 * _tmp47
_tmp64 = _tmp5 / _self[1] ** 3
_tmp65 = 2 * _tmp64
_tmp66 = _tmp34 * _tmp64
_tmp67 = -_tmp38 * _tmp66 + _tmp42 * _tmp64
_tmp68 = _tmp45 * (-_tmp21 * _tmp65 + _tmp67) - _tmp48 * _tmp66 + _tmp51 * _tmp65
_tmp69 = _tmp27 * _tmp68
_tmp70 = -_tmp65 + _tmp67
_tmp71 = _tmp4 * _tmp6
_tmp72 = _tmp28 * _tmp71
_tmp73 = _tmp17 * _tmp27
_tmp74 = 2 * _tmp57
_tmp75 = _tmp21 * _tmp74
_tmp76 = _tmp39 * _tmp57
_tmp77 = _tmp42 * _tmp57
_tmp78 = -_tmp76 + _tmp77
_tmp79 = _tmp49 * _tmp57
_tmp80 = _tmp51 * _tmp74
_tmp81 = _tmp45 * (-_tmp75 + _tmp78) - _tmp79 + _tmp80
_tmp82 = _tmp27 * _tmp81
_tmp83 = -_tmp74 + _tmp78
_tmp84 = _tmp61 * _tmp74
_tmp85 = _tmp58 * _tmp63
_tmp86 = 2 * _tmp71
_tmp87 = _tmp39 * _tmp71
_tmp88 = _tmp42 * _tmp71
_tmp89 = -_tmp87 + _tmp88
_tmp90 = -_tmp86 + _tmp89
_tmp91 = _tmp21 * _tmp86
_tmp92 = _tmp49 * _tmp71
_tmp93 = _tmp51 * _tmp86
_tmp94 = _tmp45 * (_tmp89 - _tmp91) - _tmp92 + _tmp93
_tmp95 = _tmp27 * _tmp94
_tmp96 = _tmp61 * _tmp86
_tmp97 = _tmp63 * _tmp72
_tmp98 = _self[4] * _tmp7
_tmp99 = _tmp17 - 1.0 / 2.0 * _tmp44 * _tmp98
_tmp100 = _tmp27 * _tmp99
_tmp101 = _self[5] * _tmp7
_tmp102 = -_tmp101 * _tmp46 + _tmp11 - 1
_tmp103 = -_tmp101 * _tmp41 - 2 * _tmp102 * _tmp35
_tmp104 = _tmp103 * _tmp55
_tmp105 = _tmp14 * _tmp8
_tmp106 = _tmp102 * _tmp40
_tmp107 = -_self[4] * _tmp106 + _tmp103 * _tmp45 - _tmp105 * _tmp98
_tmp108 = _tmp107 * _tmp27
_tmp109 = _tmp76 - _tmp77
_tmp110 = _tmp45 * (_tmp109 + _tmp75) + _tmp79 - _tmp80
_tmp111 = _tmp110 * _tmp27
_tmp112 = _tmp109 + _tmp74
_tmp113 = _tmp87 - _tmp88
_tmp114 = _tmp113 + _tmp86
_tmp115 = _tmp45 * (_tmp113 + _tmp91) + _tmp92 - _tmp93
_tmp116 = _tmp115 * _tmp27
# Output terms
_camera_ray = numpy.zeros(3)
_camera_ray[0] = _tmp0 * _tmp29
_camera_ray[1] = _tmp31 * _tmp4
_camera_ray[2] = -_self[4] + _tmp17 * _tmp28
_is_valid = min(
1 - max(0, -(0.0 if _tmp10 == 0 else math.copysign(1, _tmp10))),
1 - max(0, -(0.0 if _tmp22 == 0 else math.copysign(1, _tmp22))),
)
_point_D_cal = numpy.zeros((3, 6))
_point_D_cal[0, 0] = _tmp52 * _tmp53 - _tmp54 * _tmp56 - _tmp58
_point_D_cal[1, 0] = _tmp52 * _tmp59 - _tmp54 * _tmp60
_point_D_cal[2, 0] = (
_tmp17 * _tmp52 - _tmp28 * _tmp32 * _tmp63 + _tmp33 * _tmp61 - _tmp54 * _tmp62
)
_point_D_cal[0, 1] = _tmp53 * _tmp69 - _tmp56 * _tmp70
_point_D_cal[1, 1] = _tmp59 * _tmp69 - _tmp60 * _tmp70 - _tmp72
_point_D_cal[2, 1] = (
-_tmp28 * _tmp47 * _tmp66 + _tmp61 * _tmp65 - _tmp62 * _tmp70 + _tmp68 * _tmp73
)
_point_D_cal[0, 2] = -_tmp29 + _tmp53 * _tmp82 - _tmp56 * _tmp83
_point_D_cal[1, 2] = _tmp59 * _tmp82 - _tmp60 * _tmp83
_point_D_cal[2, 2] = -_tmp62 * _tmp83 + _tmp73 * _tmp81 + _tmp84 - _tmp85
_point_D_cal[0, 3] = _tmp53 * _tmp95 - _tmp56 * _tmp90
_point_D_cal[1, 3] = -_tmp31 + _tmp59 * _tmp95 - _tmp60 * _tmp90
_point_D_cal[2, 3] = -_tmp62 * _tmp90 + _tmp73 * _tmp94 + _tmp96 - _tmp97
_point_D_cal[0, 4] = _tmp100 * _tmp53
_point_D_cal[1, 4] = _tmp100 * _tmp59
_point_D_cal[2, 4] = _tmp73 * _tmp99 - 1
_point_D_cal[0, 5] = -_tmp104 * _tmp53 + _tmp108 * _tmp53
_point_D_cal[1, 5] = -_tmp103 * _tmp60 + _tmp108 * _tmp59
_point_D_cal[2, 5] = (
-_tmp104 * _tmp17 - _tmp105 * _tmp28 * _tmp7 - _tmp106 * _tmp28 + _tmp107 * _tmp73
)
_point_D_pixel = numpy.zeros((3, 2))
_point_D_pixel[0, 0] = _tmp111 * _tmp53 - _tmp112 * _tmp56 + _tmp29
_point_D_pixel[1, 0] = _tmp111 * _tmp59 - _tmp112 * _tmp60
_point_D_pixel[2, 0] = _tmp110 * _tmp73 - _tmp112 * _tmp62 - _tmp84 + _tmp85
_point_D_pixel[0, 1] = -_tmp114 * _tmp56 + _tmp116 * _tmp53
_point_D_pixel[1, 1] = -_tmp114 * _tmp60 + _tmp116 * _tmp59 + _tmp31
_point_D_pixel[2, 1] = -_tmp114 * _tmp62 + _tmp115 * _tmp73 - _tmp96 + _tmp97
return _camera_ray, _is_valid, _point_D_cal, _point_D_pixel