# -----------------------------------------------------------------------------
# This file was autogenerated by symforce from template:
# ops/CLASS/group_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 GroupOps(object):
"""
Python GroupOps implementation for :py:class:`symforce.geo.pose3.Pose3`.
"""
[docs] @staticmethod
def identity():
# type: () -> sym.Pose3
# Total ops: 0
# Input arrays
# Intermediate terms (0)
# Output terms
_res = [0.0] * 7
_res[0] = 0
_res[1] = 0
_res[2] = 0
_res[3] = 1
_res[4] = 0
_res[5] = 0
_res[6] = 0
return sym.Pose3.from_storage(_res)
[docs] @staticmethod
def inverse(a):
# type: (sym.Pose3) -> sym.Pose3
# Total ops: 49
# Input arrays
_a = a.data
# Intermediate terms (11)
_tmp0 = 2 * _a[0]
_tmp1 = _a[1] * _tmp0
_tmp2 = 2 * _a[3]
_tmp3 = _a[2] * _tmp2
_tmp4 = _a[2] * _tmp0
_tmp5 = _a[1] * _tmp2
_tmp6 = -2 * _a[1] ** 2
_tmp7 = 1 - 2 * _a[2] ** 2
_tmp8 = 2 * _a[1] * _a[2]
_tmp9 = _a[3] * _tmp0
_tmp10 = -2 * _a[0] ** 2
# Output terms
_res = [0.0] * 7
_res[0] = -_a[0]
_res[1] = -_a[1]
_res[2] = -_a[2]
_res[3] = _a[3]
_res[4] = -_a[4] * (_tmp6 + _tmp7) - _a[5] * (_tmp1 + _tmp3) - _a[6] * (_tmp4 - _tmp5)
_res[5] = -_a[4] * (_tmp1 - _tmp3) - _a[5] * (_tmp10 + _tmp7) - _a[6] * (_tmp8 + _tmp9)
_res[6] = -_a[4] * (_tmp4 + _tmp5) - _a[5] * (_tmp8 - _tmp9) - _a[6] * (_tmp10 + _tmp6 + 1)
return sym.Pose3.from_storage(_res)
[docs] @staticmethod
def compose(a, b):
# type: (sym.Pose3, sym.Pose3) -> sym.Pose3
# Total ops: 74
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (11)
_tmp0 = 2 * _a[0]
_tmp1 = _a[2] * _tmp0
_tmp2 = 2 * _a[3]
_tmp3 = _a[1] * _tmp2
_tmp4 = _a[1] * _tmp0
_tmp5 = _a[2] * _tmp2
_tmp6 = -2 * _a[2] ** 2
_tmp7 = -2 * _a[1] ** 2
_tmp8 = 2 * _a[1] * _a[2]
_tmp9 = _a[3] * _tmp0
_tmp10 = 1 - 2 * _a[0] ** 2
# Output terms
_res = [0.0] * 7
_res[0] = _a[0] * _b[3] + _a[1] * _b[2] - _a[2] * _b[1] + _a[3] * _b[0]
_res[1] = -_a[0] * _b[2] + _a[1] * _b[3] + _a[2] * _b[0] + _a[3] * _b[1]
_res[2] = _a[0] * _b[1] - _a[1] * _b[0] + _a[2] * _b[3] + _a[3] * _b[2]
_res[3] = -_a[0] * _b[0] - _a[1] * _b[1] - _a[2] * _b[2] + _a[3] * _b[3]
_res[4] = (
_a[4] + _b[4] * (_tmp6 + _tmp7 + 1) + _b[5] * (_tmp4 - _tmp5) + _b[6] * (_tmp1 + _tmp3)
)
_res[5] = (
_a[5] + _b[4] * (_tmp4 + _tmp5) + _b[5] * (_tmp10 + _tmp6) + _b[6] * (_tmp8 - _tmp9)
)
_res[6] = (
_a[6] + _b[4] * (_tmp1 - _tmp3) + _b[5] * (_tmp8 + _tmp9) + _b[6] * (_tmp10 + _tmp7)
)
return sym.Pose3.from_storage(_res)
[docs] @staticmethod
def between(a, b):
# type: (sym.Pose3, sym.Pose3) -> sym.Pose3
# Total ops: 89
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (20)
_tmp0 = 2 * _a[0]
_tmp1 = _a[2] * _tmp0
_tmp2 = 2 * _a[3]
_tmp3 = _a[1] * _tmp2
_tmp4 = _tmp1 - _tmp3
_tmp5 = _a[1] * _tmp0
_tmp6 = _a[2] * _tmp2
_tmp7 = _tmp5 + _tmp6
_tmp8 = -2 * _a[2] ** 2
_tmp9 = 1 - 2 * _a[1] ** 2
_tmp10 = _tmp8 + _tmp9
_tmp11 = _tmp5 - _tmp6
_tmp12 = 2 * _a[1] * _a[2]
_tmp13 = _a[3] * _tmp0
_tmp14 = _tmp12 + _tmp13
_tmp15 = -2 * _a[0] ** 2
_tmp16 = _tmp15 + _tmp8 + 1
_tmp17 = _tmp1 + _tmp3
_tmp18 = _tmp12 - _tmp13
_tmp19 = _tmp15 + _tmp9
# Output terms
_res = [0.0] * 7
_res[0] = -_a[0] * _b[3] - _a[1] * _b[2] + _a[2] * _b[1] + _a[3] * _b[0]
_res[1] = _a[0] * _b[2] - _a[1] * _b[3] - _a[2] * _b[0] + _a[3] * _b[1]
_res[2] = -_a[0] * _b[1] + _a[1] * _b[0] - _a[2] * _b[3] + _a[3] * _b[2]
_res[3] = _a[0] * _b[0] + _a[1] * _b[1] + _a[2] * _b[2] + _a[3] * _b[3]
_res[4] = (
-_a[4] * _tmp10
- _a[5] * _tmp7
- _a[6] * _tmp4
+ _b[4] * _tmp10
+ _b[5] * _tmp7
+ _b[6] * _tmp4
)
_res[5] = (
-_a[4] * _tmp11
- _a[5] * _tmp16
- _a[6] * _tmp14
+ _b[4] * _tmp11
+ _b[5] * _tmp16
+ _b[6] * _tmp14
)
_res[6] = (
-_a[4] * _tmp17
- _a[5] * _tmp18
- _a[6] * _tmp19
+ _b[4] * _tmp17
+ _b[5] * _tmp18
+ _b[6] * _tmp19
)
return sym.Pose3.from_storage(_res)
[docs] @staticmethod
def inverse_with_jacobian(a):
# type: (sym.Pose3) -> T.Tuple[sym.Pose3, numpy.ndarray]
# Total ops: 115
# Input arrays
_a = a.data
# Intermediate terms (48)
_tmp0 = 2 * _a[0]
_tmp1 = _a[1] * _tmp0
_tmp2 = 2 * _a[3]
_tmp3 = _a[2] * _tmp2
_tmp4 = _a[5] * (_tmp1 + _tmp3)
_tmp5 = _a[2] * _tmp0
_tmp6 = _a[1] * _tmp2
_tmp7 = -_tmp6
_tmp8 = _a[6] * (_tmp5 + _tmp7)
_tmp9 = _a[2] ** 2
_tmp10 = 2 * _tmp9
_tmp11 = -_tmp10
_tmp12 = _a[1] ** 2
_tmp13 = 2 * _tmp12
_tmp14 = -_tmp13
_tmp15 = -_tmp3
_tmp16 = _a[4] * (_tmp1 + _tmp15)
_tmp17 = 2 * _a[1] * _a[2]
_tmp18 = _a[3] * _tmp0
_tmp19 = _a[6] * (_tmp17 + _tmp18)
_tmp20 = _a[0] ** 2
_tmp21 = 2 * _tmp20
_tmp22 = 1 - _tmp21
_tmp23 = _a[4] * (_tmp5 + _tmp6)
_tmp24 = -_tmp18
_tmp25 = _a[5] * (_tmp17 + _tmp24)
_tmp26 = _a[3] ** 2
_tmp27 = -_tmp26
_tmp28 = _tmp27 + _tmp9
_tmp29 = -_tmp20
_tmp30 = _tmp12 + _tmp29
_tmp31 = _tmp28 + _tmp30
_tmp32 = -_tmp1
_tmp33 = _tmp15 + _tmp32
_tmp34 = -_tmp5
_tmp35 = _tmp34 + _tmp6
_tmp36 = -_tmp12
_tmp37 = _tmp3 + _tmp32
_tmp38 = -_tmp17
_tmp39 = _tmp24 + _tmp38
_tmp40 = _tmp20 + _tmp36
_tmp41 = _tmp28 + _tmp40
_tmp42 = _tmp34 + _tmp7
_tmp43 = _tmp18 + _tmp38
_tmp44 = -_tmp9
_tmp45 = _tmp12 + _tmp20 + _tmp27 + _tmp44
_tmp46 = _tmp26 + _tmp44
_tmp47 = _tmp21 - 1
# Output terms
_res = [0.0] * 7
_res[0] = -_a[0]
_res[1] = -_a[1]
_res[2] = -_a[2]
_res[3] = _a[3]
_res[4] = -_a[4] * (_tmp11 + _tmp14 + 1) - _tmp4 - _tmp8
_res[5] = -_a[5] * (_tmp11 + _tmp22) - _tmp16 - _tmp19
_res[6] = -_a[6] * (_tmp14 + _tmp22) - _tmp23 - _tmp25
_res_D_a = numpy.zeros((6, 6))
_res_D_a[0, 0] = _tmp31
_res_D_a[1, 0] = _tmp33
_res_D_a[2, 0] = _tmp35
_res_D_a[3, 0] = 0
_res_D_a[4, 0] = -_a[6] * (_tmp26 + _tmp29 + _tmp36 + _tmp9) - _tmp23 - _tmp25
_res_D_a[5, 0] = -_a[4] * _tmp37 - _a[5] * _tmp41 - _a[6] * _tmp39
_res_D_a[0, 1] = _tmp37
_res_D_a[1, 1] = _tmp41
_res_D_a[2, 1] = _tmp39
_res_D_a[3, 1] = -_a[4] * _tmp42 - _a[5] * _tmp43 - _a[6] * _tmp45
_res_D_a[4, 1] = 0
_res_D_a[5, 1] = -_a[4] * (_tmp40 + _tmp46) - _tmp4 - _tmp8
_res_D_a[0, 2] = _tmp42
_res_D_a[1, 2] = _tmp43
_res_D_a[2, 2] = _tmp45
_res_D_a[3, 2] = -_a[5] * (_tmp30 + _tmp46) - _tmp16 - _tmp19
_res_D_a[4, 2] = -_a[4] * _tmp31 - _a[5] * _tmp33 - _a[6] * _tmp35
_res_D_a[5, 2] = 0
_res_D_a[0, 3] = 0
_res_D_a[1, 3] = 0
_res_D_a[2, 3] = 0
_res_D_a[3, 3] = _tmp10 + _tmp13 - 1
_res_D_a[4, 3] = _tmp37
_res_D_a[5, 3] = _tmp42
_res_D_a[0, 4] = 0
_res_D_a[1, 4] = 0
_res_D_a[2, 4] = 0
_res_D_a[3, 4] = _tmp33
_res_D_a[4, 4] = _tmp10 + _tmp47
_res_D_a[5, 4] = _tmp43
_res_D_a[0, 5] = 0
_res_D_a[1, 5] = 0
_res_D_a[2, 5] = 0
_res_D_a[3, 5] = _tmp35
_res_D_a[4, 5] = _tmp39
_res_D_a[5, 5] = _tmp13 + _tmp47
return sym.Pose3.from_storage(_res), _res_D_a
[docs] @staticmethod
def compose_with_jacobians(a, b):
# type: (sym.Pose3, sym.Pose3) -> T.Tuple[sym.Pose3, numpy.ndarray, numpy.ndarray]
# Total ops: 330
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (134)
_tmp0 = _a[2] * _b[1]
_tmp1 = _a[0] * _b[3]
_tmp2 = _a[1] * _b[2]
_tmp3 = _a[3] * _b[0]
_tmp4 = -_tmp0 + _tmp1 + _tmp2 + _tmp3
_tmp5 = _a[3] * _b[1]
_tmp6 = _a[1] * _b[3]
_tmp7 = _a[0] * _b[2]
_tmp8 = _a[2] * _b[0]
_tmp9 = _tmp5 + _tmp6 - _tmp7 + _tmp8
_tmp10 = _a[0] * _b[1]
_tmp11 = _a[2] * _b[3]
_tmp12 = _a[3] * _b[2]
_tmp13 = _a[1] * _b[0]
_tmp14 = _tmp10 + _tmp11 + _tmp12 - _tmp13
_tmp15 = _a[1] * _b[1]
_tmp16 = _a[2] * _b[2]
_tmp17 = _a[0] * _b[0]
_tmp18 = _a[3] * _b[3]
_tmp19 = -_tmp15 - _tmp16 - _tmp17 + _tmp18
_tmp20 = 2 * _a[0]
_tmp21 = _a[2] * _tmp20
_tmp22 = 2 * _a[3]
_tmp23 = _a[1] * _tmp22
_tmp24 = _tmp21 + _tmp23
_tmp25 = _a[1] * _tmp20
_tmp26 = _a[2] * _tmp22
_tmp27 = -_tmp26
_tmp28 = _tmp25 + _tmp27
_tmp29 = _a[2] ** 2
_tmp30 = -2 * _tmp29
_tmp31 = _a[1] ** 2
_tmp32 = -2 * _tmp31
_tmp33 = _tmp30 + _tmp32 + 1
_tmp34 = _tmp25 + _tmp26
_tmp35 = 2 * _a[1] * _a[2]
_tmp36 = _a[0] * _tmp22
_tmp37 = -_tmp36
_tmp38 = _tmp35 + _tmp37
_tmp39 = _a[0] ** 2
_tmp40 = 1 - 2 * _tmp39
_tmp41 = _tmp30 + _tmp40
_tmp42 = -_tmp23
_tmp43 = _tmp21 + _tmp42
_tmp44 = _tmp35 + _tmp36
_tmp45 = _tmp32 + _tmp40
_tmp46 = (1.0 / 2.0) * _tmp10
_tmp47 = -_tmp46
_tmp48 = (1.0 / 2.0) * _tmp11
_tmp49 = (1.0 / 2.0) * _tmp12
_tmp50 = -_tmp49
_tmp51 = (1.0 / 2.0) * _tmp13
_tmp52 = -_tmp51
_tmp53 = _tmp50 + _tmp52
_tmp54 = _tmp47 + _tmp48 + _tmp53
_tmp55 = 2 * _tmp14
_tmp56 = (1.0 / 2.0) * _tmp6
_tmp57 = -_tmp56
_tmp58 = (1.0 / 2.0) * _tmp5
_tmp59 = (1.0 / 2.0) * _tmp7
_tmp60 = -_tmp59
_tmp61 = (1.0 / 2.0) * _tmp8
_tmp62 = -_tmp61
_tmp63 = _tmp60 + _tmp62
_tmp64 = _tmp57 + _tmp58 + _tmp63
_tmp65 = 2 * _tmp9
_tmp66 = (1.0 / 2.0) * _tmp3
_tmp67 = -_tmp66
_tmp68 = (1.0 / 2.0) * _tmp2
_tmp69 = (1.0 / 2.0) * _tmp1
_tmp70 = -_tmp69
_tmp71 = (1.0 / 2.0) * _tmp0
_tmp72 = -_tmp71
_tmp73 = _tmp70 + _tmp72
_tmp74 = _tmp67 + _tmp68 + _tmp73
_tmp75 = 2 * _tmp4
_tmp76 = (1.0 / 2.0) * _tmp15
_tmp77 = (1.0 / 2.0) * _tmp16
_tmp78 = (1.0 / 2.0) * _tmp18
_tmp79 = (1.0 / 2.0) * _tmp17
_tmp80 = _tmp78 - _tmp79
_tmp81 = _tmp76 + _tmp77 + _tmp80
_tmp82 = 2 * _tmp19
_tmp83 = -_tmp25
_tmp84 = _a[3] ** 2
_tmp85 = -_tmp84
_tmp86 = _tmp29 + _tmp85
_tmp87 = -_tmp31
_tmp88 = _tmp39 + _tmp87
_tmp89 = -_tmp35
_tmp90 = -_tmp39
_tmp91 = -_tmp76
_tmp92 = _tmp78 + _tmp79
_tmp93 = _tmp77 + _tmp91 + _tmp92
_tmp94 = -_tmp68
_tmp95 = _tmp67 + _tmp94
_tmp96 = _tmp69 + _tmp72
_tmp97 = _tmp95 + _tmp96
_tmp98 = -_tmp58
_tmp99 = _tmp57 + _tmp98
_tmp100 = _tmp60 + _tmp61
_tmp101 = _tmp100 + _tmp99
_tmp102 = -_tmp48
_tmp103 = _tmp102 + _tmp47
_tmp104 = _tmp49 + _tmp52
_tmp105 = _tmp103 + _tmp104
_tmp106 = -_tmp21
_tmp107 = -_tmp29
_tmp108 = _tmp107 + _tmp84
_tmp109 = _tmp66 + _tmp73 + _tmp94
_tmp110 = -_tmp77
_tmp111 = _tmp110 + _tmp76 + _tmp92
_tmp112 = _tmp102 + _tmp46 + _tmp53
_tmp113 = _tmp56 + _tmp63 + _tmp98
_tmp114 = _tmp31 + _tmp90
_tmp115 = _tmp104 + _tmp46 + _tmp48
_tmp116 = _tmp70 + _tmp71 + _tmp95
_tmp117 = 2 * _tmp116
_tmp118 = -_tmp117 * _tmp4
_tmp119 = _tmp59 + _tmp62 + _tmp99
_tmp120 = _tmp110 + _tmp80 + _tmp91
_tmp121 = _tmp120 * _tmp82
_tmp122 = -_tmp119 * _tmp65 + _tmp121
_tmp123 = _tmp120 * _tmp55
_tmp124 = _tmp119 * _tmp75
_tmp125 = _tmp117 * _tmp14
_tmp126 = _tmp120 * _tmp65
_tmp127 = _tmp66 + _tmp68 + _tmp96
_tmp128 = _tmp103 + _tmp50 + _tmp51
_tmp129 = -_tmp128 * _tmp55
_tmp130 = _tmp128 * _tmp65
_tmp131 = _tmp120 * _tmp75
_tmp132 = _tmp100 + _tmp56 + _tmp58
_tmp133 = 2 * _tmp132
# Output terms
_res = [0.0] * 7
_res[0] = _tmp4
_res[1] = _tmp9
_res[2] = _tmp14
_res[3] = _tmp19
_res[4] = _a[4] + _b[4] * _tmp33 + _b[5] * _tmp28 + _b[6] * _tmp24
_res[5] = _a[5] + _b[4] * _tmp34 + _b[5] * _tmp41 + _b[6] * _tmp38
_res[6] = _a[6] + _b[4] * _tmp43 + _b[5] * _tmp44 + _b[6] * _tmp45
_res_D_a = numpy.zeros((6, 6))
_res_D_a[0, 0] = _tmp54 * _tmp55 - _tmp64 * _tmp65 - _tmp74 * _tmp75 + _tmp81 * _tmp82
_res_D_a[1, 0] = _tmp54 * _tmp82 - _tmp55 * _tmp81 + _tmp64 * _tmp75 - _tmp65 * _tmp74
_res_D_a[2, 0] = -_tmp54 * _tmp75 - _tmp55 * _tmp74 + _tmp64 * _tmp82 + _tmp65 * _tmp81
_res_D_a[3, 0] = _b[5] * _tmp24 + _b[6] * (_tmp26 + _tmp83)
_res_D_a[4, 0] = _b[5] * _tmp38 + _b[6] * (_tmp86 + _tmp88)
_res_D_a[5, 0] = _b[5] * (_tmp29 + _tmp84 + _tmp87 + _tmp90) + _b[6] * (_tmp37 + _tmp89)
_res_D_a[0, 1] = -_tmp101 * _tmp75 + _tmp105 * _tmp82 + _tmp55 * _tmp93 - _tmp65 * _tmp97
_res_D_a[1, 1] = -_tmp101 * _tmp65 - _tmp105 * _tmp55 + _tmp75 * _tmp97 + _tmp82 * _tmp93
_res_D_a[2, 1] = -_tmp101 * _tmp55 + _tmp105 * _tmp65 - _tmp75 * _tmp93 + _tmp82 * _tmp97
_res_D_a[3, 1] = _b[4] * (_tmp106 + _tmp42) + _b[6] * (_tmp108 + _tmp88)
_res_D_a[4, 1] = _b[4] * (_tmp36 + _tmp89) + _b[6] * _tmp34
_res_D_a[5, 1] = _b[4] * (_tmp107 + _tmp31 + _tmp39 + _tmp85) + _b[6] * _tmp43
_res_D_a[0, 2] = _tmp109 * _tmp55 - _tmp111 * _tmp65 - _tmp112 * _tmp75 + _tmp113 * _tmp82
_res_D_a[1, 2] = _tmp109 * _tmp82 + _tmp111 * _tmp75 - _tmp112 * _tmp65 - _tmp113 * _tmp55
_res_D_a[2, 2] = -_tmp109 * _tmp75 + _tmp111 * _tmp82 - _tmp112 * _tmp55 + _tmp113 * _tmp65
_res_D_a[3, 2] = _b[4] * _tmp28 + _b[5] * (_tmp114 + _tmp86)
_res_D_a[4, 2] = _b[4] * (_tmp108 + _tmp114) + _b[5] * (_tmp27 + _tmp83)
_res_D_a[5, 2] = _b[4] * _tmp44 + _b[5] * (_tmp106 + _tmp23)
_res_D_a[0, 3] = 0
_res_D_a[1, 3] = 0
_res_D_a[2, 3] = 0
_res_D_a[3, 3] = 1
_res_D_a[4, 3] = 0
_res_D_a[5, 3] = 0
_res_D_a[0, 4] = 0
_res_D_a[1, 4] = 0
_res_D_a[2, 4] = 0
_res_D_a[3, 4] = 0
_res_D_a[4, 4] = 1
_res_D_a[5, 4] = 0
_res_D_a[0, 5] = 0
_res_D_a[1, 5] = 0
_res_D_a[2, 5] = 0
_res_D_a[3, 5] = 0
_res_D_a[4, 5] = 0
_res_D_a[5, 5] = 1
_res_D_b = numpy.zeros((6, 6))
_res_D_b[0, 0] = _tmp115 * _tmp55 + _tmp118 + _tmp122
_res_D_b[1, 0] = _tmp115 * _tmp82 - _tmp116 * _tmp65 - _tmp123 + _tmp124
_res_D_b[2, 0] = -_tmp115 * _tmp75 + _tmp119 * _tmp82 - _tmp125 + _tmp126
_res_D_b[3, 0] = 0
_res_D_b[4, 0] = 0
_res_D_b[5, 0] = 0
_res_D_b[0, 1] = _tmp123 - _tmp124 - _tmp127 * _tmp65 + _tmp128 * _tmp82
_res_D_b[1, 1] = _tmp122 + _tmp127 * _tmp75 + _tmp129
_res_D_b[2, 1] = -_tmp119 * _tmp55 + _tmp127 * _tmp82 + _tmp130 - _tmp131
_res_D_b[3, 1] = 0
_res_D_b[4, 1] = 0
_res_D_b[5, 1] = 0
_res_D_b[0, 2] = _tmp125 - _tmp126 - _tmp128 * _tmp75 + _tmp132 * _tmp82
_res_D_b[1, 2] = _tmp116 * _tmp82 - _tmp130 + _tmp131 - _tmp133 * _tmp14
_res_D_b[2, 2] = _tmp118 + _tmp121 + _tmp129 + _tmp133 * _tmp9
_res_D_b[3, 2] = 0
_res_D_b[4, 2] = 0
_res_D_b[5, 2] = 0
_res_D_b[0, 3] = 0
_res_D_b[1, 3] = 0
_res_D_b[2, 3] = 0
_res_D_b[3, 3] = _tmp33
_res_D_b[4, 3] = _tmp34
_res_D_b[5, 3] = _tmp43
_res_D_b[0, 4] = 0
_res_D_b[1, 4] = 0
_res_D_b[2, 4] = 0
_res_D_b[3, 4] = _tmp28
_res_D_b[4, 4] = _tmp41
_res_D_b[5, 4] = _tmp44
_res_D_b[0, 5] = 0
_res_D_b[1, 5] = 0
_res_D_b[2, 5] = 0
_res_D_b[3, 5] = _tmp24
_res_D_b[4, 5] = _tmp38
_res_D_b[5, 5] = _tmp45
return sym.Pose3.from_storage(_res), _res_D_a, _res_D_b
[docs] @staticmethod
def between_with_jacobians(a, b):
# type: (sym.Pose3, sym.Pose3) -> T.Tuple[sym.Pose3, numpy.ndarray, numpy.ndarray]
# Total ops: 306
# Input arrays
_a = a.data
_b = b.data
# Intermediate terms (142)
_tmp0 = _a[2] * _b[1]
_tmp1 = _a[0] * _b[3]
_tmp2 = _a[1] * _b[2]
_tmp3 = _a[3] * _b[0]
_tmp4 = _tmp0 - _tmp1 - _tmp2 + _tmp3
_tmp5 = _a[3] * _b[1]
_tmp6 = _a[1] * _b[3]
_tmp7 = _a[0] * _b[2]
_tmp8 = _a[2] * _b[0]
_tmp9 = _tmp5 - _tmp6 + _tmp7 - _tmp8
_tmp10 = _a[0] * _b[1]
_tmp11 = _a[2] * _b[3]
_tmp12 = _a[3] * _b[2]
_tmp13 = _a[1] * _b[0]
_tmp14 = -_tmp10 - _tmp11 + _tmp12 + _tmp13
_tmp15 = _a[1] * _b[1]
_tmp16 = _a[2] * _b[2]
_tmp17 = _a[0] * _b[0]
_tmp18 = _a[3] * _b[3]
_tmp19 = _tmp15 + _tmp16 + _tmp17 + _tmp18
_tmp20 = _a[1] ** 2
_tmp21 = 2 * _tmp20
_tmp22 = -_tmp21
_tmp23 = _a[2] ** 2
_tmp24 = 2 * _tmp23
_tmp25 = 1 - _tmp24
_tmp26 = _tmp22 + _tmp25
_tmp27 = 2 * _a[0]
_tmp28 = _a[1] * _tmp27
_tmp29 = 2 * _a[3]
_tmp30 = _a[2] * _tmp29
_tmp31 = _tmp28 + _tmp30
_tmp32 = _a[5] * _tmp31
_tmp33 = _a[2] * _tmp27
_tmp34 = _a[1] * _tmp29
_tmp35 = -_tmp34
_tmp36 = _tmp33 + _tmp35
_tmp37 = _a[6] * _tmp36
_tmp38 = _b[5] * _tmp31 + _b[6] * _tmp36
_tmp39 = -_tmp30
_tmp40 = _tmp28 + _tmp39
_tmp41 = _a[4] * _tmp40
_tmp42 = 2 * _a[1] * _a[2]
_tmp43 = _a[0] * _tmp29
_tmp44 = _tmp42 + _tmp43
_tmp45 = _a[6] * _tmp44
_tmp46 = _a[0] ** 2
_tmp47 = 2 * _tmp46
_tmp48 = -_tmp47
_tmp49 = _tmp25 + _tmp48
_tmp50 = _b[4] * _tmp40 + _b[6] * _tmp44
_tmp51 = _tmp33 + _tmp34
_tmp52 = _a[4] * _tmp51
_tmp53 = -_tmp43
_tmp54 = _tmp42 + _tmp53
_tmp55 = _a[5] * _tmp54
_tmp56 = _tmp22 + _tmp48 + 1
_tmp57 = _b[4] * _tmp51 + _b[5] * _tmp54
_tmp58 = (1.0 / 2.0) * _tmp10
_tmp59 = (1.0 / 2.0) * _tmp11
_tmp60 = (1.0 / 2.0) * _tmp12
_tmp61 = (1.0 / 2.0) * _tmp13
_tmp62 = -_tmp58 - _tmp59 + _tmp60 + _tmp61
_tmp63 = 2 * _tmp14
_tmp64 = _tmp62 * _tmp63
_tmp65 = (1.0 / 2.0) * _tmp5
_tmp66 = (1.0 / 2.0) * _tmp6
_tmp67 = (1.0 / 2.0) * _tmp7
_tmp68 = (1.0 / 2.0) * _tmp8
_tmp69 = -_tmp65 + _tmp66 - _tmp67 + _tmp68
_tmp70 = 2 * _tmp9
_tmp71 = -_tmp69 * _tmp70
_tmp72 = (1.0 / 2.0) * _tmp0
_tmp73 = (1.0 / 2.0) * _tmp1
_tmp74 = (1.0 / 2.0) * _tmp2
_tmp75 = (1.0 / 2.0) * _tmp3
_tmp76 = _tmp72 - _tmp73 - _tmp74 + _tmp75
_tmp77 = 2 * _tmp4
_tmp78 = _tmp76 * _tmp77
_tmp79 = (1.0 / 2.0) * _tmp15
_tmp80 = (1.0 / 2.0) * _tmp18
_tmp81 = (1.0 / 2.0) * _tmp16
_tmp82 = (1.0 / 2.0) * _tmp17
_tmp83 = -_tmp79 - _tmp80 - _tmp81 - _tmp82
_tmp84 = 2 * _tmp19
_tmp85 = _tmp83 * _tmp84
_tmp86 = -_tmp70 * _tmp76
_tmp87 = _tmp63 * _tmp83
_tmp88 = _tmp69 * _tmp77
_tmp89 = _tmp62 * _tmp84 + _tmp88
_tmp90 = _tmp70 * _tmp83
_tmp91 = -_tmp62 * _tmp77
_tmp92 = _tmp69 * _tmp84 + _tmp91
_tmp93 = -_tmp46
_tmp94 = _a[3] ** 2
_tmp95 = _tmp93 + _tmp94
_tmp96 = -_tmp20
_tmp97 = _tmp23 + _tmp96
_tmp98 = _tmp95 + _tmp97
_tmp99 = -_tmp28
_tmp100 = _tmp30 + _tmp99
_tmp101 = -_tmp42
_tmp102 = _tmp101 + _tmp53
_tmp103 = -_tmp94
_tmp104 = _tmp103 + _tmp46
_tmp105 = _tmp104 + _tmp97
_tmp106 = _tmp65 - _tmp66 + _tmp67 - _tmp68
_tmp107 = 2 * _tmp106
_tmp108 = _tmp58 + _tmp59 - _tmp60 - _tmp61
_tmp109 = _tmp108 * _tmp84 + _tmp86
_tmp110 = _tmp107 * _tmp9
_tmp111 = 2 * _tmp108
_tmp112 = -_tmp111 * _tmp14
_tmp113 = _tmp112 + _tmp78
_tmp114 = -_tmp107 * _tmp14
_tmp115 = _tmp77 * _tmp83
_tmp116 = _tmp111 * _tmp9
_tmp117 = _tmp116 + _tmp76 * _tmp84
_tmp118 = -_tmp33
_tmp119 = _tmp118 + _tmp35
_tmp120 = _tmp101 + _tmp43
_tmp121 = -_tmp23
_tmp122 = _tmp121 + _tmp20
_tmp123 = _tmp104 + _tmp122
_tmp124 = _tmp121 + _tmp46 + _tmp94 + _tmp96
_tmp125 = -_tmp72 + _tmp73 + _tmp74 - _tmp75
_tmp126 = _tmp125 * _tmp63
_tmp127 = _tmp106 * _tmp84 + _tmp126
_tmp128 = _tmp114 + _tmp125 * _tmp84
_tmp129 = -_tmp125 * _tmp77
_tmp130 = _tmp110 + _tmp129
_tmp131 = _tmp122 + _tmp95
_tmp132 = _tmp118 + _tmp34
_tmp133 = _tmp39 + _tmp99
_tmp134 = _tmp103 + _tmp20 + _tmp23 + _tmp93
_tmp135 = _tmp24 - 1
_tmp136 = _tmp79 + _tmp80 + _tmp81 + _tmp82
_tmp137 = _tmp136 * _tmp84
_tmp138 = _tmp137 + _tmp71
_tmp139 = _tmp136 * _tmp63
_tmp140 = _tmp136 * _tmp70
_tmp141 = _tmp136 * _tmp77
# Output terms
_res = [0.0] * 7
_res[0] = _tmp4
_res[1] = _tmp9
_res[2] = _tmp14
_res[3] = _tmp19
_res[4] = -_a[4] * _tmp26 + _b[4] * _tmp26 - _tmp32 - _tmp37 + _tmp38
_res[5] = -_a[5] * _tmp49 + _b[5] * _tmp49 - _tmp41 - _tmp45 + _tmp50
_res[6] = -_a[6] * _tmp56 + _b[6] * _tmp56 - _tmp52 - _tmp55 + _tmp57
_res_D_a = numpy.zeros((6, 6))
_res_D_a[0, 0] = _tmp64 + _tmp71 - _tmp78 + _tmp85
_res_D_a[1, 0] = _tmp86 - _tmp87 + _tmp89
_res_D_a[2, 0] = -_tmp63 * _tmp76 + _tmp90 + _tmp92
_res_D_a[3, 0] = 0
_res_D_a[4, 0] = -_a[6] * _tmp98 + _b[6] * _tmp98 - _tmp52 - _tmp55 + _tmp57
_res_D_a[5, 0] = (
-_a[4] * _tmp100
- _a[5] * _tmp105
- _a[6] * _tmp102
+ _b[4] * _tmp100
+ _b[5] * _tmp105
+ _b[6] * _tmp102
)
_res_D_a[0, 1] = -_tmp107 * _tmp4 + _tmp109 + _tmp87
_res_D_a[1, 1] = -_tmp110 + _tmp113 + _tmp85
_res_D_a[2, 1] = _tmp114 - _tmp115 + _tmp117
_res_D_a[3, 1] = (
-_a[4] * _tmp119
- _a[5] * _tmp120
- _a[6] * _tmp123
+ _b[4] * _tmp119
+ _b[5] * _tmp120
+ _b[6] * _tmp123
)
_res_D_a[4, 1] = 0
_res_D_a[5, 1] = -_a[4] * _tmp124 + _b[4] * _tmp124 - _tmp32 - _tmp37 + _tmp38
_res_D_a[0, 2] = _tmp127 - _tmp90 + _tmp91
_res_D_a[1, 2] = _tmp115 + _tmp128 - _tmp62 * _tmp70
_res_D_a[2, 2] = _tmp130 - _tmp64 + _tmp85
_res_D_a[3, 2] = -_a[5] * _tmp131 + _b[5] * _tmp131 - _tmp41 - _tmp45 + _tmp50
_res_D_a[4, 2] = (
-_a[4] * _tmp134
- _a[5] * _tmp133
- _a[6] * _tmp132
+ _b[4] * _tmp134
+ _b[5] * _tmp133
+ _b[6] * _tmp132
)
_res_D_a[5, 2] = 0
_res_D_a[0, 3] = 0
_res_D_a[1, 3] = 0
_res_D_a[2, 3] = 0
_res_D_a[3, 3] = _tmp135 + _tmp21
_res_D_a[4, 3] = _tmp100
_res_D_a[5, 3] = _tmp119
_res_D_a[0, 4] = 0
_res_D_a[1, 4] = 0
_res_D_a[2, 4] = 0
_res_D_a[3, 4] = _tmp133
_res_D_a[4, 4] = _tmp135 + _tmp47
_res_D_a[5, 4] = _tmp120
_res_D_a[0, 5] = 0
_res_D_a[1, 5] = 0
_res_D_a[2, 5] = 0
_res_D_a[3, 5] = _tmp132
_res_D_a[4, 5] = _tmp102
_res_D_a[5, 5] = _tmp21 + _tmp47 - 1
_res_D_b = numpy.zeros((6, 6))
_res_D_b[0, 0] = _tmp129 + _tmp138 + _tmp64
_res_D_b[1, 0] = -_tmp125 * _tmp70 - _tmp139 + _tmp89
_res_D_b[2, 0] = -_tmp126 + _tmp140 + _tmp92
_res_D_b[3, 0] = 0
_res_D_b[4, 0] = 0
_res_D_b[5, 0] = 0
_res_D_b[0, 1] = _tmp109 + _tmp139 - _tmp88
_res_D_b[1, 1] = _tmp113 + _tmp138
_res_D_b[2, 1] = _tmp117 - _tmp141 - _tmp63 * _tmp69
_res_D_b[3, 1] = 0
_res_D_b[4, 1] = 0
_res_D_b[5, 1] = 0
_res_D_b[0, 2] = -_tmp111 * _tmp4 + _tmp127 - _tmp140
_res_D_b[1, 2] = -_tmp116 + _tmp128 + _tmp141
_res_D_b[2, 2] = _tmp112 + _tmp130 + _tmp137
_res_D_b[3, 2] = 0
_res_D_b[4, 2] = 0
_res_D_b[5, 2] = 0
_res_D_b[0, 3] = 0
_res_D_b[1, 3] = 0
_res_D_b[2, 3] = 0
_res_D_b[3, 3] = _tmp26
_res_D_b[4, 3] = _tmp40
_res_D_b[5, 3] = _tmp51
_res_D_b[0, 4] = 0
_res_D_b[1, 4] = 0
_res_D_b[2, 4] = 0
_res_D_b[3, 4] = _tmp31
_res_D_b[4, 4] = _tmp49
_res_D_b[5, 4] = _tmp54
_res_D_b[0, 5] = 0
_res_D_b[1, 5] = 0
_res_D_b[2, 5] = 0
_res_D_b[3, 5] = _tmp36
_res_D_b[4, 5] = _tmp44
_res_D_b[5, 5] = _tmp56
return sym.Pose3.from_storage(_res), _res_D_a, _res_D_b