From deda3fc9f481f33ec7fbe255b73b416c16a0e512 Mon Sep 17 00:00:00 2001 From: bradley-solliday-skydio Date: Mon, 19 Dec 2022 13:40:07 -0800 Subject: [PATCH] [ImuFactor] Add an on manifold Imu Factor Unit testing is left to a follow up commit. The core use-facing api is found in - symforce/slam/imu_preintegration/preintegrated_imu_measurements.h - symforce/slam/imu_preintegration/imu_factor.h Unlike most other factors (/residual functions) in symforce, the ImuFactor is a functor so as to store the pre-integrated values with the function rather than in the `Values`. (There are a lot of parameters that are constant and unique to each factor). A python version of the `ImuFactor` class will come in a subsequent commit. Topic: on_manifold_imu_factor --- CMakeLists.txt | 1 + .../imu_manifold_preintegration_update.h | 815 ++++++ ...ld_preintegration_update_auto_derivative.h | 937 +++++++ .../factors/internal/internal_imu_factor.h | 2377 +++++++++++++++++ symforce/geo/matrix.py | 17 + symforce/pybind/CMakeLists.txt | 11 + symforce/slam/CMakeLists.txt | 24 + symforce/slam/__init__.py | 0 symforce/slam/imu_preintegration/__init__.py | 0 symforce/slam/imu_preintegration/generate.py | 73 + .../slam/imu_preintegration/imu_factor.cc | 53 + symforce/slam/imu_preintegration/imu_factor.h | 197 ++ .../imu_preintegration/imu_preintegrator.cc | 69 + .../imu_preintegration/imu_preintegrator.h | 81 + .../imu_preintegration/manifold_symbolic.py | 349 +++ .../preintegrated_imu_measurements.cc | 28 + .../preintegrated_imu_measurements.h | 66 + test/geo_matrix_test.py | 17 + test/symforce_gen_codegen_test.py | 5 + 19 files changed, 5120 insertions(+) create mode 100644 gen/cpp/sym/factors/internal/imu_manifold_preintegration_update.h create mode 100644 gen/cpp/sym/factors/internal/imu_manifold_preintegration_update_auto_derivative.h create mode 100644 gen/cpp/sym/factors/internal/internal_imu_factor.h create mode 100644 symforce/slam/CMakeLists.txt create mode 100644 symforce/slam/__init__.py create mode 100644 symforce/slam/imu_preintegration/__init__.py create mode 100644 symforce/slam/imu_preintegration/generate.py create mode 100644 symforce/slam/imu_preintegration/imu_factor.cc create mode 100644 symforce/slam/imu_preintegration/imu_factor.h create mode 100644 symforce/slam/imu_preintegration/imu_preintegrator.cc create mode 100644 symforce/slam/imu_preintegration/imu_preintegrator.h create mode 100644 symforce/slam/imu_preintegration/manifold_symbolic.py create mode 100644 symforce/slam/imu_preintegration/preintegrated_imu_measurements.cc create mode 100644 symforce/slam/imu_preintegration/preintegrated_imu_measurements.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7bc90398e..c09eda36c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -280,6 +280,7 @@ install(DIRECTORY gen/cpp/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATT if(SYMFORCE_BUILD_OPT) add_subdirectory(symforce/opt) + add_subdirectory(symforce/slam) endif() if(SYMFORCE_BUILD_CC_SYM) diff --git a/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update.h b/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update.h new file mode 100644 index 000000000..62d25361b --- /dev/null +++ b/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update.h @@ -0,0 +1,815 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * An internal helper function to update a set of preintegrated IMU measurements with a new pair of + * gyroscope and accelerometer measurements. Returns the updated preintegrated measurements. + * + * NOTE: If you are interested in this function, you should likely be using the + * IntegrateMeasurement method of the PreintegratedImuMeasurements class located in + * symforce/slam/imu_preintegration/preintegrated_imu_measurements.h. + * + * When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the + * derivatives w.r.t. to bias should all be 0. + * + * Args: + * DR (sf.Rot3): Preintegrated change in orientation + * Dv (sf.V3): Preintegrated change in velocity + * Dp (sf.V3): Preintegrated change in position + * covariance (sf.M99): Covariance [DR, Dv, Dp] in local coordinates of mean + * DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias + * Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias + * Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias + * Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias + * Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias + * accel_bias (sf.V3): Initial guess for the accelerometer measurement bias + * gyro_bias (sf.V3): Initial guess for the gyroscope measurement bias + * accel_cov_diagonal (sf.M33): The diagonal of the covariance of the accelerometer measurement + * gyro_cov_diagonal (sf.M33): The diagonal of the covariance of the gyroscope measurement + * accel_measurement (sf.V3): The accelerometer measurement + * gyro_measurement (sf.V3): The gyroscope measurement + * dt (T.Scalar): The time between the new measurements and the last + * epsilon (T.Scalar): epsilon for numerical stability + * + * Returns: + * T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: + * new_DR, + * new_Dv, + * new_Dp, + * new_covariance, + * new_DR_D_gyro_bias, + * new_Dv_D_accel_bias, + * new_Dv_D_gyro_bias, + * new_Dp_D_accel_bias + * new_Dp_D_gyro_bias, + */ +template +void ImuManifoldPreintegrationUpdate( + const sym::Rot3& DR, const Eigen::Matrix& Dv, + const Eigen::Matrix& Dp, const Eigen::Matrix& covariance, + const Eigen::Matrix& DR_D_gyro_bias, + const Eigen::Matrix& Dv_D_accel_bias, + const Eigen::Matrix& Dv_D_gyro_bias, + const Eigen::Matrix& Dp_D_accel_bias, + const Eigen::Matrix& Dp_D_gyro_bias, + const Eigen::Matrix& accel_bias, const Eigen::Matrix& gyro_bias, + const Eigen::Matrix& accel_cov_diagonal, + const Eigen::Matrix& gyro_cov_diagonal, + const Eigen::Matrix& accel_measurement, + const Eigen::Matrix& gyro_measurement, const Scalar dt, const Scalar epsilon, + sym::Rot3* const new_DR = nullptr, Eigen::Matrix* const new_Dv = nullptr, + Eigen::Matrix* const new_Dp = nullptr, + Eigen::Matrix* const new_covariance = nullptr, + Eigen::Matrix* const new_DR_D_gyro_bias = nullptr, + Eigen::Matrix* const new_Dv_D_accel_bias = nullptr, + Eigen::Matrix* const new_Dv_D_gyro_bias = nullptr, + Eigen::Matrix* const new_Dp_D_accel_bias = nullptr, + Eigen::Matrix* const new_Dp_D_gyro_bias = nullptr) { + // Total ops: 1702 + + // Input arrays + const Eigen::Matrix& _DR = DR.Data(); + + // Intermediate terms (421) + const Scalar _tmp0 = -gyro_bias(0, 0) + gyro_measurement(0, 0); + const Scalar _tmp1 = std::pow(dt, Scalar(2)); + const Scalar _tmp2 = -gyro_bias(2, 0) + gyro_measurement(2, 0); + const Scalar _tmp3 = _tmp1 * std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = -gyro_bias(1, 0) + gyro_measurement(1, 0); + const Scalar _tmp5 = _tmp1 * std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = std::pow(_tmp0, Scalar(2)) * _tmp1; + const Scalar _tmp7 = _tmp3 + _tmp5 + _tmp6; + const Scalar _tmp8 = _tmp7 + std::pow(epsilon, Scalar(2)); + const Scalar _tmp9 = std::sqrt(_tmp8); + const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp11 = std::sin(_tmp10); + const Scalar _tmp12 = _tmp11 / _tmp9; + const Scalar _tmp13 = _tmp12 * dt; + const Scalar _tmp14 = _tmp0 * _tmp13; + const Scalar _tmp15 = _tmp2 * dt; + const Scalar _tmp16 = _tmp12 * _tmp15; + const Scalar _tmp17 = _tmp13 * _tmp4; + const Scalar _tmp18 = std::cos(_tmp10); + const Scalar _tmp19 = -2 * std::pow(_DR[2], Scalar(2)); + const Scalar _tmp20 = 1 - 2 * std::pow(_DR[1], Scalar(2)); + const Scalar _tmp21 = _tmp19 + _tmp20; + const Scalar _tmp22 = -accel_bias(0, 0) + accel_measurement(0, 0); + const Scalar _tmp23 = 2 * _DR[0] * _DR[1]; + const Scalar _tmp24 = 2 * _DR[2]; + const Scalar _tmp25 = _DR[3] * _tmp24; + const Scalar _tmp26 = _tmp23 - _tmp25; + const Scalar _tmp27 = -accel_bias(1, 0) + accel_measurement(1, 0); + const Scalar _tmp28 = 2 * _DR[3]; + const Scalar _tmp29 = _DR[1] * _tmp28; + const Scalar _tmp30 = _DR[0] * _tmp24; + const Scalar _tmp31 = _tmp29 + _tmp30; + const Scalar _tmp32 = -accel_bias(2, 0) + accel_measurement(2, 0); + const Scalar _tmp33 = _tmp21 * _tmp22 + _tmp26 * _tmp27 + _tmp31 * _tmp32; + const Scalar _tmp34 = -2 * std::pow(_DR[0], Scalar(2)); + const Scalar _tmp35 = _tmp19 + _tmp34 + 1; + const Scalar _tmp36 = _tmp23 + _tmp25; + const Scalar _tmp37 = _DR[0] * _tmp28; + const Scalar _tmp38 = _DR[1] * _tmp24; + const Scalar _tmp39 = -_tmp37 + _tmp38; + const Scalar _tmp40 = _tmp22 * _tmp36 + _tmp27 * _tmp35 + _tmp32 * _tmp39; + const Scalar _tmp41 = _tmp20 + _tmp34; + const Scalar _tmp42 = -_tmp29 + _tmp30; + const Scalar _tmp43 = _tmp37 + _tmp38; + const Scalar _tmp44 = _tmp22 * _tmp42 + _tmp27 * _tmp43 + _tmp32 * _tmp41; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp46 = -_tmp3; + const Scalar _tmp47 = -_tmp5; + const Scalar _tmp48 = _tmp7 + std::sqrt(epsilon); + const Scalar _tmp49 = std::sqrt(_tmp48); + const Scalar _tmp50 = (_tmp49 - std::sin(_tmp49)) / (_tmp48 * std::sqrt(_tmp48)); + const Scalar _tmp51 = _tmp50 * (_tmp46 + _tmp47) + 1; + const Scalar _tmp52 = dt * gyro_cov_diagonal(0, 0); + const Scalar _tmp53 = 2 * std::pow(_tmp11, Scalar(2)) / _tmp8; + const Scalar _tmp54 = -_tmp5 * _tmp53; + const Scalar _tmp55 = -_tmp3 * _tmp53; + const Scalar _tmp56 = _tmp54 + _tmp55 + 1; + const Scalar _tmp57 = 2 * _tmp18; + const Scalar _tmp58 = _tmp17 * _tmp57; + const Scalar _tmp59 = _tmp1 * _tmp2; + const Scalar _tmp60 = _tmp0 * _tmp53; + const Scalar _tmp61 = _tmp59 * _tmp60; + const Scalar _tmp62 = -_tmp58 + _tmp61; + const Scalar _tmp63 = _tmp16 * _tmp57; + const Scalar _tmp64 = _tmp1 * _tmp4; + const Scalar _tmp65 = _tmp60 * _tmp64; + const Scalar _tmp66 = _tmp63 + _tmp65; + const Scalar _tmp67 = + _tmp56 * covariance(0, 0) + _tmp62 * covariance(2, 0) + _tmp66 * covariance(1, 0); + const Scalar _tmp68 = + _tmp56 * covariance(0, 1) + _tmp62 * covariance(2, 1) + _tmp66 * covariance(1, 1); + const Scalar _tmp69 = + _tmp56 * covariance(0, 2) + _tmp62 * covariance(2, 2) + _tmp66 * covariance(1, 2); + const Scalar _tmp70 = _tmp0 * _tmp50; + const Scalar _tmp71 = _tmp59 * _tmp70; + const Scalar _tmp72 = (1 - std::cos(_tmp49)) / _tmp48; + const Scalar _tmp73 = _tmp72 * dt; + const Scalar _tmp74 = _tmp4 * _tmp73; + const Scalar _tmp75 = _tmp71 - _tmp74; + const Scalar _tmp76 = dt * gyro_cov_diagonal(2, 0); + const Scalar _tmp77 = _tmp64 * _tmp70; + const Scalar _tmp78 = _tmp15 * _tmp72; + const Scalar _tmp79 = _tmp77 + _tmp78; + const Scalar _tmp80 = dt * gyro_cov_diagonal(1, 0); + const Scalar _tmp81 = -_tmp53 * _tmp6 + 1; + const Scalar _tmp82 = _tmp55 + _tmp81; + const Scalar _tmp83 = _tmp14 * _tmp57; + const Scalar _tmp84 = _tmp2 * _tmp64; + const Scalar _tmp85 = _tmp53 * _tmp84; + const Scalar _tmp86 = _tmp83 + _tmp85; + const Scalar _tmp87 = -_tmp63 + _tmp65; + const Scalar _tmp88 = + _tmp82 * covariance(1, 0) + _tmp86 * covariance(2, 0) + _tmp87 * covariance(0, 0); + const Scalar _tmp89 = + _tmp82 * covariance(1, 1) + _tmp86 * covariance(2, 1) + _tmp87 * covariance(0, 1); + const Scalar _tmp90 = + _tmp82 * covariance(1, 2) + _tmp86 * covariance(2, 2) + _tmp87 * covariance(0, 2); + const Scalar _tmp91 = _tmp50 * _tmp84; + const Scalar _tmp92 = _tmp0 * _tmp73; + const Scalar _tmp93 = _tmp91 + _tmp92; + const Scalar _tmp94 = _tmp93 * dt; + const Scalar _tmp95 = -_tmp6; + const Scalar _tmp96 = _tmp50 * (_tmp46 + _tmp95) + 1; + const Scalar _tmp97 = _tmp96 * dt; + const Scalar _tmp98 = _tmp97 * gyro_cov_diagonal(1, 0); + const Scalar _tmp99 = _tmp51 * dt; + const Scalar _tmp100 = _tmp77 - _tmp78; + const Scalar _tmp101 = _tmp100 * gyro_cov_diagonal(0, 0); + const Scalar _tmp102 = + _tmp101 * _tmp99 + _tmp75 * _tmp94 * gyro_cov_diagonal(2, 0) + _tmp79 * _tmp98; + const Scalar _tmp103 = _tmp54 + _tmp81; + const Scalar _tmp104 = -_tmp83 + _tmp85; + const Scalar _tmp105 = _tmp58 + _tmp61; + const Scalar _tmp106 = + _tmp103 * covariance(2, 0) + _tmp104 * covariance(1, 0) + _tmp105 * covariance(0, 0); + const Scalar _tmp107 = + _tmp103 * covariance(2, 1) + _tmp104 * covariance(1, 1) + _tmp105 * covariance(0, 1); + const Scalar _tmp108 = + _tmp103 * covariance(2, 2) + _tmp104 * covariance(1, 2) + _tmp105 * covariance(0, 2); + const Scalar _tmp109 = _tmp50 * (_tmp47 + _tmp95) + 1; + const Scalar _tmp110 = _tmp109 * dt; + const Scalar _tmp111 = _tmp110 * gyro_cov_diagonal(2, 0); + const Scalar _tmp112 = _tmp71 + _tmp74; + const Scalar _tmp113 = _tmp91 - _tmp92; + const Scalar _tmp114 = _tmp113 * dt; + const Scalar _tmp115 = _tmp111 * _tmp75 + _tmp112 * _tmp99 * gyro_cov_diagonal(0, 0) + + _tmp114 * _tmp79 * gyro_cov_diagonal(1, 0); + const Scalar _tmp116 = _tmp31 * dt; + const Scalar _tmp117 = _tmp26 * dt; + const Scalar _tmp118 = _tmp116 * _tmp27 - _tmp117 * _tmp32; + const Scalar _tmp119 = _tmp118 * covariance(0, 0); + const Scalar _tmp120 = _tmp21 * dt; + const Scalar _tmp121 = -_tmp116 * _tmp22 + _tmp120 * _tmp32; + const Scalar _tmp122 = _tmp121 * covariance(1, 0); + const Scalar _tmp123 = _tmp117 * _tmp22 - _tmp120 * _tmp27; + const Scalar _tmp124 = _tmp123 * covariance(2, 0); + const Scalar _tmp125 = _tmp119 + _tmp122 + _tmp124 + covariance(3, 0); + const Scalar _tmp126 = _tmp118 * covariance(0, 2); + const Scalar _tmp127 = _tmp121 * covariance(1, 2); + const Scalar _tmp128 = _tmp123 * covariance(2, 2); + const Scalar _tmp129 = _tmp126 + _tmp127 + _tmp128 + covariance(3, 2); + const Scalar _tmp130 = _tmp118 * covariance(0, 1); + const Scalar _tmp131 = _tmp121 * covariance(1, 1); + const Scalar _tmp132 = _tmp123 * covariance(2, 1); + const Scalar _tmp133 = _tmp130 + _tmp131 + _tmp132 + covariance(3, 1); + const Scalar _tmp134 = _tmp39 * dt; + const Scalar _tmp135 = _tmp36 * dt; + const Scalar _tmp136 = -_tmp134 * _tmp22 + _tmp135 * _tmp32; + const Scalar _tmp137 = _tmp136 * covariance(1, 0); + const Scalar _tmp138 = _tmp35 * dt; + const Scalar _tmp139 = _tmp134 * _tmp27 - _tmp138 * _tmp32; + const Scalar _tmp140 = _tmp139 * covariance(0, 0); + const Scalar _tmp141 = -_tmp135 * _tmp27 + _tmp138 * _tmp22; + const Scalar _tmp142 = _tmp141 * covariance(2, 0); + const Scalar _tmp143 = _tmp137 + _tmp140 + _tmp142 + covariance(4, 0); + const Scalar _tmp144 = _tmp136 * covariance(1, 1); + const Scalar _tmp145 = _tmp139 * covariance(0, 1); + const Scalar _tmp146 = _tmp141 * covariance(2, 1); + const Scalar _tmp147 = _tmp144 + _tmp145 + _tmp146 + covariance(4, 1); + const Scalar _tmp148 = _tmp136 * covariance(1, 2); + const Scalar _tmp149 = _tmp139 * covariance(0, 2); + const Scalar _tmp150 = _tmp141 * covariance(2, 2); + const Scalar _tmp151 = _tmp148 + _tmp149 + _tmp150 + covariance(4, 2); + const Scalar _tmp152 = _tmp43 * dt; + const Scalar _tmp153 = _tmp41 * dt; + const Scalar _tmp154 = -_tmp152 * _tmp32 + _tmp153 * _tmp27; + const Scalar _tmp155 = _tmp154 * covariance(0, 1); + const Scalar _tmp156 = _tmp42 * dt; + const Scalar _tmp157 = -_tmp153 * _tmp22 + _tmp156 * _tmp32; + const Scalar _tmp158 = _tmp157 * covariance(1, 1); + const Scalar _tmp159 = _tmp152 * _tmp22 - _tmp156 * _tmp27; + const Scalar _tmp160 = _tmp159 * covariance(2, 1); + const Scalar _tmp161 = _tmp155 + _tmp158 + _tmp160 + covariance(5, 1); + const Scalar _tmp162 = _tmp154 * covariance(0, 2); + const Scalar _tmp163 = _tmp157 * covariance(1, 2); + const Scalar _tmp164 = _tmp159 * covariance(2, 2); + const Scalar _tmp165 = _tmp162 + _tmp163 + _tmp164 + covariance(5, 2); + const Scalar _tmp166 = _tmp154 * covariance(0, 0); + const Scalar _tmp167 = _tmp157 * covariance(1, 0); + const Scalar _tmp168 = _tmp159 * covariance(2, 0); + const Scalar _tmp169 = _tmp166 + _tmp167 + _tmp168 + covariance(5, 0); + const Scalar _tmp170 = (Scalar(1) / Scalar(2)) * dt; + const Scalar _tmp171 = _tmp130 * _tmp170 + _tmp131 * _tmp170 + _tmp132 * _tmp170 + + covariance(3, 1) * dt + covariance(6, 1); + const Scalar _tmp172 = _tmp126 * _tmp170 + _tmp127 * _tmp170 + _tmp128 * _tmp170 + + covariance(3, 2) * dt + covariance(6, 2); + const Scalar _tmp173 = _tmp119 * _tmp170 + _tmp122 * _tmp170 + _tmp124 * _tmp170 + + covariance(3, 0) * dt + covariance(6, 0); + const Scalar _tmp174 = _tmp144 * _tmp170 + _tmp145 * _tmp170 + _tmp146 * _tmp170 + + covariance(4, 1) * dt + covariance(7, 1); + const Scalar _tmp175 = _tmp148 * _tmp170 + _tmp149 * _tmp170 + _tmp150 * _tmp170 + + covariance(4, 2) * dt + covariance(7, 2); + const Scalar _tmp176 = _tmp137 * _tmp170 + _tmp140 * _tmp170 + _tmp142 * _tmp170 + + covariance(4, 0) * dt + covariance(7, 0); + const Scalar _tmp177 = _tmp155 * _tmp170 + _tmp158 * _tmp170 + _tmp160 * _tmp170 + + covariance(5, 1) * dt + covariance(8, 1); + const Scalar _tmp178 = _tmp162 * _tmp170 + _tmp163 * _tmp170 + _tmp164 * _tmp170 + + covariance(5, 2) * dt + covariance(8, 2); + const Scalar _tmp179 = _tmp166 * _tmp170 + _tmp167 * _tmp170 + _tmp168 * _tmp170 + + covariance(5, 0) * dt + covariance(8, 0); + const Scalar _tmp180 = _tmp112 * dt; + const Scalar _tmp181 = _tmp101 * _tmp180 + _tmp111 * _tmp93 + _tmp113 * _tmp98; + const Scalar _tmp182 = _tmp118 * _tmp67; + const Scalar _tmp183 = _tmp123 * _tmp69; + const Scalar _tmp184 = _tmp121 * _tmp68; + const Scalar _tmp185 = + _tmp56 * covariance(0, 3) + _tmp62 * covariance(2, 3) + _tmp66 * covariance(1, 3); + const Scalar _tmp186 = _tmp118 * _tmp88; + const Scalar _tmp187 = _tmp123 * _tmp90; + const Scalar _tmp188 = _tmp121 * _tmp89; + const Scalar _tmp189 = + _tmp82 * covariance(1, 3) + _tmp86 * covariance(2, 3) + _tmp87 * covariance(0, 3); + const Scalar _tmp190 = _tmp106 * _tmp118; + const Scalar _tmp191 = _tmp108 * _tmp123; + const Scalar _tmp192 = _tmp107 * _tmp121; + const Scalar _tmp193 = + _tmp103 * covariance(2, 3) + _tmp104 * covariance(1, 3) + _tmp105 * covariance(0, 3); + const Scalar _tmp194 = _tmp123 * _tmp129; + const Scalar _tmp195 = _tmp121 * _tmp133; + const Scalar _tmp196 = std::pow(_tmp31, Scalar(2)); + const Scalar _tmp197 = accel_cov_diagonal(2, 0) * dt; + const Scalar _tmp198 = std::pow(_tmp26, Scalar(2)); + const Scalar _tmp199 = accel_cov_diagonal(1, 0) * dt; + const Scalar _tmp200 = std::pow(_tmp21, Scalar(2)); + const Scalar _tmp201 = accel_cov_diagonal(0, 0) * dt; + const Scalar _tmp202 = _tmp118 * _tmp125; + const Scalar _tmp203 = _tmp118 * covariance(0, 3); + const Scalar _tmp204 = _tmp121 * covariance(1, 3); + const Scalar _tmp205 = _tmp123 * covariance(2, 3); + const Scalar _tmp206 = _tmp203 + _tmp204 + _tmp205 + covariance(3, 3); + const Scalar _tmp207 = _tmp118 * _tmp143; + const Scalar _tmp208 = _tmp123 * _tmp151; + const Scalar _tmp209 = _tmp121 * _tmp147; + const Scalar _tmp210 = _tmp136 * covariance(1, 3); + const Scalar _tmp211 = _tmp139 * covariance(0, 3); + const Scalar _tmp212 = _tmp141 * covariance(2, 3); + const Scalar _tmp213 = _tmp210 + _tmp211 + _tmp212 + covariance(4, 3); + const Scalar _tmp214 = _tmp134 * accel_cov_diagonal(2, 0); + const Scalar _tmp215 = _tmp120 * accel_cov_diagonal(0, 0); + const Scalar _tmp216 = + _tmp117 * _tmp35 * accel_cov_diagonal(1, 0) + _tmp214 * _tmp31 + _tmp215 * _tmp36; + const Scalar _tmp217 = _tmp121 * _tmp161; + const Scalar _tmp218 = _tmp123 * _tmp165; + const Scalar _tmp219 = _tmp118 * _tmp169; + const Scalar _tmp220 = _tmp154 * covariance(0, 3); + const Scalar _tmp221 = _tmp157 * covariance(1, 3); + const Scalar _tmp222 = _tmp159 * covariance(2, 3); + const Scalar _tmp223 = _tmp220 + _tmp221 + _tmp222 + covariance(5, 3); + const Scalar _tmp224 = _tmp152 * accel_cov_diagonal(1, 0); + const Scalar _tmp225 = _tmp41 * accel_cov_diagonal(2, 0); + const Scalar _tmp226 = _tmp116 * _tmp225 + _tmp215 * _tmp42 + _tmp224 * _tmp26; + const Scalar _tmp227 = _tmp123 * _tmp172; + const Scalar _tmp228 = _tmp121 * _tmp171; + const Scalar _tmp229 = _tmp118 * _tmp173; + const Scalar _tmp230 = _tmp170 * _tmp203 + _tmp170 * _tmp204 + _tmp170 * _tmp205 + + covariance(3, 3) * dt + covariance(6, 3); + const Scalar _tmp231 = _tmp196 * accel_cov_diagonal(2, 0); + const Scalar _tmp232 = _tmp45 * accel_cov_diagonal(1, 0); + const Scalar _tmp233 = _tmp200 * accel_cov_diagonal(0, 0); + const Scalar _tmp234 = _tmp198 * _tmp232 + _tmp231 * _tmp45 + _tmp233 * _tmp45; + const Scalar _tmp235 = _tmp118 * _tmp176; + const Scalar _tmp236 = _tmp121 * _tmp174; + const Scalar _tmp237 = _tmp123 * _tmp175; + const Scalar _tmp238 = _tmp170 * _tmp210 + _tmp170 * _tmp211 + _tmp170 * _tmp212 + + covariance(4, 3) * dt + covariance(7, 3); + const Scalar _tmp239 = _tmp31 * _tmp45; + const Scalar _tmp240 = _tmp39 * accel_cov_diagonal(2, 0); + const Scalar _tmp241 = _tmp35 * _tmp45; + const Scalar _tmp242 = _tmp241 * accel_cov_diagonal(1, 0); + const Scalar _tmp243 = _tmp36 * _tmp45; + const Scalar _tmp244 = _tmp21 * accel_cov_diagonal(0, 0); + const Scalar _tmp245 = _tmp239 * _tmp240 + _tmp242 * _tmp26 + _tmp243 * _tmp244; + const Scalar _tmp246 = _tmp118 * _tmp179; + const Scalar _tmp247 = _tmp121 * _tmp177; + const Scalar _tmp248 = _tmp123 * _tmp178; + const Scalar _tmp249 = _tmp170 * _tmp220 + _tmp170 * _tmp221 + _tmp170 * _tmp222 + + covariance(5, 3) * dt + covariance(8, 3); + const Scalar _tmp250 = _tmp26 * _tmp45; + const Scalar _tmp251 = _tmp21 * _tmp45; + const Scalar _tmp252 = _tmp42 * accel_cov_diagonal(0, 0); + const Scalar _tmp253 = _tmp41 * _tmp45; + const Scalar _tmp254 = _tmp250 * _tmp43 * accel_cov_diagonal(1, 0) + _tmp251 * _tmp252 + + _tmp253 * _tmp31 * accel_cov_diagonal(2, 0); + const Scalar _tmp255 = _tmp139 * _tmp67; + const Scalar _tmp256 = _tmp141 * _tmp69; + const Scalar _tmp257 = _tmp136 * _tmp68; + const Scalar _tmp258 = + _tmp56 * covariance(0, 4) + _tmp62 * covariance(2, 4) + _tmp66 * covariance(1, 4); + const Scalar _tmp259 = _tmp139 * _tmp88; + const Scalar _tmp260 = _tmp141 * _tmp90; + const Scalar _tmp261 = _tmp136 * _tmp89; + const Scalar _tmp262 = + _tmp82 * covariance(1, 4) + _tmp86 * covariance(2, 4) + _tmp87 * covariance(0, 4); + const Scalar _tmp263 = _tmp108 * _tmp141; + const Scalar _tmp264 = _tmp106 * _tmp139; + const Scalar _tmp265 = _tmp107 * _tmp136; + const Scalar _tmp266 = + _tmp103 * covariance(2, 4) + _tmp104 * covariance(1, 4) + _tmp105 * covariance(0, 4); + const Scalar _tmp267 = _tmp133 * _tmp136; + const Scalar _tmp268 = _tmp125 * _tmp139; + const Scalar _tmp269 = _tmp129 * _tmp141; + const Scalar _tmp270 = _tmp118 * covariance(0, 4); + const Scalar _tmp271 = _tmp121 * covariance(1, 4); + const Scalar _tmp272 = _tmp123 * covariance(2, 4); + const Scalar _tmp273 = _tmp270 + _tmp271 + _tmp272 + covariance(3, 4); + const Scalar _tmp274 = _tmp136 * _tmp147; + const Scalar _tmp275 = _tmp139 * _tmp143; + const Scalar _tmp276 = std::pow(_tmp36, Scalar(2)); + const Scalar _tmp277 = _tmp141 * _tmp151; + const Scalar _tmp278 = std::pow(_tmp35, Scalar(2)); + const Scalar _tmp279 = std::pow(_tmp39, Scalar(2)); + const Scalar _tmp280 = _tmp136 * covariance(1, 4); + const Scalar _tmp281 = _tmp139 * covariance(0, 4); + const Scalar _tmp282 = _tmp141 * covariance(2, 4); + const Scalar _tmp283 = _tmp280 + _tmp281 + _tmp282 + covariance(4, 4); + const Scalar _tmp284 = _tmp136 * _tmp161; + const Scalar _tmp285 = _tmp141 * _tmp165; + const Scalar _tmp286 = _tmp139 * _tmp169; + const Scalar _tmp287 = _tmp154 * covariance(0, 4); + const Scalar _tmp288 = _tmp157 * covariance(1, 4); + const Scalar _tmp289 = _tmp159 * covariance(2, 4); + const Scalar _tmp290 = _tmp287 + _tmp288 + _tmp289 + covariance(5, 4); + const Scalar _tmp291 = _tmp135 * _tmp252 + _tmp214 * _tmp41 + _tmp224 * _tmp35; + const Scalar _tmp292 = _tmp141 * _tmp172; + const Scalar _tmp293 = _tmp136 * _tmp171; + const Scalar _tmp294 = _tmp139 * _tmp173; + const Scalar _tmp295 = _tmp170 * _tmp270 + _tmp170 * _tmp271 + _tmp170 * _tmp272 + + covariance(3, 4) * dt + covariance(6, 4); + const Scalar _tmp296 = _tmp141 * _tmp175; + const Scalar _tmp297 = _tmp136 * _tmp174; + const Scalar _tmp298 = _tmp139 * _tmp176; + const Scalar _tmp299 = _tmp170 * _tmp280 + _tmp170 * _tmp281 + _tmp170 * _tmp282 + + covariance(4, 4) * dt + covariance(7, 4); + const Scalar _tmp300 = _tmp276 * accel_cov_diagonal(0, 0); + const Scalar _tmp301 = _tmp279 * accel_cov_diagonal(2, 0); + const Scalar _tmp302 = _tmp232 * _tmp278 + _tmp300 * _tmp45 + _tmp301 * _tmp45; + const Scalar _tmp303 = _tmp136 * _tmp177; + const Scalar _tmp304 = _tmp141 * _tmp178; + const Scalar _tmp305 = _tmp139 * _tmp179; + const Scalar _tmp306 = _tmp170 * _tmp287 + _tmp170 * _tmp288 + _tmp170 * _tmp289 + + covariance(5, 4) * dt + covariance(8, 4); + const Scalar _tmp307 = _tmp240 * _tmp253 + _tmp242 * _tmp43 + _tmp243 * _tmp252; + const Scalar _tmp308 = _tmp157 * _tmp68; + const Scalar _tmp309 = _tmp159 * _tmp69; + const Scalar _tmp310 = _tmp154 * _tmp67; + const Scalar _tmp311 = + _tmp56 * covariance(0, 5) + _tmp62 * covariance(2, 5) + _tmp66 * covariance(1, 5); + const Scalar _tmp312 = _tmp154 * _tmp88; + const Scalar _tmp313 = _tmp157 * _tmp89; + const Scalar _tmp314 = _tmp159 * _tmp90; + const Scalar _tmp315 = + _tmp82 * covariance(1, 5) + _tmp86 * covariance(2, 5) + _tmp87 * covariance(0, 5); + const Scalar _tmp316 = _tmp106 * _tmp154; + const Scalar _tmp317 = _tmp108 * _tmp159; + const Scalar _tmp318 = _tmp107 * _tmp157; + const Scalar _tmp319 = + _tmp103 * covariance(2, 5) + _tmp104 * covariance(1, 5) + _tmp105 * covariance(0, 5); + const Scalar _tmp320 = _tmp133 * _tmp157; + const Scalar _tmp321 = _tmp125 * _tmp154; + const Scalar _tmp322 = _tmp129 * _tmp159; + const Scalar _tmp323 = _tmp118 * covariance(0, 5); + const Scalar _tmp324 = _tmp121 * covariance(1, 5); + const Scalar _tmp325 = _tmp123 * covariance(2, 5); + const Scalar _tmp326 = _tmp323 + _tmp324 + _tmp325 + covariance(3, 5); + const Scalar _tmp327 = _tmp147 * _tmp157; + const Scalar _tmp328 = _tmp143 * _tmp154; + const Scalar _tmp329 = _tmp151 * _tmp159; + const Scalar _tmp330 = _tmp136 * covariance(1, 5); + const Scalar _tmp331 = _tmp139 * covariance(0, 5); + const Scalar _tmp332 = _tmp141 * covariance(2, 5); + const Scalar _tmp333 = _tmp330 + _tmp331 + _tmp332 + covariance(4, 5); + const Scalar _tmp334 = _tmp157 * _tmp161; + const Scalar _tmp335 = _tmp159 * _tmp165; + const Scalar _tmp336 = std::pow(_tmp42, Scalar(2)); + const Scalar _tmp337 = std::pow(_tmp41, Scalar(2)); + const Scalar _tmp338 = _tmp154 * _tmp169; + const Scalar _tmp339 = std::pow(_tmp43, Scalar(2)); + const Scalar _tmp340 = _tmp154 * covariance(0, 5); + const Scalar _tmp341 = _tmp157 * covariance(1, 5); + const Scalar _tmp342 = _tmp159 * covariance(2, 5); + const Scalar _tmp343 = _tmp340 + _tmp341 + _tmp342 + covariance(5, 5); + const Scalar _tmp344 = _tmp159 * _tmp172; + const Scalar _tmp345 = _tmp157 * _tmp171; + const Scalar _tmp346 = _tmp154 * _tmp173; + const Scalar _tmp347 = _tmp170 * _tmp323 + _tmp170 * _tmp324 + _tmp170 * _tmp325 + + covariance(3, 5) * dt + covariance(6, 5); + const Scalar _tmp348 = _tmp157 * _tmp174; + const Scalar _tmp349 = _tmp159 * _tmp175; + const Scalar _tmp350 = _tmp154 * _tmp176; + const Scalar _tmp351 = _tmp170 * _tmp330 + _tmp170 * _tmp331 + _tmp170 * _tmp332 + + covariance(4, 5) * dt + covariance(7, 5); + const Scalar _tmp352 = _tmp157 * _tmp177; + const Scalar _tmp353 = _tmp159 * _tmp178; + const Scalar _tmp354 = _tmp154 * _tmp179; + const Scalar _tmp355 = _tmp170 * _tmp340 + _tmp170 * _tmp341 + _tmp170 * _tmp342 + + covariance(5, 5) * dt + covariance(8, 5); + const Scalar _tmp356 = _tmp336 * accel_cov_diagonal(0, 0); + const Scalar _tmp357 = _tmp337 * accel_cov_diagonal(2, 0); + const Scalar _tmp358 = _tmp232 * _tmp339 + _tmp356 * _tmp45 + _tmp357 * _tmp45; + const Scalar _tmp359 = _tmp118 * covariance(0, 6); + const Scalar _tmp360 = _tmp121 * covariance(1, 6); + const Scalar _tmp361 = _tmp123 * covariance(2, 6); + const Scalar _tmp362 = _tmp136 * covariance(1, 6); + const Scalar _tmp363 = _tmp139 * covariance(0, 6); + const Scalar _tmp364 = _tmp141 * covariance(2, 6); + const Scalar _tmp365 = _tmp154 * covariance(0, 6); + const Scalar _tmp366 = _tmp157 * covariance(1, 6); + const Scalar _tmp367 = _tmp159 * covariance(2, 6); + const Scalar _tmp368 = (Scalar(1) / Scalar(4)) * [&]() { + const Scalar base = dt; + return base * base * base; + }(); + const Scalar _tmp369 = _tmp368 * accel_cov_diagonal(1, 0); + const Scalar _tmp370 = _tmp31 * _tmp368; + const Scalar _tmp371 = _tmp26 * _tmp369; + const Scalar _tmp372 = _tmp36 * _tmp368; + const Scalar _tmp373 = _tmp240 * _tmp370 + _tmp244 * _tmp372 + _tmp35 * _tmp371; + const Scalar _tmp374 = _tmp21 * _tmp252 * _tmp368 + _tmp225 * _tmp370 + _tmp371 * _tmp43; + const Scalar _tmp375 = _tmp118 * covariance(0, 7); + const Scalar _tmp376 = _tmp121 * covariance(1, 7); + const Scalar _tmp377 = _tmp123 * covariance(2, 7); + const Scalar _tmp378 = _tmp136 * covariance(1, 7); + const Scalar _tmp379 = _tmp139 * covariance(0, 7); + const Scalar _tmp380 = _tmp141 * covariance(2, 7); + const Scalar _tmp381 = _tmp154 * covariance(0, 7); + const Scalar _tmp382 = _tmp157 * covariance(1, 7); + const Scalar _tmp383 = _tmp159 * covariance(2, 7); + const Scalar _tmp384 = _tmp225 * _tmp368 * _tmp39 + _tmp252 * _tmp372 + _tmp35 * _tmp369 * _tmp43; + const Scalar _tmp385 = _tmp118 * covariance(0, 8); + const Scalar _tmp386 = _tmp121 * covariance(1, 8); + const Scalar _tmp387 = _tmp123 * covariance(2, 8); + const Scalar _tmp388 = _tmp136 * covariance(1, 8); + const Scalar _tmp389 = _tmp139 * covariance(0, 8); + const Scalar _tmp390 = _tmp141 * covariance(2, 8); + const Scalar _tmp391 = _tmp154 * covariance(0, 8); + const Scalar _tmp392 = _tmp157 * covariance(1, 8); + const Scalar _tmp393 = _tmp159 * covariance(2, 8); + const Scalar _tmp394 = DR_D_gyro_bias(2, 0) * _tmp123; + const Scalar _tmp395 = DR_D_gyro_bias(1, 0) * _tmp121; + const Scalar _tmp396 = DR_D_gyro_bias(0, 0) * _tmp118; + const Scalar _tmp397 = DR_D_gyro_bias(2, 0) * _tmp141; + const Scalar _tmp398 = DR_D_gyro_bias(1, 0) * _tmp136; + const Scalar _tmp399 = DR_D_gyro_bias(0, 0) * _tmp139; + const Scalar _tmp400 = DR_D_gyro_bias(2, 0) * _tmp159; + const Scalar _tmp401 = DR_D_gyro_bias(1, 0) * _tmp157; + const Scalar _tmp402 = DR_D_gyro_bias(0, 0) * _tmp154; + const Scalar _tmp403 = DR_D_gyro_bias(2, 1) * _tmp123; + const Scalar _tmp404 = DR_D_gyro_bias(1, 1) * _tmp121; + const Scalar _tmp405 = DR_D_gyro_bias(0, 1) * _tmp118; + const Scalar _tmp406 = DR_D_gyro_bias(2, 1) * _tmp141; + const Scalar _tmp407 = DR_D_gyro_bias(1, 1) * _tmp136; + const Scalar _tmp408 = DR_D_gyro_bias(0, 1) * _tmp139; + const Scalar _tmp409 = DR_D_gyro_bias(2, 1) * _tmp159; + const Scalar _tmp410 = DR_D_gyro_bias(1, 1) * _tmp157; + const Scalar _tmp411 = DR_D_gyro_bias(0, 1) * _tmp154; + const Scalar _tmp412 = DR_D_gyro_bias(2, 2) * _tmp123; + const Scalar _tmp413 = DR_D_gyro_bias(1, 2) * _tmp121; + const Scalar _tmp414 = DR_D_gyro_bias(0, 2) * _tmp118; + const Scalar _tmp415 = DR_D_gyro_bias(2, 2) * _tmp141; + const Scalar _tmp416 = DR_D_gyro_bias(1, 2) * _tmp136; + const Scalar _tmp417 = DR_D_gyro_bias(0, 2) * _tmp139; + const Scalar _tmp418 = DR_D_gyro_bias(2, 2) * _tmp159; + const Scalar _tmp419 = DR_D_gyro_bias(1, 2) * _tmp157; + const Scalar _tmp420 = DR_D_gyro_bias(0, 2) * _tmp154; + + // Output terms (9) + if (new_DR != nullptr) { + Eigen::Matrix _new_DR; + + _new_DR[0] = _DR[0] * _tmp18 + _DR[1] * _tmp16 - _DR[2] * _tmp17 + _DR[3] * _tmp14; + _new_DR[1] = -_DR[0] * _tmp16 + _DR[1] * _tmp18 + _DR[2] * _tmp14 + _DR[3] * _tmp17; + _new_DR[2] = _DR[0] * _tmp17 - _DR[1] * _tmp14 + _DR[2] * _tmp18 + _DR[3] * _tmp16; + _new_DR[3] = -_DR[0] * _tmp14 - _DR[1] * _tmp17 - _DR[2] * _tmp16 + _DR[3] * _tmp18; + + *new_DR = sym::Rot3(_new_DR); + } + + if (new_Dv != nullptr) { + Eigen::Matrix& _new_Dv = (*new_Dv); + + _new_Dv(0, 0) = Dv(0, 0) + _tmp33 * dt; + _new_Dv(1, 0) = Dv(1, 0) + _tmp40 * dt; + _new_Dv(2, 0) = Dv(2, 0) + _tmp44 * dt; + } + + if (new_Dp != nullptr) { + Eigen::Matrix& _new_Dp = (*new_Dp); + + _new_Dp(0, 0) = Dp(0, 0) + Dv(0, 0) * dt + _tmp33 * _tmp45; + _new_Dp(1, 0) = Dp(1, 0) + Dv(1, 0) * dt + _tmp40 * _tmp45; + _new_Dp(2, 0) = Dp(2, 0) + Dv(2, 0) * dt + _tmp44 * _tmp45; + } + + if (new_covariance != nullptr) { + Eigen::Matrix& _new_covariance = (*new_covariance); + + _new_covariance(0, 0) = + std::pow(_tmp51, Scalar(2)) * _tmp52 + _tmp56 * _tmp67 + _tmp62 * _tmp69 + _tmp66 * _tmp68 + + std::pow(_tmp75, Scalar(2)) * _tmp76 + std::pow(_tmp79, Scalar(2)) * _tmp80; + _new_covariance(1, 0) = _tmp102 + _tmp56 * _tmp88 + _tmp62 * _tmp90 + _tmp66 * _tmp89; + _new_covariance(2, 0) = _tmp106 * _tmp56 + _tmp107 * _tmp66 + _tmp108 * _tmp62 + _tmp115; + _new_covariance(3, 0) = _tmp125 * _tmp56 + _tmp129 * _tmp62 + _tmp133 * _tmp66; + _new_covariance(4, 0) = _tmp143 * _tmp56 + _tmp147 * _tmp66 + _tmp151 * _tmp62; + _new_covariance(5, 0) = _tmp161 * _tmp66 + _tmp165 * _tmp62 + _tmp169 * _tmp56; + _new_covariance(6, 0) = _tmp171 * _tmp66 + _tmp172 * _tmp62 + _tmp173 * _tmp56; + _new_covariance(7, 0) = _tmp174 * _tmp66 + _tmp175 * _tmp62 + _tmp176 * _tmp56; + _new_covariance(8, 0) = _tmp177 * _tmp66 + _tmp178 * _tmp62 + _tmp179 * _tmp56; + _new_covariance(0, 1) = _tmp102 + _tmp67 * _tmp87 + _tmp68 * _tmp82 + _tmp69 * _tmp86; + _new_covariance(1, 1) = + std::pow(_tmp100, Scalar(2)) * _tmp52 + _tmp76 * std::pow(_tmp93, Scalar(2)) + + _tmp80 * std::pow(_tmp96, Scalar(2)) + _tmp82 * _tmp89 + _tmp86 * _tmp90 + _tmp87 * _tmp88; + _new_covariance(2, 1) = _tmp106 * _tmp87 + _tmp107 * _tmp82 + _tmp108 * _tmp86 + _tmp181; + _new_covariance(3, 1) = _tmp125 * _tmp87 + _tmp129 * _tmp86 + _tmp133 * _tmp82; + _new_covariance(4, 1) = _tmp143 * _tmp87 + _tmp147 * _tmp82 + _tmp151 * _tmp86; + _new_covariance(5, 1) = _tmp161 * _tmp82 + _tmp165 * _tmp86 + _tmp169 * _tmp87; + _new_covariance(6, 1) = _tmp171 * _tmp82 + _tmp172 * _tmp86 + _tmp173 * _tmp87; + _new_covariance(7, 1) = _tmp174 * _tmp82 + _tmp175 * _tmp86 + _tmp176 * _tmp87; + _new_covariance(8, 1) = _tmp177 * _tmp82 + _tmp178 * _tmp86 + _tmp179 * _tmp87; + _new_covariance(0, 2) = _tmp103 * _tmp69 + _tmp104 * _tmp68 + _tmp105 * _tmp67 + _tmp115; + _new_covariance(1, 2) = _tmp103 * _tmp90 + _tmp104 * _tmp89 + _tmp105 * _tmp88 + _tmp181; + _new_covariance(2, 2) = _tmp103 * _tmp108 + _tmp104 * _tmp107 + _tmp105 * _tmp106 + + std::pow(_tmp109, Scalar(2)) * _tmp76 + + std::pow(_tmp112, Scalar(2)) * _tmp52 + + std::pow(_tmp113, Scalar(2)) * _tmp80; + _new_covariance(3, 2) = _tmp103 * _tmp129 + _tmp104 * _tmp133 + _tmp105 * _tmp125; + _new_covariance(4, 2) = _tmp103 * _tmp151 + _tmp104 * _tmp147 + _tmp105 * _tmp143; + _new_covariance(5, 2) = _tmp103 * _tmp165 + _tmp104 * _tmp161 + _tmp105 * _tmp169; + _new_covariance(6, 2) = _tmp103 * _tmp172 + _tmp104 * _tmp171 + _tmp105 * _tmp173; + _new_covariance(7, 2) = _tmp103 * _tmp175 + _tmp104 * _tmp174 + _tmp105 * _tmp176; + _new_covariance(8, 2) = _tmp103 * _tmp178 + _tmp104 * _tmp177 + _tmp105 * _tmp179; + _new_covariance(0, 3) = _tmp182 + _tmp183 + _tmp184 + _tmp185; + _new_covariance(1, 3) = _tmp186 + _tmp187 + _tmp188 + _tmp189; + _new_covariance(2, 3) = _tmp190 + _tmp191 + _tmp192 + _tmp193; + _new_covariance(3, 3) = _tmp194 + _tmp195 + _tmp196 * _tmp197 + _tmp198 * _tmp199 + + _tmp200 * _tmp201 + _tmp202 + _tmp206; + _new_covariance(4, 3) = _tmp207 + _tmp208 + _tmp209 + _tmp213 + _tmp216; + _new_covariance(5, 3) = _tmp217 + _tmp218 + _tmp219 + _tmp223 + _tmp226; + _new_covariance(6, 3) = _tmp227 + _tmp228 + _tmp229 + _tmp230 + _tmp234; + _new_covariance(7, 3) = _tmp235 + _tmp236 + _tmp237 + _tmp238 + _tmp245; + _new_covariance(8, 3) = _tmp246 + _tmp247 + _tmp248 + _tmp249 + _tmp254; + _new_covariance(0, 4) = _tmp255 + _tmp256 + _tmp257 + _tmp258; + _new_covariance(1, 4) = _tmp259 + _tmp260 + _tmp261 + _tmp262; + _new_covariance(2, 4) = _tmp263 + _tmp264 + _tmp265 + _tmp266; + _new_covariance(3, 4) = _tmp216 + _tmp267 + _tmp268 + _tmp269 + _tmp273; + _new_covariance(4, 4) = _tmp197 * _tmp279 + _tmp199 * _tmp278 + _tmp201 * _tmp276 + _tmp274 + + _tmp275 + _tmp277 + _tmp283; + _new_covariance(5, 4) = _tmp284 + _tmp285 + _tmp286 + _tmp290 + _tmp291; + _new_covariance(6, 4) = _tmp245 + _tmp292 + _tmp293 + _tmp294 + _tmp295; + _new_covariance(7, 4) = _tmp296 + _tmp297 + _tmp298 + _tmp299 + _tmp302; + _new_covariance(8, 4) = _tmp303 + _tmp304 + _tmp305 + _tmp306 + _tmp307; + _new_covariance(0, 5) = _tmp308 + _tmp309 + _tmp310 + _tmp311; + _new_covariance(1, 5) = _tmp312 + _tmp313 + _tmp314 + _tmp315; + _new_covariance(2, 5) = _tmp316 + _tmp317 + _tmp318 + _tmp319; + _new_covariance(3, 5) = _tmp226 + _tmp320 + _tmp321 + _tmp322 + _tmp326; + _new_covariance(4, 5) = _tmp291 + _tmp327 + _tmp328 + _tmp329 + _tmp333; + _new_covariance(5, 5) = _tmp197 * _tmp337 + _tmp199 * _tmp339 + _tmp201 * _tmp336 + _tmp334 + + _tmp335 + _tmp338 + _tmp343; + _new_covariance(6, 5) = _tmp254 + _tmp344 + _tmp345 + _tmp346 + _tmp347; + _new_covariance(7, 5) = _tmp307 + _tmp348 + _tmp349 + _tmp350 + _tmp351; + _new_covariance(8, 5) = _tmp352 + _tmp353 + _tmp354 + _tmp355 + _tmp358; + _new_covariance(0, 6) = _tmp170 * _tmp182 + _tmp170 * _tmp183 + _tmp170 * _tmp184 + + _tmp185 * dt + _tmp56 * covariance(0, 6) + _tmp62 * covariance(2, 6) + + _tmp66 * covariance(1, 6); + _new_covariance(1, 6) = _tmp170 * _tmp186 + _tmp170 * _tmp187 + _tmp170 * _tmp188 + + _tmp189 * dt + _tmp82 * covariance(1, 6) + _tmp86 * covariance(2, 6) + + _tmp87 * covariance(0, 6); + _new_covariance(2, 6) = _tmp103 * covariance(2, 6) + _tmp104 * covariance(1, 6) + + _tmp105 * covariance(0, 6) + _tmp170 * _tmp190 + _tmp170 * _tmp191 + + _tmp170 * _tmp192 + _tmp193 * dt; + _new_covariance(3, 6) = _tmp170 * _tmp194 + _tmp170 * _tmp195 + _tmp170 * _tmp202 + + _tmp206 * dt + _tmp234 + _tmp359 + _tmp360 + _tmp361 + covariance(3, 6); + _new_covariance(4, 6) = _tmp170 * _tmp207 + _tmp170 * _tmp208 + _tmp170 * _tmp209 + + _tmp213 * dt + _tmp245 + _tmp362 + _tmp363 + _tmp364 + covariance(4, 6); + _new_covariance(5, 6) = _tmp170 * _tmp217 + _tmp170 * _tmp218 + _tmp170 * _tmp219 + + _tmp223 * dt + _tmp254 + _tmp365 + _tmp366 + _tmp367 + covariance(5, 6); + _new_covariance(6, 6) = _tmp170 * _tmp227 + _tmp170 * _tmp228 + _tmp170 * _tmp229 + + _tmp170 * _tmp359 + _tmp170 * _tmp360 + _tmp170 * _tmp361 + + _tmp198 * _tmp369 + _tmp230 * dt + _tmp231 * _tmp368 + + _tmp233 * _tmp368 + covariance(3, 6) * dt + covariance(6, 6); + _new_covariance(7, 6) = _tmp170 * _tmp235 + _tmp170 * _tmp236 + _tmp170 * _tmp237 + + _tmp170 * _tmp362 + _tmp170 * _tmp363 + _tmp170 * _tmp364 + + _tmp238 * dt + _tmp373 + covariance(4, 6) * dt + covariance(7, 6); + _new_covariance(8, 6) = _tmp170 * _tmp246 + _tmp170 * _tmp247 + _tmp170 * _tmp248 + + _tmp170 * _tmp365 + _tmp170 * _tmp366 + _tmp170 * _tmp367 + + _tmp249 * dt + _tmp374 + covariance(5, 6) * dt + covariance(8, 6); + _new_covariance(0, 7) = _tmp170 * _tmp255 + _tmp170 * _tmp256 + _tmp170 * _tmp257 + + _tmp258 * dt + _tmp56 * covariance(0, 7) + _tmp62 * covariance(2, 7) + + _tmp66 * covariance(1, 7); + _new_covariance(1, 7) = _tmp170 * _tmp259 + _tmp170 * _tmp260 + _tmp170 * _tmp261 + + _tmp262 * dt + _tmp82 * covariance(1, 7) + _tmp86 * covariance(2, 7) + + _tmp87 * covariance(0, 7); + _new_covariance(2, 7) = _tmp103 * covariance(2, 7) + _tmp104 * covariance(1, 7) + + _tmp105 * covariance(0, 7) + _tmp170 * _tmp263 + _tmp170 * _tmp264 + + _tmp170 * _tmp265 + _tmp266 * dt; + _new_covariance(3, 7) = _tmp170 * _tmp267 + _tmp170 * _tmp268 + _tmp170 * _tmp269 + _tmp245 + + _tmp273 * dt + _tmp375 + _tmp376 + _tmp377 + covariance(3, 7); + _new_covariance(4, 7) = _tmp170 * _tmp274 + _tmp170 * _tmp275 + _tmp170 * _tmp277 + + _tmp283 * dt + _tmp302 + _tmp378 + _tmp379 + _tmp380 + covariance(4, 7); + _new_covariance(5, 7) = _tmp170 * _tmp284 + _tmp170 * _tmp285 + _tmp170 * _tmp286 + + _tmp290 * dt + _tmp307 + _tmp381 + _tmp382 + _tmp383 + covariance(5, 7); + _new_covariance(6, 7) = _tmp170 * _tmp292 + _tmp170 * _tmp293 + _tmp170 * _tmp294 + + _tmp170 * _tmp375 + _tmp170 * _tmp376 + _tmp170 * _tmp377 + + _tmp295 * dt + _tmp373 + covariance(3, 7) * dt + covariance(6, 7); + _new_covariance(7, 7) = _tmp170 * _tmp296 + _tmp170 * _tmp297 + _tmp170 * _tmp298 + + _tmp170 * _tmp378 + _tmp170 * _tmp379 + _tmp170 * _tmp380 + + _tmp278 * _tmp369 + _tmp299 * dt + _tmp300 * _tmp368 + + _tmp301 * _tmp368 + covariance(4, 7) * dt + covariance(7, 7); + _new_covariance(8, 7) = _tmp170 * _tmp303 + _tmp170 * _tmp304 + _tmp170 * _tmp305 + + _tmp170 * _tmp381 + _tmp170 * _tmp382 + _tmp170 * _tmp383 + + _tmp306 * dt + _tmp384 + covariance(5, 7) * dt + covariance(8, 7); + _new_covariance(0, 8) = _tmp170 * _tmp308 + _tmp170 * _tmp309 + _tmp170 * _tmp310 + + _tmp311 * dt + _tmp56 * covariance(0, 8) + _tmp62 * covariance(2, 8) + + _tmp66 * covariance(1, 8); + _new_covariance(1, 8) = _tmp170 * _tmp312 + _tmp170 * _tmp313 + _tmp170 * _tmp314 + + _tmp315 * dt + _tmp82 * covariance(1, 8) + _tmp86 * covariance(2, 8) + + _tmp87 * covariance(0, 8); + _new_covariance(2, 8) = _tmp103 * covariance(2, 8) + _tmp104 * covariance(1, 8) + + _tmp105 * covariance(0, 8) + _tmp170 * _tmp316 + _tmp170 * _tmp317 + + _tmp170 * _tmp318 + _tmp319 * dt; + _new_covariance(3, 8) = _tmp170 * _tmp320 + _tmp170 * _tmp321 + _tmp170 * _tmp322 + _tmp254 + + _tmp326 * dt + _tmp385 + _tmp386 + _tmp387 + covariance(3, 8); + _new_covariance(4, 8) = _tmp170 * _tmp327 + _tmp170 * _tmp328 + _tmp170 * _tmp329 + _tmp307 + + _tmp333 * dt + _tmp388 + _tmp389 + _tmp390 + covariance(4, 8); + _new_covariance(5, 8) = _tmp170 * _tmp334 + _tmp170 * _tmp335 + _tmp170 * _tmp338 + + _tmp343 * dt + _tmp358 + _tmp391 + _tmp392 + _tmp393 + covariance(5, 8); + _new_covariance(6, 8) = _tmp170 * _tmp344 + _tmp170 * _tmp345 + _tmp170 * _tmp346 + + _tmp170 * _tmp385 + _tmp170 * _tmp386 + _tmp170 * _tmp387 + + _tmp347 * dt + _tmp374 + covariance(3, 8) * dt + covariance(6, 8); + _new_covariance(7, 8) = _tmp170 * _tmp348 + _tmp170 * _tmp349 + _tmp170 * _tmp350 + + _tmp170 * _tmp388 + _tmp170 * _tmp389 + _tmp170 * _tmp390 + + _tmp351 * dt + _tmp384 + covariance(4, 8) * dt + covariance(7, 8); + _new_covariance(8, 8) = _tmp170 * _tmp352 + _tmp170 * _tmp353 + _tmp170 * _tmp354 + + _tmp170 * _tmp391 + _tmp170 * _tmp392 + _tmp170 * _tmp393 + + _tmp339 * _tmp369 + _tmp355 * dt + _tmp356 * _tmp368 + + _tmp357 * _tmp368 + covariance(5, 8) * dt + covariance(8, 8); + } + + if (new_DR_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_DR_D_gyro_bias = (*new_DR_D_gyro_bias); + + _new_DR_D_gyro_bias(0, 0) = DR_D_gyro_bias(0, 0) * _tmp56 + DR_D_gyro_bias(1, 0) * _tmp66 + + DR_D_gyro_bias(2, 0) * _tmp62 - _tmp99; + _new_DR_D_gyro_bias(1, 0) = DR_D_gyro_bias(0, 0) * _tmp87 + DR_D_gyro_bias(1, 0) * _tmp82 + + DR_D_gyro_bias(2, 0) * _tmp86 - _tmp100 * dt; + _new_DR_D_gyro_bias(2, 0) = DR_D_gyro_bias(0, 0) * _tmp105 + DR_D_gyro_bias(1, 0) * _tmp104 + + DR_D_gyro_bias(2, 0) * _tmp103 - _tmp180; + _new_DR_D_gyro_bias(0, 1) = DR_D_gyro_bias(0, 1) * _tmp56 + DR_D_gyro_bias(1, 1) * _tmp66 + + DR_D_gyro_bias(2, 1) * _tmp62 - _tmp79 * dt; + _new_DR_D_gyro_bias(1, 1) = DR_D_gyro_bias(0, 1) * _tmp87 + DR_D_gyro_bias(1, 1) * _tmp82 + + DR_D_gyro_bias(2, 1) * _tmp86 - _tmp97; + _new_DR_D_gyro_bias(2, 1) = DR_D_gyro_bias(0, 1) * _tmp105 + DR_D_gyro_bias(1, 1) * _tmp104 + + DR_D_gyro_bias(2, 1) * _tmp103 - _tmp114; + _new_DR_D_gyro_bias(0, 2) = DR_D_gyro_bias(0, 2) * _tmp56 + DR_D_gyro_bias(1, 2) * _tmp66 + + DR_D_gyro_bias(2, 2) * _tmp62 - _tmp75 * dt; + _new_DR_D_gyro_bias(1, 2) = DR_D_gyro_bias(0, 2) * _tmp87 + DR_D_gyro_bias(1, 2) * _tmp82 + + DR_D_gyro_bias(2, 2) * _tmp86 - _tmp94; + _new_DR_D_gyro_bias(2, 2) = DR_D_gyro_bias(0, 2) * _tmp105 + DR_D_gyro_bias(1, 2) * _tmp104 + + DR_D_gyro_bias(2, 2) * _tmp103 - _tmp110; + } + + if (new_Dv_D_accel_bias != nullptr) { + Eigen::Matrix& _new_Dv_D_accel_bias = (*new_Dv_D_accel_bias); + + _new_Dv_D_accel_bias(0, 0) = Dv_D_accel_bias(0, 0) - _tmp120; + _new_Dv_D_accel_bias(1, 0) = Dv_D_accel_bias(1, 0) - _tmp135; + _new_Dv_D_accel_bias(2, 0) = Dv_D_accel_bias(2, 0) - _tmp156; + _new_Dv_D_accel_bias(0, 1) = Dv_D_accel_bias(0, 1) - _tmp117; + _new_Dv_D_accel_bias(1, 1) = Dv_D_accel_bias(1, 1) - _tmp138; + _new_Dv_D_accel_bias(2, 1) = Dv_D_accel_bias(2, 1) - _tmp152; + _new_Dv_D_accel_bias(0, 2) = Dv_D_accel_bias(0, 2) - _tmp116; + _new_Dv_D_accel_bias(1, 2) = Dv_D_accel_bias(1, 2) - _tmp134; + _new_Dv_D_accel_bias(2, 2) = Dv_D_accel_bias(2, 2) - _tmp153; + } + + if (new_Dv_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_Dv_D_gyro_bias = (*new_Dv_D_gyro_bias); + + _new_Dv_D_gyro_bias(0, 0) = Dv_D_gyro_bias(0, 0) + _tmp394 + _tmp395 + _tmp396; + _new_Dv_D_gyro_bias(1, 0) = Dv_D_gyro_bias(1, 0) + _tmp397 + _tmp398 + _tmp399; + _new_Dv_D_gyro_bias(2, 0) = Dv_D_gyro_bias(2, 0) + _tmp400 + _tmp401 + _tmp402; + _new_Dv_D_gyro_bias(0, 1) = Dv_D_gyro_bias(0, 1) + _tmp403 + _tmp404 + _tmp405; + _new_Dv_D_gyro_bias(1, 1) = Dv_D_gyro_bias(1, 1) + _tmp406 + _tmp407 + _tmp408; + _new_Dv_D_gyro_bias(2, 1) = Dv_D_gyro_bias(2, 1) + _tmp409 + _tmp410 + _tmp411; + _new_Dv_D_gyro_bias(0, 2) = Dv_D_gyro_bias(0, 2) + _tmp412 + _tmp413 + _tmp414; + _new_Dv_D_gyro_bias(1, 2) = Dv_D_gyro_bias(1, 2) + _tmp415 + _tmp416 + _tmp417; + _new_Dv_D_gyro_bias(2, 2) = Dv_D_gyro_bias(2, 2) + _tmp418 + _tmp419 + _tmp420; + } + + if (new_Dp_D_accel_bias != nullptr) { + Eigen::Matrix& _new_Dp_D_accel_bias = (*new_Dp_D_accel_bias); + + _new_Dp_D_accel_bias(0, 0) = Dp_D_accel_bias(0, 0) + Dv_D_accel_bias(0, 0) * dt - _tmp251; + _new_Dp_D_accel_bias(1, 0) = Dp_D_accel_bias(1, 0) + Dv_D_accel_bias(1, 0) * dt - _tmp243; + _new_Dp_D_accel_bias(2, 0) = + Dp_D_accel_bias(2, 0) + Dv_D_accel_bias(2, 0) * dt - _tmp42 * _tmp45; + _new_Dp_D_accel_bias(0, 1) = Dp_D_accel_bias(0, 1) + Dv_D_accel_bias(0, 1) * dt - _tmp250; + _new_Dp_D_accel_bias(1, 1) = Dp_D_accel_bias(1, 1) + Dv_D_accel_bias(1, 1) * dt - _tmp241; + _new_Dp_D_accel_bias(2, 1) = + Dp_D_accel_bias(2, 1) + Dv_D_accel_bias(2, 1) * dt - _tmp43 * _tmp45; + _new_Dp_D_accel_bias(0, 2) = Dp_D_accel_bias(0, 2) + Dv_D_accel_bias(0, 2) * dt - _tmp239; + _new_Dp_D_accel_bias(1, 2) = + Dp_D_accel_bias(1, 2) + Dv_D_accel_bias(1, 2) * dt - _tmp39 * _tmp45; + _new_Dp_D_accel_bias(2, 2) = Dp_D_accel_bias(2, 2) + Dv_D_accel_bias(2, 2) * dt - _tmp253; + } + + if (new_Dp_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_Dp_D_gyro_bias = (*new_Dp_D_gyro_bias); + + _new_Dp_D_gyro_bias(0, 0) = Dp_D_gyro_bias(0, 0) + Dv_D_gyro_bias(0, 0) * dt + + _tmp170 * _tmp394 + _tmp170 * _tmp395 + _tmp170 * _tmp396; + _new_Dp_D_gyro_bias(1, 0) = Dp_D_gyro_bias(1, 0) + Dv_D_gyro_bias(1, 0) * dt + + _tmp170 * _tmp397 + _tmp170 * _tmp398 + _tmp170 * _tmp399; + _new_Dp_D_gyro_bias(2, 0) = Dp_D_gyro_bias(2, 0) + Dv_D_gyro_bias(2, 0) * dt + + _tmp170 * _tmp400 + _tmp170 * _tmp401 + _tmp170 * _tmp402; + _new_Dp_D_gyro_bias(0, 1) = Dp_D_gyro_bias(0, 1) + Dv_D_gyro_bias(0, 1) * dt + + _tmp170 * _tmp403 + _tmp170 * _tmp404 + _tmp170 * _tmp405; + _new_Dp_D_gyro_bias(1, 1) = Dp_D_gyro_bias(1, 1) + Dv_D_gyro_bias(1, 1) * dt + + _tmp170 * _tmp406 + _tmp170 * _tmp407 + _tmp170 * _tmp408; + _new_Dp_D_gyro_bias(2, 1) = Dp_D_gyro_bias(2, 1) + Dv_D_gyro_bias(2, 1) * dt + + _tmp170 * _tmp409 + _tmp170 * _tmp410 + _tmp170 * _tmp411; + _new_Dp_D_gyro_bias(0, 2) = Dp_D_gyro_bias(0, 2) + Dv_D_gyro_bias(0, 2) * dt + + _tmp170 * _tmp412 + _tmp170 * _tmp413 + _tmp170 * _tmp414; + _new_Dp_D_gyro_bias(1, 2) = Dp_D_gyro_bias(1, 2) + Dv_D_gyro_bias(1, 2) * dt + + _tmp170 * _tmp415 + _tmp170 * _tmp416 + _tmp170 * _tmp417; + _new_Dp_D_gyro_bias(2, 2) = Dp_D_gyro_bias(2, 2) + Dv_D_gyro_bias(2, 2) * dt + + _tmp170 * _tmp418 + _tmp170 * _tmp419 + _tmp170 * _tmp420; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update_auto_derivative.h b/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update_auto_derivative.h new file mode 100644 index 000000000..a03fbf3b5 --- /dev/null +++ b/gen/cpp/sym/factors/internal/imu_manifold_preintegration_update_auto_derivative.h @@ -0,0 +1,937 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to + * help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output. + * Since this function is more expensive, there is no reason to use it instead. + * + * An internal helper function to update a set of preintegrated IMU measurements with a new pair of + * gyroscope and accelerometer measurements. Returns the updated preintegrated measurements. + * + * NOTE: If you are interested in this function, you should likely be using the + * IntegrateMeasurement method of the PreintegratedImuMeasurements class located in + * symforce/slam/imu_preintegration/preintegrated_imu_measurements.h. + * + * When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the + * derivatives w.r.t. to bias should all be 0. + * + * Args: + * DR (sf.Rot3): Preintegrated change in orientation + * Dv (sf.V3): Preintegrated change in velocity + * Dp (sf.V3): Preintegrated change in position + * covariance (sf.M99): Covariance [DR, Dv, Dp] in local coordinates of mean + * DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias + * Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias + * Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias + * Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias + * Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias + * accel_bias (sf.V3): Initial guess for the accelerometer measurement bias + * gyro_bias (sf.V3): Initial guess for the gyroscope measurement bias + * accel_cov_diagonal (sf.M33): The diagonal of the covariance of the accelerometer measurement + * gyro_cov_diagonal (sf.M33): The diagonal of the covariance of the gyroscope measurement + * accel_measurement (sf.V3): The accelerometer measurement + * gyro_measurement (sf.V3): The gyroscope measurement + * dt (T.Scalar): The time between the new measurements and the last + * epsilon (T.Scalar): epsilon for numerical stability + * + * Returns: + * T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: + * new_DR, + * new_Dv, + * new_Dp, + * new_covariance, + * new_DR_D_gyro_bias, + * new_Dv_D_accel_bias, + * new_Dv_D_gyro_bias, + * new_Dp_D_accel_bias + * new_Dp_D_gyro_bias, + */ +template +void ImuManifoldPreintegrationUpdateAutoDerivative( + const sym::Rot3& DR, const Eigen::Matrix& Dv, + const Eigen::Matrix& Dp, const Eigen::Matrix& covariance, + const Eigen::Matrix& DR_D_gyro_bias, + const Eigen::Matrix& Dv_D_accel_bias, + const Eigen::Matrix& Dv_D_gyro_bias, + const Eigen::Matrix& Dp_D_accel_bias, + const Eigen::Matrix& Dp_D_gyro_bias, + const Eigen::Matrix& accel_bias, const Eigen::Matrix& gyro_bias, + const Eigen::Matrix& accel_cov_diagonal, + const Eigen::Matrix& gyro_cov_diagonal, + const Eigen::Matrix& accel_measurement, + const Eigen::Matrix& gyro_measurement, const Scalar dt, const Scalar epsilon, + sym::Rot3* const new_DR = nullptr, Eigen::Matrix* const new_Dv = nullptr, + Eigen::Matrix* const new_Dp = nullptr, + Eigen::Matrix* const new_covariance = nullptr, + Eigen::Matrix* const new_DR_D_gyro_bias = nullptr, + Eigen::Matrix* const new_Dv_D_accel_bias = nullptr, + Eigen::Matrix* const new_Dv_D_gyro_bias = nullptr, + Eigen::Matrix* const new_Dp_D_accel_bias = nullptr, + Eigen::Matrix* const new_Dp_D_gyro_bias = nullptr) { + // Total ops: 2169 + + // Input arrays + const Eigen::Matrix& _DR = DR.Data(); + + // Intermediate terms (439) + const Scalar _tmp0 = -gyro_bias(0, 0) + gyro_measurement(0, 0); + const Scalar _tmp1 = std::pow(dt, Scalar(2)); + const Scalar _tmp2 = -gyro_bias(2, 0) + gyro_measurement(2, 0); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = -gyro_bias(1, 0) + gyro_measurement(1, 0); + const Scalar _tmp5 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp6 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp7 = _tmp1 * _tmp3 + _tmp1 * _tmp5 + _tmp1 * _tmp6 + std::pow(epsilon, Scalar(2)); + const Scalar _tmp8 = std::sqrt(_tmp7); + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp10 = std::sin(_tmp9); + const Scalar _tmp11 = _DR[3] * _tmp10; + const Scalar _tmp12 = Scalar(1.0) / (_tmp8); + const Scalar _tmp13 = _tmp12 * dt; + const Scalar _tmp14 = _tmp11 * _tmp13; + const Scalar _tmp15 = _tmp0 * _tmp14; + const Scalar _tmp16 = _tmp10 * _tmp13; + const Scalar _tmp17 = _DR[1] * _tmp16; + const Scalar _tmp18 = _tmp17 * _tmp2; + const Scalar _tmp19 = _DR[2] * _tmp16; + const Scalar _tmp20 = _tmp19 * _tmp4; + const Scalar _tmp21 = std::cos(_tmp9); + const Scalar _tmp22 = _DR[0] * _tmp21; + const Scalar _tmp23 = _tmp15 + _tmp18 - _tmp20 + _tmp22; + const Scalar _tmp24 = _DR[0] * _tmp16; + const Scalar _tmp25 = _tmp2 * _tmp24; + const Scalar _tmp26 = _tmp14 * _tmp4; + const Scalar _tmp27 = _tmp0 * _tmp19; + const Scalar _tmp28 = _DR[1] * _tmp21; + const Scalar _tmp29 = -_tmp25 + _tmp26 + _tmp27 + _tmp28; + const Scalar _tmp30 = _tmp24 * _tmp4; + const Scalar _tmp31 = _tmp14 * _tmp2; + const Scalar _tmp32 = _tmp0 * _tmp17; + const Scalar _tmp33 = _DR[2] * _tmp21; + const Scalar _tmp34 = _tmp30 + _tmp31 - _tmp32 + _tmp33; + const Scalar _tmp35 = _DR[3] * _tmp21; + const Scalar _tmp36 = _tmp0 * _tmp24; + const Scalar _tmp37 = _tmp17 * _tmp4; + const Scalar _tmp38 = _tmp19 * _tmp2; + const Scalar _tmp39 = _tmp35 - _tmp36 - _tmp37 - _tmp38; + const Scalar _tmp40 = std::pow(_DR[1], Scalar(2)); + const Scalar _tmp41 = -2 * _tmp40; + const Scalar _tmp42 = std::pow(_DR[2], Scalar(2)); + const Scalar _tmp43 = -2 * _tmp42; + const Scalar _tmp44 = _tmp41 + _tmp43 + 1; + const Scalar _tmp45 = -accel_bias(0, 0) + accel_measurement(0, 0); + const Scalar _tmp46 = 2 * _DR[1]; + const Scalar _tmp47 = _DR[0] * _tmp46; + const Scalar _tmp48 = 2 * _DR[2] * _DR[3]; + const Scalar _tmp49 = -_tmp48; + const Scalar _tmp50 = _tmp47 + _tmp49; + const Scalar _tmp51 = -accel_bias(1, 0) + accel_measurement(1, 0); + const Scalar _tmp52 = _DR[3] * _tmp46; + const Scalar _tmp53 = 2 * _DR[0]; + const Scalar _tmp54 = _DR[2] * _tmp53; + const Scalar _tmp55 = _tmp52 + _tmp54; + const Scalar _tmp56 = -accel_bias(2, 0) + accel_measurement(2, 0); + const Scalar _tmp57 = _tmp44 * _tmp45 + _tmp50 * _tmp51 + _tmp55 * _tmp56; + const Scalar _tmp58 = std::pow(_DR[0], Scalar(2)); + const Scalar _tmp59 = 1 - 2 * _tmp58; + const Scalar _tmp60 = _tmp43 + _tmp59; + const Scalar _tmp61 = _tmp47 + _tmp48; + const Scalar _tmp62 = _DR[3] * _tmp53; + const Scalar _tmp63 = -_tmp62; + const Scalar _tmp64 = _DR[2] * _tmp46; + const Scalar _tmp65 = _tmp63 + _tmp64; + const Scalar _tmp66 = _tmp45 * _tmp61 + _tmp51 * _tmp60 + _tmp56 * _tmp65; + const Scalar _tmp67 = _tmp41 + _tmp59; + const Scalar _tmp68 = -_tmp52; + const Scalar _tmp69 = _tmp54 + _tmp68; + const Scalar _tmp70 = _tmp62 + _tmp64; + const Scalar _tmp71 = _tmp45 * _tmp69 + _tmp51 * _tmp70 + _tmp56 * _tmp67; + const Scalar _tmp72 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp73 = _DR[0] * _tmp10; + const Scalar _tmp74 = _tmp12 * _tmp72; + const Scalar _tmp75 = _tmp73 * _tmp74; + const Scalar _tmp76 = _tmp2 * _tmp75; + const Scalar _tmp77 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp78 = [&]() { + const Scalar base = dt; + return base * base * base; + }(); + const Scalar _tmp79 = _tmp78 / _tmp7; + const Scalar _tmp80 = _tmp3 * _tmp79; + const Scalar _tmp81 = _tmp77 * _tmp80; + const Scalar _tmp82 = _tmp78 / (_tmp7 * std::sqrt(_tmp7)); + const Scalar _tmp83 = _tmp3 * _tmp82; + const Scalar _tmp84 = _DR[1] * _tmp10; + const Scalar _tmp85 = _tmp83 * _tmp84; + const Scalar _tmp86 = _DR[2] * _tmp10; + const Scalar _tmp87 = _tmp4 * _tmp86; + const Scalar _tmp88 = _tmp2 * _tmp82; + const Scalar _tmp89 = _tmp87 * _tmp88; + const Scalar _tmp90 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp91 = _tmp79 * _tmp90; + const Scalar _tmp92 = _tmp2 * _tmp4; + const Scalar _tmp93 = _tmp91 * _tmp92; + const Scalar _tmp94 = _tmp89 - _tmp93; + const Scalar _tmp95 = (Scalar(1) / Scalar(2)) * _tmp35; + const Scalar _tmp96 = _tmp79 * _tmp95; + const Scalar _tmp97 = _tmp0 * _tmp96; + const Scalar _tmp98 = _tmp2 * _tmp97; + const Scalar _tmp99 = _tmp0 * _tmp82; + const Scalar _tmp100 = _tmp2 * _tmp99; + const Scalar _tmp101 = _tmp100 * _tmp11; + const Scalar _tmp102 = -_tmp101 + _tmp98; + const Scalar _tmp103 = _tmp102 + _tmp17 - _tmp76 + _tmp81 - _tmp85 + _tmp94; + const Scalar _tmp104 = 2 * _tmp39; + const Scalar _tmp105 = -_tmp24; + const Scalar _tmp106 = _tmp74 * _tmp84; + const Scalar _tmp107 = _tmp106 * _tmp2; + const Scalar _tmp108 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp109 = _tmp108 * _tmp80; + const Scalar _tmp110 = _tmp73 * _tmp83; + const Scalar _tmp111 = _tmp92 * _tmp96; + const Scalar _tmp112 = _tmp11 * _tmp4; + const Scalar _tmp113 = _tmp112 * _tmp88; + const Scalar _tmp114 = _tmp111 - _tmp113; + const Scalar _tmp115 = _tmp100 * _tmp86; + const Scalar _tmp116 = _tmp0 * _tmp2; + const Scalar _tmp117 = _tmp116 * _tmp91; + const Scalar _tmp118 = -_tmp115 + _tmp117; + const Scalar _tmp119 = _tmp105 - _tmp107 - _tmp109 + _tmp110 + _tmp114 + _tmp118; + const Scalar _tmp120 = 2 * _tmp34; + const Scalar _tmp121 = _tmp74 * _tmp86; + const Scalar _tmp122 = _tmp121 * _tmp2; + const Scalar _tmp123 = _tmp80 * _tmp95; + const Scalar _tmp124 = _tmp11 * _tmp83; + const Scalar _tmp125 = _tmp108 * _tmp79; + const Scalar _tmp126 = _tmp125 * _tmp92; + const Scalar _tmp127 = _tmp4 * _tmp73; + const Scalar _tmp128 = _tmp127 * _tmp88; + const Scalar _tmp129 = _tmp126 - _tmp128; + const Scalar _tmp130 = _tmp100 * _tmp84; + const Scalar _tmp131 = _tmp77 * _tmp79; + const Scalar _tmp132 = _tmp116 * _tmp131; + const Scalar _tmp133 = _tmp130 - _tmp132; + const Scalar _tmp134 = -_tmp122 + _tmp123 - _tmp124 + _tmp129 + _tmp133 + _tmp14; + const Scalar _tmp135 = 2 * _tmp29; + const Scalar _tmp136 = -_tmp19; + const Scalar _tmp137 = _tmp11 * _tmp74; + const Scalar _tmp138 = _tmp137 * _tmp2; + const Scalar _tmp139 = _tmp80 * _tmp90; + const Scalar _tmp140 = _tmp83 * _tmp86; + const Scalar _tmp141 = _tmp116 * _tmp125; + const Scalar _tmp142 = _tmp100 * _tmp73; + const Scalar _tmp143 = -_tmp141 + _tmp142; + const Scalar _tmp144 = _tmp4 * _tmp84; + const Scalar _tmp145 = _tmp144 * _tmp88; + const Scalar _tmp146 = _tmp131 * _tmp92; + const Scalar _tmp147 = _tmp145 - _tmp146; + const Scalar _tmp148 = _tmp136 - _tmp138 - _tmp139 + _tmp140 + _tmp143 + _tmp147; + const Scalar _tmp149 = 2 * _tmp23; + const Scalar _tmp150 = + _tmp103 * _tmp104 + _tmp119 * _tmp120 - _tmp134 * _tmp135 - _tmp148 * _tmp149; + const Scalar _tmp151 = Scalar(1.0) / (dt); + const Scalar _tmp152 = _tmp151 * gyro_cov_diagonal(2, 0); + const Scalar _tmp153 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp154 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp155 = -_tmp154; + const Scalar _tmp156 = -Scalar(1) / Scalar(2) * _tmp25; + const Scalar _tmp157 = _tmp156 - _tmp77; + const Scalar _tmp158 = _tmp153 + _tmp155 + _tmp157; + const Scalar _tmp159 = (Scalar(1) / Scalar(2)) * _tmp36; + const Scalar _tmp160 = (Scalar(1) / Scalar(2)) * _tmp37; + const Scalar _tmp161 = (Scalar(1) / Scalar(2)) * _tmp38; + const Scalar _tmp162 = _tmp161 + _tmp95; + const Scalar _tmp163 = -_tmp159 + _tmp160 + _tmp162; + const Scalar _tmp164 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp165 = -_tmp164; + const Scalar _tmp166 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp167 = -Scalar(1) / Scalar(2) * _tmp20; + const Scalar _tmp168 = -_tmp108; + const Scalar _tmp169 = _tmp165 + _tmp166 + _tmp167 + _tmp168; + const Scalar _tmp170 = (Scalar(1) / Scalar(2)) * _tmp30; + const Scalar _tmp171 = -_tmp170; + const Scalar _tmp172 = (Scalar(1) / Scalar(2)) * _tmp31; + const Scalar _tmp173 = -Scalar(1) / Scalar(2) * _tmp32; + const Scalar _tmp174 = -_tmp172 + _tmp173; + const Scalar _tmp175 = _tmp171 + _tmp174 + _tmp90; + const Scalar _tmp176 = + _tmp104 * _tmp163 + _tmp120 * _tmp175 - _tmp135 * _tmp158 - _tmp149 * _tmp169; + const Scalar _tmp177 = -_tmp90; + const Scalar _tmp178 = _tmp171 + _tmp172 + _tmp173 + _tmp177; + const Scalar _tmp179 = -_tmp166 + _tmp167; + const Scalar _tmp180 = _tmp108 + _tmp165 + _tmp179; + const Scalar _tmp181 = -_tmp153; + const Scalar _tmp182 = _tmp154 + _tmp157 + _tmp181; + const Scalar _tmp183 = _tmp159 - _tmp160 + _tmp162; + const Scalar _tmp184 = + _tmp104 * _tmp178 + _tmp120 * _tmp183 - _tmp135 * _tmp180 - _tmp149 * _tmp182; + const Scalar _tmp185 = _tmp170 + _tmp174 + _tmp177; + const Scalar _tmp186 = _tmp159 + _tmp160 - _tmp161 + _tmp95; + const Scalar _tmp187 = _tmp155 + _tmp156 + _tmp181 + _tmp77; + const Scalar _tmp188 = _tmp164 + _tmp168 + _tmp179; + const Scalar _tmp189 = + _tmp104 * _tmp187 + _tmp120 * _tmp188 - _tmp135 * _tmp186 - _tmp149 * _tmp185; + const Scalar _tmp190 = + _tmp176 * covariance(0, 0) + _tmp184 * covariance(1, 0) + _tmp189 * covariance(2, 0); + const Scalar _tmp191 = _tmp0 * _tmp137; + const Scalar _tmp192 = _tmp125 * _tmp6; + const Scalar _tmp193 = _tmp6 * _tmp82; + const Scalar _tmp194 = _tmp193 * _tmp73; + const Scalar _tmp195 = _tmp144 * _tmp99; + const Scalar _tmp196 = _tmp0 * _tmp4; + const Scalar _tmp197 = _tmp131 * _tmp196; + const Scalar _tmp198 = _tmp195 - _tmp197; + const Scalar _tmp199 = _tmp115 - _tmp117; + const Scalar _tmp200 = _tmp105 - _tmp191 - _tmp192 + _tmp194 + _tmp198 + _tmp199; + const Scalar _tmp201 = -_tmp17; + const Scalar _tmp202 = _tmp0 * _tmp121; + const Scalar _tmp203 = _tmp131 * _tmp6; + const Scalar _tmp204 = _tmp193 * _tmp84; + const Scalar _tmp205 = _tmp125 * _tmp196; + const Scalar _tmp206 = _tmp127 * _tmp99; + const Scalar _tmp207 = _tmp205 - _tmp206; + const Scalar _tmp208 = _tmp102 + _tmp201 - _tmp202 - _tmp203 + _tmp204 + _tmp207; + const Scalar _tmp209 = _tmp0 * _tmp106; + const Scalar _tmp210 = _tmp6 * _tmp91; + const Scalar _tmp211 = _tmp193 * _tmp86; + const Scalar _tmp212 = _tmp4 * _tmp97; + const Scalar _tmp213 = _tmp112 * _tmp99; + const Scalar _tmp214 = _tmp212 - _tmp213; + const Scalar _tmp215 = _tmp143 + _tmp19 - _tmp209 + _tmp210 - _tmp211 + _tmp214; + const Scalar _tmp216 = _tmp0 * _tmp75; + const Scalar _tmp217 = _tmp6 * _tmp96; + const Scalar _tmp218 = _tmp11 * _tmp193; + const Scalar _tmp219 = _tmp87 * _tmp99; + const Scalar _tmp220 = _tmp196 * _tmp91; + const Scalar _tmp221 = _tmp219 - _tmp220; + const Scalar _tmp222 = -_tmp130 + _tmp132; + const Scalar _tmp223 = _tmp14 - _tmp216 + _tmp217 - _tmp218 + _tmp221 + _tmp222; + const Scalar _tmp224 = + _tmp104 * _tmp223 + _tmp120 * _tmp215 - _tmp135 * _tmp208 - _tmp149 * _tmp200; + const Scalar _tmp225 = _tmp151 * gyro_cov_diagonal(0, 0); + const Scalar _tmp226 = + _tmp176 * covariance(0, 1) + _tmp184 * covariance(1, 1) + _tmp189 * covariance(2, 1); + const Scalar _tmp227 = _tmp144 * _tmp74; + const Scalar _tmp228 = _tmp5 * _tmp96; + const Scalar _tmp229 = _tmp5 * _tmp82; + const Scalar _tmp230 = _tmp11 * _tmp229; + const Scalar _tmp231 = -_tmp126 + _tmp128; + const Scalar _tmp232 = -_tmp219 + _tmp220; + const Scalar _tmp233 = _tmp14 - _tmp227 + _tmp228 - _tmp230 + _tmp231 + _tmp232; + const Scalar _tmp234 = _tmp74 * _tmp87; + const Scalar _tmp235 = _tmp125 * _tmp5; + const Scalar _tmp236 = _tmp229 * _tmp73; + const Scalar _tmp237 = _tmp114 + _tmp198 - _tmp234 + _tmp235 - _tmp236 + _tmp24; + const Scalar _tmp238 = _tmp112 * _tmp74; + const Scalar _tmp239 = _tmp131 * _tmp5; + const Scalar _tmp240 = _tmp229 * _tmp84; + const Scalar _tmp241 = -_tmp205 + _tmp206; + const Scalar _tmp242 = _tmp201 - _tmp238 - _tmp239 + _tmp240 + _tmp241 + _tmp94; + const Scalar _tmp243 = _tmp127 * _tmp74; + const Scalar _tmp244 = _tmp5 * _tmp91; + const Scalar _tmp245 = _tmp229 * _tmp86; + const Scalar _tmp246 = -_tmp145 + _tmp146; + const Scalar _tmp247 = _tmp136 + _tmp214 - _tmp243 - _tmp244 + _tmp245 + _tmp246; + const Scalar _tmp248 = + _tmp104 * _tmp247 + _tmp120 * _tmp233 - _tmp135 * _tmp237 - _tmp149 * _tmp242; + const Scalar _tmp249 = _tmp151 * gyro_cov_diagonal(1, 0); + const Scalar _tmp250 = + _tmp176 * covariance(0, 2) + _tmp184 * covariance(1, 2) + _tmp189 * covariance(2, 2); + const Scalar _tmp251 = + _tmp104 * _tmp175 - _tmp120 * _tmp163 - _tmp135 * _tmp169 + _tmp149 * _tmp158; + const Scalar _tmp252 = + _tmp104 * _tmp188 - _tmp120 * _tmp187 - _tmp135 * _tmp185 + _tmp149 * _tmp186; + const Scalar _tmp253 = + _tmp104 * _tmp183 - _tmp120 * _tmp178 - _tmp135 * _tmp182 + _tmp149 * _tmp180; + const Scalar _tmp254 = + _tmp251 * covariance(0, 1) + _tmp252 * covariance(2, 1) + _tmp253 * covariance(1, 1); + const Scalar _tmp255 = + _tmp251 * covariance(0, 2) + _tmp252 * covariance(2, 2) + _tmp253 * covariance(1, 2); + const Scalar _tmp256 = + _tmp251 * covariance(0, 0) + _tmp252 * covariance(2, 0) + _tmp253 * covariance(1, 0); + const Scalar _tmp257 = + _tmp104 * _tmp215 - _tmp120 * _tmp223 - _tmp135 * _tmp200 + _tmp149 * _tmp208; + const Scalar _tmp258 = _tmp224 * _tmp225; + const Scalar _tmp259 = + _tmp104 * _tmp233 - _tmp120 * _tmp247 - _tmp135 * _tmp242 + _tmp149 * _tmp237; + const Scalar _tmp260 = + -_tmp103 * _tmp120 + _tmp104 * _tmp119 + _tmp134 * _tmp149 - _tmp135 * _tmp148; + const Scalar _tmp261 = _tmp150 * _tmp152; + const Scalar _tmp262 = _tmp248 * _tmp249 * _tmp259 + _tmp257 * _tmp258 + _tmp260 * _tmp261; + const Scalar _tmp263 = + _tmp104 * _tmp180 - _tmp120 * _tmp182 + _tmp135 * _tmp178 - _tmp149 * _tmp183; + const Scalar _tmp264 = + _tmp104 * _tmp186 - _tmp120 * _tmp185 + _tmp135 * _tmp187 - _tmp149 * _tmp188; + const Scalar _tmp265 = + _tmp104 * _tmp158 - _tmp120 * _tmp169 + _tmp135 * _tmp163 - _tmp149 * _tmp175; + const Scalar _tmp266 = + _tmp263 * covariance(1, 0) + _tmp264 * covariance(2, 0) + _tmp265 * covariance(0, 0); + const Scalar _tmp267 = + _tmp263 * covariance(1, 2) + _tmp264 * covariance(2, 2) + _tmp265 * covariance(0, 2); + const Scalar _tmp268 = + _tmp263 * covariance(1, 1) + _tmp264 * covariance(2, 1) + _tmp265 * covariance(0, 1); + const Scalar _tmp269 = + _tmp103 * _tmp135 + _tmp104 * _tmp134 - _tmp119 * _tmp149 - _tmp120 * _tmp148; + const Scalar _tmp270 = + _tmp104 * _tmp208 - _tmp120 * _tmp200 + _tmp135 * _tmp223 - _tmp149 * _tmp215; + const Scalar _tmp271 = + _tmp104 * _tmp237 - _tmp120 * _tmp242 + _tmp135 * _tmp247 - _tmp149 * _tmp233; + const Scalar _tmp272 = _tmp249 * _tmp271; + const Scalar _tmp273 = _tmp248 * _tmp272 + _tmp258 * _tmp270 + _tmp261 * _tmp269; + const Scalar _tmp274 = -_tmp58; + const Scalar _tmp275 = _tmp274 + _tmp40; + const Scalar _tmp276 = std::pow(_DR[3], Scalar(2)); + const Scalar _tmp277 = -_tmp276; + const Scalar _tmp278 = _tmp277 + _tmp42; + const Scalar _tmp279 = _tmp45 * _tmp50 + _tmp51 * (_tmp275 + _tmp278); + const Scalar _tmp280 = _tmp279 * dt; + const Scalar _tmp281 = -_tmp42; + const Scalar _tmp282 = _tmp276 + _tmp281; + const Scalar _tmp283 = -_tmp40; + const Scalar _tmp284 = _tmp283 + _tmp58; + const Scalar _tmp285 = -_tmp54; + const Scalar _tmp286 = _tmp45 * (_tmp285 + _tmp68) + _tmp56 * (_tmp282 + _tmp284); + const Scalar _tmp287 = _tmp286 * dt; + const Scalar _tmp288 = -_tmp47; + const Scalar _tmp289 = _tmp51 * _tmp55 + _tmp56 * (_tmp288 + _tmp48); + const Scalar _tmp290 = _tmp289 * dt; + const Scalar _tmp291 = _tmp280 * covariance(2, 2) + _tmp287 * covariance(1, 2) + + _tmp290 * covariance(0, 2) + covariance(3, 2); + const Scalar _tmp292 = _tmp280 * covariance(2, 1) + _tmp287 * covariance(1, 1) + + _tmp290 * covariance(0, 1) + covariance(3, 1); + const Scalar _tmp293 = _tmp280 * covariance(2, 0) + _tmp287 * covariance(1, 0) + + _tmp290 * covariance(0, 0) + covariance(3, 0); + const Scalar _tmp294 = -_tmp64; + const Scalar _tmp295 = _tmp45 * (_tmp294 + _tmp62) + _tmp56 * _tmp61; + const Scalar _tmp296 = _tmp295 * dt; + const Scalar _tmp297 = _tmp51 * _tmp65 + _tmp56 * (_tmp278 + _tmp284); + const Scalar _tmp298 = _tmp297 * dt; + const Scalar _tmp299 = _tmp45 * (_tmp275 + _tmp282) + _tmp51 * (_tmp288 + _tmp49); + const Scalar _tmp300 = _tmp299 * dt; + const Scalar _tmp301 = _tmp296 * covariance(1, 2) + _tmp298 * covariance(0, 2) + + _tmp300 * covariance(2, 2) + covariance(4, 2); + const Scalar _tmp302 = _tmp296 * covariance(1, 0) + _tmp298 * covariance(0, 0) + + _tmp300 * covariance(2, 0) + covariance(4, 0); + const Scalar _tmp303 = _tmp296 * covariance(1, 1) + _tmp298 * covariance(0, 1) + + _tmp300 * covariance(2, 1) + covariance(4, 1); + const Scalar _tmp304 = _tmp45 * _tmp70 + _tmp51 * (_tmp285 + _tmp52); + const Scalar _tmp305 = _tmp304 * dt; + const Scalar _tmp306 = + _tmp51 * (_tmp274 + _tmp276 + _tmp283 + _tmp42) + _tmp56 * (_tmp294 + _tmp63); + const Scalar _tmp307 = _tmp306 * dt; + const Scalar _tmp308 = _tmp45 * (_tmp277 + _tmp281 + _tmp40 + _tmp58) + _tmp56 * _tmp69; + const Scalar _tmp309 = _tmp308 * dt; + const Scalar _tmp310 = _tmp305 * covariance(2, 1) + _tmp307 * covariance(0, 1) + + _tmp309 * covariance(1, 1) + covariance(5, 1); + const Scalar _tmp311 = _tmp305 * covariance(2, 2) + _tmp307 * covariance(0, 2) + + _tmp309 * covariance(1, 2) + covariance(5, 2); + const Scalar _tmp312 = _tmp305 * covariance(2, 0) + _tmp307 * covariance(0, 0) + + _tmp309 * covariance(1, 0) + covariance(5, 0); + const Scalar _tmp313 = _tmp279 * _tmp72; + const Scalar _tmp314 = _tmp286 * _tmp72; + const Scalar _tmp315 = _tmp289 * _tmp72; + const Scalar _tmp316 = _tmp313 * covariance(2, 0) + _tmp314 * covariance(1, 0) + + _tmp315 * covariance(0, 0) + covariance(3, 0) * dt + covariance(6, 0); + const Scalar _tmp317 = _tmp313 * covariance(2, 2) + _tmp314 * covariance(1, 2) + + _tmp315 * covariance(0, 2) + covariance(3, 2) * dt + covariance(6, 2); + const Scalar _tmp318 = _tmp313 * covariance(2, 1) + _tmp314 * covariance(1, 1) + + _tmp315 * covariance(0, 1) + covariance(3, 1) * dt + covariance(6, 1); + const Scalar _tmp319 = _tmp295 * _tmp72; + const Scalar _tmp320 = _tmp297 * _tmp72; + const Scalar _tmp321 = _tmp299 * _tmp72; + const Scalar _tmp322 = _tmp319 * covariance(1, 0) + _tmp320 * covariance(0, 0) + + _tmp321 * covariance(2, 0) + covariance(4, 0) * dt + covariance(7, 0); + const Scalar _tmp323 = _tmp319 * covariance(1, 2) + _tmp320 * covariance(0, 2) + + _tmp321 * covariance(2, 2) + covariance(4, 2) * dt + covariance(7, 2); + const Scalar _tmp324 = _tmp319 * covariance(1, 1) + _tmp320 * covariance(0, 1) + + _tmp321 * covariance(2, 1) + covariance(4, 1) * dt + covariance(7, 1); + const Scalar _tmp325 = _tmp304 * _tmp72; + const Scalar _tmp326 = _tmp306 * _tmp72; + const Scalar _tmp327 = _tmp308 * _tmp72; + const Scalar _tmp328 = _tmp325 * covariance(2, 2) + _tmp326 * covariance(0, 2) + + _tmp327 * covariance(1, 2) + covariance(5, 2) * dt + covariance(8, 2); + const Scalar _tmp329 = _tmp325 * covariance(2, 0) + _tmp326 * covariance(0, 0) + + _tmp327 * covariance(1, 0) + covariance(5, 0) * dt + covariance(8, 0); + const Scalar _tmp330 = _tmp325 * covariance(2, 1) + _tmp326 * covariance(0, 1) + + _tmp327 * covariance(1, 1) + covariance(5, 1) * dt + covariance(8, 1); + const Scalar _tmp331 = + _tmp152 * _tmp260 * _tmp269 + _tmp225 * _tmp257 * _tmp270 + _tmp259 * _tmp272; + const Scalar _tmp332 = + _tmp176 * covariance(0, 3) + _tmp184 * covariance(1, 3) + _tmp189 * covariance(2, 3); + const Scalar _tmp333 = + _tmp251 * covariance(0, 3) + _tmp252 * covariance(2, 3) + _tmp253 * covariance(1, 3); + const Scalar _tmp334 = + _tmp263 * covariance(1, 3) + _tmp264 * covariance(2, 3) + _tmp265 * covariance(0, 3); + const Scalar _tmp335 = std::pow(_tmp55, Scalar(2)); + const Scalar _tmp336 = accel_cov_diagonal(2, 0) * dt; + const Scalar _tmp337 = std::pow(_tmp50, Scalar(2)); + const Scalar _tmp338 = accel_cov_diagonal(1, 0) * dt; + const Scalar _tmp339 = std::pow(_tmp44, Scalar(2)); + const Scalar _tmp340 = accel_cov_diagonal(0, 0) * dt; + const Scalar _tmp341 = _tmp280 * covariance(2, 3) + _tmp287 * covariance(1, 3) + + _tmp290 * covariance(0, 3) + covariance(3, 3); + const Scalar _tmp342 = _tmp296 * covariance(1, 3) + _tmp298 * covariance(0, 3) + + _tmp300 * covariance(2, 3) + covariance(4, 3); + const Scalar _tmp343 = _tmp65 * dt; + const Scalar _tmp344 = _tmp55 * accel_cov_diagonal(2, 0); + const Scalar _tmp345 = _tmp50 * dt; + const Scalar _tmp346 = _tmp60 * accel_cov_diagonal(1, 0); + const Scalar _tmp347 = _tmp61 * dt; + const Scalar _tmp348 = _tmp44 * accel_cov_diagonal(0, 0); + const Scalar _tmp349 = _tmp343 * _tmp344 + _tmp345 * _tmp346 + _tmp347 * _tmp348; + const Scalar _tmp350 = _tmp305 * covariance(2, 3) + _tmp307 * covariance(0, 3) + + _tmp309 * covariance(1, 3) + covariance(5, 3); + const Scalar _tmp351 = _tmp70 * dt; + const Scalar _tmp352 = _tmp351 * accel_cov_diagonal(1, 0); + const Scalar _tmp353 = _tmp69 * dt; + const Scalar _tmp354 = _tmp67 * dt; + const Scalar _tmp355 = _tmp344 * _tmp354 + _tmp348 * _tmp353 + _tmp352 * _tmp50; + const Scalar _tmp356 = _tmp313 * covariance(2, 3) + _tmp314 * covariance(1, 3) + + _tmp315 * covariance(0, 3) + covariance(3, 3) * dt + covariance(6, 3); + const Scalar _tmp357 = _tmp335 * accel_cov_diagonal(2, 0); + const Scalar _tmp358 = _tmp337 * accel_cov_diagonal(1, 0); + const Scalar _tmp359 = _tmp339 * accel_cov_diagonal(0, 0); + const Scalar _tmp360 = _tmp357 * _tmp72 + _tmp358 * _tmp72 + _tmp359 * _tmp72; + const Scalar _tmp361 = _tmp319 * covariance(1, 3) + _tmp320 * covariance(0, 3) + + _tmp321 * covariance(2, 3) + covariance(4, 3) * dt + covariance(7, 3); + const Scalar _tmp362 = _tmp65 * _tmp72; + const Scalar _tmp363 = _tmp60 * _tmp72; + const Scalar _tmp364 = _tmp50 * accel_cov_diagonal(1, 0); + const Scalar _tmp365 = _tmp44 * _tmp72; + const Scalar _tmp366 = _tmp61 * accel_cov_diagonal(0, 0); + const Scalar _tmp367 = _tmp344 * _tmp362 + _tmp363 * _tmp364 + _tmp365 * _tmp366; + const Scalar _tmp368 = _tmp325 * covariance(2, 3) + _tmp326 * covariance(0, 3) + + _tmp327 * covariance(1, 3) + covariance(5, 3) * dt + covariance(8, 3); + const Scalar _tmp369 = _tmp70 * _tmp72; + const Scalar _tmp370 = _tmp69 * _tmp72; + const Scalar _tmp371 = _tmp67 * _tmp72; + const Scalar _tmp372 = _tmp344 * _tmp371 + _tmp348 * _tmp370 + _tmp364 * _tmp369; + const Scalar _tmp373 = + _tmp176 * covariance(0, 4) + _tmp184 * covariance(1, 4) + _tmp189 * covariance(2, 4); + const Scalar _tmp374 = _tmp256 * dt; + const Scalar _tmp375 = + _tmp251 * covariance(0, 4) + _tmp252 * covariance(2, 4) + _tmp253 * covariance(1, 4); + const Scalar _tmp376 = + _tmp263 * covariance(1, 4) + _tmp264 * covariance(2, 4) + _tmp265 * covariance(0, 4); + const Scalar _tmp377 = _tmp293 * dt; + const Scalar _tmp378 = _tmp280 * covariance(2, 4) + _tmp287 * covariance(1, 4) + + _tmp290 * covariance(0, 4) + covariance(3, 4); + const Scalar _tmp379 = std::pow(_tmp61, Scalar(2)); + const Scalar _tmp380 = std::pow(_tmp60, Scalar(2)); + const Scalar _tmp381 = std::pow(_tmp65, Scalar(2)); + const Scalar _tmp382 = _tmp296 * covariance(1, 4) + _tmp298 * covariance(0, 4) + + _tmp300 * covariance(2, 4) + covariance(4, 4); + const Scalar _tmp383 = _tmp312 * dt; + const Scalar _tmp384 = _tmp305 * covariance(2, 4) + _tmp307 * covariance(0, 4) + + _tmp309 * covariance(1, 4) + covariance(5, 4); + const Scalar _tmp385 = _tmp65 * accel_cov_diagonal(2, 0); + const Scalar _tmp386 = _tmp352 * _tmp60 + _tmp353 * _tmp366 + _tmp354 * _tmp385; + const Scalar _tmp387 = _tmp313 * covariance(2, 4) + _tmp314 * covariance(1, 4) + + _tmp315 * covariance(0, 4) + covariance(3, 4) * dt + covariance(6, 4); + const Scalar _tmp388 = _tmp322 * dt; + const Scalar _tmp389 = _tmp319 * covariance(1, 4) + _tmp320 * covariance(0, 4) + + _tmp321 * covariance(2, 4) + covariance(4, 4) * dt + covariance(7, 4); + const Scalar _tmp390 = _tmp379 * accel_cov_diagonal(0, 0); + const Scalar _tmp391 = _tmp380 * accel_cov_diagonal(1, 0); + const Scalar _tmp392 = _tmp381 * accel_cov_diagonal(2, 0); + const Scalar _tmp393 = _tmp390 * _tmp72 + _tmp391 * _tmp72 + _tmp392 * _tmp72; + const Scalar _tmp394 = _tmp325 * covariance(2, 4) + _tmp326 * covariance(0, 4) + + _tmp327 * covariance(1, 4) + covariance(5, 4) * dt + covariance(8, 4); + const Scalar _tmp395 = + _tmp346 * _tmp369 + _tmp362 * _tmp67 * accel_cov_diagonal(2, 0) + _tmp366 * _tmp370; + const Scalar _tmp396 = + _tmp176 * covariance(0, 5) + _tmp184 * covariance(1, 5) + _tmp189 * covariance(2, 5); + const Scalar _tmp397 = + _tmp251 * covariance(0, 5) + _tmp252 * covariance(2, 5) + _tmp253 * covariance(1, 5); + const Scalar _tmp398 = + _tmp263 * covariance(1, 5) + _tmp264 * covariance(2, 5) + _tmp265 * covariance(0, 5); + const Scalar _tmp399 = _tmp280 * covariance(2, 5) + _tmp287 * covariance(1, 5) + + _tmp290 * covariance(0, 5) + covariance(3, 5); + const Scalar _tmp400 = _tmp296 * covariance(1, 5) + _tmp298 * covariance(0, 5) + + _tmp300 * covariance(2, 5) + covariance(4, 5); + const Scalar _tmp401 = std::pow(_tmp69, Scalar(2)); + const Scalar _tmp402 = std::pow(_tmp67, Scalar(2)); + const Scalar _tmp403 = std::pow(_tmp70, Scalar(2)); + const Scalar _tmp404 = _tmp305 * covariance(2, 5) + _tmp307 * covariance(0, 5) + + _tmp309 * covariance(1, 5) + covariance(5, 5); + const Scalar _tmp405 = _tmp313 * covariance(2, 5) + _tmp314 * covariance(1, 5) + + _tmp315 * covariance(0, 5) + covariance(3, 5) * dt + covariance(6, 5); + const Scalar _tmp406 = _tmp319 * covariance(1, 5) + _tmp320 * covariance(0, 5) + + _tmp321 * covariance(2, 5) + covariance(4, 5) * dt + covariance(7, 5); + const Scalar _tmp407 = _tmp325 * covariance(2, 5) + _tmp326 * covariance(0, 5) + + _tmp327 * covariance(1, 5) + covariance(5, 5) * dt + covariance(8, 5); + const Scalar _tmp408 = _tmp401 * accel_cov_diagonal(0, 0); + const Scalar _tmp409 = _tmp402 * accel_cov_diagonal(2, 0); + const Scalar _tmp410 = _tmp403 * accel_cov_diagonal(1, 0); + const Scalar _tmp411 = _tmp408 * _tmp72 + _tmp409 * _tmp72 + _tmp410 * _tmp72; + const Scalar _tmp412 = _tmp312 * _tmp72; + const Scalar _tmp413 = (Scalar(1) / Scalar(4)) * _tmp78; + const Scalar _tmp414 = _tmp344 * _tmp413; + const Scalar _tmp415 = _tmp346 * _tmp413 * _tmp50 + _tmp348 * _tmp413 * _tmp61 + _tmp414 * _tmp65; + const Scalar _tmp416 = _tmp413 * _tmp70; + const Scalar _tmp417 = _tmp413 * _tmp69; + const Scalar _tmp418 = _tmp348 * _tmp417 + _tmp364 * _tmp416 + _tmp414 * _tmp67; + const Scalar _tmp419 = _tmp346 * _tmp416 + _tmp366 * _tmp417 + _tmp385 * _tmp413 * _tmp67; + const Scalar _tmp420 = -_tmp195 + _tmp197; + const Scalar _tmp421 = _tmp118 + _tmp191 + _tmp192 - _tmp194 + _tmp24 + _tmp420; + const Scalar _tmp422 = _tmp101 - _tmp98; + const Scalar _tmp423 = _tmp17 + _tmp202 + _tmp203 - _tmp204 + _tmp241 + _tmp422; + const Scalar _tmp424 = _tmp141 - _tmp142; + const Scalar _tmp425 = -_tmp212 + _tmp213; + const Scalar _tmp426 = _tmp136 + _tmp209 - _tmp210 + _tmp211 + _tmp424 + _tmp425; + const Scalar _tmp427 = -_tmp14; + const Scalar _tmp428 = _tmp133 + _tmp216 - _tmp217 + _tmp218 + _tmp232 + _tmp427; + const Scalar _tmp429 = _tmp129 + _tmp221 + _tmp227 - _tmp228 + _tmp230 + _tmp427; + const Scalar _tmp430 = -_tmp111 + _tmp113; + const Scalar _tmp431 = _tmp105 + _tmp234 - _tmp235 + _tmp236 + _tmp420 + _tmp430; + const Scalar _tmp432 = -_tmp89 + _tmp93; + const Scalar _tmp433 = _tmp17 + _tmp207 + _tmp238 + _tmp239 - _tmp240 + _tmp432; + const Scalar _tmp434 = _tmp147 + _tmp19 + _tmp243 + _tmp244 - _tmp245 + _tmp425; + const Scalar _tmp435 = _tmp201 + _tmp422 + _tmp432 + _tmp76 - _tmp81 + _tmp85; + const Scalar _tmp436 = _tmp107 + _tmp109 - _tmp110 + _tmp199 + _tmp24 + _tmp430; + const Scalar _tmp437 = _tmp122 - _tmp123 + _tmp124 + _tmp222 + _tmp231 + _tmp427; + const Scalar _tmp438 = _tmp138 + _tmp139 - _tmp140 + _tmp19 + _tmp246 + _tmp424; + + // Output terms (9) + if (new_DR != nullptr) { + Eigen::Matrix _new_DR; + + _new_DR[0] = _tmp23; + _new_DR[1] = _tmp29; + _new_DR[2] = _tmp34; + _new_DR[3] = _tmp39; + + *new_DR = sym::Rot3(_new_DR); + } + + if (new_Dv != nullptr) { + Eigen::Matrix& _new_Dv = (*new_Dv); + + _new_Dv(0, 0) = Dv(0, 0) + _tmp57 * dt; + _new_Dv(1, 0) = Dv(1, 0) + _tmp66 * dt; + _new_Dv(2, 0) = Dv(2, 0) + _tmp71 * dt; + } + + if (new_Dp != nullptr) { + Eigen::Matrix& _new_Dp = (*new_Dp); + + _new_Dp(0, 0) = Dp(0, 0) + Dv(0, 0) * dt + _tmp57 * _tmp72; + _new_Dp(1, 0) = Dp(1, 0) + Dv(1, 0) * dt + _tmp66 * _tmp72; + _new_Dp(2, 0) = Dp(2, 0) + Dv(2, 0) * dt + _tmp71 * _tmp72; + } + + if (new_covariance != nullptr) { + Eigen::Matrix& _new_covariance = (*new_covariance); + + _new_covariance(0, 0) = std::pow(_tmp150, Scalar(2)) * _tmp152 + _tmp176 * _tmp190 + + _tmp184 * _tmp226 + _tmp189 * _tmp250 + + std::pow(_tmp224, Scalar(2)) * _tmp225 + + std::pow(_tmp248, Scalar(2)) * _tmp249; + _new_covariance(1, 0) = _tmp176 * _tmp256 + _tmp184 * _tmp254 + _tmp189 * _tmp255 + _tmp262; + _new_covariance(2, 0) = _tmp176 * _tmp266 + _tmp184 * _tmp268 + _tmp189 * _tmp267 + _tmp273; + _new_covariance(3, 0) = _tmp176 * _tmp293 + _tmp184 * _tmp292 + _tmp189 * _tmp291; + _new_covariance(4, 0) = _tmp176 * _tmp302 + _tmp184 * _tmp303 + _tmp189 * _tmp301; + _new_covariance(5, 0) = _tmp176 * _tmp312 + _tmp184 * _tmp310 + _tmp189 * _tmp311; + _new_covariance(6, 0) = _tmp176 * _tmp316 + _tmp184 * _tmp318 + _tmp189 * _tmp317; + _new_covariance(7, 0) = _tmp176 * _tmp322 + _tmp184 * _tmp324 + _tmp189 * _tmp323; + _new_covariance(8, 0) = _tmp176 * _tmp329 + _tmp184 * _tmp330 + _tmp189 * _tmp328; + _new_covariance(0, 1) = _tmp190 * _tmp251 + _tmp226 * _tmp253 + _tmp250 * _tmp252 + _tmp262; + _new_covariance(1, 1) = _tmp152 * std::pow(_tmp260, Scalar(2)) + + _tmp225 * std::pow(_tmp257, Scalar(2)) + + _tmp249 * std::pow(_tmp259, Scalar(2)) + _tmp251 * _tmp256 + + _tmp252 * _tmp255 + _tmp253 * _tmp254; + _new_covariance(2, 1) = _tmp251 * _tmp266 + _tmp252 * _tmp267 + _tmp253 * _tmp268 + _tmp331; + _new_covariance(3, 1) = _tmp251 * _tmp293 + _tmp252 * _tmp291 + _tmp253 * _tmp292; + _new_covariance(4, 1) = _tmp251 * _tmp302 + _tmp252 * _tmp301 + _tmp253 * _tmp303; + _new_covariance(5, 1) = _tmp251 * _tmp312 + _tmp252 * _tmp311 + _tmp253 * _tmp310; + _new_covariance(6, 1) = _tmp251 * _tmp316 + _tmp252 * _tmp317 + _tmp253 * _tmp318; + _new_covariance(7, 1) = _tmp251 * _tmp322 + _tmp252 * _tmp323 + _tmp253 * _tmp324; + _new_covariance(8, 1) = _tmp251 * _tmp329 + _tmp252 * _tmp328 + _tmp253 * _tmp330; + _new_covariance(0, 2) = _tmp190 * _tmp265 + _tmp226 * _tmp263 + _tmp250 * _tmp264 + _tmp273; + _new_covariance(1, 2) = _tmp254 * _tmp263 + _tmp255 * _tmp264 + _tmp256 * _tmp265 + _tmp331; + _new_covariance(2, 2) = _tmp152 * std::pow(_tmp269, Scalar(2)) + + _tmp225 * std::pow(_tmp270, Scalar(2)) + + _tmp249 * std::pow(_tmp271, Scalar(2)) + _tmp263 * _tmp268 + + _tmp264 * _tmp267 + _tmp265 * _tmp266; + _new_covariance(3, 2) = _tmp263 * _tmp292 + _tmp264 * _tmp291 + _tmp265 * _tmp293; + _new_covariance(4, 2) = _tmp263 * _tmp303 + _tmp264 * _tmp301 + _tmp265 * _tmp302; + _new_covariance(5, 2) = _tmp263 * _tmp310 + _tmp264 * _tmp311 + _tmp265 * _tmp312; + _new_covariance(6, 2) = _tmp263 * _tmp318 + _tmp264 * _tmp317 + _tmp265 * _tmp316; + _new_covariance(7, 2) = _tmp263 * _tmp324 + _tmp264 * _tmp323 + _tmp265 * _tmp322; + _new_covariance(8, 2) = _tmp263 * _tmp330 + _tmp264 * _tmp328 + _tmp265 * _tmp329; + _new_covariance(0, 3) = _tmp190 * _tmp290 + _tmp226 * _tmp287 + _tmp250 * _tmp280 + _tmp332; + _new_covariance(1, 3) = _tmp254 * _tmp287 + _tmp255 * _tmp280 + _tmp256 * _tmp290 + _tmp333; + _new_covariance(2, 3) = _tmp266 * _tmp290 + _tmp267 * _tmp280 + _tmp268 * _tmp287 + _tmp334; + _new_covariance(3, 3) = _tmp280 * _tmp291 + _tmp287 * _tmp292 + _tmp290 * _tmp293 + + _tmp335 * _tmp336 + _tmp337 * _tmp338 + _tmp339 * _tmp340 + _tmp341; + _new_covariance(4, 3) = + _tmp280 * _tmp301 + _tmp287 * _tmp303 + _tmp290 * _tmp302 + _tmp342 + _tmp349; + _new_covariance(5, 3) = + _tmp280 * _tmp311 + _tmp287 * _tmp310 + _tmp290 * _tmp312 + _tmp350 + _tmp355; + _new_covariance(6, 3) = + _tmp280 * _tmp317 + _tmp287 * _tmp318 + _tmp290 * _tmp316 + _tmp356 + _tmp360; + _new_covariance(7, 3) = + _tmp280 * _tmp323 + _tmp287 * _tmp324 + _tmp290 * _tmp322 + _tmp361 + _tmp367; + _new_covariance(8, 3) = + _tmp280 * _tmp328 + _tmp287 * _tmp330 + _tmp290 * _tmp329 + _tmp368 + _tmp372; + _new_covariance(0, 4) = _tmp190 * _tmp298 + _tmp226 * _tmp296 + _tmp250 * _tmp300 + _tmp373; + _new_covariance(1, 4) = _tmp254 * _tmp296 + _tmp255 * _tmp300 + _tmp297 * _tmp374 + _tmp375; + _new_covariance(2, 4) = _tmp266 * _tmp298 + _tmp267 * _tmp300 + _tmp268 * _tmp296 + _tmp376; + _new_covariance(3, 4) = + _tmp291 * _tmp300 + _tmp292 * _tmp296 + _tmp297 * _tmp377 + _tmp349 + _tmp378; + _new_covariance(4, 4) = _tmp296 * _tmp303 + _tmp298 * _tmp302 + _tmp300 * _tmp301 + + _tmp336 * _tmp381 + _tmp338 * _tmp380 + _tmp340 * _tmp379 + _tmp382; + _new_covariance(5, 4) = + _tmp296 * _tmp310 + _tmp297 * _tmp383 + _tmp300 * _tmp311 + _tmp384 + _tmp386; + _new_covariance(6, 4) = + _tmp296 * _tmp318 + _tmp298 * _tmp316 + _tmp300 * _tmp317 + _tmp367 + _tmp387; + _new_covariance(7, 4) = + _tmp296 * _tmp324 + _tmp297 * _tmp388 + _tmp300 * _tmp323 + _tmp389 + _tmp393; + _new_covariance(8, 4) = + _tmp296 * _tmp330 + _tmp298 * _tmp329 + _tmp300 * _tmp328 + _tmp394 + _tmp395; + _new_covariance(0, 5) = _tmp190 * _tmp307 + _tmp226 * _tmp309 + _tmp250 * _tmp305 + _tmp396; + _new_covariance(1, 5) = _tmp254 * _tmp309 + _tmp255 * _tmp305 + _tmp306 * _tmp374 + _tmp397; + _new_covariance(2, 5) = _tmp266 * _tmp307 + _tmp267 * _tmp305 + _tmp268 * _tmp309 + _tmp398; + _new_covariance(3, 5) = + _tmp291 * _tmp305 + _tmp292 * _tmp309 + _tmp306 * _tmp377 + _tmp355 + _tmp399; + _new_covariance(4, 5) = + _tmp301 * _tmp305 + _tmp302 * _tmp307 + _tmp303 * _tmp309 + _tmp386 + _tmp400; + _new_covariance(5, 5) = _tmp305 * _tmp311 + _tmp306 * _tmp383 + _tmp309 * _tmp310 + + _tmp336 * _tmp402 + _tmp338 * _tmp403 + _tmp340 * _tmp401 + _tmp404; + _new_covariance(6, 5) = + _tmp305 * _tmp317 + _tmp307 * _tmp316 + _tmp309 * _tmp318 + _tmp372 + _tmp405; + _new_covariance(7, 5) = + _tmp305 * _tmp323 + _tmp306 * _tmp388 + _tmp309 * _tmp324 + _tmp395 + _tmp406; + _new_covariance(8, 5) = + _tmp305 * _tmp328 + _tmp307 * _tmp329 + _tmp309 * _tmp330 + _tmp407 + _tmp411; + _new_covariance(0, 6) = _tmp176 * covariance(0, 6) + _tmp184 * covariance(1, 6) + + _tmp189 * covariance(2, 6) + _tmp190 * _tmp315 + _tmp226 * _tmp314 + + _tmp250 * _tmp313 + _tmp332 * dt; + _new_covariance(1, 6) = _tmp251 * covariance(0, 6) + _tmp252 * covariance(2, 6) + + _tmp253 * covariance(1, 6) + _tmp254 * _tmp314 + _tmp255 * _tmp313 + + _tmp256 * _tmp315 + _tmp333 * dt; + _new_covariance(2, 6) = _tmp263 * covariance(1, 6) + _tmp264 * covariance(2, 6) + + _tmp265 * covariance(0, 6) + _tmp266 * _tmp315 + _tmp267 * _tmp313 + + _tmp268 * _tmp314 + _tmp334 * dt; + _new_covariance(3, 6) = _tmp280 * covariance(2, 6) + _tmp287 * covariance(1, 6) + + _tmp290 * covariance(0, 6) + _tmp291 * _tmp313 + _tmp292 * _tmp314 + + _tmp293 * _tmp315 + _tmp341 * dt + _tmp360 + covariance(3, 6); + _new_covariance(4, 6) = _tmp296 * covariance(1, 6) + _tmp298 * covariance(0, 6) + + _tmp300 * covariance(2, 6) + _tmp301 * _tmp313 + _tmp302 * _tmp315 + + _tmp303 * _tmp314 + _tmp342 * dt + _tmp367 + covariance(4, 6); + _new_covariance(5, 6) = _tmp289 * _tmp412 + _tmp305 * covariance(2, 6) + + _tmp307 * covariance(0, 6) + _tmp309 * covariance(1, 6) + + _tmp310 * _tmp314 + _tmp311 * _tmp313 + _tmp350 * dt + _tmp372 + + covariance(5, 6); + _new_covariance(6, 6) = _tmp313 * _tmp317 + _tmp313 * covariance(2, 6) + _tmp314 * _tmp318 + + _tmp314 * covariance(1, 6) + _tmp315 * _tmp316 + + _tmp315 * covariance(0, 6) + _tmp356 * dt + _tmp357 * _tmp413 + + _tmp358 * _tmp413 + _tmp359 * _tmp413 + covariance(3, 6) * dt + + covariance(6, 6); + _new_covariance(7, 6) = _tmp313 * _tmp323 + _tmp314 * _tmp324 + _tmp315 * _tmp322 + + _tmp319 * covariance(1, 6) + _tmp320 * covariance(0, 6) + + _tmp321 * covariance(2, 6) + _tmp361 * dt + _tmp415 + + covariance(4, 6) * dt + covariance(7, 6); + _new_covariance(8, 6) = _tmp313 * _tmp328 + _tmp314 * _tmp330 + _tmp315 * _tmp329 + + _tmp325 * covariance(2, 6) + _tmp326 * covariance(0, 6) + + _tmp327 * covariance(1, 6) + _tmp368 * dt + _tmp418 + + covariance(5, 6) * dt + covariance(8, 6); + _new_covariance(0, 7) = _tmp176 * covariance(0, 7) + _tmp184 * covariance(1, 7) + + _tmp189 * covariance(2, 7) + _tmp190 * _tmp320 + _tmp226 * _tmp319 + + _tmp250 * _tmp321 + _tmp373 * dt; + _new_covariance(1, 7) = _tmp251 * covariance(0, 7) + _tmp252 * covariance(2, 7) + + _tmp253 * covariance(1, 7) + _tmp254 * _tmp319 + _tmp255 * _tmp321 + + _tmp256 * _tmp320 + _tmp375 * dt; + _new_covariance(2, 7) = _tmp263 * covariance(1, 7) + _tmp264 * covariance(2, 7) + + _tmp265 * covariance(0, 7) + _tmp266 * _tmp320 + _tmp267 * _tmp321 + + _tmp268 * _tmp319 + _tmp376 * dt; + _new_covariance(3, 7) = _tmp280 * covariance(2, 7) + _tmp287 * covariance(1, 7) + + _tmp290 * covariance(0, 7) + _tmp291 * _tmp321 + _tmp292 * _tmp319 + + _tmp293 * _tmp320 + _tmp367 + _tmp378 * dt + covariance(3, 7); + _new_covariance(4, 7) = _tmp296 * covariance(1, 7) + _tmp298 * covariance(0, 7) + + _tmp300 * covariance(2, 7) + _tmp301 * _tmp321 + _tmp302 * _tmp320 + + _tmp303 * _tmp319 + _tmp382 * dt + _tmp393 + covariance(4, 7); + _new_covariance(5, 7) = _tmp297 * _tmp412 + _tmp305 * covariance(2, 7) + + _tmp307 * covariance(0, 7) + _tmp309 * covariance(1, 7) + + _tmp310 * _tmp319 + _tmp311 * _tmp321 + _tmp384 * dt + _tmp395 + + covariance(5, 7); + _new_covariance(6, 7) = _tmp313 * covariance(2, 7) + _tmp314 * covariance(1, 7) + + _tmp315 * covariance(0, 7) + _tmp316 * _tmp320 + _tmp317 * _tmp321 + + _tmp318 * _tmp319 + _tmp387 * dt + _tmp415 + covariance(3, 7) * dt + + covariance(6, 7); + _new_covariance(7, 7) = _tmp319 * _tmp324 + _tmp319 * covariance(1, 7) + _tmp320 * _tmp322 + + _tmp320 * covariance(0, 7) + _tmp321 * _tmp323 + + _tmp321 * covariance(2, 7) + _tmp389 * dt + _tmp390 * _tmp413 + + _tmp391 * _tmp413 + _tmp392 * _tmp413 + covariance(4, 7) * dt + + covariance(7, 7); + _new_covariance(8, 7) = _tmp319 * _tmp330 + _tmp320 * _tmp329 + _tmp321 * _tmp328 + + _tmp325 * covariance(2, 7) + _tmp326 * covariance(0, 7) + + _tmp327 * covariance(1, 7) + _tmp394 * dt + _tmp419 + + covariance(5, 7) * dt + covariance(8, 7); + _new_covariance(0, 8) = _tmp176 * covariance(0, 8) + _tmp184 * covariance(1, 8) + + _tmp189 * covariance(2, 8) + _tmp190 * _tmp326 + _tmp226 * _tmp327 + + _tmp250 * _tmp325 + _tmp396 * dt; + _new_covariance(1, 8) = _tmp251 * covariance(0, 8) + _tmp252 * covariance(2, 8) + + _tmp253 * covariance(1, 8) + _tmp254 * _tmp327 + _tmp255 * _tmp325 + + _tmp256 * _tmp326 + _tmp397 * dt; + _new_covariance(2, 8) = _tmp263 * covariance(1, 8) + _tmp264 * covariance(2, 8) + + _tmp265 * covariance(0, 8) + _tmp266 * _tmp326 + _tmp267 * _tmp325 + + _tmp268 * _tmp327 + _tmp398 * dt; + _new_covariance(3, 8) = _tmp280 * covariance(2, 8) + _tmp287 * covariance(1, 8) + + _tmp290 * covariance(0, 8) + _tmp291 * _tmp325 + _tmp292 * _tmp327 + + _tmp293 * _tmp326 + _tmp372 + _tmp399 * dt + covariance(3, 8); + _new_covariance(4, 8) = _tmp296 * covariance(1, 8) + _tmp298 * covariance(0, 8) + + _tmp300 * covariance(2, 8) + _tmp301 * _tmp325 + _tmp302 * _tmp326 + + _tmp303 * _tmp327 + _tmp395 + _tmp400 * dt + covariance(4, 8); + _new_covariance(5, 8) = _tmp305 * covariance(2, 8) + _tmp306 * _tmp412 + + _tmp307 * covariance(0, 8) + _tmp309 * covariance(1, 8) + + _tmp310 * _tmp327 + _tmp311 * _tmp325 + _tmp404 * dt + _tmp411 + + covariance(5, 8); + _new_covariance(6, 8) = _tmp313 * covariance(2, 8) + _tmp314 * covariance(1, 8) + + _tmp315 * covariance(0, 8) + _tmp316 * _tmp326 + _tmp317 * _tmp325 + + _tmp318 * _tmp327 + _tmp405 * dt + _tmp418 + covariance(3, 8) * dt + + covariance(6, 8); + _new_covariance(7, 8) = _tmp319 * covariance(1, 8) + _tmp320 * covariance(0, 8) + + _tmp321 * covariance(2, 8) + _tmp322 * _tmp326 + _tmp323 * _tmp325 + + _tmp324 * _tmp327 + _tmp406 * dt + _tmp419 + covariance(4, 8) * dt + + covariance(7, 8); + _new_covariance(8, 8) = _tmp325 * _tmp328 + _tmp325 * covariance(2, 8) + _tmp326 * _tmp329 + + _tmp326 * covariance(0, 8) + _tmp327 * _tmp330 + + _tmp327 * covariance(1, 8) + _tmp407 * dt + _tmp408 * _tmp413 + + _tmp409 * _tmp413 + _tmp410 * _tmp413 + covariance(5, 8) * dt + + covariance(8, 8); + } + + if (new_DR_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_DR_D_gyro_bias = (*new_DR_D_gyro_bias); + + _new_DR_D_gyro_bias(0, 0) = DR_D_gyro_bias(0, 0) * _tmp176 + DR_D_gyro_bias(1, 0) * _tmp184 + + DR_D_gyro_bias(2, 0) * _tmp189 + _tmp104 * _tmp428 + + _tmp120 * _tmp426 - _tmp135 * _tmp423 - _tmp149 * _tmp421; + _new_DR_D_gyro_bias(1, 0) = DR_D_gyro_bias(0, 0) * _tmp251 + DR_D_gyro_bias(1, 0) * _tmp253 + + DR_D_gyro_bias(2, 0) * _tmp252 + _tmp104 * _tmp426 - + _tmp120 * _tmp428 - _tmp135 * _tmp421 + _tmp149 * _tmp423; + _new_DR_D_gyro_bias(2, 0) = DR_D_gyro_bias(0, 0) * _tmp265 + DR_D_gyro_bias(1, 0) * _tmp263 + + DR_D_gyro_bias(2, 0) * _tmp264 + _tmp104 * _tmp423 - + _tmp120 * _tmp421 + _tmp135 * _tmp428 - _tmp149 * _tmp426; + _new_DR_D_gyro_bias(0, 1) = DR_D_gyro_bias(0, 1) * _tmp176 + DR_D_gyro_bias(1, 1) * _tmp184 + + DR_D_gyro_bias(2, 1) * _tmp189 + _tmp104 * _tmp434 + + _tmp120 * _tmp429 - _tmp135 * _tmp431 - _tmp149 * _tmp433; + _new_DR_D_gyro_bias(1, 1) = DR_D_gyro_bias(0, 1) * _tmp251 + DR_D_gyro_bias(1, 1) * _tmp253 + + DR_D_gyro_bias(2, 1) * _tmp252 + _tmp104 * _tmp429 - + _tmp120 * _tmp434 - _tmp135 * _tmp433 + _tmp149 * _tmp431; + _new_DR_D_gyro_bias(2, 1) = DR_D_gyro_bias(0, 1) * _tmp265 + DR_D_gyro_bias(1, 1) * _tmp263 + + DR_D_gyro_bias(2, 1) * _tmp264 + _tmp104 * _tmp431 - + _tmp120 * _tmp433 + _tmp135 * _tmp434 - _tmp149 * _tmp429; + _new_DR_D_gyro_bias(0, 2) = DR_D_gyro_bias(0, 2) * _tmp176 + DR_D_gyro_bias(1, 2) * _tmp184 + + DR_D_gyro_bias(2, 2) * _tmp189 + _tmp104 * _tmp435 + + _tmp120 * _tmp436 - _tmp135 * _tmp437 - _tmp149 * _tmp438; + _new_DR_D_gyro_bias(1, 2) = DR_D_gyro_bias(0, 2) * _tmp251 + DR_D_gyro_bias(1, 2) * _tmp253 + + DR_D_gyro_bias(2, 2) * _tmp252 + _tmp104 * _tmp436 - + _tmp120 * _tmp435 - _tmp135 * _tmp438 + _tmp149 * _tmp437; + _new_DR_D_gyro_bias(2, 2) = DR_D_gyro_bias(0, 2) * _tmp265 + DR_D_gyro_bias(1, 2) * _tmp263 + + DR_D_gyro_bias(2, 2) * _tmp264 + _tmp104 * _tmp437 - + _tmp120 * _tmp438 + _tmp135 * _tmp435 - _tmp149 * _tmp436; + } + + if (new_Dv_D_accel_bias != nullptr) { + Eigen::Matrix& _new_Dv_D_accel_bias = (*new_Dv_D_accel_bias); + + _new_Dv_D_accel_bias(0, 0) = Dv_D_accel_bias(0, 0) - _tmp44 * dt; + _new_Dv_D_accel_bias(1, 0) = Dv_D_accel_bias(1, 0) - _tmp347; + _new_Dv_D_accel_bias(2, 0) = Dv_D_accel_bias(2, 0) - _tmp353; + _new_Dv_D_accel_bias(0, 1) = Dv_D_accel_bias(0, 1) - _tmp345; + _new_Dv_D_accel_bias(1, 1) = Dv_D_accel_bias(1, 1) - _tmp60 * dt; + _new_Dv_D_accel_bias(2, 1) = Dv_D_accel_bias(2, 1) - _tmp351; + _new_Dv_D_accel_bias(0, 2) = Dv_D_accel_bias(0, 2) - _tmp55 * dt; + _new_Dv_D_accel_bias(1, 2) = Dv_D_accel_bias(1, 2) - _tmp343; + _new_Dv_D_accel_bias(2, 2) = Dv_D_accel_bias(2, 2) - _tmp354; + } + + if (new_Dv_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_Dv_D_gyro_bias = (*new_Dv_D_gyro_bias); + + _new_Dv_D_gyro_bias(0, 0) = DR_D_gyro_bias(0, 0) * _tmp290 + DR_D_gyro_bias(1, 0) * _tmp287 + + DR_D_gyro_bias(2, 0) * _tmp280 + Dv_D_gyro_bias(0, 0); + _new_Dv_D_gyro_bias(1, 0) = DR_D_gyro_bias(0, 0) * _tmp298 + DR_D_gyro_bias(1, 0) * _tmp296 + + DR_D_gyro_bias(2, 0) * _tmp300 + Dv_D_gyro_bias(1, 0); + _new_Dv_D_gyro_bias(2, 0) = DR_D_gyro_bias(0, 0) * _tmp307 + DR_D_gyro_bias(1, 0) * _tmp309 + + DR_D_gyro_bias(2, 0) * _tmp305 + Dv_D_gyro_bias(2, 0); + _new_Dv_D_gyro_bias(0, 1) = DR_D_gyro_bias(0, 1) * _tmp290 + DR_D_gyro_bias(1, 1) * _tmp287 + + DR_D_gyro_bias(2, 1) * _tmp280 + Dv_D_gyro_bias(0, 1); + _new_Dv_D_gyro_bias(1, 1) = DR_D_gyro_bias(0, 1) * _tmp298 + DR_D_gyro_bias(1, 1) * _tmp296 + + DR_D_gyro_bias(2, 1) * _tmp300 + Dv_D_gyro_bias(1, 1); + _new_Dv_D_gyro_bias(2, 1) = DR_D_gyro_bias(0, 1) * _tmp307 + DR_D_gyro_bias(1, 1) * _tmp309 + + DR_D_gyro_bias(2, 1) * _tmp305 + Dv_D_gyro_bias(2, 1); + _new_Dv_D_gyro_bias(0, 2) = DR_D_gyro_bias(0, 2) * _tmp290 + DR_D_gyro_bias(1, 2) * _tmp287 + + DR_D_gyro_bias(2, 2) * _tmp280 + Dv_D_gyro_bias(0, 2); + _new_Dv_D_gyro_bias(1, 2) = DR_D_gyro_bias(0, 2) * _tmp298 + DR_D_gyro_bias(1, 2) * _tmp296 + + DR_D_gyro_bias(2, 2) * _tmp300 + Dv_D_gyro_bias(1, 2); + _new_Dv_D_gyro_bias(2, 2) = DR_D_gyro_bias(0, 2) * _tmp307 + DR_D_gyro_bias(1, 2) * _tmp309 + + DR_D_gyro_bias(2, 2) * _tmp305 + Dv_D_gyro_bias(2, 2); + } + + if (new_Dp_D_accel_bias != nullptr) { + Eigen::Matrix& _new_Dp_D_accel_bias = (*new_Dp_D_accel_bias); + + _new_Dp_D_accel_bias(0, 0) = Dp_D_accel_bias(0, 0) + Dv_D_accel_bias(0, 0) * dt - _tmp365; + _new_Dp_D_accel_bias(1, 0) = + Dp_D_accel_bias(1, 0) + Dv_D_accel_bias(1, 0) * dt - _tmp61 * _tmp72; + _new_Dp_D_accel_bias(2, 0) = Dp_D_accel_bias(2, 0) + Dv_D_accel_bias(2, 0) * dt - _tmp370; + _new_Dp_D_accel_bias(0, 1) = + Dp_D_accel_bias(0, 1) + Dv_D_accel_bias(0, 1) * dt - _tmp50 * _tmp72; + _new_Dp_D_accel_bias(1, 1) = Dp_D_accel_bias(1, 1) + Dv_D_accel_bias(1, 1) * dt - _tmp363; + _new_Dp_D_accel_bias(2, 1) = Dp_D_accel_bias(2, 1) + Dv_D_accel_bias(2, 1) * dt - _tmp369; + _new_Dp_D_accel_bias(0, 2) = + Dp_D_accel_bias(0, 2) + Dv_D_accel_bias(0, 2) * dt - _tmp55 * _tmp72; + _new_Dp_D_accel_bias(1, 2) = Dp_D_accel_bias(1, 2) + Dv_D_accel_bias(1, 2) * dt - _tmp362; + _new_Dp_D_accel_bias(2, 2) = Dp_D_accel_bias(2, 2) + Dv_D_accel_bias(2, 2) * dt - _tmp371; + } + + if (new_Dp_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_Dp_D_gyro_bias = (*new_Dp_D_gyro_bias); + + _new_Dp_D_gyro_bias(0, 0) = DR_D_gyro_bias(0, 0) * _tmp315 + DR_D_gyro_bias(1, 0) * _tmp314 + + DR_D_gyro_bias(2, 0) * _tmp313 + Dp_D_gyro_bias(0, 0) + + Dv_D_gyro_bias(0, 0) * dt; + _new_Dp_D_gyro_bias(1, 0) = DR_D_gyro_bias(0, 0) * _tmp320 + DR_D_gyro_bias(1, 0) * _tmp319 + + DR_D_gyro_bias(2, 0) * _tmp321 + Dp_D_gyro_bias(1, 0) + + Dv_D_gyro_bias(1, 0) * dt; + _new_Dp_D_gyro_bias(2, 0) = DR_D_gyro_bias(0, 0) * _tmp326 + DR_D_gyro_bias(1, 0) * _tmp327 + + DR_D_gyro_bias(2, 0) * _tmp325 + Dp_D_gyro_bias(2, 0) + + Dv_D_gyro_bias(2, 0) * dt; + _new_Dp_D_gyro_bias(0, 1) = DR_D_gyro_bias(0, 1) * _tmp315 + DR_D_gyro_bias(1, 1) * _tmp314 + + DR_D_gyro_bias(2, 1) * _tmp313 + Dp_D_gyro_bias(0, 1) + + Dv_D_gyro_bias(0, 1) * dt; + _new_Dp_D_gyro_bias(1, 1) = DR_D_gyro_bias(0, 1) * _tmp320 + DR_D_gyro_bias(1, 1) * _tmp319 + + DR_D_gyro_bias(2, 1) * _tmp321 + Dp_D_gyro_bias(1, 1) + + Dv_D_gyro_bias(1, 1) * dt; + _new_Dp_D_gyro_bias(2, 1) = DR_D_gyro_bias(0, 1) * _tmp326 + DR_D_gyro_bias(1, 1) * _tmp327 + + DR_D_gyro_bias(2, 1) * _tmp325 + Dp_D_gyro_bias(2, 1) + + Dv_D_gyro_bias(2, 1) * dt; + _new_Dp_D_gyro_bias(0, 2) = DR_D_gyro_bias(0, 2) * _tmp315 + DR_D_gyro_bias(1, 2) * _tmp314 + + DR_D_gyro_bias(2, 2) * _tmp313 + Dp_D_gyro_bias(0, 2) + + Dv_D_gyro_bias(0, 2) * dt; + _new_Dp_D_gyro_bias(1, 2) = DR_D_gyro_bias(0, 2) * _tmp320 + DR_D_gyro_bias(1, 2) * _tmp319 + + DR_D_gyro_bias(2, 2) * _tmp321 + Dp_D_gyro_bias(1, 2) + + Dv_D_gyro_bias(1, 2) * dt; + _new_Dp_D_gyro_bias(2, 2) = DR_D_gyro_bias(0, 2) * _tmp326 + DR_D_gyro_bias(1, 2) * _tmp327 + + DR_D_gyro_bias(2, 2) * _tmp325 + Dp_D_gyro_bias(2, 2) + + Dv_D_gyro_bias(2, 2) * dt; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/internal/internal_imu_factor.h b/gen/cpp/sym/factors/internal/internal_imu_factor.h new file mode 100644 index 000000000..025f93533 --- /dev/null +++ b/gen/cpp/sym/factors/internal/internal_imu_factor.h @@ -0,0 +1,2377 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * An internal helper function to linearize the difference between the orientation, velocity, and + * position at one time step and those of another time step. The expected difference is calculated + * from the preintegrated IMU measurements between those time steps. + * + * NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use + * the one found in symforce/slam/imu_preintegration/imu_factor.h. + * + * Time step i is the time of the first IMU measurement of the interval. + * Time step j is the time after the last IMU measurement of the interval. + * + * Assumes sqrt_info is lower triangular and only reads the lower triangle. + * + * Args: + * pose_i (sf.Pose3): Pose at time step i (world_T_body) + * vel_i (sf.V3): Velocity at time step i (world frame) + * pose_j (sf.Pose3): Pose at time step j (world_T_body) + * vel_j (sf.V3): Velocity at time step j (world frame) + * accel_bias_i (sf.V3): The accelerometer bias between timesteps i and j + * gyro_bias_i (sf.V3): The gyroscope bias between timesteps i and j + * DR (sf.Rot3): Preintegrated estimate for pose_i.R.inverse() * pose_j.R + * Dv (sf.V3): Preintegrated estimate for vel_j - vel_i in the body frame at timestep i + * Dp (sf.V3): Preintegrated estimate for position change (before velocity and gravity + * corrections) in the body frame at timestep i: + * R_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2), + * where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i + * sqrt_info (sf.M99): sqrt info matrix of DR('s tangent space), Dv, Dp + * DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat + * Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat + * Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat + * Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat + * Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat + * accel_bias_hat (sf.V3): The accelerometer bias used during preintegration + * gyro_bias_hat (sf.V3): The gyroscope bias used during preintegration + * gravity (sf.V3): Acceleration due to gravity (in the same frame as pose_x and vel_x), + * i.e., the vector which when added to the accelerometer measurements gives the + * true acceleration (up to bias and noise) of the IMU. + * dt (T.Scalar): The time between timestep i and j: t_j - t_i + * epsilon (T.Scalar): epsilon used for numerical stability + * + * Outputs: + * res: 9dof product residual between the orientations, then velocities, then positions. + * jacobian: (9x24) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), + * accel_bias_i (3), gyro_bias_i (3) + * hessian: (24x24) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), + * accel_bias_i (3), gyro_bias_i (3) + * rhs: (24x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), + * accel_bias_i (3), gyro_bias_i (3) + */ +template +void InternalImuFactor(const sym::Pose3& pose_i, const Eigen::Matrix& vel_i, + const sym::Pose3& pose_j, const Eigen::Matrix& vel_j, + const Eigen::Matrix& accel_bias_i, + const Eigen::Matrix& gyro_bias_i, const sym::Rot3& DR, + const Eigen::Matrix& Dv, const Eigen::Matrix& Dp, + const Eigen::Matrix& sqrt_info, + const Eigen::Matrix& DR_D_gyro_bias, + const Eigen::Matrix& Dv_D_accel_bias, + const Eigen::Matrix& Dv_D_gyro_bias, + const Eigen::Matrix& Dp_D_accel_bias, + const Eigen::Matrix& Dp_D_gyro_bias, + const Eigen::Matrix& accel_bias_hat, + const Eigen::Matrix& gyro_bias_hat, + const Eigen::Matrix& gravity, const Scalar dt, + const Scalar epsilon, Eigen::Matrix* const res = nullptr, + Eigen::Matrix* const jacobian = nullptr, + Eigen::Matrix* const hessian = nullptr, + Eigen::Matrix* const rhs = nullptr) { + // Total ops: 5964 + + // Input arrays + const Eigen::Matrix& _pose_i = pose_i.Data(); + const Eigen::Matrix& _pose_j = pose_j.Data(); + const Eigen::Matrix& _DR = DR.Data(); + + // Intermediate terms (770) + const Scalar _tmp0 = -gyro_bias_hat(0, 0) + gyro_bias_i(0, 0); + const Scalar _tmp1 = -gyro_bias_hat(2, 0) + gyro_bias_i(2, 0); + const Scalar _tmp2 = -gyro_bias_hat(1, 0) + gyro_bias_i(1, 0); + const Scalar _tmp3 = + DR_D_gyro_bias(2, 0) * _tmp0 + DR_D_gyro_bias(2, 1) * _tmp2 + DR_D_gyro_bias(2, 2) * _tmp1; + const Scalar _tmp4 = + DR_D_gyro_bias(0, 0) * _tmp0 + DR_D_gyro_bias(0, 1) * _tmp2 + DR_D_gyro_bias(0, 2) * _tmp1; + const Scalar _tmp5 = + DR_D_gyro_bias(1, 0) * _tmp0 + DR_D_gyro_bias(1, 1) * _tmp2 + DR_D_gyro_bias(1, 2) * _tmp1; + const Scalar _tmp6 = std::pow(_tmp3, Scalar(2)) + std::pow(_tmp4, Scalar(2)) + + std::pow(_tmp5, Scalar(2)) + std::pow(epsilon, Scalar(2)); + const Scalar _tmp7 = std::sqrt(_tmp6); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp9 = std::cos(_tmp8); + const Scalar _tmp10 = _DR[0] * _tmp9; + const Scalar _tmp11 = std::sin(_tmp8); + const Scalar _tmp12 = _tmp11 / _tmp7; + const Scalar _tmp13 = _tmp12 * _tmp5; + const Scalar _tmp14 = _DR[3] * _tmp12; + const Scalar _tmp15 = _DR[1] * _tmp12; + const Scalar _tmp16 = -_DR[2] * _tmp13 + _tmp10 + _tmp14 * _tmp4 + _tmp15 * _tmp3; + const Scalar _tmp17 = _pose_i[0] * _tmp16; + const Scalar _tmp18 = _DR[1] * _tmp9; + const Scalar _tmp19 = _DR[0] * _tmp12; + const Scalar _tmp20 = _DR[2] * _tmp12; + const Scalar _tmp21 = _DR[3] * _tmp13 + _tmp18 - _tmp19 * _tmp3 + _tmp20 * _tmp4; + const Scalar _tmp22 = _pose_i[1] * _tmp21; + const Scalar _tmp23 = _DR[2] * _tmp9; + const Scalar _tmp24 = _tmp14 * _tmp3 - _tmp15 * _tmp4 + _tmp19 * _tmp5 + _tmp23; + const Scalar _tmp25 = _pose_i[2] * _tmp24; + const Scalar _tmp26 = _DR[3] * _tmp9; + const Scalar _tmp27 = -_DR[1] * _tmp13 - _tmp19 * _tmp4 - _tmp20 * _tmp3 + _tmp26; + const Scalar _tmp28 = _pose_i[3] * _tmp27; + const Scalar _tmp29 = -_tmp17 - _tmp22 - _tmp25 + _tmp28; + const Scalar _tmp30 = _pose_j[0] * _tmp29; + const Scalar _tmp31 = _pose_i[3] * _tmp16; + const Scalar _tmp32 = _pose_i[0] * _tmp27; + const Scalar _tmp33 = _pose_i[2] * _tmp21; + const Scalar _tmp34 = _pose_i[1] * _tmp24; + const Scalar _tmp35 = -_tmp31 - _tmp32 + _tmp33 - _tmp34; + const Scalar _tmp36 = _pose_j[3] * _tmp35; + const Scalar _tmp37 = _pose_i[2] * _tmp16; + const Scalar _tmp38 = _pose_i[1] * _tmp27; + const Scalar _tmp39 = _pose_i[3] * _tmp21; + const Scalar _tmp40 = _pose_i[0] * _tmp24; + const Scalar _tmp41 = -_tmp37 - _tmp38 - _tmp39 + _tmp40; + const Scalar _tmp42 = _pose_j[2] * _tmp41; + const Scalar _tmp43 = _pose_i[1] * _tmp16; + const Scalar _tmp44 = _pose_i[2] * _tmp27; + const Scalar _tmp45 = _pose_i[0] * _tmp21; + const Scalar _tmp46 = _pose_i[3] * _tmp24; + const Scalar _tmp47 = _tmp43 - _tmp44 - _tmp45 - _tmp46; + const Scalar _tmp48 = _pose_j[1] * _tmp47; + const Scalar _tmp49 = _tmp30 + _tmp36 + _tmp42 - _tmp48; + const Scalar _tmp50 = _tmp49 * sqrt_info(0, 0); + const Scalar _tmp51 = _pose_j[0] * _tmp35; + const Scalar _tmp52 = _pose_j[1] * _tmp41; + const Scalar _tmp53 = _pose_j[2] * _tmp47; + const Scalar _tmp54 = _tmp51 + _tmp52 + _tmp53; + const Scalar _tmp55 = _pose_j[3] * _tmp29; + const Scalar _tmp56 = + 2 * std::min(0, (((-_tmp54 + _tmp55) > 0) - ((-_tmp54 + _tmp55) < 0))) + 1; + const Scalar _tmp57 = 2 * _tmp56; + const Scalar _tmp58 = -_tmp55; + const Scalar _tmp59 = 1 - epsilon; + const Scalar _tmp60 = std::min(_tmp59, std::fabs(_tmp54 + _tmp58)); + const Scalar _tmp61 = std::acos(_tmp60) / std::sqrt(Scalar(1 - std::pow(_tmp60, Scalar(2)))); + const Scalar _tmp62 = _tmp57 * _tmp61; + const Scalar _tmp63 = _tmp50 * _tmp62; + const Scalar _tmp64 = _pose_j[1] * _tmp29; + const Scalar _tmp65 = _pose_j[2] * _tmp35; + const Scalar _tmp66 = _pose_j[3] * _tmp41; + const Scalar _tmp67 = _pose_j[0] * _tmp47; + const Scalar _tmp68 = _tmp64 - _tmp65 + _tmp66 + _tmp67; + const Scalar _tmp69 = _tmp62 * _tmp68; + const Scalar _tmp70 = _tmp49 * _tmp62; + const Scalar _tmp71 = _tmp69 * sqrt_info(1, 1) + _tmp70 * sqrt_info(1, 0); + const Scalar _tmp72 = _pose_j[2] * _tmp29; + const Scalar _tmp73 = _pose_j[1] * _tmp35; + const Scalar _tmp74 = _pose_j[0] * _tmp41; + const Scalar _tmp75 = _pose_j[3] * _tmp47; + const Scalar _tmp76 = _tmp72 + _tmp73 - _tmp74 + _tmp75; + const Scalar _tmp77 = _tmp62 * _tmp76; + const Scalar _tmp78 = + _tmp69 * sqrt_info(2, 1) + _tmp70 * sqrt_info(2, 0) + _tmp77 * sqrt_info(2, 2); + const Scalar _tmp79 = std::pow(_pose_i[1], Scalar(2)); + const Scalar _tmp80 = -2 * _tmp79; + const Scalar _tmp81 = std::pow(_pose_i[2], Scalar(2)); + const Scalar _tmp82 = 1 - 2 * _tmp81; + const Scalar _tmp83 = _tmp80 + _tmp82; + const Scalar _tmp84 = -dt * gravity(0, 0) - vel_i(0, 0) + vel_j(0, 0); + const Scalar _tmp85 = -accel_bias_hat(0, 0) + accel_bias_i(0, 0); + const Scalar _tmp86 = -accel_bias_hat(1, 0) + accel_bias_i(1, 0); + const Scalar _tmp87 = -accel_bias_hat(2, 0) + accel_bias_i(2, 0); + const Scalar _tmp88 = 2 * _pose_i[3]; + const Scalar _tmp89 = _pose_i[2] * _tmp88; + const Scalar _tmp90 = 2 * _pose_i[0]; + const Scalar _tmp91 = _pose_i[1] * _tmp90; + const Scalar _tmp92 = _tmp89 + _tmp91; + const Scalar _tmp93 = -dt * gravity(1, 0) - vel_i(1, 0) + vel_j(1, 0); + const Scalar _tmp94 = _pose_i[1] * _tmp88; + const Scalar _tmp95 = -_tmp94; + const Scalar _tmp96 = _pose_i[2] * _tmp90; + const Scalar _tmp97 = _tmp95 + _tmp96; + const Scalar _tmp98 = -dt * gravity(2, 0) - vel_i(2, 0) + vel_j(2, 0); + const Scalar _tmp99 = _tmp92 * _tmp93 + _tmp97 * _tmp98; + const Scalar _tmp100 = -Dv(0, 0) - Dv_D_accel_bias(0, 0) * _tmp85 - + Dv_D_accel_bias(0, 1) * _tmp86 - Dv_D_accel_bias(0, 2) * _tmp87 - + Dv_D_gyro_bias(0, 0) * _tmp0 - Dv_D_gyro_bias(0, 1) * _tmp2 - + Dv_D_gyro_bias(0, 2) * _tmp1 + _tmp83 * _tmp84 + _tmp99; + const Scalar _tmp101 = _tmp100 * sqrt_info(3, 3) + _tmp69 * sqrt_info(3, 1) + + _tmp70 * sqrt_info(3, 0) + _tmp77 * sqrt_info(3, 2); + const Scalar _tmp102 = std::pow(_pose_i[0], Scalar(2)); + const Scalar _tmp103 = -2 * _tmp102; + const Scalar _tmp104 = _tmp103 + _tmp82; + const Scalar _tmp105 = -_tmp89; + const Scalar _tmp106 = _tmp105 + _tmp91; + const Scalar _tmp107 = _pose_i[3] * _tmp90; + const Scalar _tmp108 = 2 * _pose_i[1] * _pose_i[2]; + const Scalar _tmp109 = _tmp107 + _tmp108; + const Scalar _tmp110 = _tmp106 * _tmp84 + _tmp109 * _tmp98; + const Scalar _tmp111 = -Dv(1, 0) - Dv_D_accel_bias(1, 0) * _tmp85 - + Dv_D_accel_bias(1, 1) * _tmp86 - Dv_D_accel_bias(1, 2) * _tmp87 - + Dv_D_gyro_bias(1, 0) * _tmp0 - Dv_D_gyro_bias(1, 1) * _tmp2 - + Dv_D_gyro_bias(1, 2) * _tmp1 + _tmp104 * _tmp93 + _tmp110; + const Scalar _tmp112 = _tmp100 * sqrt_info(4, 3) + _tmp111 * sqrt_info(4, 4) + + _tmp69 * sqrt_info(4, 1) + _tmp70 * sqrt_info(4, 0) + + _tmp77 * sqrt_info(4, 2); + const Scalar _tmp113 = _tmp103 + _tmp80 + 1; + const Scalar _tmp114 = _tmp94 + _tmp96; + const Scalar _tmp115 = -_tmp107; + const Scalar _tmp116 = _tmp108 + _tmp115; + const Scalar _tmp117 = _tmp114 * _tmp84 + _tmp116 * _tmp93; + const Scalar _tmp118 = -Dv(2, 0) - Dv_D_accel_bias(2, 0) * _tmp85 - + Dv_D_accel_bias(2, 1) * _tmp86 - Dv_D_accel_bias(2, 2) * _tmp87 - + Dv_D_gyro_bias(2, 0) * _tmp0 - Dv_D_gyro_bias(2, 1) * _tmp2 - + Dv_D_gyro_bias(2, 2) * _tmp1 + _tmp113 * _tmp98 + _tmp117; + const Scalar _tmp119 = _tmp100 * sqrt_info(5, 3) + _tmp111 * sqrt_info(5, 4) + + _tmp118 * sqrt_info(5, 5) + _tmp69 * sqrt_info(5, 1) + + _tmp70 * sqrt_info(5, 0) + _tmp77 * sqrt_info(5, 2); + const Scalar _tmp120 = (Scalar(1) / Scalar(2)) * std::pow(dt, Scalar(2)); + const Scalar _tmp121 = -_pose_i[4] + _pose_j[4] - _tmp120 * gravity(0, 0) - dt * vel_i(0, 0); + const Scalar _tmp122 = -_pose_i[6] + _pose_j[6] - _tmp120 * gravity(2, 0) - dt * vel_i(2, 0); + const Scalar _tmp123 = -_pose_i[5] + _pose_j[5] - _tmp120 * gravity(1, 0) - dt * vel_i(1, 0); + const Scalar _tmp124 = _tmp122 * _tmp97 + _tmp123 * _tmp92; + const Scalar _tmp125 = -Dp(0, 0) - Dp_D_accel_bias(0, 0) * _tmp85 - + Dp_D_accel_bias(0, 1) * _tmp86 - Dp_D_accel_bias(0, 2) * _tmp87 - + Dp_D_gyro_bias(0, 0) * _tmp0 - Dp_D_gyro_bias(0, 1) * _tmp2 - + Dp_D_gyro_bias(0, 2) * _tmp1 + _tmp121 * _tmp83 + _tmp124; + const Scalar _tmp126 = _tmp57 * sqrt_info(6, 2); + const Scalar _tmp127 = _tmp100 * sqrt_info(6, 3) + _tmp111 * sqrt_info(6, 4) + + _tmp118 * sqrt_info(6, 5) + _tmp125 * sqrt_info(6, 6) + + _tmp126 * _tmp61 * _tmp76 + _tmp69 * sqrt_info(6, 1) + + _tmp70 * sqrt_info(6, 0); + const Scalar _tmp128 = _tmp106 * _tmp121 + _tmp109 * _tmp122; + const Scalar _tmp129 = -Dp(1, 0) - Dp_D_accel_bias(1, 0) * _tmp85 - + Dp_D_accel_bias(1, 1) * _tmp86 - Dp_D_accel_bias(1, 2) * _tmp87 - + Dp_D_gyro_bias(1, 0) * _tmp0 - Dp_D_gyro_bias(1, 1) * _tmp2 - + Dp_D_gyro_bias(1, 2) * _tmp1 + _tmp104 * _tmp123 + _tmp128; + const Scalar _tmp130 = _tmp100 * sqrt_info(7, 3) + _tmp111 * sqrt_info(7, 4) + + _tmp118 * sqrt_info(7, 5) + _tmp125 * sqrt_info(7, 6) + + _tmp129 * sqrt_info(7, 7) + _tmp69 * sqrt_info(7, 1) + + _tmp70 * sqrt_info(7, 0) + _tmp77 * sqrt_info(7, 2); + const Scalar _tmp131 = _tmp114 * _tmp121 + _tmp116 * _tmp123; + const Scalar _tmp132 = + _tmp100 * sqrt_info(8, 3) + _tmp111 * sqrt_info(8, 4) + _tmp118 * sqrt_info(8, 5) + + _tmp125 * sqrt_info(8, 6) + _tmp129 * sqrt_info(8, 7) + _tmp69 * sqrt_info(8, 1) + + _tmp70 * sqrt_info(8, 0) + _tmp77 * sqrt_info(8, 2) + + sqrt_info(8, 8) * (-Dp(2, 0) - Dp_D_accel_bias(2, 0) * _tmp85 - + Dp_D_accel_bias(2, 1) * _tmp86 - Dp_D_accel_bias(2, 2) * _tmp87 - + Dp_D_gyro_bias(2, 0) * _tmp0 - Dp_D_gyro_bias(2, 1) * _tmp2 - + Dp_D_gyro_bias(2, 2) * _tmp1 + _tmp113 * _tmp122 + _tmp131); + const Scalar _tmp133 = (Scalar(1) / Scalar(2)) * _tmp38; + const Scalar _tmp134 = (Scalar(1) / Scalar(2)) * _tmp40; + const Scalar _tmp135 = (Scalar(1) / Scalar(2)) * _tmp37; + const Scalar _tmp136 = (Scalar(1) / Scalar(2)) * _tmp39; + const Scalar _tmp137 = _tmp135 - _tmp136; + const Scalar _tmp138 = _tmp133 + _tmp134 + _tmp137; + const Scalar _tmp139 = (Scalar(1) / Scalar(2)) * _tmp43; + const Scalar _tmp140 = (Scalar(1) / Scalar(2)) * _tmp46; + const Scalar _tmp141 = (Scalar(1) / Scalar(2)) * _tmp44; + const Scalar _tmp142 = (Scalar(1) / Scalar(2)) * _tmp45; + const Scalar _tmp143 = -_tmp141 + _tmp142; + const Scalar _tmp144 = _tmp139 + _tmp140 + _tmp143; + const Scalar _tmp145 = (Scalar(1) / Scalar(2)) * _tmp31; + const Scalar _tmp146 = -_tmp145; + const Scalar _tmp147 = (Scalar(1) / Scalar(2)) * _tmp32; + const Scalar _tmp148 = -_tmp147; + const Scalar _tmp149 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp150 = (Scalar(1) / Scalar(2)) * _tmp34; + const Scalar _tmp151 = _tmp146 + _tmp148 - _tmp149 + _tmp150; + const Scalar _tmp152 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp153 = -Scalar(1) / Scalar(2) * _tmp28; + const Scalar _tmp154 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp155 = -_tmp154; + const Scalar _tmp156 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp157 = -_tmp156; + const Scalar _tmp158 = _tmp152 + _tmp153 + _tmp155 + _tmp157; + const Scalar _tmp159 = -_tmp51 - _tmp52 - _tmp53 + _tmp55; + const Scalar _tmp160 = std::fabs(_tmp159); + const Scalar _tmp161 = std::min(_tmp160, _tmp59); + const Scalar _tmp162 = std::acos(_tmp161); + const Scalar _tmp163 = 1 - std::pow(_tmp161, Scalar(2)); + const Scalar _tmp164 = std::pow(_tmp163, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp165 = _tmp164 * _tmp57; + const Scalar _tmp166 = _tmp162 * _tmp165; + const Scalar _tmp167 = _tmp166 * (_pose_j[0] * _tmp151 - _pose_j[1] * _tmp138 + + _pose_j[2] * _tmp144 + _pose_j[3] * _tmp158); + const Scalar _tmp168 = + -_pose_j[0] * _tmp158 - _pose_j[1] * _tmp144 - _pose_j[2] * _tmp138 + _pose_j[3] * _tmp151; + const Scalar _tmp169 = (((_tmp159) > 0) - ((_tmp159) < 0)); + const Scalar _tmp170 = _tmp56 * ((((-_tmp160 + _tmp59) > 0) - ((-_tmp160 + _tmp59) < 0)) + 1); + const Scalar _tmp171 = _tmp170 / _tmp163; + const Scalar _tmp172 = _tmp169 * _tmp171; + const Scalar _tmp173 = _tmp168 * _tmp172; + const Scalar _tmp174 = std::pow(_tmp163, Scalar(Scalar(-3) / Scalar(2))); + const Scalar _tmp175 = _tmp161 * _tmp162 * _tmp170 * _tmp174; + const Scalar _tmp176 = _tmp169 * _tmp175; + const Scalar _tmp177 = _tmp168 * _tmp176; + const Scalar _tmp178 = _tmp167 * sqrt_info(0, 0) - _tmp173 * _tmp50 + _tmp177 * _tmp50; + const Scalar _tmp179 = _tmp68 * sqrt_info(1, 1); + const Scalar _tmp180 = _tmp166 * (_pose_j[0] * _tmp138 + _pose_j[1] * _tmp151 - + _pose_j[2] * _tmp158 + _pose_j[3] * _tmp144); + const Scalar _tmp181 = _tmp173 * _tmp49; + const Scalar _tmp182 = _tmp49 * sqrt_info(1, 0); + const Scalar _tmp183 = _tmp167 * sqrt_info(1, 0) - _tmp173 * _tmp179 + _tmp177 * _tmp179 + + _tmp177 * _tmp182 + _tmp180 * sqrt_info(1, 1) - _tmp181 * sqrt_info(1, 0); + const Scalar _tmp184 = _tmp68 * sqrt_info(2, 1); + const Scalar _tmp185 = + -_pose_j[0] * _tmp144 + _pose_j[1] * _tmp158 + _pose_j[2] * _tmp151 + _pose_j[3] * _tmp138; + const Scalar _tmp186 = _tmp166 * _tmp185; + const Scalar _tmp187 = _tmp49 * sqrt_info(2, 0); + const Scalar _tmp188 = _tmp172 * _tmp76; + const Scalar _tmp189 = _tmp168 * _tmp188; + const Scalar _tmp190 = _tmp177 * _tmp76; + const Scalar _tmp191 = _tmp167 * sqrt_info(2, 0) - _tmp173 * _tmp184 - _tmp173 * _tmp187 + + _tmp177 * _tmp184 + _tmp177 * _tmp187 + _tmp180 * sqrt_info(2, 1) + + _tmp186 * sqrt_info(2, 2) - _tmp189 * sqrt_info(2, 2) + + _tmp190 * sqrt_info(2, 2); + const Scalar _tmp192 = _tmp68 * sqrt_info(3, 1); + const Scalar _tmp193 = _tmp49 * sqrt_info(3, 0); + const Scalar _tmp194 = _tmp76 * sqrt_info(3, 2); + const Scalar _tmp195 = _tmp167 * sqrt_info(3, 0) - _tmp173 * _tmp192 - _tmp173 * _tmp193 + + _tmp177 * _tmp192 + _tmp177 * _tmp193 + _tmp177 * _tmp194 + + _tmp180 * sqrt_info(3, 1) + _tmp186 * sqrt_info(3, 2) - + _tmp189 * sqrt_info(3, 2); + const Scalar _tmp196 = _tmp68 * sqrt_info(4, 1); + const Scalar _tmp197 = -_tmp79; + const Scalar _tmp198 = std::pow(_pose_i[3], Scalar(2)); + const Scalar _tmp199 = _tmp197 + _tmp198; + const Scalar _tmp200 = -_tmp102; + const Scalar _tmp201 = _tmp200 + _tmp81; + const Scalar _tmp202 = _tmp199 + _tmp201; + const Scalar _tmp203 = _tmp117 + _tmp202 * _tmp98; + const Scalar _tmp204 = _tmp173 * _tmp68; + const Scalar _tmp205 = _tmp177 * _tmp49; + const Scalar _tmp206 = _tmp167 * sqrt_info(4, 0) + _tmp177 * _tmp196 + _tmp180 * sqrt_info(4, 1) - + _tmp181 * sqrt_info(4, 0) + _tmp186 * sqrt_info(4, 2) - + _tmp189 * sqrt_info(4, 2) + _tmp190 * sqrt_info(4, 2) + + _tmp203 * sqrt_info(4, 4) - _tmp204 * sqrt_info(4, 1) + + _tmp205 * sqrt_info(4, 0); + const Scalar _tmp207 = _tmp68 * sqrt_info(5, 1); + const Scalar _tmp208 = -_tmp91; + const Scalar _tmp209 = _tmp208 + _tmp89; + const Scalar _tmp210 = -_tmp198; + const Scalar _tmp211 = _tmp102 + _tmp197 + _tmp210 + _tmp81; + const Scalar _tmp212 = -_tmp108; + const Scalar _tmp213 = _tmp115 + _tmp212; + const Scalar _tmp214 = _tmp209 * _tmp84 + _tmp211 * _tmp93 + _tmp213 * _tmp98; + const Scalar _tmp215 = _tmp167 * sqrt_info(5, 0) + _tmp177 * _tmp207 + _tmp180 * sqrt_info(5, 1) - + _tmp181 * sqrt_info(5, 0) + _tmp186 * sqrt_info(5, 2) - + _tmp189 * sqrt_info(5, 2) + _tmp190 * sqrt_info(5, 2) + + _tmp203 * sqrt_info(5, 4) - _tmp204 * sqrt_info(5, 1) + + _tmp205 * sqrt_info(5, 0) + _tmp214 * sqrt_info(5, 5); + const Scalar _tmp216 = _tmp68 * sqrt_info(6, 1); + const Scalar _tmp217 = _tmp126 * _tmp164; + const Scalar _tmp218 = _tmp162 * _tmp185; + const Scalar _tmp219 = _tmp49 * sqrt_info(6, 0); + const Scalar _tmp220 = _tmp167 * sqrt_info(6, 0) - _tmp173 * _tmp216 - _tmp173 * _tmp219 + + _tmp177 * _tmp216 + _tmp177 * _tmp219 + _tmp180 * sqrt_info(6, 1) - + _tmp189 * sqrt_info(6, 2) + _tmp190 * sqrt_info(6, 2) + + _tmp203 * sqrt_info(6, 4) + _tmp214 * sqrt_info(6, 5) + _tmp217 * _tmp218; + const Scalar _tmp221 = _tmp68 * sqrt_info(7, 1); + const Scalar _tmp222 = _tmp122 * _tmp202 + _tmp131; + const Scalar _tmp223 = _tmp165 * sqrt_info(7, 2); + const Scalar _tmp224 = _tmp49 * sqrt_info(7, 0); + const Scalar _tmp225 = _tmp167 * sqrt_info(7, 0) - _tmp173 * _tmp221 + _tmp177 * _tmp221 + + _tmp177 * _tmp224 + _tmp180 * sqrt_info(7, 1) - _tmp181 * sqrt_info(7, 0) - + _tmp189 * sqrt_info(7, 2) + _tmp190 * sqrt_info(7, 2) + + _tmp203 * sqrt_info(7, 4) + _tmp214 * sqrt_info(7, 5) + _tmp218 * _tmp223 + + _tmp222 * sqrt_info(7, 7); + const Scalar _tmp226 = _tmp68 * sqrt_info(8, 1); + const Scalar _tmp227 = _tmp49 * sqrt_info(8, 0); + const Scalar _tmp228 = _tmp188 * sqrt_info(8, 2); + const Scalar _tmp229 = _tmp76 * sqrt_info(8, 2); + const Scalar _tmp230 = + _tmp167 * sqrt_info(8, 0) - _tmp168 * _tmp228 - _tmp173 * _tmp227 + _tmp177 * _tmp226 + + _tmp177 * _tmp227 + _tmp177 * _tmp229 + _tmp180 * sqrt_info(8, 1) + + _tmp186 * sqrt_info(8, 2) + _tmp203 * sqrt_info(8, 4) - _tmp204 * sqrt_info(8, 1) + + _tmp214 * sqrt_info(8, 5) + _tmp222 * sqrt_info(8, 7) + + sqrt_info(8, 8) * (_tmp121 * _tmp209 + _tmp122 * _tmp213 + _tmp123 * _tmp211); + const Scalar _tmp231 = -_tmp133; + const Scalar _tmp232 = -_tmp134 + _tmp137 + _tmp231; + const Scalar _tmp233 = -_tmp140; + const Scalar _tmp234 = _tmp139 + _tmp141 + _tmp142 + _tmp233; + const Scalar _tmp235 = _tmp149 + _tmp150; + const Scalar _tmp236 = _tmp145 + _tmp148 + _tmp235; + const Scalar _tmp237 = -_tmp152 + _tmp153; + const Scalar _tmp238 = _tmp154 + _tmp157 + _tmp237; + const Scalar _tmp239 = + -_pose_j[0] * _tmp234 - _pose_j[1] * _tmp238 - _pose_j[2] * _tmp236 + _pose_j[3] * _tmp232; + const Scalar _tmp240 = _tmp172 * _tmp239; + const Scalar _tmp241 = + _pose_j[0] * _tmp232 - _pose_j[1] * _tmp236 + _pose_j[2] * _tmp238 + _pose_j[3] * _tmp234; + const Scalar _tmp242 = _tmp166 * _tmp241; + const Scalar _tmp243 = _tmp176 * _tmp239; + const Scalar _tmp244 = -_tmp240 * _tmp50 + _tmp242 * sqrt_info(0, 0) + _tmp243 * _tmp50; + const Scalar _tmp245 = _tmp240 * _tmp68; + const Scalar _tmp246 = _tmp240 * _tmp49; + const Scalar _tmp247 = _tmp166 * (_pose_j[0] * _tmp236 + _pose_j[1] * _tmp232 - + _pose_j[2] * _tmp234 + _pose_j[3] * _tmp238); + const Scalar _tmp248 = _tmp179 * _tmp243 + _tmp182 * _tmp243 + _tmp242 * sqrt_info(1, 0) - + _tmp245 * sqrt_info(1, 1) - _tmp246 * sqrt_info(1, 0) + + _tmp247 * sqrt_info(1, 1); + const Scalar _tmp249 = + -_pose_j[0] * _tmp238 + _pose_j[1] * _tmp234 + _pose_j[2] * _tmp232 + _pose_j[3] * _tmp236; + const Scalar _tmp250 = _tmp166 * _tmp249; + const Scalar _tmp251 = _tmp166 * sqrt_info(2, 0); + const Scalar _tmp252 = _tmp188 * _tmp239; + const Scalar _tmp253 = _tmp243 * _tmp76; + const Scalar _tmp254 = _tmp184 * _tmp243 - _tmp187 * _tmp240 + _tmp187 * _tmp243 + + _tmp241 * _tmp251 - _tmp245 * sqrt_info(2, 1) + _tmp247 * sqrt_info(2, 1) + + _tmp250 * sqrt_info(2, 2) - _tmp252 * sqrt_info(2, 2) + + _tmp253 * sqrt_info(2, 2); + const Scalar _tmp255 = -_tmp96; + const Scalar _tmp256 = _tmp255 + _tmp95; + const Scalar _tmp257 = -_tmp81; + const Scalar _tmp258 = _tmp102 + _tmp257; + const Scalar _tmp259 = _tmp210 + _tmp79; + const Scalar _tmp260 = _tmp258 + _tmp259; + const Scalar _tmp261 = _tmp107 + _tmp212; + const Scalar _tmp262 = _tmp256 * _tmp84 + _tmp260 * _tmp98 + _tmp261 * _tmp93; + const Scalar _tmp263 = -_tmp192 * _tmp240 + _tmp192 * _tmp243 + _tmp193 * _tmp243 + + _tmp194 * _tmp243 + _tmp242 * sqrt_info(3, 0) - _tmp246 * sqrt_info(3, 0) + + _tmp247 * sqrt_info(3, 1) + _tmp250 * sqrt_info(3, 2) - + _tmp252 * sqrt_info(3, 2) + _tmp262 * sqrt_info(3, 3); + const Scalar _tmp264 = _tmp49 * sqrt_info(4, 0); + const Scalar _tmp265 = _tmp196 * _tmp243 + _tmp242 * sqrt_info(4, 0) + _tmp243 * _tmp264 - + _tmp245 * sqrt_info(4, 1) - _tmp246 * sqrt_info(4, 0) + + _tmp247 * sqrt_info(4, 1) + _tmp250 * sqrt_info(4, 2) - + _tmp252 * sqrt_info(4, 2) + _tmp253 * sqrt_info(4, 2) + + _tmp262 * sqrt_info(4, 3); + const Scalar _tmp266 = _tmp199 + _tmp258; + const Scalar _tmp267 = _tmp266 * _tmp84 + _tmp99; + const Scalar _tmp268 = _tmp49 * sqrt_info(5, 0); + const Scalar _tmp269 = _tmp207 * _tmp243 + _tmp242 * sqrt_info(5, 0) + _tmp243 * _tmp268 - + _tmp245 * sqrt_info(5, 1) - _tmp246 * sqrt_info(5, 0) + + _tmp247 * sqrt_info(5, 1) + _tmp250 * sqrt_info(5, 2) - + _tmp252 * sqrt_info(5, 2) + _tmp253 * sqrt_info(5, 2) + + _tmp262 * sqrt_info(5, 3) + _tmp267 * sqrt_info(5, 5); + const Scalar _tmp270 = _tmp121 * _tmp256 + _tmp122 * _tmp260 + _tmp123 * _tmp261; + const Scalar _tmp271 = _tmp162 * _tmp249; + const Scalar _tmp272 = _tmp216 * _tmp243 + _tmp217 * _tmp271 - _tmp219 * _tmp240 + + _tmp219 * _tmp243 + _tmp242 * sqrt_info(6, 0) - _tmp245 * sqrt_info(6, 1) + + _tmp247 * sqrt_info(6, 1) - _tmp252 * sqrt_info(6, 2) + + _tmp253 * sqrt_info(6, 2) + _tmp262 * sqrt_info(6, 3) + + _tmp267 * sqrt_info(6, 5) + _tmp270 * sqrt_info(6, 6); + const Scalar _tmp273 = -_tmp221 * _tmp240 + _tmp221 * _tmp243 + _tmp223 * _tmp271 + + _tmp224 * _tmp243 + _tmp242 * sqrt_info(7, 0) - _tmp246 * sqrt_info(7, 0) + + _tmp247 * sqrt_info(7, 1) - _tmp252 * sqrt_info(7, 2) + + _tmp253 * sqrt_info(7, 2) + _tmp262 * sqrt_info(7, 3) + + _tmp267 * sqrt_info(7, 5) + _tmp270 * sqrt_info(7, 6); + const Scalar _tmp274 = _tmp226 * _tmp243 - _tmp227 * _tmp240 + _tmp227 * _tmp243 - + _tmp228 * _tmp239 + _tmp229 * _tmp243 + _tmp242 * sqrt_info(8, 0) - + _tmp245 * sqrt_info(8, 1) + _tmp247 * sqrt_info(8, 1) + + _tmp250 * sqrt_info(8, 2) + _tmp262 * sqrt_info(8, 3) + + _tmp267 * sqrt_info(8, 5) + _tmp270 * sqrt_info(8, 6) + + sqrt_info(8, 8) * (_tmp121 * _tmp266 + _tmp124); + const Scalar _tmp275 = _tmp134 + _tmp135 + _tmp136 + _tmp231; + const Scalar _tmp276 = -_tmp139 + _tmp143 + _tmp233; + const Scalar _tmp277 = _tmp146 + _tmp147 + _tmp235; + const Scalar _tmp278 = _tmp155 + _tmp156 + _tmp237; + const Scalar _tmp279 = + -_pose_j[0] * _tmp275 - _pose_j[1] * _tmp277 - _pose_j[2] * _tmp278 + _pose_j[3] * _tmp276; + const Scalar _tmp280 = _tmp176 * _tmp279; + const Scalar _tmp281 = _tmp172 * _tmp279; + const Scalar _tmp282 = _tmp166 * (_pose_j[0] * _tmp276 - _pose_j[1] * _tmp278 + + _pose_j[2] * _tmp277 + _pose_j[3] * _tmp275); + const Scalar _tmp283 = _tmp280 * _tmp50 - _tmp281 * _tmp50 + _tmp282 * sqrt_info(0, 0); + const Scalar _tmp284 = _tmp281 * _tmp49; + const Scalar _tmp285 = _tmp166 * (_pose_j[0] * _tmp278 + _pose_j[1] * _tmp276 - + _pose_j[2] * _tmp275 + _pose_j[3] * _tmp277); + const Scalar _tmp286 = _tmp179 * _tmp280 - _tmp179 * _tmp281 + _tmp182 * _tmp280 + + _tmp282 * sqrt_info(1, 0) - _tmp284 * sqrt_info(1, 0) + + _tmp285 * sqrt_info(1, 1); + const Scalar _tmp287 = _tmp188 * _tmp279; + const Scalar _tmp288 = _tmp281 * _tmp68; + const Scalar _tmp289 = _tmp280 * _tmp76; + const Scalar _tmp290 = _tmp162 * (-_pose_j[0] * _tmp277 + _pose_j[1] * _tmp275 + + _pose_j[2] * _tmp276 + _pose_j[3] * _tmp278); + const Scalar _tmp291 = _tmp165 * _tmp290; + const Scalar _tmp292 = _tmp184 * _tmp280 + _tmp187 * _tmp280 - _tmp187 * _tmp281 + + _tmp282 * sqrt_info(2, 0) + _tmp285 * sqrt_info(2, 1) - + _tmp287 * sqrt_info(2, 2) - _tmp288 * sqrt_info(2, 1) + + _tmp289 * sqrt_info(2, 2) + _tmp291 * sqrt_info(2, 2); + const Scalar _tmp293 = _tmp198 + _tmp200 + _tmp257 + _tmp79; + const Scalar _tmp294 = _tmp110 + _tmp293 * _tmp93; + const Scalar _tmp295 = _tmp192 * _tmp280 - _tmp192 * _tmp281 + _tmp193 * _tmp280 + + _tmp194 * _tmp280 + _tmp282 * sqrt_info(3, 0) - _tmp284 * sqrt_info(3, 0) + + _tmp285 * sqrt_info(3, 1) - _tmp287 * sqrt_info(3, 2) + + _tmp291 * sqrt_info(3, 2) + _tmp294 * sqrt_info(3, 3); + const Scalar _tmp296 = _tmp280 * _tmp49; + const Scalar _tmp297 = _tmp105 + _tmp208; + const Scalar _tmp298 = _tmp255 + _tmp94; + const Scalar _tmp299 = _tmp201 + _tmp259; + const Scalar _tmp300 = _tmp297 * _tmp93 + _tmp298 * _tmp98 + _tmp299 * _tmp84; + const Scalar _tmp301 = _tmp196 * _tmp280 + _tmp282 * sqrt_info(4, 0) - _tmp284 * sqrt_info(4, 0) + + _tmp285 * sqrt_info(4, 1) - _tmp287 * sqrt_info(4, 2) - + _tmp288 * sqrt_info(4, 1) + _tmp289 * sqrt_info(4, 2) + + _tmp291 * sqrt_info(4, 2) + _tmp294 * sqrt_info(4, 3) + + _tmp296 * sqrt_info(4, 0) + _tmp300 * sqrt_info(4, 4); + const Scalar _tmp302 = _tmp207 * _tmp280 + _tmp282 * sqrt_info(5, 0) - _tmp284 * sqrt_info(5, 0) + + _tmp285 * sqrt_info(5, 1) - _tmp287 * sqrt_info(5, 2) - + _tmp288 * sqrt_info(5, 1) + _tmp289 * sqrt_info(5, 2) + + _tmp291 * sqrt_info(5, 2) + _tmp294 * sqrt_info(5, 3) + + _tmp296 * sqrt_info(5, 0) + _tmp300 * sqrt_info(5, 4); + const Scalar _tmp303 = _tmp123 * _tmp293 + _tmp128; + const Scalar _tmp304 = _tmp216 * _tmp280 + _tmp217 * _tmp290 + _tmp219 * _tmp280 - + _tmp219 * _tmp281 + _tmp282 * sqrt_info(6, 0) + _tmp285 * sqrt_info(6, 1) - + _tmp287 * sqrt_info(6, 2) - _tmp288 * sqrt_info(6, 1) + + _tmp289 * sqrt_info(6, 2) + _tmp294 * sqrt_info(6, 3) + + _tmp300 * sqrt_info(6, 4) + _tmp303 * sqrt_info(6, 6); + const Scalar _tmp305 = _tmp121 * _tmp299 + _tmp122 * _tmp298 + _tmp123 * _tmp297; + const Scalar _tmp306 = + _tmp221 * _tmp280 - _tmp221 * _tmp281 + _tmp223 * _tmp290 + _tmp224 * _tmp280 + + _tmp282 * sqrt_info(7, 0) - _tmp284 * sqrt_info(7, 0) + _tmp285 * sqrt_info(7, 1) - + _tmp287 * sqrt_info(7, 2) + _tmp289 * sqrt_info(7, 2) + _tmp294 * sqrt_info(7, 3) + + _tmp300 * sqrt_info(7, 4) + _tmp303 * sqrt_info(7, 6) + _tmp305 * sqrt_info(7, 7); + const Scalar _tmp307 = + _tmp226 * _tmp280 + _tmp227 * _tmp280 - _tmp227 * _tmp281 + _tmp229 * _tmp280 + + _tmp282 * sqrt_info(8, 0) + _tmp285 * sqrt_info(8, 1) - _tmp287 * sqrt_info(8, 2) - + _tmp288 * sqrt_info(8, 1) + _tmp291 * sqrt_info(8, 2) + _tmp294 * sqrt_info(8, 3) + + _tmp300 * sqrt_info(8, 4) + _tmp303 * sqrt_info(8, 6) + _tmp305 * sqrt_info(8, 7); + const Scalar _tmp308 = _tmp83 * sqrt_info(6, 6); + const Scalar _tmp309 = _tmp106 * sqrt_info(7, 7); + const Scalar _tmp310 = _tmp83 * sqrt_info(7, 6); + const Scalar _tmp311 = -_tmp309 - _tmp310; + const Scalar _tmp312 = _tmp106 * sqrt_info(8, 7); + const Scalar _tmp313 = _tmp83 * sqrt_info(8, 6); + const Scalar _tmp314 = _tmp114 * sqrt_info(8, 8); + const Scalar _tmp315 = -_tmp312 - _tmp313 - _tmp314; + const Scalar _tmp316 = _tmp92 * sqrt_info(6, 6); + const Scalar _tmp317 = _tmp92 * sqrt_info(7, 6); + const Scalar _tmp318 = _tmp104 * sqrt_info(7, 7); + const Scalar _tmp319 = -_tmp317 - _tmp318; + const Scalar _tmp320 = _tmp116 * sqrt_info(8, 8); + const Scalar _tmp321 = _tmp92 * sqrt_info(8, 6); + const Scalar _tmp322 = _tmp104 * sqrt_info(8, 7); + const Scalar _tmp323 = -_tmp320 - _tmp321 - _tmp322; + const Scalar _tmp324 = _tmp97 * sqrt_info(6, 6); + const Scalar _tmp325 = _tmp109 * sqrt_info(7, 7); + const Scalar _tmp326 = _tmp97 * sqrt_info(7, 6); + const Scalar _tmp327 = -_tmp325 - _tmp326; + const Scalar _tmp328 = _tmp109 * sqrt_info(8, 7); + const Scalar _tmp329 = _tmp113 * sqrt_info(8, 8); + const Scalar _tmp330 = _tmp97 * sqrt_info(8, 6); + const Scalar _tmp331 = -_tmp328 - _tmp329 - _tmp330; + const Scalar _tmp332 = _tmp83 * sqrt_info(3, 3); + const Scalar _tmp333 = _tmp106 * sqrt_info(4, 4); + const Scalar _tmp334 = _tmp83 * sqrt_info(4, 3); + const Scalar _tmp335 = -_tmp333 - _tmp334; + const Scalar _tmp336 = _tmp106 * sqrt_info(5, 4); + const Scalar _tmp337 = _tmp83 * sqrt_info(5, 3); + const Scalar _tmp338 = _tmp114 * sqrt_info(5, 5); + const Scalar _tmp339 = -_tmp336 - _tmp337 - _tmp338; + const Scalar _tmp340 = _tmp106 * sqrt_info(6, 4); + const Scalar _tmp341 = _tmp83 * sqrt_info(6, 3); + const Scalar _tmp342 = _tmp114 * sqrt_info(6, 5); + const Scalar _tmp343 = -_tmp308 * dt - _tmp340 - _tmp341 - _tmp342; + const Scalar _tmp344 = _tmp106 * sqrt_info(7, 4); + const Scalar _tmp345 = _tmp83 * sqrt_info(7, 3); + const Scalar _tmp346 = _tmp114 * sqrt_info(7, 5); + const Scalar _tmp347 = -_tmp309 * dt - _tmp310 * dt - _tmp344 - _tmp345 - _tmp346; + const Scalar _tmp348 = _tmp106 * sqrt_info(8, 4); + const Scalar _tmp349 = _tmp83 * sqrt_info(8, 3); + const Scalar _tmp350 = _tmp114 * sqrt_info(8, 5); + const Scalar _tmp351 = -_tmp312 * dt - _tmp313 * dt - _tmp314 * dt - _tmp348 - _tmp349 - _tmp350; + const Scalar _tmp352 = _tmp92 * sqrt_info(3, 3); + const Scalar _tmp353 = _tmp92 * sqrt_info(4, 3); + const Scalar _tmp354 = _tmp104 * sqrt_info(4, 4); + const Scalar _tmp355 = -_tmp353 - _tmp354; + const Scalar _tmp356 = _tmp116 * sqrt_info(5, 5); + const Scalar _tmp357 = _tmp92 * sqrt_info(5, 3); + const Scalar _tmp358 = _tmp104 * sqrt_info(5, 4); + const Scalar _tmp359 = -_tmp356 - _tmp357 - _tmp358; + const Scalar _tmp360 = _tmp116 * sqrt_info(6, 5); + const Scalar _tmp361 = _tmp92 * sqrt_info(6, 3); + const Scalar _tmp362 = _tmp104 * sqrt_info(6, 4); + const Scalar _tmp363 = -_tmp316 * dt - _tmp360 - _tmp361 - _tmp362; + const Scalar _tmp364 = _tmp116 * sqrt_info(7, 5); + const Scalar _tmp365 = _tmp92 * sqrt_info(7, 3); + const Scalar _tmp366 = _tmp104 * sqrt_info(7, 4); + const Scalar _tmp367 = -_tmp317 * dt - _tmp318 * dt - _tmp364 - _tmp365 - _tmp366; + const Scalar _tmp368 = _tmp116 * sqrt_info(8, 5); + const Scalar _tmp369 = _tmp92 * sqrt_info(8, 3); + const Scalar _tmp370 = _tmp104 * sqrt_info(8, 4); + const Scalar _tmp371 = -_tmp320 * dt - _tmp321 * dt - _tmp322 * dt - _tmp368 - _tmp369 - _tmp370; + const Scalar _tmp372 = _tmp97 * sqrt_info(3, 3); + const Scalar _tmp373 = _tmp109 * sqrt_info(4, 4); + const Scalar _tmp374 = _tmp97 * sqrt_info(4, 3); + const Scalar _tmp375 = -_tmp373 - _tmp374; + const Scalar _tmp376 = _tmp109 * sqrt_info(5, 4); + const Scalar _tmp377 = _tmp113 * sqrt_info(5, 5); + const Scalar _tmp378 = _tmp97 * sqrt_info(5, 3); + const Scalar _tmp379 = -_tmp376 - _tmp377 - _tmp378; + const Scalar _tmp380 = _tmp109 * sqrt_info(6, 4); + const Scalar _tmp381 = _tmp113 * sqrt_info(6, 5); + const Scalar _tmp382 = _tmp97 * sqrt_info(6, 3); + const Scalar _tmp383 = -_tmp324 * dt - _tmp380 - _tmp381 - _tmp382; + const Scalar _tmp384 = _tmp109 * sqrt_info(7, 4); + const Scalar _tmp385 = _tmp113 * sqrt_info(7, 5); + const Scalar _tmp386 = _tmp97 * sqrt_info(7, 3); + const Scalar _tmp387 = -_tmp325 * dt - _tmp326 * dt - _tmp384 - _tmp385 - _tmp386; + const Scalar _tmp388 = _tmp109 * sqrt_info(8, 4); + const Scalar _tmp389 = _tmp113 * sqrt_info(8, 5); + const Scalar _tmp390 = _tmp97 * sqrt_info(8, 3); + const Scalar _tmp391 = -_tmp328 * dt - _tmp329 * dt - _tmp330 * dt - _tmp388 - _tmp389 - _tmp390; + const Scalar _tmp392 = (Scalar(1) / Scalar(2)) * _tmp30; + const Scalar _tmp393 = (Scalar(1) / Scalar(2)) * _tmp36; + const Scalar _tmp394 = (Scalar(1) / Scalar(2)) * _tmp42; + const Scalar _tmp395 = (Scalar(1) / Scalar(2)) * _tmp48; + const Scalar _tmp396 = -_tmp392 - _tmp393 - _tmp394 + _tmp395; + const Scalar _tmp397 = _tmp176 * _tmp396; + const Scalar _tmp398 = _tmp172 * _tmp396; + const Scalar _tmp399 = -Scalar(1) / Scalar(2) * _tmp51 - Scalar(1) / Scalar(2) * _tmp52 - + Scalar(1) / Scalar(2) * _tmp53 + (Scalar(1) / Scalar(2)) * _tmp55; + const Scalar _tmp400 = _tmp166 * _tmp399; + const Scalar _tmp401 = _tmp397 * _tmp50 - _tmp398 * _tmp50 + _tmp400 * sqrt_info(0, 0); + const Scalar _tmp402 = (Scalar(1) / Scalar(2)) * _tmp72; + const Scalar _tmp403 = (Scalar(1) / Scalar(2)) * _tmp73; + const Scalar _tmp404 = (Scalar(1) / Scalar(2)) * _tmp74; + const Scalar _tmp405 = (Scalar(1) / Scalar(2)) * _tmp75; + const Scalar _tmp406 = _tmp166 * (_tmp402 + _tmp403 - _tmp404 + _tmp405); + const Scalar _tmp407 = _tmp398 * _tmp49; + const Scalar _tmp408 = _tmp398 * _tmp68; + const Scalar _tmp409 = _tmp179 * _tmp397 + _tmp182 * _tmp397 + _tmp400 * sqrt_info(1, 0) + + _tmp406 * sqrt_info(1, 1) - _tmp407 * sqrt_info(1, 0) - + _tmp408 * sqrt_info(1, 1); + const Scalar _tmp410 = _tmp176 * _tmp76; + const Scalar _tmp411 = _tmp396 * _tmp410; + const Scalar _tmp412 = (Scalar(1) / Scalar(2)) * _tmp64; + const Scalar _tmp413 = (Scalar(1) / Scalar(2)) * _tmp65; + const Scalar _tmp414 = (Scalar(1) / Scalar(2)) * _tmp66; + const Scalar _tmp415 = (Scalar(1) / Scalar(2)) * _tmp67; + const Scalar _tmp416 = -_tmp412 + _tmp413 - _tmp414 - _tmp415; + const Scalar _tmp417 = _tmp162 * _tmp416; + const Scalar _tmp418 = _tmp165 * _tmp417; + const Scalar _tmp419 = _tmp188 * _tmp396; + const Scalar _tmp420 = _tmp184 * _tmp397 + _tmp187 * _tmp397 - _tmp187 * _tmp398 + + _tmp400 * sqrt_info(2, 0) + _tmp406 * sqrt_info(2, 1) - + _tmp408 * sqrt_info(2, 1) + _tmp411 * sqrt_info(2, 2) + + _tmp418 * sqrt_info(2, 2) - _tmp419 * sqrt_info(2, 2); + const Scalar _tmp421 = _tmp176 * _tmp194; + const Scalar _tmp422 = _tmp192 * _tmp397 - _tmp192 * _tmp398 + _tmp193 * _tmp397 + + _tmp396 * _tmp421 + _tmp400 * sqrt_info(3, 0) + _tmp406 * sqrt_info(3, 1) - + _tmp407 * sqrt_info(3, 0) + _tmp418 * sqrt_info(3, 2) - + _tmp419 * sqrt_info(3, 2); + const Scalar _tmp423 = _tmp196 * _tmp397 + _tmp264 * _tmp397 + _tmp400 * sqrt_info(4, 0) + + _tmp406 * sqrt_info(4, 1) - _tmp407 * sqrt_info(4, 0) - + _tmp408 * sqrt_info(4, 1) + _tmp411 * sqrt_info(4, 2) + + _tmp418 * sqrt_info(4, 2) - _tmp419 * sqrt_info(4, 2); + const Scalar _tmp424 = _tmp410 * sqrt_info(5, 2); + const Scalar _tmp425 = _tmp207 * _tmp397 + _tmp268 * _tmp397 + _tmp396 * _tmp424 + + _tmp400 * sqrt_info(5, 0) + _tmp406 * sqrt_info(5, 1) - + _tmp407 * sqrt_info(5, 0) - _tmp408 * sqrt_info(5, 1) + + _tmp418 * sqrt_info(5, 2) - _tmp419 * sqrt_info(5, 2); + const Scalar _tmp426 = _tmp216 * _tmp397 + _tmp217 * _tmp417 + _tmp219 * _tmp397 - + _tmp219 * _tmp398 + _tmp400 * sqrt_info(6, 0) + _tmp406 * sqrt_info(6, 1) - + _tmp408 * sqrt_info(6, 1) + _tmp411 * sqrt_info(6, 2) - + _tmp419 * sqrt_info(6, 2); + const Scalar _tmp427 = _tmp221 * _tmp397 - _tmp221 * _tmp398 + _tmp223 * _tmp417 + + _tmp224 * _tmp397 + _tmp400 * sqrt_info(7, 0) + _tmp406 * sqrt_info(7, 1) - + _tmp407 * sqrt_info(7, 0) + _tmp411 * sqrt_info(7, 2) - + _tmp419 * sqrt_info(7, 2); + const Scalar _tmp428 = _tmp176 * _tmp229; + const Scalar _tmp429 = _tmp226 * _tmp397 + _tmp227 * _tmp397 - _tmp227 * _tmp398 - + _tmp228 * _tmp396 + _tmp396 * _tmp428 + _tmp400 * sqrt_info(8, 0) + + _tmp406 * sqrt_info(8, 1) - _tmp408 * sqrt_info(8, 1) + + _tmp418 * sqrt_info(8, 2); + const Scalar _tmp430 = _tmp161 * _tmp169 * _tmp170 * _tmp174 * _tmp417; + const Scalar _tmp431 = _tmp172 * _tmp416; + const Scalar _tmp432 = -_tmp402 - _tmp403 + _tmp404 - _tmp405; + const Scalar _tmp433 = _tmp166 * _tmp432; + const Scalar _tmp434 = _tmp430 * _tmp50 - _tmp431 * _tmp50 + _tmp433 * sqrt_info(0, 0); + const Scalar _tmp435 = _tmp431 * _tmp49; + const Scalar _tmp436 = _tmp432 * sqrt_info(1, 0); + const Scalar _tmp437 = _tmp166 * _tmp436 + _tmp179 * _tmp430 - _tmp179 * _tmp431 + + _tmp182 * _tmp430 + _tmp400 * sqrt_info(1, 1) - _tmp435 * sqrt_info(1, 0); + const Scalar _tmp438 = _tmp188 * _tmp416; + const Scalar _tmp439 = _tmp430 * _tmp76; + const Scalar _tmp440 = _tmp431 * _tmp68; + const Scalar _tmp441 = _tmp392 + _tmp393 + _tmp394 - _tmp395; + const Scalar _tmp442 = _tmp166 * _tmp441; + const Scalar _tmp443 = _tmp184 * _tmp430 + _tmp187 * _tmp430 - _tmp187 * _tmp431 + + _tmp400 * sqrt_info(2, 1) + _tmp433 * sqrt_info(2, 0) - + _tmp438 * sqrt_info(2, 2) + _tmp439 * sqrt_info(2, 2) - + _tmp440 * sqrt_info(2, 1) + _tmp442 * sqrt_info(2, 2); + const Scalar _tmp444 = _tmp192 * _tmp430 - _tmp192 * _tmp431 + _tmp193 * _tmp430 - + _tmp193 * _tmp431 + _tmp194 * _tmp430 + _tmp400 * sqrt_info(3, 1) + + _tmp433 * sqrt_info(3, 0) - _tmp438 * sqrt_info(3, 2) + + _tmp442 * sqrt_info(3, 2); + const Scalar _tmp445 = _tmp430 * _tmp49; + const Scalar _tmp446 = _tmp196 * _tmp430 + _tmp400 * sqrt_info(4, 1) + _tmp433 * sqrt_info(4, 0) - + _tmp435 * sqrt_info(4, 0) - _tmp438 * sqrt_info(4, 2) + + _tmp439 * sqrt_info(4, 2) - _tmp440 * sqrt_info(4, 1) + + _tmp442 * sqrt_info(4, 2) + _tmp445 * sqrt_info(4, 0); + const Scalar _tmp447 = _tmp207 * _tmp430 + _tmp400 * sqrt_info(5, 1) + _tmp433 * sqrt_info(5, 0) - + _tmp435 * sqrt_info(5, 0) - _tmp438 * sqrt_info(5, 2) + + _tmp439 * sqrt_info(5, 2) - _tmp440 * sqrt_info(5, 1) + + _tmp442 * sqrt_info(5, 2) + _tmp445 * sqrt_info(5, 0); + const Scalar _tmp448 = _tmp162 * _tmp441; + const Scalar _tmp449 = _tmp216 * _tmp430 - _tmp216 * _tmp431 + _tmp217 * _tmp448 + + _tmp219 * _tmp430 - _tmp219 * _tmp431 + _tmp400 * sqrt_info(6, 1) + + _tmp433 * sqrt_info(6, 0) - _tmp438 * sqrt_info(6, 2) + + _tmp439 * sqrt_info(6, 2); + const Scalar _tmp450 = _tmp221 * _tmp430 - _tmp221 * _tmp431 + _tmp223 * _tmp448 + + _tmp400 * sqrt_info(7, 1) + _tmp433 * sqrt_info(7, 0) - + _tmp435 * sqrt_info(7, 0) - _tmp438 * sqrt_info(7, 2) + + _tmp439 * sqrt_info(7, 2) + _tmp445 * sqrt_info(7, 0); + const Scalar _tmp451 = _tmp226 * _tmp430 + _tmp227 * _tmp430 - _tmp227 * _tmp431 + + _tmp229 * _tmp430 + _tmp400 * sqrt_info(8, 1) + _tmp433 * sqrt_info(8, 0) - + _tmp438 * sqrt_info(8, 2) - _tmp440 * sqrt_info(8, 1) + + _tmp442 * sqrt_info(8, 2); + const Scalar _tmp452 = _tmp166 * (_tmp412 - _tmp413 + _tmp414 + _tmp415); + const Scalar _tmp453 = _tmp172 * _tmp432; + const Scalar _tmp454 = _tmp176 * _tmp432; + const Scalar _tmp455 = _tmp452 * sqrt_info(0, 0) - _tmp453 * _tmp50 + _tmp454 * _tmp50; + const Scalar _tmp456 = _tmp436 * _tmp49; + const Scalar _tmp457 = _tmp166 * _tmp396; + const Scalar _tmp458 = _tmp453 * _tmp68; + const Scalar _tmp459 = -_tmp172 * _tmp456 + _tmp176 * _tmp456 + _tmp179 * _tmp454 + + _tmp452 * sqrt_info(1, 0) + _tmp457 * sqrt_info(1, 1) - + _tmp458 * sqrt_info(1, 1); + const Scalar _tmp460 = _tmp453 * _tmp76; + const Scalar _tmp461 = _tmp410 * _tmp432; + const Scalar _tmp462 = _tmp184 * _tmp454 - _tmp187 * _tmp453 + _tmp187 * _tmp454 + + _tmp400 * sqrt_info(2, 2) + _tmp452 * sqrt_info(2, 0) + + _tmp457 * sqrt_info(2, 1) - _tmp458 * sqrt_info(2, 1) - + _tmp460 * sqrt_info(2, 2) + _tmp461 * sqrt_info(2, 2); + const Scalar _tmp463 = _tmp192 * _tmp454 - _tmp193 * _tmp453 + _tmp193 * _tmp454 - + _tmp194 * _tmp453 + _tmp400 * sqrt_info(3, 2) + _tmp421 * _tmp432 + + _tmp452 * sqrt_info(3, 0) + _tmp457 * sqrt_info(3, 1) - + _tmp458 * sqrt_info(3, 1); + const Scalar _tmp464 = _tmp453 * _tmp49; + const Scalar _tmp465 = _tmp196 * _tmp454 + _tmp264 * _tmp454 + _tmp400 * sqrt_info(4, 2) + + _tmp452 * sqrt_info(4, 0) + _tmp457 * sqrt_info(4, 1) - + _tmp458 * sqrt_info(4, 1) - _tmp460 * sqrt_info(4, 2) + + _tmp461 * sqrt_info(4, 2) - _tmp464 * sqrt_info(4, 0); + const Scalar _tmp466 = _tmp207 * _tmp454 + _tmp268 * _tmp454 + _tmp400 * sqrt_info(5, 2) + + _tmp424 * _tmp432 + _tmp452 * sqrt_info(5, 0) + _tmp457 * sqrt_info(5, 1) - + _tmp458 * sqrt_info(5, 1) - _tmp460 * sqrt_info(5, 2) - + _tmp464 * sqrt_info(5, 0); + const Scalar _tmp467 = _tmp162 * _tmp399; + const Scalar _tmp468 = _tmp216 * _tmp454 + _tmp217 * _tmp467 - _tmp219 * _tmp453 + + _tmp219 * _tmp454 + _tmp452 * sqrt_info(6, 0) + _tmp457 * sqrt_info(6, 1) - + _tmp458 * sqrt_info(6, 1) - _tmp460 * sqrt_info(6, 2) + + _tmp461 * sqrt_info(6, 2); + const Scalar _tmp469 = -_tmp221 * _tmp453 + _tmp221 * _tmp454 + _tmp223 * _tmp467 + + _tmp224 * _tmp454 + _tmp452 * sqrt_info(7, 0) + _tmp457 * sqrt_info(7, 1) - + _tmp460 * sqrt_info(7, 2) + _tmp461 * sqrt_info(7, 2) - + _tmp464 * sqrt_info(7, 0); + const Scalar _tmp470 = _tmp226 * _tmp454 - _tmp227 * _tmp453 + _tmp227 * _tmp454 + + _tmp400 * sqrt_info(8, 2) + _tmp428 * _tmp432 + _tmp452 * sqrt_info(8, 0) + + _tmp457 * sqrt_info(8, 1) - _tmp458 * sqrt_info(8, 1) - + _tmp460 * sqrt_info(8, 2); + const Scalar _tmp471 = _tmp309 + _tmp310; + const Scalar _tmp472 = _tmp312 + _tmp313 + _tmp314; + const Scalar _tmp473 = _tmp317 + _tmp318; + const Scalar _tmp474 = _tmp320 + _tmp321 + _tmp322; + const Scalar _tmp475 = _tmp325 + _tmp326; + const Scalar _tmp476 = _tmp328 + _tmp329 + _tmp330; + const Scalar _tmp477 = _tmp333 + _tmp334; + const Scalar _tmp478 = _tmp336 + _tmp337 + _tmp338; + const Scalar _tmp479 = _tmp340 + _tmp341 + _tmp342; + const Scalar _tmp480 = _tmp344 + _tmp345 + _tmp346; + const Scalar _tmp481 = _tmp348 + _tmp349 + _tmp350; + const Scalar _tmp482 = _tmp353 + _tmp354; + const Scalar _tmp483 = _tmp356 + _tmp357 + _tmp358; + const Scalar _tmp484 = _tmp360 + _tmp361 + _tmp362; + const Scalar _tmp485 = _tmp364 + _tmp365 + _tmp366; + const Scalar _tmp486 = _tmp368 + _tmp369 + _tmp370; + const Scalar _tmp487 = _tmp373 + _tmp374; + const Scalar _tmp488 = _tmp376 + _tmp377 + _tmp378; + const Scalar _tmp489 = _tmp380 + _tmp381 + _tmp382; + const Scalar _tmp490 = _tmp384 + _tmp385 + _tmp386; + const Scalar _tmp491 = _tmp388 + _tmp389 + _tmp390; + const Scalar _tmp492 = Dv_D_accel_bias(0, 0) * sqrt_info(3, 3); + const Scalar _tmp493 = + -Dv_D_accel_bias(0, 0) * sqrt_info(4, 3) - Dv_D_accel_bias(1, 0) * sqrt_info(4, 4); + const Scalar _tmp494 = -Dv_D_accel_bias(0, 0) * sqrt_info(5, 3) - + Dv_D_accel_bias(1, 0) * sqrt_info(5, 4) - + Dv_D_accel_bias(2, 0) * sqrt_info(5, 5); + const Scalar _tmp495 = + -Dp_D_accel_bias(0, 0) * sqrt_info(6, 6) - Dv_D_accel_bias(0, 0) * sqrt_info(6, 3) - + Dv_D_accel_bias(1, 0) * sqrt_info(6, 4) - Dv_D_accel_bias(2, 0) * sqrt_info(6, 5); + const Scalar _tmp496 = + -Dp_D_accel_bias(0, 0) * sqrt_info(7, 6) - Dp_D_accel_bias(1, 0) * sqrt_info(7, 7) - + Dv_D_accel_bias(0, 0) * sqrt_info(7, 3) - Dv_D_accel_bias(1, 0) * sqrt_info(7, 4) - + Dv_D_accel_bias(2, 0) * sqrt_info(7, 5); + const Scalar _tmp497 = + -Dp_D_accel_bias(0, 0) * sqrt_info(8, 6) - Dp_D_accel_bias(1, 0) * sqrt_info(8, 7) - + Dp_D_accel_bias(2, 0) * sqrt_info(8, 8) - Dv_D_accel_bias(0, 0) * sqrt_info(8, 3) - + Dv_D_accel_bias(1, 0) * sqrt_info(8, 4) - Dv_D_accel_bias(2, 0) * sqrt_info(8, 5); + const Scalar _tmp498 = Dv_D_accel_bias(0, 1) * sqrt_info(3, 3); + const Scalar _tmp499 = + -Dv_D_accel_bias(0, 1) * sqrt_info(4, 3) - Dv_D_accel_bias(1, 1) * sqrt_info(4, 4); + const Scalar _tmp500 = -Dv_D_accel_bias(0, 1) * sqrt_info(5, 3) - + Dv_D_accel_bias(1, 1) * sqrt_info(5, 4) - + Dv_D_accel_bias(2, 1) * sqrt_info(5, 5); + const Scalar _tmp501 = + -Dp_D_accel_bias(0, 1) * sqrt_info(6, 6) - Dv_D_accel_bias(0, 1) * sqrt_info(6, 3) - + Dv_D_accel_bias(1, 1) * sqrt_info(6, 4) - Dv_D_accel_bias(2, 1) * sqrt_info(6, 5); + const Scalar _tmp502 = + -Dp_D_accel_bias(0, 1) * sqrt_info(7, 6) - Dp_D_accel_bias(1, 1) * sqrt_info(7, 7) - + Dv_D_accel_bias(0, 1) * sqrt_info(7, 3) - Dv_D_accel_bias(1, 1) * sqrt_info(7, 4) - + Dv_D_accel_bias(2, 1) * sqrt_info(7, 5); + const Scalar _tmp503 = + -Dp_D_accel_bias(0, 1) * sqrt_info(8, 6) - Dp_D_accel_bias(1, 1) * sqrt_info(8, 7) - + Dp_D_accel_bias(2, 1) * sqrt_info(8, 8) - Dv_D_accel_bias(0, 1) * sqrt_info(8, 3) - + Dv_D_accel_bias(1, 1) * sqrt_info(8, 4) - Dv_D_accel_bias(2, 1) * sqrt_info(8, 5); + const Scalar _tmp504 = Dv_D_accel_bias(0, 2) * sqrt_info(3, 3); + const Scalar _tmp505 = + -Dv_D_accel_bias(0, 2) * sqrt_info(4, 3) - Dv_D_accel_bias(1, 2) * sqrt_info(4, 4); + const Scalar _tmp506 = -Dv_D_accel_bias(0, 2) * sqrt_info(5, 3) - + Dv_D_accel_bias(1, 2) * sqrt_info(5, 4) - + Dv_D_accel_bias(2, 2) * sqrt_info(5, 5); + const Scalar _tmp507 = + -Dp_D_accel_bias(0, 2) * sqrt_info(6, 6) - Dv_D_accel_bias(0, 2) * sqrt_info(6, 3) - + Dv_D_accel_bias(1, 2) * sqrt_info(6, 4) - Dv_D_accel_bias(2, 2) * sqrt_info(6, 5); + const Scalar _tmp508 = + -Dp_D_accel_bias(0, 2) * sqrt_info(7, 6) - Dp_D_accel_bias(1, 2) * sqrt_info(7, 7) - + Dv_D_accel_bias(0, 2) * sqrt_info(7, 3) - Dv_D_accel_bias(1, 2) * sqrt_info(7, 4) - + Dv_D_accel_bias(2, 2) * sqrt_info(7, 5); + const Scalar _tmp509 = + -Dp_D_accel_bias(0, 2) * sqrt_info(8, 6) - Dp_D_accel_bias(1, 2) * sqrt_info(8, 7) - + Dp_D_accel_bias(2, 2) * sqrt_info(8, 8) - Dv_D_accel_bias(0, 2) * sqrt_info(8, 3) - + Dv_D_accel_bias(1, 2) * sqrt_info(8, 4) - Dv_D_accel_bias(2, 2) * sqrt_info(8, 5); + const Scalar _tmp510 = 2 * _tmp5; + const Scalar _tmp511 = 2 * _tmp4; + const Scalar _tmp512 = 2 * _tmp3; + const Scalar _tmp513 = DR_D_gyro_bias(0, 0) * _tmp511 + DR_D_gyro_bias(1, 0) * _tmp510 + + DR_D_gyro_bias(2, 0) * _tmp512; + const Scalar _tmp514 = (Scalar(1) / Scalar(2)) * _tmp11 / (_tmp6 * std::sqrt(_tmp6)); + const Scalar _tmp515 = _tmp513 * _tmp514; + const Scalar _tmp516 = _tmp3 * _tmp515; + const Scalar _tmp517 = (Scalar(1) / Scalar(4)) * _tmp513; + const Scalar _tmp518 = Scalar(1.0) / (_tmp6); + const Scalar _tmp519 = _tmp5 * _tmp518; + const Scalar _tmp520 = _tmp517 * _tmp519; + const Scalar _tmp521 = _tmp4 * _tmp518; + const Scalar _tmp522 = _tmp517 * _tmp521; + const Scalar _tmp523 = _DR[3] * _tmp4; + const Scalar _tmp524 = _tmp5 * _tmp514; + const Scalar _tmp525 = _tmp513 * _tmp524; + const Scalar _tmp526 = _tmp3 * _tmp518; + const Scalar _tmp527 = _tmp18 * _tmp526; + const Scalar _tmp528 = DR_D_gyro_bias(0, 0) * _tmp14 - DR_D_gyro_bias(1, 0) * _tmp20 + + DR_D_gyro_bias(2, 0) * _tmp15 - _DR[1] * _tmp516 + _DR[2] * _tmp525 - + _tmp19 * _tmp517 - _tmp23 * _tmp520 + _tmp26 * _tmp522 - + _tmp515 * _tmp523 + _tmp517 * _tmp527; + const Scalar _tmp529 = _tmp517 * _tmp526; + const Scalar _tmp530 = _DR[0] * _tmp513; + const Scalar _tmp531 = _tmp514 * _tmp530; + const Scalar _tmp532 = -DR_D_gyro_bias(0, 0) * _tmp19 - DR_D_gyro_bias(1, 0) * _tmp15 - + DR_D_gyro_bias(2, 0) * _tmp20 + _DR[1] * _tmp525 + _DR[2] * _tmp516 - + _tmp10 * _tmp522 - _tmp14 * _tmp517 - _tmp18 * _tmp520 - _tmp23 * _tmp529 + + _tmp4 * _tmp531; + const Scalar _tmp533 = _DR[1] * _tmp4; + const Scalar _tmp534 = -DR_D_gyro_bias(0, 0) * _tmp15 + DR_D_gyro_bias(1, 0) * _tmp19 + + DR_D_gyro_bias(2, 0) * _tmp14 - _DR[3] * _tmp516 + _tmp10 * _tmp520 - + _tmp18 * _tmp522 - _tmp20 * _tmp517 + _tmp26 * _tmp529 + + _tmp515 * _tmp533 - _tmp524 * _tmp530; + const Scalar _tmp535 = _DR[2] * _tmp4; + const Scalar _tmp536 = _tmp10 * _tmp526; + const Scalar _tmp537 = DR_D_gyro_bias(0, 0) * _tmp20 + DR_D_gyro_bias(1, 0) * _tmp14 - + DR_D_gyro_bias(2, 0) * _tmp19 - _DR[3] * _tmp525 - _tmp15 * _tmp517 + + _tmp23 * _tmp522 + _tmp26 * _tmp520 + _tmp3 * _tmp531 - _tmp515 * _tmp535 - + _tmp517 * _tmp536; + const Scalar _tmp538 = + -_pose_i[0] * _tmp532 - _pose_i[1] * _tmp534 + _pose_i[2] * _tmp537 - _pose_i[3] * _tmp528; + const Scalar _tmp539 = + _pose_i[0] * _tmp534 - _pose_i[1] * _tmp532 - _pose_i[2] * _tmp528 - _pose_i[3] * _tmp537; + const Scalar _tmp540 = + -_pose_i[0] * _tmp528 - _pose_i[1] * _tmp537 - _pose_i[2] * _tmp534 + _pose_i[3] * _tmp532; + const Scalar _tmp541 = + -_pose_i[0] * _tmp537 + _pose_i[1] * _tmp528 - _pose_i[2] * _tmp532 - _pose_i[3] * _tmp534; + const Scalar _tmp542 = _tmp166 * (_pose_j[0] * _tmp540 - _pose_j[1] * _tmp541 + + _pose_j[2] * _tmp539 + _pose_j[3] * _tmp538); + const Scalar _tmp543 = + _pose_j[0] * _tmp538 + _pose_j[1] * _tmp539 + _pose_j[2] * _tmp541 - _pose_j[3] * _tmp540; + const Scalar _tmp544 = (((_tmp54 + _tmp58) > 0) - ((_tmp54 + _tmp58) < 0)); + const Scalar _tmp545 = _tmp171 * _tmp544; + const Scalar _tmp546 = _tmp543 * _tmp545; + const Scalar _tmp547 = _tmp175 * _tmp544; + const Scalar _tmp548 = _tmp543 * _tmp547; + const Scalar _tmp549 = -_tmp50 * _tmp546 + _tmp50 * _tmp548 + _tmp542 * sqrt_info(0, 0); + const Scalar _tmp550 = _tmp166 * (_pose_j[0] * _tmp541 + _pose_j[1] * _tmp540 - + _pose_j[2] * _tmp538 + _pose_j[3] * _tmp539); + const Scalar _tmp551 = _tmp49 * _tmp546; + const Scalar _tmp552 = -_tmp179 * _tmp546 + _tmp179 * _tmp548 + _tmp182 * _tmp548 + + _tmp542 * sqrt_info(1, 0) + _tmp550 * sqrt_info(1, 1) - + _tmp551 * sqrt_info(1, 0); + const Scalar _tmp553 = + -_pose_j[0] * _tmp539 + _pose_j[1] * _tmp538 + _pose_j[2] * _tmp540 + _pose_j[3] * _tmp541; + const Scalar _tmp554 = _tmp166 * _tmp553; + const Scalar _tmp555 = _tmp548 * _tmp76; + const Scalar _tmp556 = _tmp546 * _tmp76; + const Scalar _tmp557 = -_tmp184 * _tmp546 + _tmp184 * _tmp548 - _tmp187 * _tmp546 + + _tmp187 * _tmp548 + _tmp542 * sqrt_info(2, 0) + _tmp550 * sqrt_info(2, 1) + + _tmp554 * sqrt_info(2, 2) + _tmp555 * sqrt_info(2, 2) - + _tmp556 * sqrt_info(2, 2); + const Scalar _tmp558 = -Dv_D_gyro_bias(0, 0) * sqrt_info(3, 3) - _tmp192 * _tmp546 + + _tmp192 * _tmp548 - _tmp193 * _tmp546 + _tmp193 * _tmp548 - + _tmp194 * _tmp546 + _tmp194 * _tmp548 + _tmp542 * sqrt_info(3, 0) + + _tmp550 * sqrt_info(3, 1) + _tmp554 * sqrt_info(3, 2); + const Scalar _tmp559 = _tmp546 * _tmp68; + const Scalar _tmp560 = + -Dv_D_gyro_bias(0, 0) * sqrt_info(4, 3) - Dv_D_gyro_bias(1, 0) * sqrt_info(4, 4) + + _tmp196 * _tmp548 + _tmp264 * _tmp548 + _tmp542 * sqrt_info(4, 0) + + _tmp550 * sqrt_info(4, 1) - _tmp551 * sqrt_info(4, 0) + _tmp554 * sqrt_info(4, 2) + + _tmp555 * sqrt_info(4, 2) - _tmp556 * sqrt_info(4, 2) - _tmp559 * sqrt_info(4, 1); + const Scalar _tmp561 = + -Dv_D_gyro_bias(0, 0) * sqrt_info(5, 3) - Dv_D_gyro_bias(1, 0) * sqrt_info(5, 4) - + Dv_D_gyro_bias(2, 0) * sqrt_info(5, 5) + _tmp207 * _tmp548 + _tmp268 * _tmp548 + + _tmp542 * sqrt_info(5, 0) + _tmp550 * sqrt_info(5, 1) - _tmp551 * sqrt_info(5, 0) + + _tmp554 * sqrt_info(5, 2) + _tmp555 * sqrt_info(5, 2) - _tmp556 * sqrt_info(5, 2) - + _tmp559 * sqrt_info(5, 1); + const Scalar _tmp562 = _tmp162 * _tmp553; + const Scalar _tmp563 = + -Dp_D_gyro_bias(0, 0) * sqrt_info(6, 6) - Dv_D_gyro_bias(0, 0) * sqrt_info(6, 3) - + Dv_D_gyro_bias(1, 0) * sqrt_info(6, 4) - Dv_D_gyro_bias(2, 0) * sqrt_info(6, 5) - + _tmp216 * _tmp546 + _tmp216 * _tmp548 + _tmp217 * _tmp562 - _tmp219 * _tmp546 + + _tmp219 * _tmp548 + _tmp542 * sqrt_info(6, 0) + _tmp550 * sqrt_info(6, 1) + + _tmp555 * sqrt_info(6, 2) - _tmp556 * sqrt_info(6, 2); + const Scalar _tmp564 = + -Dp_D_gyro_bias(0, 0) * sqrt_info(7, 6) - Dp_D_gyro_bias(1, 0) * sqrt_info(7, 7) - + Dv_D_gyro_bias(0, 0) * sqrt_info(7, 3) - Dv_D_gyro_bias(1, 0) * sqrt_info(7, 4) - + Dv_D_gyro_bias(2, 0) * sqrt_info(7, 5) - _tmp221 * _tmp546 + _tmp221 * _tmp548 + + _tmp223 * _tmp562 + _tmp224 * _tmp548 + _tmp542 * sqrt_info(7, 0) + + _tmp550 * sqrt_info(7, 1) - _tmp551 * sqrt_info(7, 0) + _tmp555 * sqrt_info(7, 2) - + _tmp556 * sqrt_info(7, 2); + const Scalar _tmp565 = + -Dp_D_gyro_bias(0, 0) * sqrt_info(8, 6) - Dp_D_gyro_bias(1, 0) * sqrt_info(8, 7) - + Dp_D_gyro_bias(2, 0) * sqrt_info(8, 8) - Dv_D_gyro_bias(0, 0) * sqrt_info(8, 3) - + Dv_D_gyro_bias(1, 0) * sqrt_info(8, 4) - Dv_D_gyro_bias(2, 0) * sqrt_info(8, 5) + + _tmp226 * _tmp548 - _tmp227 * _tmp546 + _tmp227 * _tmp548 + _tmp229 * _tmp548 + + _tmp542 * sqrt_info(8, 0) + _tmp550 * sqrt_info(8, 1) + _tmp554 * sqrt_info(8, 2) - + _tmp556 * sqrt_info(8, 2) - _tmp559 * sqrt_info(8, 1); + const Scalar _tmp566 = DR_D_gyro_bias(0, 1) * _tmp511 + DR_D_gyro_bias(1, 1) * _tmp510 + + DR_D_gyro_bias(2, 1) * _tmp512; + const Scalar _tmp567 = _tmp524 * _tmp566; + const Scalar _tmp568 = (Scalar(1) / Scalar(4)) * _tmp566; + const Scalar _tmp569 = _tmp519 * _tmp568; + const Scalar _tmp570 = _tmp23 * _tmp568; + const Scalar _tmp571 = _tmp514 * _tmp566; + const Scalar _tmp572 = _tmp3 * _tmp571; + const Scalar _tmp573 = _DR[0] * _tmp4; + const Scalar _tmp574 = _tmp521 * _tmp568; + const Scalar _tmp575 = -DR_D_gyro_bias(0, 1) * _tmp19 - DR_D_gyro_bias(1, 1) * _tmp15 - + DR_D_gyro_bias(2, 1) * _tmp20 + _DR[1] * _tmp567 + _DR[2] * _tmp572 - + _tmp10 * _tmp574 - _tmp14 * _tmp568 - _tmp18 * _tmp569 - + _tmp526 * _tmp570 + _tmp571 * _tmp573; + const Scalar _tmp576 = _tmp26 * _tmp568; + const Scalar _tmp577 = DR_D_gyro_bias(0, 1) * _tmp14 - DR_D_gyro_bias(1, 1) * _tmp20 + + DR_D_gyro_bias(2, 1) * _tmp15 - _DR[1] * _tmp572 + _DR[2] * _tmp567 - + _tmp19 * _tmp568 - _tmp519 * _tmp570 + _tmp521 * _tmp576 - + _tmp523 * _tmp571 + _tmp527 * _tmp568; + const Scalar _tmp578 = DR_D_gyro_bias(0, 1) * _tmp20 + DR_D_gyro_bias(1, 1) * _tmp14 - + DR_D_gyro_bias(2, 1) * _tmp19 + _DR[0] * _tmp572 - _DR[3] * _tmp567 - + _tmp15 * _tmp568 + _tmp26 * _tmp569 + _tmp521 * _tmp570 - + _tmp535 * _tmp571 - _tmp536 * _tmp568; + const Scalar _tmp579 = -DR_D_gyro_bias(0, 1) * _tmp15 + DR_D_gyro_bias(1, 1) * _tmp19 + + DR_D_gyro_bias(2, 1) * _tmp14 - _DR[0] * _tmp567 - _DR[3] * _tmp572 + + _tmp10 * _tmp569 - _tmp18 * _tmp574 - _tmp20 * _tmp568 + + _tmp526 * _tmp576 + _tmp533 * _tmp571; + const Scalar _tmp580 = + -_pose_i[0] * _tmp578 + _pose_i[1] * _tmp577 - _pose_i[2] * _tmp575 - _pose_i[3] * _tmp579; + const Scalar _tmp581 = + _pose_i[0] * _tmp579 - _pose_i[1] * _tmp575 - _pose_i[2] * _tmp577 - _pose_i[3] * _tmp578; + const Scalar _tmp582 = + -_pose_i[0] * _tmp575 - _pose_i[1] * _tmp579 + _pose_i[2] * _tmp578 - _pose_i[3] * _tmp577; + const Scalar _tmp583 = + -_pose_i[0] * _tmp577 - _pose_i[1] * _tmp578 - _pose_i[2] * _tmp579 + _pose_i[3] * _tmp575; + const Scalar _tmp584 = + _pose_j[0] * _tmp582 + _pose_j[1] * _tmp581 + _pose_j[2] * _tmp580 - _pose_j[3] * _tmp583; + const Scalar _tmp585 = _tmp545 * _tmp584; + const Scalar _tmp586 = + _pose_j[0] * _tmp583 - _pose_j[1] * _tmp580 + _pose_j[2] * _tmp581 + _pose_j[3] * _tmp582; + const Scalar _tmp587 = _tmp166 * _tmp586; + const Scalar _tmp588 = _tmp547 * _tmp584; + const Scalar _tmp589 = -_tmp50 * _tmp585 + _tmp50 * _tmp588 + _tmp587 * sqrt_info(0, 0); + const Scalar _tmp590 = _tmp49 * _tmp585; + const Scalar _tmp591 = _tmp166 * (_pose_j[0] * _tmp580 + _pose_j[1] * _tmp583 - + _pose_j[2] * _tmp582 + _pose_j[3] * _tmp581); + const Scalar _tmp592 = -_tmp179 * _tmp585 + _tmp179 * _tmp588 + _tmp182 * _tmp588 + + _tmp587 * sqrt_info(1, 0) - _tmp590 * sqrt_info(1, 0) + + _tmp591 * sqrt_info(1, 1); + const Scalar _tmp593 = + -_pose_j[0] * _tmp581 + _pose_j[1] * _tmp582 + _pose_j[2] * _tmp583 + _pose_j[3] * _tmp580; + const Scalar _tmp594 = _tmp166 * _tmp593; + const Scalar _tmp595 = _tmp588 * _tmp76; + const Scalar _tmp596 = _tmp585 * _tmp76; + const Scalar _tmp597 = _tmp585 * _tmp68; + const Scalar _tmp598 = _tmp184 * _tmp588 - _tmp187 * _tmp585 + _tmp187 * _tmp588 + + _tmp251 * _tmp586 + _tmp591 * sqrt_info(2, 1) + _tmp594 * sqrt_info(2, 2) + + _tmp595 * sqrt_info(2, 2) - _tmp596 * sqrt_info(2, 2) - + _tmp597 * sqrt_info(2, 1); + const Scalar _tmp599 = -Dv_D_gyro_bias(0, 1) * sqrt_info(3, 3) - _tmp192 * _tmp585 + + _tmp192 * _tmp588 + _tmp193 * _tmp588 - _tmp194 * _tmp585 + + _tmp194 * _tmp588 + _tmp587 * sqrt_info(3, 0) - _tmp590 * sqrt_info(3, 0) + + _tmp591 * sqrt_info(3, 1) + _tmp594 * sqrt_info(3, 2); + const Scalar _tmp600 = _tmp588 * _tmp68; + const Scalar _tmp601 = + -Dv_D_gyro_bias(0, 1) * sqrt_info(4, 3) - Dv_D_gyro_bias(1, 1) * sqrt_info(4, 4) + + _tmp264 * _tmp588 + _tmp587 * sqrt_info(4, 0) - _tmp590 * sqrt_info(4, 0) + + _tmp591 * sqrt_info(4, 1) + _tmp594 * sqrt_info(4, 2) + _tmp595 * sqrt_info(4, 2) - + _tmp596 * sqrt_info(4, 2) - _tmp597 * sqrt_info(4, 1) + _tmp600 * sqrt_info(4, 1); + const Scalar _tmp602 = + -Dv_D_gyro_bias(0, 1) * sqrt_info(5, 3) - Dv_D_gyro_bias(1, 1) * sqrt_info(5, 4) - + Dv_D_gyro_bias(2, 1) * sqrt_info(5, 5) + _tmp207 * _tmp588 + _tmp268 * _tmp588 + + _tmp587 * sqrt_info(5, 0) - _tmp590 * sqrt_info(5, 0) + _tmp591 * sqrt_info(5, 1) + + _tmp594 * sqrt_info(5, 2) + _tmp595 * sqrt_info(5, 2) - _tmp596 * sqrt_info(5, 2) - + _tmp597 * sqrt_info(5, 1); + const Scalar _tmp603 = _tmp162 * _tmp593; + const Scalar _tmp604 = + -Dp_D_gyro_bias(0, 1) * sqrt_info(6, 6) - Dv_D_gyro_bias(0, 1) * sqrt_info(6, 3) - + Dv_D_gyro_bias(1, 1) * sqrt_info(6, 4) - Dv_D_gyro_bias(2, 1) * sqrt_info(6, 5) - + _tmp216 * _tmp585 + _tmp216 * _tmp588 + _tmp217 * _tmp603 + _tmp219 * _tmp588 + + _tmp587 * sqrt_info(6, 0) - _tmp590 * sqrt_info(6, 0) + _tmp591 * sqrt_info(6, 1) + + _tmp595 * sqrt_info(6, 2) - _tmp596 * sqrt_info(6, 2); + const Scalar _tmp605 = + -Dp_D_gyro_bias(0, 1) * sqrt_info(7, 6) - Dp_D_gyro_bias(1, 1) * sqrt_info(7, 7) - + Dv_D_gyro_bias(0, 1) * sqrt_info(7, 3) - Dv_D_gyro_bias(1, 1) * sqrt_info(7, 4) - + Dv_D_gyro_bias(2, 1) * sqrt_info(7, 5) - _tmp221 * _tmp585 + _tmp221 * _tmp588 + + _tmp223 * _tmp603 + _tmp224 * _tmp588 + _tmp587 * sqrt_info(7, 0) - + _tmp590 * sqrt_info(7, 0) + _tmp591 * sqrt_info(7, 1) + _tmp595 * sqrt_info(7, 2) - + _tmp596 * sqrt_info(7, 2); + const Scalar _tmp606 = + -Dp_D_gyro_bias(0, 1) * sqrt_info(8, 6) - Dp_D_gyro_bias(1, 1) * sqrt_info(8, 7) - + Dp_D_gyro_bias(2, 1) * sqrt_info(8, 8) - Dv_D_gyro_bias(0, 1) * sqrt_info(8, 3) - + Dv_D_gyro_bias(1, 1) * sqrt_info(8, 4) - Dv_D_gyro_bias(2, 1) * sqrt_info(8, 5) - + _tmp227 * _tmp585 + _tmp227 * _tmp588 + _tmp229 * _tmp588 + _tmp587 * sqrt_info(8, 0) + + _tmp591 * sqrt_info(8, 1) + _tmp594 * sqrt_info(8, 2) - _tmp596 * sqrt_info(8, 2) - + _tmp597 * sqrt_info(8, 1) + _tmp600 * sqrt_info(8, 1); + const Scalar _tmp607 = DR_D_gyro_bias(0, 2) * _tmp511 + DR_D_gyro_bias(1, 2) * _tmp510 + + DR_D_gyro_bias(2, 2) * _tmp512; + const Scalar _tmp608 = (Scalar(1) / Scalar(4)) * _tmp607; + const Scalar _tmp609 = _tmp519 * _tmp608; + const Scalar _tmp610 = _tmp514 * _tmp607; + const Scalar _tmp611 = _tmp3 * _tmp610; + const Scalar _tmp612 = _tmp26 * _tmp608; + const Scalar _tmp613 = _tmp524 * _tmp607; + const Scalar _tmp614 = DR_D_gyro_bias(0, 2) * _tmp14 - DR_D_gyro_bias(1, 2) * _tmp20 + + DR_D_gyro_bias(2, 2) * _tmp15 - _DR[1] * _tmp611 + _DR[2] * _tmp613 - + _tmp19 * _tmp608 - _tmp23 * _tmp609 + _tmp521 * _tmp612 - + _tmp523 * _tmp610 + _tmp527 * _tmp608; + const Scalar _tmp615 = _tmp23 * _tmp608; + const Scalar _tmp616 = _tmp10 * _tmp608; + const Scalar _tmp617 = DR_D_gyro_bias(0, 2) * _tmp20 + DR_D_gyro_bias(1, 2) * _tmp14 - + DR_D_gyro_bias(2, 2) * _tmp19 + _DR[0] * _tmp611 - _DR[3] * _tmp613 - + _tmp15 * _tmp608 + _tmp26 * _tmp609 + _tmp521 * _tmp615 - + _tmp526 * _tmp616 - _tmp535 * _tmp610; + const Scalar _tmp618 = -DR_D_gyro_bias(0, 2) * _tmp15 + DR_D_gyro_bias(1, 2) * _tmp19 + + DR_D_gyro_bias(2, 2) * _tmp14 - _DR[0] * _tmp613 - _DR[3] * _tmp611 + + _tmp10 * _tmp609 - _tmp18 * _tmp521 * _tmp608 - _tmp20 * _tmp608 + + _tmp526 * _tmp612 + _tmp533 * _tmp610; + const Scalar _tmp619 = -DR_D_gyro_bias(0, 2) * _tmp19 - DR_D_gyro_bias(1, 2) * _tmp15 - + DR_D_gyro_bias(2, 2) * _tmp20 + _DR[1] * _tmp613 + _DR[2] * _tmp611 - + _tmp14 * _tmp608 - _tmp18 * _tmp609 - _tmp521 * _tmp616 - + _tmp526 * _tmp615 + _tmp573 * _tmp610; + const Scalar _tmp620 = + -_pose_i[0] * _tmp619 - _pose_i[1] * _tmp618 + _pose_i[2] * _tmp617 - _pose_i[3] * _tmp614; + const Scalar _tmp621 = + _pose_i[0] * _tmp618 - _pose_i[1] * _tmp619 - _pose_i[2] * _tmp614 - _pose_i[3] * _tmp617; + const Scalar _tmp622 = + -_pose_i[0] * _tmp617 + _pose_i[1] * _tmp614 - _pose_i[2] * _tmp619 - _pose_i[3] * _tmp618; + const Scalar _tmp623 = + -_pose_i[0] * _tmp614 - _pose_i[1] * _tmp617 - _pose_i[2] * _tmp618 + _pose_i[3] * _tmp619; + const Scalar _tmp624 = _tmp166 * (_pose_j[0] * _tmp623 - _pose_j[1] * _tmp622 + + _pose_j[2] * _tmp621 + _pose_j[3] * _tmp620); + const Scalar _tmp625 = + _pose_j[0] * _tmp620 + _pose_j[1] * _tmp621 + _pose_j[2] * _tmp622 - _pose_j[3] * _tmp623; + const Scalar _tmp626 = _tmp547 * _tmp625; + const Scalar _tmp627 = _tmp545 * _tmp625; + const Scalar _tmp628 = _tmp50 * _tmp626 - _tmp50 * _tmp627 + _tmp624 * sqrt_info(0, 0); + const Scalar _tmp629 = _tmp166 * (_pose_j[0] * _tmp622 + _pose_j[1] * _tmp623 - + _pose_j[2] * _tmp620 + _pose_j[3] * _tmp621); + const Scalar _tmp630 = _tmp49 * _tmp627; + const Scalar _tmp631 = _tmp179 * _tmp626 - _tmp179 * _tmp627 + _tmp182 * _tmp626 + + _tmp624 * sqrt_info(1, 0) + _tmp629 * sqrt_info(1, 1) - + _tmp630 * sqrt_info(1, 0); + const Scalar _tmp632 = + -_pose_j[0] * _tmp621 + _pose_j[1] * _tmp620 + _pose_j[2] * _tmp623 + _pose_j[3] * _tmp622; + const Scalar _tmp633 = _tmp166 * _tmp632; + const Scalar _tmp634 = _tmp627 * _tmp76; + const Scalar _tmp635 = _tmp626 * _tmp76; + const Scalar _tmp636 = _tmp627 * _tmp68; + const Scalar _tmp637 = _tmp184 * _tmp626 + _tmp187 * _tmp626 - _tmp187 * _tmp627 + + _tmp624 * sqrt_info(2, 0) + _tmp629 * sqrt_info(2, 1) + + _tmp633 * sqrt_info(2, 2) - _tmp634 * sqrt_info(2, 2) + + _tmp635 * sqrt_info(2, 2) - _tmp636 * sqrt_info(2, 1); + const Scalar _tmp638 = -Dv_D_gyro_bias(0, 2) * sqrt_info(3, 3) + _tmp192 * _tmp626 - + _tmp192 * _tmp627 + _tmp193 * _tmp626 - _tmp193 * _tmp627 + + _tmp194 * _tmp626 + _tmp624 * sqrt_info(3, 0) + _tmp629 * sqrt_info(3, 1) + + _tmp633 * sqrt_info(3, 2) - _tmp634 * sqrt_info(3, 2); + const Scalar _tmp639 = _tmp49 * _tmp626; + const Scalar _tmp640 = + -Dv_D_gyro_bias(0, 2) * sqrt_info(4, 3) - Dv_D_gyro_bias(1, 2) * sqrt_info(4, 4) + + _tmp196 * _tmp626 + _tmp624 * sqrt_info(4, 0) + _tmp629 * sqrt_info(4, 1) - + _tmp630 * sqrt_info(4, 0) + _tmp633 * sqrt_info(4, 2) - _tmp634 * sqrt_info(4, 2) + + _tmp635 * sqrt_info(4, 2) - _tmp636 * sqrt_info(4, 1) + _tmp639 * sqrt_info(4, 0); + const Scalar _tmp641 = + -Dv_D_gyro_bias(0, 2) * sqrt_info(5, 3) - Dv_D_gyro_bias(1, 2) * sqrt_info(5, 4) - + Dv_D_gyro_bias(2, 2) * sqrt_info(5, 5) + _tmp207 * _tmp626 + _tmp624 * sqrt_info(5, 0) + + _tmp629 * sqrt_info(5, 1) - _tmp630 * sqrt_info(5, 0) + _tmp633 * sqrt_info(5, 2) - + _tmp634 * sqrt_info(5, 2) + _tmp635 * sqrt_info(5, 2) - _tmp636 * sqrt_info(5, 1) + + _tmp639 * sqrt_info(5, 0); + const Scalar _tmp642 = _tmp162 * _tmp632; + const Scalar _tmp643 = + -Dp_D_gyro_bias(0, 2) * sqrt_info(6, 6) - Dv_D_gyro_bias(0, 2) * sqrt_info(6, 3) - + Dv_D_gyro_bias(1, 2) * sqrt_info(6, 4) - Dv_D_gyro_bias(2, 2) * sqrt_info(6, 5) + + _tmp216 * _tmp626 - _tmp216 * _tmp627 + _tmp217 * _tmp642 + _tmp219 * _tmp626 - + _tmp219 * _tmp627 + _tmp624 * sqrt_info(6, 0) + _tmp629 * sqrt_info(6, 1) - + _tmp634 * sqrt_info(6, 2) + _tmp635 * sqrt_info(6, 2); + const Scalar _tmp644 = + -Dp_D_gyro_bias(0, 2) * sqrt_info(7, 6) - Dp_D_gyro_bias(1, 2) * sqrt_info(7, 7) - + Dv_D_gyro_bias(0, 2) * sqrt_info(7, 3) - Dv_D_gyro_bias(1, 2) * sqrt_info(7, 4) - + Dv_D_gyro_bias(2, 2) * sqrt_info(7, 5) + _tmp221 * _tmp626 - _tmp221 * _tmp627 + + _tmp223 * _tmp642 + _tmp224 * _tmp626 + _tmp624 * sqrt_info(7, 0) + + _tmp629 * sqrt_info(7, 1) - _tmp630 * sqrt_info(7, 0) - _tmp634 * sqrt_info(7, 2) + + _tmp635 * sqrt_info(7, 2); + const Scalar _tmp645 = + -Dp_D_gyro_bias(0, 2) * sqrt_info(8, 6) - Dp_D_gyro_bias(1, 2) * sqrt_info(8, 7) - + Dp_D_gyro_bias(2, 2) * sqrt_info(8, 8) - Dv_D_gyro_bias(0, 2) * sqrt_info(8, 3) - + Dv_D_gyro_bias(1, 2) * sqrt_info(8, 4) - Dv_D_gyro_bias(2, 2) * sqrt_info(8, 5) + + _tmp226 * _tmp626 + _tmp227 * _tmp626 - _tmp227 * _tmp627 + _tmp229 * _tmp626 + + _tmp624 * sqrt_info(8, 0) + _tmp629 * sqrt_info(8, 1) + _tmp633 * sqrt_info(8, 2) - + _tmp634 * sqrt_info(8, 2) - _tmp636 * sqrt_info(8, 1); + const Scalar _tmp646 = _tmp220 * _tmp308; + const Scalar _tmp647 = _tmp220 * _tmp316; + const Scalar _tmp648 = _tmp220 * _tmp324; + const Scalar _tmp649 = _tmp195 * _tmp332; + const Scalar _tmp650 = _tmp195 * _tmp352; + const Scalar _tmp651 = _tmp195 * _tmp372; + const Scalar _tmp652 = _tmp272 * _tmp308; + const Scalar _tmp653 = _tmp272 * _tmp316; + const Scalar _tmp654 = _tmp272 * _tmp324; + const Scalar _tmp655 = _tmp263 * _tmp332; + const Scalar _tmp656 = _tmp263 * _tmp352; + const Scalar _tmp657 = _tmp263 * _tmp372; + const Scalar _tmp658 = _tmp304 * _tmp308; + const Scalar _tmp659 = _tmp304 * _tmp316; + const Scalar _tmp660 = _tmp304 * _tmp324; + const Scalar _tmp661 = _tmp295 * _tmp332; + const Scalar _tmp662 = _tmp295 * _tmp352; + const Scalar _tmp663 = _tmp295 * _tmp372; + const Scalar _tmp664 = std::pow(_tmp83, Scalar(2)); + const Scalar _tmp665 = std::pow(sqrt_info(6, 6), Scalar(2)); + const Scalar _tmp666 = _tmp664 * _tmp665; + const Scalar _tmp667 = _tmp665 * _tmp92; + const Scalar _tmp668 = _tmp667 * _tmp83; + const Scalar _tmp669 = _tmp665 * _tmp83 * _tmp97; + const Scalar _tmp670 = _tmp308 * _tmp343; + const Scalar _tmp671 = _tmp308 * _tmp363; + const Scalar _tmp672 = _tmp308 * _tmp383; + const Scalar _tmp673 = _tmp308 * _tmp426; + const Scalar _tmp674 = _tmp308 * _tmp449; + const Scalar _tmp675 = _tmp308 * _tmp468; + const Scalar _tmp676 = -_tmp668; + const Scalar _tmp677 = -_tmp669; + const Scalar _tmp678 = _tmp308 * _tmp479; + const Scalar _tmp679 = _tmp308 * _tmp484; + const Scalar _tmp680 = _tmp308 * _tmp489; + const Scalar _tmp681 = _tmp308 * _tmp495; + const Scalar _tmp682 = _tmp308 * _tmp501; + const Scalar _tmp683 = _tmp308 * _tmp507; + const Scalar _tmp684 = _tmp308 * _tmp563; + const Scalar _tmp685 = _tmp308 * _tmp604; + const Scalar _tmp686 = _tmp308 * _tmp643; + const Scalar _tmp687 = std::pow(_tmp92, Scalar(2)); + const Scalar _tmp688 = _tmp665 * _tmp687; + const Scalar _tmp689 = _tmp667 * _tmp97; + const Scalar _tmp690 = _tmp316 * _tmp343; + const Scalar _tmp691 = _tmp316 * _tmp363; + const Scalar _tmp692 = _tmp316 * _tmp383; + const Scalar _tmp693 = _tmp316 * _tmp426; + const Scalar _tmp694 = _tmp316 * _tmp449; + const Scalar _tmp695 = _tmp316 * _tmp468; + const Scalar _tmp696 = -_tmp689; + const Scalar _tmp697 = _tmp316 * _tmp479; + const Scalar _tmp698 = _tmp316 * _tmp484; + const Scalar _tmp699 = _tmp316 * _tmp489; + const Scalar _tmp700 = _tmp316 * _tmp495; + const Scalar _tmp701 = _tmp316 * _tmp501; + const Scalar _tmp702 = _tmp316 * _tmp507; + const Scalar _tmp703 = _tmp316 * _tmp563; + const Scalar _tmp704 = _tmp316 * _tmp604; + const Scalar _tmp705 = _tmp316 * _tmp643; + const Scalar _tmp706 = std::pow(_tmp97, Scalar(2)); + const Scalar _tmp707 = _tmp665 * _tmp706; + const Scalar _tmp708 = _tmp324 * _tmp343; + const Scalar _tmp709 = _tmp324 * _tmp363; + const Scalar _tmp710 = _tmp324 * _tmp383; + const Scalar _tmp711 = _tmp324 * _tmp426; + const Scalar _tmp712 = _tmp324 * _tmp449; + const Scalar _tmp713 = _tmp324 * _tmp468; + const Scalar _tmp714 = _tmp324 * _tmp479; + const Scalar _tmp715 = _tmp324 * _tmp484; + const Scalar _tmp716 = _tmp324 * _tmp489; + const Scalar _tmp717 = _tmp324 * _tmp495; + const Scalar _tmp718 = _tmp324 * _tmp501; + const Scalar _tmp719 = _tmp324 * _tmp507; + const Scalar _tmp720 = _tmp324 * _tmp563; + const Scalar _tmp721 = _tmp324 * _tmp604; + const Scalar _tmp722 = _tmp324 * _tmp643; + const Scalar _tmp723 = std::pow(sqrt_info(3, 3), Scalar(2)); + const Scalar _tmp724 = _tmp664 * _tmp723; + const Scalar _tmp725 = _tmp723 * _tmp92; + const Scalar _tmp726 = _tmp725 * _tmp83; + const Scalar _tmp727 = _tmp723 * _tmp83; + const Scalar _tmp728 = _tmp727 * _tmp97; + const Scalar _tmp729 = _tmp332 * _tmp422; + const Scalar _tmp730 = _tmp332 * _tmp444; + const Scalar _tmp731 = _tmp332 * _tmp463; + const Scalar _tmp732 = -_tmp726; + const Scalar _tmp733 = -_tmp728; + const Scalar _tmp734 = Dv_D_accel_bias(0, 0) * _tmp727; + const Scalar _tmp735 = Dv_D_accel_bias(0, 1) * _tmp723; + const Scalar _tmp736 = _tmp735 * _tmp83; + const Scalar _tmp737 = Dv_D_accel_bias(0, 2) * _tmp727; + const Scalar _tmp738 = _tmp332 * _tmp558; + const Scalar _tmp739 = _tmp332 * _tmp599; + const Scalar _tmp740 = _tmp332 * _tmp638; + const Scalar _tmp741 = _tmp687 * _tmp723; + const Scalar _tmp742 = _tmp725 * _tmp97; + const Scalar _tmp743 = _tmp352 * _tmp422; + const Scalar _tmp744 = _tmp352 * _tmp444; + const Scalar _tmp745 = _tmp352 * _tmp463; + const Scalar _tmp746 = -_tmp742; + const Scalar _tmp747 = Dv_D_accel_bias(0, 0) * _tmp725; + const Scalar _tmp748 = Dv_D_accel_bias(0, 1) * _tmp725; + const Scalar _tmp749 = Dv_D_accel_bias(0, 2) * _tmp725; + const Scalar _tmp750 = _tmp352 * _tmp558; + const Scalar _tmp751 = _tmp352 * _tmp599; + const Scalar _tmp752 = _tmp352 * _tmp638; + const Scalar _tmp753 = _tmp706 * _tmp723; + const Scalar _tmp754 = _tmp372 * _tmp422; + const Scalar _tmp755 = _tmp372 * _tmp444; + const Scalar _tmp756 = _tmp372 * _tmp463; + const Scalar _tmp757 = Dv_D_accel_bias(0, 0) * _tmp723 * _tmp97; + const Scalar _tmp758 = _tmp735 * _tmp97; + const Scalar _tmp759 = Dv_D_accel_bias(0, 2) * _tmp723; + const Scalar _tmp760 = _tmp759 * _tmp97; + const Scalar _tmp761 = _tmp372 * _tmp558; + const Scalar _tmp762 = _tmp372 * _tmp599; + const Scalar _tmp763 = _tmp372 * _tmp638; + const Scalar _tmp764 = _tmp127 * _tmp308; + const Scalar _tmp765 = _tmp127 * _tmp316; + const Scalar _tmp766 = _tmp127 * _tmp324; + const Scalar _tmp767 = _tmp101 * _tmp332; + const Scalar _tmp768 = _tmp101 * _tmp352; + const Scalar _tmp769 = _tmp101 * _tmp372; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp63; + _res(1, 0) = _tmp71; + _res(2, 0) = _tmp78; + _res(3, 0) = _tmp101; + _res(4, 0) = _tmp112; + _res(5, 0) = _tmp119; + _res(6, 0) = _tmp127; + _res(7, 0) = _tmp130; + _res(8, 0) = _tmp132; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp178; + _jacobian(1, 0) = _tmp183; + _jacobian(2, 0) = _tmp191; + _jacobian(3, 0) = _tmp195; + _jacobian(4, 0) = _tmp206; + _jacobian(5, 0) = _tmp215; + _jacobian(6, 0) = _tmp220; + _jacobian(7, 0) = _tmp225; + _jacobian(8, 0) = _tmp230; + _jacobian(0, 1) = _tmp244; + _jacobian(1, 1) = _tmp248; + _jacobian(2, 1) = _tmp254; + _jacobian(3, 1) = _tmp263; + _jacobian(4, 1) = _tmp265; + _jacobian(5, 1) = _tmp269; + _jacobian(6, 1) = _tmp272; + _jacobian(7, 1) = _tmp273; + _jacobian(8, 1) = _tmp274; + _jacobian(0, 2) = _tmp283; + _jacobian(1, 2) = _tmp286; + _jacobian(2, 2) = _tmp292; + _jacobian(3, 2) = _tmp295; + _jacobian(4, 2) = _tmp301; + _jacobian(5, 2) = _tmp302; + _jacobian(6, 2) = _tmp304; + _jacobian(7, 2) = _tmp306; + _jacobian(8, 2) = _tmp307; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(3, 3) = 0; + _jacobian(4, 3) = 0; + _jacobian(5, 3) = 0; + _jacobian(6, 3) = -_tmp308; + _jacobian(7, 3) = _tmp311; + _jacobian(8, 3) = _tmp315; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(3, 4) = 0; + _jacobian(4, 4) = 0; + _jacobian(5, 4) = 0; + _jacobian(6, 4) = -_tmp316; + _jacobian(7, 4) = _tmp319; + _jacobian(8, 4) = _tmp323; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(3, 5) = 0; + _jacobian(4, 5) = 0; + _jacobian(5, 5) = 0; + _jacobian(6, 5) = -_tmp324; + _jacobian(7, 5) = _tmp327; + _jacobian(8, 5) = _tmp331; + _jacobian(0, 6) = 0; + _jacobian(1, 6) = 0; + _jacobian(2, 6) = 0; + _jacobian(3, 6) = -_tmp332; + _jacobian(4, 6) = _tmp335; + _jacobian(5, 6) = _tmp339; + _jacobian(6, 6) = _tmp343; + _jacobian(7, 6) = _tmp347; + _jacobian(8, 6) = _tmp351; + _jacobian(0, 7) = 0; + _jacobian(1, 7) = 0; + _jacobian(2, 7) = 0; + _jacobian(3, 7) = -_tmp352; + _jacobian(4, 7) = _tmp355; + _jacobian(5, 7) = _tmp359; + _jacobian(6, 7) = _tmp363; + _jacobian(7, 7) = _tmp367; + _jacobian(8, 7) = _tmp371; + _jacobian(0, 8) = 0; + _jacobian(1, 8) = 0; + _jacobian(2, 8) = 0; + _jacobian(3, 8) = -_tmp372; + _jacobian(4, 8) = _tmp375; + _jacobian(5, 8) = _tmp379; + _jacobian(6, 8) = _tmp383; + _jacobian(7, 8) = _tmp387; + _jacobian(8, 8) = _tmp391; + _jacobian(0, 9) = _tmp401; + _jacobian(1, 9) = _tmp409; + _jacobian(2, 9) = _tmp420; + _jacobian(3, 9) = _tmp422; + _jacobian(4, 9) = _tmp423; + _jacobian(5, 9) = _tmp425; + _jacobian(6, 9) = _tmp426; + _jacobian(7, 9) = _tmp427; + _jacobian(8, 9) = _tmp429; + _jacobian(0, 10) = _tmp434; + _jacobian(1, 10) = _tmp437; + _jacobian(2, 10) = _tmp443; + _jacobian(3, 10) = _tmp444; + _jacobian(4, 10) = _tmp446; + _jacobian(5, 10) = _tmp447; + _jacobian(6, 10) = _tmp449; + _jacobian(7, 10) = _tmp450; + _jacobian(8, 10) = _tmp451; + _jacobian(0, 11) = _tmp455; + _jacobian(1, 11) = _tmp459; + _jacobian(2, 11) = _tmp462; + _jacobian(3, 11) = _tmp463; + _jacobian(4, 11) = _tmp465; + _jacobian(5, 11) = _tmp466; + _jacobian(6, 11) = _tmp468; + _jacobian(7, 11) = _tmp469; + _jacobian(8, 11) = _tmp470; + _jacobian(0, 12) = 0; + _jacobian(1, 12) = 0; + _jacobian(2, 12) = 0; + _jacobian(3, 12) = 0; + _jacobian(4, 12) = 0; + _jacobian(5, 12) = 0; + _jacobian(6, 12) = _tmp308; + _jacobian(7, 12) = _tmp471; + _jacobian(8, 12) = _tmp472; + _jacobian(0, 13) = 0; + _jacobian(1, 13) = 0; + _jacobian(2, 13) = 0; + _jacobian(3, 13) = 0; + _jacobian(4, 13) = 0; + _jacobian(5, 13) = 0; + _jacobian(6, 13) = _tmp316; + _jacobian(7, 13) = _tmp473; + _jacobian(8, 13) = _tmp474; + _jacobian(0, 14) = 0; + _jacobian(1, 14) = 0; + _jacobian(2, 14) = 0; + _jacobian(3, 14) = 0; + _jacobian(4, 14) = 0; + _jacobian(5, 14) = 0; + _jacobian(6, 14) = _tmp324; + _jacobian(7, 14) = _tmp475; + _jacobian(8, 14) = _tmp476; + _jacobian(0, 15) = 0; + _jacobian(1, 15) = 0; + _jacobian(2, 15) = 0; + _jacobian(3, 15) = _tmp332; + _jacobian(4, 15) = _tmp477; + _jacobian(5, 15) = _tmp478; + _jacobian(6, 15) = _tmp479; + _jacobian(7, 15) = _tmp480; + _jacobian(8, 15) = _tmp481; + _jacobian(0, 16) = 0; + _jacobian(1, 16) = 0; + _jacobian(2, 16) = 0; + _jacobian(3, 16) = _tmp352; + _jacobian(4, 16) = _tmp482; + _jacobian(5, 16) = _tmp483; + _jacobian(6, 16) = _tmp484; + _jacobian(7, 16) = _tmp485; + _jacobian(8, 16) = _tmp486; + _jacobian(0, 17) = 0; + _jacobian(1, 17) = 0; + _jacobian(2, 17) = 0; + _jacobian(3, 17) = _tmp372; + _jacobian(4, 17) = _tmp487; + _jacobian(5, 17) = _tmp488; + _jacobian(6, 17) = _tmp489; + _jacobian(7, 17) = _tmp490; + _jacobian(8, 17) = _tmp491; + _jacobian(0, 18) = 0; + _jacobian(1, 18) = 0; + _jacobian(2, 18) = 0; + _jacobian(3, 18) = -_tmp492; + _jacobian(4, 18) = _tmp493; + _jacobian(5, 18) = _tmp494; + _jacobian(6, 18) = _tmp495; + _jacobian(7, 18) = _tmp496; + _jacobian(8, 18) = _tmp497; + _jacobian(0, 19) = 0; + _jacobian(1, 19) = 0; + _jacobian(2, 19) = 0; + _jacobian(3, 19) = -_tmp498; + _jacobian(4, 19) = _tmp499; + _jacobian(5, 19) = _tmp500; + _jacobian(6, 19) = _tmp501; + _jacobian(7, 19) = _tmp502; + _jacobian(8, 19) = _tmp503; + _jacobian(0, 20) = 0; + _jacobian(1, 20) = 0; + _jacobian(2, 20) = 0; + _jacobian(3, 20) = -_tmp504; + _jacobian(4, 20) = _tmp505; + _jacobian(5, 20) = _tmp506; + _jacobian(6, 20) = _tmp507; + _jacobian(7, 20) = _tmp508; + _jacobian(8, 20) = _tmp509; + _jacobian(0, 21) = _tmp549; + _jacobian(1, 21) = _tmp552; + _jacobian(2, 21) = _tmp557; + _jacobian(3, 21) = _tmp558; + _jacobian(4, 21) = _tmp560; + _jacobian(5, 21) = _tmp561; + _jacobian(6, 21) = _tmp563; + _jacobian(7, 21) = _tmp564; + _jacobian(8, 21) = _tmp565; + _jacobian(0, 22) = _tmp589; + _jacobian(1, 22) = _tmp592; + _jacobian(2, 22) = _tmp598; + _jacobian(3, 22) = _tmp599; + _jacobian(4, 22) = _tmp601; + _jacobian(5, 22) = _tmp602; + _jacobian(6, 22) = _tmp604; + _jacobian(7, 22) = _tmp605; + _jacobian(8, 22) = _tmp606; + _jacobian(0, 23) = _tmp628; + _jacobian(1, 23) = _tmp631; + _jacobian(2, 23) = _tmp637; + _jacobian(3, 23) = _tmp638; + _jacobian(4, 23) = _tmp640; + _jacobian(5, 23) = _tmp641; + _jacobian(6, 23) = _tmp643; + _jacobian(7, 23) = _tmp644; + _jacobian(8, 23) = _tmp645; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp178, Scalar(2)) + std::pow(_tmp183, Scalar(2)) + std::pow(_tmp191, Scalar(2)) + + std::pow(_tmp195, Scalar(2)) + std::pow(_tmp206, Scalar(2)) + std::pow(_tmp215, Scalar(2)) + + std::pow(_tmp220, Scalar(2)) + std::pow(_tmp225, Scalar(2)) + std::pow(_tmp230, Scalar(2)); + _hessian(1, 0) = _tmp178 * _tmp244 + _tmp183 * _tmp248 + _tmp191 * _tmp254 + _tmp195 * _tmp263 + + _tmp206 * _tmp265 + _tmp215 * _tmp269 + _tmp220 * _tmp272 + _tmp225 * _tmp273 + + _tmp230 * _tmp274; + _hessian(2, 0) = _tmp178 * _tmp283 + _tmp183 * _tmp286 + _tmp191 * _tmp292 + _tmp195 * _tmp295 + + _tmp206 * _tmp301 + _tmp215 * _tmp302 + _tmp220 * _tmp304 + _tmp225 * _tmp306 + + _tmp230 * _tmp307; + _hessian(3, 0) = _tmp225 * _tmp311 + _tmp230 * _tmp315 - _tmp646; + _hessian(4, 0) = _tmp225 * _tmp319 + _tmp230 * _tmp323 - _tmp647; + _hessian(5, 0) = _tmp225 * _tmp327 + _tmp230 * _tmp331 - _tmp648; + _hessian(6, 0) = _tmp206 * _tmp335 + _tmp215 * _tmp339 + _tmp220 * _tmp343 + _tmp225 * _tmp347 + + _tmp230 * _tmp351 - _tmp649; + _hessian(7, 0) = _tmp206 * _tmp355 + _tmp215 * _tmp359 + _tmp220 * _tmp363 + _tmp225 * _tmp367 + + _tmp230 * _tmp371 - _tmp650; + _hessian(8, 0) = _tmp206 * _tmp375 + _tmp215 * _tmp379 + _tmp220 * _tmp383 + _tmp225 * _tmp387 + + _tmp230 * _tmp391 - _tmp651; + _hessian(9, 0) = _tmp178 * _tmp401 + _tmp183 * _tmp409 + _tmp191 * _tmp420 + _tmp195 * _tmp422 + + _tmp206 * _tmp423 + _tmp215 * _tmp425 + _tmp220 * _tmp426 + _tmp225 * _tmp427 + + _tmp230 * _tmp429; + _hessian(10, 0) = _tmp178 * _tmp434 + _tmp183 * _tmp437 + _tmp191 * _tmp443 + + _tmp195 * _tmp444 + _tmp206 * _tmp446 + _tmp215 * _tmp447 + + _tmp220 * _tmp449 + _tmp225 * _tmp450 + _tmp230 * _tmp451; + _hessian(11, 0) = _tmp178 * _tmp455 + _tmp183 * _tmp459 + _tmp191 * _tmp462 + + _tmp195 * _tmp463 + _tmp206 * _tmp465 + _tmp215 * _tmp466 + + _tmp220 * _tmp468 + _tmp225 * _tmp469 + _tmp230 * _tmp470; + _hessian(12, 0) = _tmp225 * _tmp471 + _tmp230 * _tmp472 + _tmp646; + _hessian(13, 0) = _tmp225 * _tmp473 + _tmp230 * _tmp474 + _tmp647; + _hessian(14, 0) = _tmp225 * _tmp475 + _tmp230 * _tmp476 + _tmp648; + _hessian(15, 0) = _tmp206 * _tmp477 + _tmp215 * _tmp478 + _tmp220 * _tmp479 + + _tmp225 * _tmp480 + _tmp230 * _tmp481 + _tmp649; + _hessian(16, 0) = _tmp206 * _tmp482 + _tmp215 * _tmp483 + _tmp220 * _tmp484 + + _tmp225 * _tmp485 + _tmp230 * _tmp486 + _tmp650; + _hessian(17, 0) = _tmp206 * _tmp487 + _tmp215 * _tmp488 + _tmp220 * _tmp489 + + _tmp225 * _tmp490 + _tmp230 * _tmp491 + _tmp651; + _hessian(18, 0) = -_tmp195 * _tmp492 + _tmp206 * _tmp493 + _tmp215 * _tmp494 + + _tmp220 * _tmp495 + _tmp225 * _tmp496 + _tmp230 * _tmp497; + _hessian(19, 0) = -_tmp195 * _tmp498 + _tmp206 * _tmp499 + _tmp215 * _tmp500 + + _tmp220 * _tmp501 + _tmp225 * _tmp502 + _tmp230 * _tmp503; + _hessian(20, 0) = -_tmp195 * _tmp504 + _tmp206 * _tmp505 + _tmp215 * _tmp506 + + _tmp220 * _tmp507 + _tmp225 * _tmp508 + _tmp230 * _tmp509; + _hessian(21, 0) = _tmp178 * _tmp549 + _tmp183 * _tmp552 + _tmp191 * _tmp557 + + _tmp195 * _tmp558 + _tmp206 * _tmp560 + _tmp215 * _tmp561 + + _tmp220 * _tmp563 + _tmp225 * _tmp564 + _tmp230 * _tmp565; + _hessian(22, 0) = _tmp178 * _tmp589 + _tmp183 * _tmp592 + _tmp191 * _tmp598 + + _tmp195 * _tmp599 + _tmp206 * _tmp601 + _tmp215 * _tmp602 + + _tmp220 * _tmp604 + _tmp225 * _tmp605 + _tmp230 * _tmp606; + _hessian(23, 0) = _tmp178 * _tmp628 + _tmp183 * _tmp631 + _tmp191 * _tmp637 + + _tmp195 * _tmp638 + _tmp206 * _tmp640 + _tmp215 * _tmp641 + + _tmp220 * _tmp643 + _tmp225 * _tmp644 + _tmp230 * _tmp645; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp244, Scalar(2)) + std::pow(_tmp248, Scalar(2)) + std::pow(_tmp254, Scalar(2)) + + std::pow(_tmp263, Scalar(2)) + std::pow(_tmp265, Scalar(2)) + std::pow(_tmp269, Scalar(2)) + + std::pow(_tmp272, Scalar(2)) + std::pow(_tmp273, Scalar(2)) + std::pow(_tmp274, Scalar(2)); + _hessian(2, 1) = _tmp244 * _tmp283 + _tmp248 * _tmp286 + _tmp254 * _tmp292 + _tmp263 * _tmp295 + + _tmp265 * _tmp301 + _tmp269 * _tmp302 + _tmp272 * _tmp304 + _tmp273 * _tmp306 + + _tmp274 * _tmp307; + _hessian(3, 1) = _tmp273 * _tmp311 + _tmp274 * _tmp315 - _tmp652; + _hessian(4, 1) = _tmp273 * _tmp319 + _tmp274 * _tmp323 - _tmp653; + _hessian(5, 1) = _tmp273 * _tmp327 + _tmp274 * _tmp331 - _tmp654; + _hessian(6, 1) = _tmp265 * _tmp335 + _tmp269 * _tmp339 + _tmp272 * _tmp343 + _tmp273 * _tmp347 + + _tmp274 * _tmp351 - _tmp655; + _hessian(7, 1) = _tmp265 * _tmp355 + _tmp269 * _tmp359 + _tmp272 * _tmp363 + _tmp273 * _tmp367 + + _tmp274 * _tmp371 - _tmp656; + _hessian(8, 1) = _tmp265 * _tmp375 + _tmp269 * _tmp379 + _tmp272 * _tmp383 + _tmp273 * _tmp387 + + _tmp274 * _tmp391 - _tmp657; + _hessian(9, 1) = _tmp244 * _tmp401 + _tmp248 * _tmp409 + _tmp254 * _tmp420 + _tmp263 * _tmp422 + + _tmp265 * _tmp423 + _tmp269 * _tmp425 + _tmp272 * _tmp426 + _tmp273 * _tmp427 + + _tmp274 * _tmp429; + _hessian(10, 1) = _tmp244 * _tmp434 + _tmp248 * _tmp437 + _tmp254 * _tmp443 + + _tmp263 * _tmp444 + _tmp265 * _tmp446 + _tmp269 * _tmp447 + + _tmp272 * _tmp449 + _tmp273 * _tmp450 + _tmp274 * _tmp451; + _hessian(11, 1) = _tmp244 * _tmp455 + _tmp248 * _tmp459 + _tmp254 * _tmp462 + + _tmp263 * _tmp463 + _tmp265 * _tmp465 + _tmp269 * _tmp466 + + _tmp272 * _tmp468 + _tmp273 * _tmp469 + _tmp274 * _tmp470; + _hessian(12, 1) = _tmp273 * _tmp471 + _tmp274 * _tmp472 + _tmp652; + _hessian(13, 1) = _tmp273 * _tmp473 + _tmp274 * _tmp474 + _tmp653; + _hessian(14, 1) = _tmp273 * _tmp475 + _tmp274 * _tmp476 + _tmp654; + _hessian(15, 1) = _tmp265 * _tmp477 + _tmp269 * _tmp478 + _tmp272 * _tmp479 + + _tmp273 * _tmp480 + _tmp274 * _tmp481 + _tmp655; + _hessian(16, 1) = _tmp265 * _tmp482 + _tmp269 * _tmp483 + _tmp272 * _tmp484 + + _tmp273 * _tmp485 + _tmp274 * _tmp486 + _tmp656; + _hessian(17, 1) = _tmp265 * _tmp487 + _tmp269 * _tmp488 + _tmp272 * _tmp489 + + _tmp273 * _tmp490 + _tmp274 * _tmp491 + _tmp657; + _hessian(18, 1) = -_tmp263 * _tmp492 + _tmp265 * _tmp493 + _tmp269 * _tmp494 + + _tmp272 * _tmp495 + _tmp273 * _tmp496 + _tmp274 * _tmp497; + _hessian(19, 1) = -_tmp263 * _tmp498 + _tmp265 * _tmp499 + _tmp269 * _tmp500 + + _tmp272 * _tmp501 + _tmp273 * _tmp502 + _tmp274 * _tmp503; + _hessian(20, 1) = -_tmp263 * _tmp504 + _tmp265 * _tmp505 + _tmp269 * _tmp506 + + _tmp272 * _tmp507 + _tmp273 * _tmp508 + _tmp274 * _tmp509; + _hessian(21, 1) = _tmp244 * _tmp549 + _tmp248 * _tmp552 + _tmp254 * _tmp557 + + _tmp263 * _tmp558 + _tmp265 * _tmp560 + _tmp269 * _tmp561 + + _tmp272 * _tmp563 + _tmp273 * _tmp564 + _tmp274 * _tmp565; + _hessian(22, 1) = _tmp244 * _tmp589 + _tmp248 * _tmp592 + _tmp254 * _tmp598 + + _tmp263 * _tmp599 + _tmp265 * _tmp601 + _tmp269 * _tmp602 + + _tmp272 * _tmp604 + _tmp273 * _tmp605 + _tmp274 * _tmp606; + _hessian(23, 1) = _tmp244 * _tmp628 + _tmp248 * _tmp631 + _tmp254 * _tmp637 + + _tmp263 * _tmp638 + _tmp265 * _tmp640 + _tmp269 * _tmp641 + + _tmp272 * _tmp643 + _tmp273 * _tmp644 + _tmp274 * _tmp645; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp283, Scalar(2)) + std::pow(_tmp286, Scalar(2)) + std::pow(_tmp292, Scalar(2)) + + std::pow(_tmp295, Scalar(2)) + std::pow(_tmp301, Scalar(2)) + std::pow(_tmp302, Scalar(2)) + + std::pow(_tmp304, Scalar(2)) + std::pow(_tmp306, Scalar(2)) + std::pow(_tmp307, Scalar(2)); + _hessian(3, 2) = _tmp306 * _tmp311 + _tmp307 * _tmp315 - _tmp658; + _hessian(4, 2) = _tmp306 * _tmp319 + _tmp307 * _tmp323 - _tmp659; + _hessian(5, 2) = _tmp306 * _tmp327 + _tmp307 * _tmp331 - _tmp660; + _hessian(6, 2) = _tmp301 * _tmp335 + _tmp302 * _tmp339 + _tmp304 * _tmp343 + _tmp306 * _tmp347 + + _tmp307 * _tmp351 - _tmp661; + _hessian(7, 2) = _tmp301 * _tmp355 + _tmp302 * _tmp359 + _tmp304 * _tmp363 + _tmp306 * _tmp367 + + _tmp307 * _tmp371 - _tmp662; + _hessian(8, 2) = _tmp301 * _tmp375 + _tmp302 * _tmp379 + _tmp304 * _tmp383 + _tmp306 * _tmp387 + + _tmp307 * _tmp391 - _tmp663; + _hessian(9, 2) = _tmp283 * _tmp401 + _tmp286 * _tmp409 + _tmp292 * _tmp420 + _tmp295 * _tmp422 + + _tmp301 * _tmp423 + _tmp302 * _tmp425 + _tmp304 * _tmp426 + _tmp306 * _tmp427 + + _tmp307 * _tmp429; + _hessian(10, 2) = _tmp283 * _tmp434 + _tmp286 * _tmp437 + _tmp292 * _tmp443 + + _tmp295 * _tmp444 + _tmp301 * _tmp446 + _tmp302 * _tmp447 + + _tmp304 * _tmp449 + _tmp306 * _tmp450 + _tmp307 * _tmp451; + _hessian(11, 2) = _tmp283 * _tmp455 + _tmp286 * _tmp459 + _tmp292 * _tmp462 + + _tmp295 * _tmp463 + _tmp301 * _tmp465 + _tmp302 * _tmp466 + + _tmp304 * _tmp468 + _tmp306 * _tmp469 + _tmp307 * _tmp470; + _hessian(12, 2) = _tmp306 * _tmp471 + _tmp307 * _tmp472 + _tmp658; + _hessian(13, 2) = _tmp306 * _tmp473 + _tmp307 * _tmp474 + _tmp659; + _hessian(14, 2) = _tmp306 * _tmp475 + _tmp307 * _tmp476 + _tmp660; + _hessian(15, 2) = _tmp301 * _tmp477 + _tmp302 * _tmp478 + _tmp304 * _tmp479 + + _tmp306 * _tmp480 + _tmp307 * _tmp481 + _tmp661; + _hessian(16, 2) = _tmp301 * _tmp482 + _tmp302 * _tmp483 + _tmp304 * _tmp484 + + _tmp306 * _tmp485 + _tmp307 * _tmp486 + _tmp662; + _hessian(17, 2) = _tmp301 * _tmp487 + _tmp302 * _tmp488 + _tmp304 * _tmp489 + + _tmp306 * _tmp490 + _tmp307 * _tmp491 + _tmp663; + _hessian(18, 2) = -_tmp295 * _tmp492 + _tmp301 * _tmp493 + _tmp302 * _tmp494 + + _tmp304 * _tmp495 + _tmp306 * _tmp496 + _tmp307 * _tmp497; + _hessian(19, 2) = -_tmp295 * _tmp498 + _tmp301 * _tmp499 + _tmp302 * _tmp500 + + _tmp304 * _tmp501 + _tmp306 * _tmp502 + _tmp307 * _tmp503; + _hessian(20, 2) = -_tmp295 * _tmp504 + _tmp301 * _tmp505 + _tmp302 * _tmp506 + + _tmp304 * _tmp507 + _tmp306 * _tmp508 + _tmp307 * _tmp509; + _hessian(21, 2) = _tmp283 * _tmp549 + _tmp286 * _tmp552 + _tmp292 * _tmp557 + + _tmp295 * _tmp558 + _tmp301 * _tmp560 + _tmp302 * _tmp561 + + _tmp304 * _tmp563 + _tmp306 * _tmp564 + _tmp307 * _tmp565; + _hessian(22, 2) = _tmp283 * _tmp589 + _tmp286 * _tmp592 + _tmp292 * _tmp598 + + _tmp295 * _tmp599 + _tmp301 * _tmp601 + _tmp302 * _tmp602 + + _tmp304 * _tmp604 + _tmp306 * _tmp605 + _tmp307 * _tmp606; + _hessian(23, 2) = _tmp283 * _tmp628 + _tmp286 * _tmp631 + _tmp292 * _tmp637 + + _tmp295 * _tmp638 + _tmp301 * _tmp640 + _tmp302 * _tmp641 + + _tmp304 * _tmp643 + _tmp306 * _tmp644 + _tmp307 * _tmp645; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = std::pow(_tmp311, Scalar(2)) + std::pow(_tmp315, Scalar(2)) + _tmp666; + _hessian(4, 3) = _tmp311 * _tmp319 + _tmp315 * _tmp323 + _tmp668; + _hessian(5, 3) = _tmp311 * _tmp327 + _tmp315 * _tmp331 + _tmp669; + _hessian(6, 3) = _tmp311 * _tmp347 + _tmp315 * _tmp351 - _tmp670; + _hessian(7, 3) = _tmp311 * _tmp367 + _tmp315 * _tmp371 - _tmp671; + _hessian(8, 3) = _tmp311 * _tmp387 + _tmp315 * _tmp391 - _tmp672; + _hessian(9, 3) = _tmp311 * _tmp427 + _tmp315 * _tmp429 - _tmp673; + _hessian(10, 3) = _tmp311 * _tmp450 + _tmp315 * _tmp451 - _tmp674; + _hessian(11, 3) = _tmp311 * _tmp469 + _tmp315 * _tmp470 - _tmp675; + _hessian(12, 3) = _tmp311 * _tmp471 + _tmp315 * _tmp472 - _tmp666; + _hessian(13, 3) = _tmp311 * _tmp473 + _tmp315 * _tmp474 + _tmp676; + _hessian(14, 3) = _tmp311 * _tmp475 + _tmp315 * _tmp476 + _tmp677; + _hessian(15, 3) = _tmp311 * _tmp480 + _tmp315 * _tmp481 - _tmp678; + _hessian(16, 3) = _tmp311 * _tmp485 + _tmp315 * _tmp486 - _tmp679; + _hessian(17, 3) = _tmp311 * _tmp490 + _tmp315 * _tmp491 - _tmp680; + _hessian(18, 3) = _tmp311 * _tmp496 + _tmp315 * _tmp497 - _tmp681; + _hessian(19, 3) = _tmp311 * _tmp502 + _tmp315 * _tmp503 - _tmp682; + _hessian(20, 3) = _tmp311 * _tmp508 + _tmp315 * _tmp509 - _tmp683; + _hessian(21, 3) = _tmp311 * _tmp564 + _tmp315 * _tmp565 - _tmp684; + _hessian(22, 3) = _tmp311 * _tmp605 + _tmp315 * _tmp606 - _tmp685; + _hessian(23, 3) = _tmp311 * _tmp644 + _tmp315 * _tmp645 - _tmp686; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = std::pow(_tmp319, Scalar(2)) + std::pow(_tmp323, Scalar(2)) + _tmp688; + _hessian(5, 4) = _tmp319 * _tmp327 + _tmp323 * _tmp331 + _tmp689; + _hessian(6, 4) = _tmp319 * _tmp347 + _tmp323 * _tmp351 - _tmp690; + _hessian(7, 4) = _tmp319 * _tmp367 + _tmp323 * _tmp371 - _tmp691; + _hessian(8, 4) = _tmp319 * _tmp387 + _tmp323 * _tmp391 - _tmp692; + _hessian(9, 4) = _tmp319 * _tmp427 + _tmp323 * _tmp429 - _tmp693; + _hessian(10, 4) = _tmp319 * _tmp450 + _tmp323 * _tmp451 - _tmp694; + _hessian(11, 4) = _tmp319 * _tmp469 + _tmp323 * _tmp470 - _tmp695; + _hessian(12, 4) = _tmp319 * _tmp471 + _tmp323 * _tmp472 + _tmp676; + _hessian(13, 4) = _tmp319 * _tmp473 + _tmp323 * _tmp474 - _tmp688; + _hessian(14, 4) = _tmp319 * _tmp475 + _tmp323 * _tmp476 + _tmp696; + _hessian(15, 4) = _tmp319 * _tmp480 + _tmp323 * _tmp481 - _tmp697; + _hessian(16, 4) = _tmp319 * _tmp485 + _tmp323 * _tmp486 - _tmp698; + _hessian(17, 4) = _tmp319 * _tmp490 + _tmp323 * _tmp491 - _tmp699; + _hessian(18, 4) = _tmp319 * _tmp496 + _tmp323 * _tmp497 - _tmp700; + _hessian(19, 4) = _tmp319 * _tmp502 + _tmp323 * _tmp503 - _tmp701; + _hessian(20, 4) = _tmp319 * _tmp508 + _tmp323 * _tmp509 - _tmp702; + _hessian(21, 4) = _tmp319 * _tmp564 + _tmp323 * _tmp565 - _tmp703; + _hessian(22, 4) = _tmp319 * _tmp605 + _tmp323 * _tmp606 - _tmp704; + _hessian(23, 4) = _tmp319 * _tmp644 + _tmp323 * _tmp645 - _tmp705; + _hessian(0, 5) = 0; + _hessian(1, 5) = 0; + _hessian(2, 5) = 0; + _hessian(3, 5) = 0; + _hessian(4, 5) = 0; + _hessian(5, 5) = std::pow(_tmp327, Scalar(2)) + std::pow(_tmp331, Scalar(2)) + _tmp707; + _hessian(6, 5) = _tmp327 * _tmp347 + _tmp331 * _tmp351 - _tmp708; + _hessian(7, 5) = _tmp327 * _tmp367 + _tmp331 * _tmp371 - _tmp709; + _hessian(8, 5) = _tmp327 * _tmp387 + _tmp331 * _tmp391 - _tmp710; + _hessian(9, 5) = _tmp327 * _tmp427 + _tmp331 * _tmp429 - _tmp711; + _hessian(10, 5) = _tmp327 * _tmp450 + _tmp331 * _tmp451 - _tmp712; + _hessian(11, 5) = _tmp327 * _tmp469 + _tmp331 * _tmp470 - _tmp713; + _hessian(12, 5) = _tmp327 * _tmp471 + _tmp331 * _tmp472 + _tmp677; + _hessian(13, 5) = _tmp327 * _tmp473 + _tmp331 * _tmp474 + _tmp696; + _hessian(14, 5) = _tmp327 * _tmp475 + _tmp331 * _tmp476 - _tmp707; + _hessian(15, 5) = _tmp327 * _tmp480 + _tmp331 * _tmp481 - _tmp714; + _hessian(16, 5) = _tmp327 * _tmp485 + _tmp331 * _tmp486 - _tmp715; + _hessian(17, 5) = _tmp327 * _tmp490 + _tmp331 * _tmp491 - _tmp716; + _hessian(18, 5) = _tmp327 * _tmp496 + _tmp331 * _tmp497 - _tmp717; + _hessian(19, 5) = _tmp327 * _tmp502 + _tmp331 * _tmp503 - _tmp718; + _hessian(20, 5) = _tmp327 * _tmp508 + _tmp331 * _tmp509 - _tmp719; + _hessian(21, 5) = _tmp327 * _tmp564 + _tmp331 * _tmp565 - _tmp720; + _hessian(22, 5) = _tmp327 * _tmp605 + _tmp331 * _tmp606 - _tmp721; + _hessian(23, 5) = _tmp327 * _tmp644 + _tmp331 * _tmp645 - _tmp722; + _hessian(0, 6) = 0; + _hessian(1, 6) = 0; + _hessian(2, 6) = 0; + _hessian(3, 6) = 0; + _hessian(4, 6) = 0; + _hessian(5, 6) = 0; + _hessian(6, 6) = std::pow(_tmp335, Scalar(2)) + std::pow(_tmp339, Scalar(2)) + + std::pow(_tmp343, Scalar(2)) + std::pow(_tmp347, Scalar(2)) + + std::pow(_tmp351, Scalar(2)) + _tmp724; + _hessian(7, 6) = _tmp335 * _tmp355 + _tmp339 * _tmp359 + _tmp343 * _tmp363 + _tmp347 * _tmp367 + + _tmp351 * _tmp371 + _tmp726; + _hessian(8, 6) = _tmp335 * _tmp375 + _tmp339 * _tmp379 + _tmp343 * _tmp383 + _tmp347 * _tmp387 + + _tmp351 * _tmp391 + _tmp728; + _hessian(9, 6) = _tmp335 * _tmp423 + _tmp339 * _tmp425 + _tmp343 * _tmp426 + _tmp347 * _tmp427 + + _tmp351 * _tmp429 - _tmp729; + _hessian(10, 6) = _tmp335 * _tmp446 + _tmp339 * _tmp447 + _tmp343 * _tmp449 + + _tmp347 * _tmp450 + _tmp351 * _tmp451 - _tmp730; + _hessian(11, 6) = _tmp335 * _tmp465 + _tmp339 * _tmp466 + _tmp343 * _tmp468 + + _tmp347 * _tmp469 + _tmp351 * _tmp470 - _tmp731; + _hessian(12, 6) = _tmp347 * _tmp471 + _tmp351 * _tmp472 + _tmp670; + _hessian(13, 6) = _tmp347 * _tmp473 + _tmp351 * _tmp474 + _tmp690; + _hessian(14, 6) = _tmp347 * _tmp475 + _tmp351 * _tmp476 + _tmp708; + _hessian(15, 6) = _tmp335 * _tmp477 + _tmp339 * _tmp478 + _tmp343 * _tmp479 + + _tmp347 * _tmp480 + _tmp351 * _tmp481 - _tmp724; + _hessian(16, 6) = _tmp335 * _tmp482 + _tmp339 * _tmp483 + _tmp343 * _tmp484 + + _tmp347 * _tmp485 + _tmp351 * _tmp486 + _tmp732; + _hessian(17, 6) = _tmp335 * _tmp487 + _tmp339 * _tmp488 + _tmp343 * _tmp489 + + _tmp347 * _tmp490 + _tmp351 * _tmp491 + _tmp733; + _hessian(18, 6) = _tmp335 * _tmp493 + _tmp339 * _tmp494 + _tmp343 * _tmp495 + + _tmp347 * _tmp496 + _tmp351 * _tmp497 + _tmp734; + _hessian(19, 6) = _tmp335 * _tmp499 + _tmp339 * _tmp500 + _tmp343 * _tmp501 + + _tmp347 * _tmp502 + _tmp351 * _tmp503 + _tmp736; + _hessian(20, 6) = _tmp335 * _tmp505 + _tmp339 * _tmp506 + _tmp343 * _tmp507 + + _tmp347 * _tmp508 + _tmp351 * _tmp509 + _tmp737; + _hessian(21, 6) = _tmp335 * _tmp560 + _tmp339 * _tmp561 + _tmp343 * _tmp563 + + _tmp347 * _tmp564 + _tmp351 * _tmp565 - _tmp738; + _hessian(22, 6) = _tmp335 * _tmp601 + _tmp339 * _tmp602 + _tmp343 * _tmp604 + + _tmp347 * _tmp605 + _tmp351 * _tmp606 - _tmp739; + _hessian(23, 6) = _tmp335 * _tmp640 + _tmp339 * _tmp641 + _tmp343 * _tmp643 + + _tmp347 * _tmp644 + _tmp351 * _tmp645 - _tmp740; + _hessian(0, 7) = 0; + _hessian(1, 7) = 0; + _hessian(2, 7) = 0; + _hessian(3, 7) = 0; + _hessian(4, 7) = 0; + _hessian(5, 7) = 0; + _hessian(6, 7) = 0; + _hessian(7, 7) = std::pow(_tmp355, Scalar(2)) + std::pow(_tmp359, Scalar(2)) + + std::pow(_tmp363, Scalar(2)) + std::pow(_tmp367, Scalar(2)) + + std::pow(_tmp371, Scalar(2)) + _tmp741; + _hessian(8, 7) = _tmp355 * _tmp375 + _tmp359 * _tmp379 + _tmp363 * _tmp383 + _tmp367 * _tmp387 + + _tmp371 * _tmp391 + _tmp742; + _hessian(9, 7) = _tmp355 * _tmp423 + _tmp359 * _tmp425 + _tmp363 * _tmp426 + _tmp367 * _tmp427 + + _tmp371 * _tmp429 - _tmp743; + _hessian(10, 7) = _tmp355 * _tmp446 + _tmp359 * _tmp447 + _tmp363 * _tmp449 + + _tmp367 * _tmp450 + _tmp371 * _tmp451 - _tmp744; + _hessian(11, 7) = _tmp355 * _tmp465 + _tmp359 * _tmp466 + _tmp363 * _tmp468 + + _tmp367 * _tmp469 + _tmp371 * _tmp470 - _tmp745; + _hessian(12, 7) = _tmp367 * _tmp471 + _tmp371 * _tmp472 + _tmp671; + _hessian(13, 7) = _tmp367 * _tmp473 + _tmp371 * _tmp474 + _tmp691; + _hessian(14, 7) = _tmp367 * _tmp475 + _tmp371 * _tmp476 + _tmp709; + _hessian(15, 7) = _tmp355 * _tmp477 + _tmp359 * _tmp478 + _tmp363 * _tmp479 + + _tmp367 * _tmp480 + _tmp371 * _tmp481 + _tmp732; + _hessian(16, 7) = _tmp355 * _tmp482 + _tmp359 * _tmp483 + _tmp363 * _tmp484 + + _tmp367 * _tmp485 + _tmp371 * _tmp486 - _tmp741; + _hessian(17, 7) = _tmp355 * _tmp487 + _tmp359 * _tmp488 + _tmp363 * _tmp489 + + _tmp367 * _tmp490 + _tmp371 * _tmp491 + _tmp746; + _hessian(18, 7) = _tmp355 * _tmp493 + _tmp359 * _tmp494 + _tmp363 * _tmp495 + + _tmp367 * _tmp496 + _tmp371 * _tmp497 + _tmp747; + _hessian(19, 7) = _tmp355 * _tmp499 + _tmp359 * _tmp500 + _tmp363 * _tmp501 + + _tmp367 * _tmp502 + _tmp371 * _tmp503 + _tmp748; + _hessian(20, 7) = _tmp355 * _tmp505 + _tmp359 * _tmp506 + _tmp363 * _tmp507 + + _tmp367 * _tmp508 + _tmp371 * _tmp509 + _tmp749; + _hessian(21, 7) = _tmp355 * _tmp560 + _tmp359 * _tmp561 + _tmp363 * _tmp563 + + _tmp367 * _tmp564 + _tmp371 * _tmp565 - _tmp750; + _hessian(22, 7) = _tmp355 * _tmp601 + _tmp359 * _tmp602 + _tmp363 * _tmp604 + + _tmp367 * _tmp605 + _tmp371 * _tmp606 - _tmp751; + _hessian(23, 7) = _tmp355 * _tmp640 + _tmp359 * _tmp641 + _tmp363 * _tmp643 + + _tmp367 * _tmp644 + _tmp371 * _tmp645 - _tmp752; + _hessian(0, 8) = 0; + _hessian(1, 8) = 0; + _hessian(2, 8) = 0; + _hessian(3, 8) = 0; + _hessian(4, 8) = 0; + _hessian(5, 8) = 0; + _hessian(6, 8) = 0; + _hessian(7, 8) = 0; + _hessian(8, 8) = std::pow(_tmp375, Scalar(2)) + std::pow(_tmp379, Scalar(2)) + + std::pow(_tmp383, Scalar(2)) + std::pow(_tmp387, Scalar(2)) + + std::pow(_tmp391, Scalar(2)) + _tmp753; + _hessian(9, 8) = _tmp375 * _tmp423 + _tmp379 * _tmp425 + _tmp383 * _tmp426 + _tmp387 * _tmp427 + + _tmp391 * _tmp429 - _tmp754; + _hessian(10, 8) = _tmp375 * _tmp446 + _tmp379 * _tmp447 + _tmp383 * _tmp449 + + _tmp387 * _tmp450 + _tmp391 * _tmp451 - _tmp755; + _hessian(11, 8) = _tmp375 * _tmp465 + _tmp379 * _tmp466 + _tmp383 * _tmp468 + + _tmp387 * _tmp469 + _tmp391 * _tmp470 - _tmp756; + _hessian(12, 8) = _tmp387 * _tmp471 + _tmp391 * _tmp472 + _tmp672; + _hessian(13, 8) = _tmp387 * _tmp473 + _tmp391 * _tmp474 + _tmp692; + _hessian(14, 8) = _tmp387 * _tmp475 + _tmp391 * _tmp476 + _tmp710; + _hessian(15, 8) = _tmp375 * _tmp477 + _tmp379 * _tmp478 + _tmp383 * _tmp479 + + _tmp387 * _tmp480 + _tmp391 * _tmp481 + _tmp733; + _hessian(16, 8) = _tmp375 * _tmp482 + _tmp379 * _tmp483 + _tmp383 * _tmp484 + + _tmp387 * _tmp485 + _tmp391 * _tmp486 + _tmp746; + _hessian(17, 8) = _tmp375 * _tmp487 + _tmp379 * _tmp488 + _tmp383 * _tmp489 + + _tmp387 * _tmp490 + _tmp391 * _tmp491 - _tmp753; + _hessian(18, 8) = _tmp375 * _tmp493 + _tmp379 * _tmp494 + _tmp383 * _tmp495 + + _tmp387 * _tmp496 + _tmp391 * _tmp497 + _tmp757; + _hessian(19, 8) = _tmp375 * _tmp499 + _tmp379 * _tmp500 + _tmp383 * _tmp501 + + _tmp387 * _tmp502 + _tmp391 * _tmp503 + _tmp758; + _hessian(20, 8) = _tmp375 * _tmp505 + _tmp379 * _tmp506 + _tmp383 * _tmp507 + + _tmp387 * _tmp508 + _tmp391 * _tmp509 + _tmp760; + _hessian(21, 8) = _tmp375 * _tmp560 + _tmp379 * _tmp561 + _tmp383 * _tmp563 + + _tmp387 * _tmp564 + _tmp391 * _tmp565 - _tmp761; + _hessian(22, 8) = _tmp375 * _tmp601 + _tmp379 * _tmp602 + _tmp383 * _tmp604 + + _tmp387 * _tmp605 + _tmp391 * _tmp606 - _tmp762; + _hessian(23, 8) = _tmp375 * _tmp640 + _tmp379 * _tmp641 + _tmp383 * _tmp643 + + _tmp387 * _tmp644 + _tmp391 * _tmp645 - _tmp763; + _hessian(0, 9) = 0; + _hessian(1, 9) = 0; + _hessian(2, 9) = 0; + _hessian(3, 9) = 0; + _hessian(4, 9) = 0; + _hessian(5, 9) = 0; + _hessian(6, 9) = 0; + _hessian(7, 9) = 0; + _hessian(8, 9) = 0; + _hessian(9, 9) = + std::pow(_tmp401, Scalar(2)) + std::pow(_tmp409, Scalar(2)) + std::pow(_tmp420, Scalar(2)) + + std::pow(_tmp422, Scalar(2)) + std::pow(_tmp423, Scalar(2)) + std::pow(_tmp425, Scalar(2)) + + std::pow(_tmp426, Scalar(2)) + std::pow(_tmp427, Scalar(2)) + std::pow(_tmp429, Scalar(2)); + _hessian(10, 9) = _tmp401 * _tmp434 + _tmp409 * _tmp437 + _tmp420 * _tmp443 + + _tmp422 * _tmp444 + _tmp423 * _tmp446 + _tmp425 * _tmp447 + + _tmp426 * _tmp449 + _tmp427 * _tmp450 + _tmp429 * _tmp451; + _hessian(11, 9) = _tmp401 * _tmp455 + _tmp409 * _tmp459 + _tmp420 * _tmp462 + + _tmp422 * _tmp463 + _tmp423 * _tmp465 + _tmp425 * _tmp466 + + _tmp426 * _tmp468 + _tmp427 * _tmp469 + _tmp429 * _tmp470; + _hessian(12, 9) = _tmp427 * _tmp471 + _tmp429 * _tmp472 + _tmp673; + _hessian(13, 9) = _tmp427 * _tmp473 + _tmp429 * _tmp474 + _tmp693; + _hessian(14, 9) = _tmp427 * _tmp475 + _tmp429 * _tmp476 + _tmp711; + _hessian(15, 9) = _tmp423 * _tmp477 + _tmp425 * _tmp478 + _tmp426 * _tmp479 + + _tmp427 * _tmp480 + _tmp429 * _tmp481 + _tmp729; + _hessian(16, 9) = _tmp423 * _tmp482 + _tmp425 * _tmp483 + _tmp426 * _tmp484 + + _tmp427 * _tmp485 + _tmp429 * _tmp486 + _tmp743; + _hessian(17, 9) = _tmp423 * _tmp487 + _tmp425 * _tmp488 + _tmp426 * _tmp489 + + _tmp427 * _tmp490 + _tmp429 * _tmp491 + _tmp754; + _hessian(18, 9) = -_tmp422 * _tmp492 + _tmp423 * _tmp493 + _tmp425 * _tmp494 + + _tmp426 * _tmp495 + _tmp427 * _tmp496 + _tmp429 * _tmp497; + _hessian(19, 9) = -_tmp422 * _tmp498 + _tmp423 * _tmp499 + _tmp425 * _tmp500 + + _tmp426 * _tmp501 + _tmp427 * _tmp502 + _tmp429 * _tmp503; + _hessian(20, 9) = -_tmp422 * _tmp504 + _tmp423 * _tmp505 + _tmp425 * _tmp506 + + _tmp426 * _tmp507 + _tmp427 * _tmp508 + _tmp429 * _tmp509; + _hessian(21, 9) = _tmp401 * _tmp549 + _tmp409 * _tmp552 + _tmp420 * _tmp557 + + _tmp422 * _tmp558 + _tmp423 * _tmp560 + _tmp425 * _tmp561 + + _tmp426 * _tmp563 + _tmp427 * _tmp564 + _tmp429 * _tmp565; + _hessian(22, 9) = _tmp401 * _tmp589 + _tmp409 * _tmp592 + _tmp420 * _tmp598 + + _tmp422 * _tmp599 + _tmp423 * _tmp601 + _tmp425 * _tmp602 + + _tmp426 * _tmp604 + _tmp427 * _tmp605 + _tmp429 * _tmp606; + _hessian(23, 9) = _tmp401 * _tmp628 + _tmp409 * _tmp631 + _tmp420 * _tmp637 + + _tmp422 * _tmp638 + _tmp423 * _tmp640 + _tmp425 * _tmp641 + + _tmp426 * _tmp643 + _tmp427 * _tmp644 + _tmp429 * _tmp645; + _hessian(0, 10) = 0; + _hessian(1, 10) = 0; + _hessian(2, 10) = 0; + _hessian(3, 10) = 0; + _hessian(4, 10) = 0; + _hessian(5, 10) = 0; + _hessian(6, 10) = 0; + _hessian(7, 10) = 0; + _hessian(8, 10) = 0; + _hessian(9, 10) = 0; + _hessian(10, 10) = + std::pow(_tmp434, Scalar(2)) + std::pow(_tmp437, Scalar(2)) + std::pow(_tmp443, Scalar(2)) + + std::pow(_tmp444, Scalar(2)) + std::pow(_tmp446, Scalar(2)) + std::pow(_tmp447, Scalar(2)) + + std::pow(_tmp449, Scalar(2)) + std::pow(_tmp450, Scalar(2)) + std::pow(_tmp451, Scalar(2)); + _hessian(11, 10) = _tmp434 * _tmp455 + _tmp437 * _tmp459 + _tmp443 * _tmp462 + + _tmp444 * _tmp463 + _tmp446 * _tmp465 + _tmp447 * _tmp466 + + _tmp449 * _tmp468 + _tmp450 * _tmp469 + _tmp451 * _tmp470; + _hessian(12, 10) = _tmp450 * _tmp471 + _tmp451 * _tmp472 + _tmp674; + _hessian(13, 10) = _tmp450 * _tmp473 + _tmp451 * _tmp474 + _tmp694; + _hessian(14, 10) = _tmp450 * _tmp475 + _tmp451 * _tmp476 + _tmp712; + _hessian(15, 10) = _tmp446 * _tmp477 + _tmp447 * _tmp478 + _tmp449 * _tmp479 + + _tmp450 * _tmp480 + _tmp451 * _tmp481 + _tmp730; + _hessian(16, 10) = _tmp446 * _tmp482 + _tmp447 * _tmp483 + _tmp449 * _tmp484 + + _tmp450 * _tmp485 + _tmp451 * _tmp486 + _tmp744; + _hessian(17, 10) = _tmp446 * _tmp487 + _tmp447 * _tmp488 + _tmp449 * _tmp489 + + _tmp450 * _tmp490 + _tmp451 * _tmp491 + _tmp755; + _hessian(18, 10) = -_tmp444 * _tmp492 + _tmp446 * _tmp493 + _tmp447 * _tmp494 + + _tmp449 * _tmp495 + _tmp450 * _tmp496 + _tmp451 * _tmp497; + _hessian(19, 10) = -_tmp444 * _tmp498 + _tmp446 * _tmp499 + _tmp447 * _tmp500 + + _tmp449 * _tmp501 + _tmp450 * _tmp502 + _tmp451 * _tmp503; + _hessian(20, 10) = -_tmp444 * _tmp504 + _tmp446 * _tmp505 + _tmp447 * _tmp506 + + _tmp449 * _tmp507 + _tmp450 * _tmp508 + _tmp451 * _tmp509; + _hessian(21, 10) = _tmp434 * _tmp549 + _tmp437 * _tmp552 + _tmp443 * _tmp557 + + _tmp444 * _tmp558 + _tmp446 * _tmp560 + _tmp447 * _tmp561 + + _tmp449 * _tmp563 + _tmp450 * _tmp564 + _tmp451 * _tmp565; + _hessian(22, 10) = _tmp434 * _tmp589 + _tmp437 * _tmp592 + _tmp443 * _tmp598 + + _tmp444 * _tmp599 + _tmp446 * _tmp601 + _tmp447 * _tmp602 + + _tmp449 * _tmp604 + _tmp450 * _tmp605 + _tmp451 * _tmp606; + _hessian(23, 10) = _tmp434 * _tmp628 + _tmp437 * _tmp631 + _tmp443 * _tmp637 + + _tmp444 * _tmp638 + _tmp446 * _tmp640 + _tmp447 * _tmp641 + + _tmp449 * _tmp643 + _tmp450 * _tmp644 + _tmp451 * _tmp645; + _hessian(0, 11) = 0; + _hessian(1, 11) = 0; + _hessian(2, 11) = 0; + _hessian(3, 11) = 0; + _hessian(4, 11) = 0; + _hessian(5, 11) = 0; + _hessian(6, 11) = 0; + _hessian(7, 11) = 0; + _hessian(8, 11) = 0; + _hessian(9, 11) = 0; + _hessian(10, 11) = 0; + _hessian(11, 11) = + std::pow(_tmp455, Scalar(2)) + std::pow(_tmp459, Scalar(2)) + std::pow(_tmp462, Scalar(2)) + + std::pow(_tmp463, Scalar(2)) + std::pow(_tmp465, Scalar(2)) + std::pow(_tmp466, Scalar(2)) + + std::pow(_tmp468, Scalar(2)) + std::pow(_tmp469, Scalar(2)) + std::pow(_tmp470, Scalar(2)); + _hessian(12, 11) = _tmp469 * _tmp471 + _tmp470 * _tmp472 + _tmp675; + _hessian(13, 11) = _tmp469 * _tmp473 + _tmp470 * _tmp474 + _tmp695; + _hessian(14, 11) = _tmp469 * _tmp475 + _tmp470 * _tmp476 + _tmp713; + _hessian(15, 11) = _tmp465 * _tmp477 + _tmp466 * _tmp478 + _tmp468 * _tmp479 + + _tmp469 * _tmp480 + _tmp470 * _tmp481 + _tmp731; + _hessian(16, 11) = _tmp465 * _tmp482 + _tmp466 * _tmp483 + _tmp468 * _tmp484 + + _tmp469 * _tmp485 + _tmp470 * _tmp486 + _tmp745; + _hessian(17, 11) = _tmp465 * _tmp487 + _tmp466 * _tmp488 + _tmp468 * _tmp489 + + _tmp469 * _tmp490 + _tmp470 * _tmp491 + _tmp756; + _hessian(18, 11) = -_tmp463 * _tmp492 + _tmp465 * _tmp493 + _tmp466 * _tmp494 + + _tmp468 * _tmp495 + _tmp469 * _tmp496 + _tmp470 * _tmp497; + _hessian(19, 11) = -_tmp463 * _tmp498 + _tmp465 * _tmp499 + _tmp466 * _tmp500 + + _tmp468 * _tmp501 + _tmp469 * _tmp502 + _tmp470 * _tmp503; + _hessian(20, 11) = -_tmp463 * _tmp504 + _tmp465 * _tmp505 + _tmp466 * _tmp506 + + _tmp468 * _tmp507 + _tmp469 * _tmp508 + _tmp470 * _tmp509; + _hessian(21, 11) = _tmp455 * _tmp549 + _tmp459 * _tmp552 + _tmp462 * _tmp557 + + _tmp463 * _tmp558 + _tmp465 * _tmp560 + _tmp466 * _tmp561 + + _tmp468 * _tmp563 + _tmp469 * _tmp564 + _tmp470 * _tmp565; + _hessian(22, 11) = _tmp455 * _tmp589 + _tmp459 * _tmp592 + _tmp462 * _tmp598 + + _tmp463 * _tmp599 + _tmp465 * _tmp601 + _tmp466 * _tmp602 + + _tmp468 * _tmp604 + _tmp469 * _tmp605 + _tmp470 * _tmp606; + _hessian(23, 11) = _tmp455 * _tmp628 + _tmp459 * _tmp631 + _tmp462 * _tmp637 + + _tmp463 * _tmp638 + _tmp465 * _tmp640 + _tmp466 * _tmp641 + + _tmp468 * _tmp643 + _tmp469 * _tmp644 + _tmp470 * _tmp645; + _hessian(0, 12) = 0; + _hessian(1, 12) = 0; + _hessian(2, 12) = 0; + _hessian(3, 12) = 0; + _hessian(4, 12) = 0; + _hessian(5, 12) = 0; + _hessian(6, 12) = 0; + _hessian(7, 12) = 0; + _hessian(8, 12) = 0; + _hessian(9, 12) = 0; + _hessian(10, 12) = 0; + _hessian(11, 12) = 0; + _hessian(12, 12) = std::pow(_tmp471, Scalar(2)) + std::pow(_tmp472, Scalar(2)) + _tmp666; + _hessian(13, 12) = _tmp471 * _tmp473 + _tmp472 * _tmp474 + _tmp668; + _hessian(14, 12) = _tmp471 * _tmp475 + _tmp472 * _tmp476 + _tmp669; + _hessian(15, 12) = _tmp471 * _tmp480 + _tmp472 * _tmp481 + _tmp678; + _hessian(16, 12) = _tmp471 * _tmp485 + _tmp472 * _tmp486 + _tmp679; + _hessian(17, 12) = _tmp471 * _tmp490 + _tmp472 * _tmp491 + _tmp680; + _hessian(18, 12) = _tmp471 * _tmp496 + _tmp472 * _tmp497 + _tmp681; + _hessian(19, 12) = _tmp471 * _tmp502 + _tmp472 * _tmp503 + _tmp682; + _hessian(20, 12) = _tmp471 * _tmp508 + _tmp472 * _tmp509 + _tmp683; + _hessian(21, 12) = _tmp471 * _tmp564 + _tmp472 * _tmp565 + _tmp684; + _hessian(22, 12) = _tmp471 * _tmp605 + _tmp472 * _tmp606 + _tmp685; + _hessian(23, 12) = _tmp471 * _tmp644 + _tmp472 * _tmp645 + _tmp686; + _hessian(0, 13) = 0; + _hessian(1, 13) = 0; + _hessian(2, 13) = 0; + _hessian(3, 13) = 0; + _hessian(4, 13) = 0; + _hessian(5, 13) = 0; + _hessian(6, 13) = 0; + _hessian(7, 13) = 0; + _hessian(8, 13) = 0; + _hessian(9, 13) = 0; + _hessian(10, 13) = 0; + _hessian(11, 13) = 0; + _hessian(12, 13) = 0; + _hessian(13, 13) = std::pow(_tmp473, Scalar(2)) + std::pow(_tmp474, Scalar(2)) + _tmp688; + _hessian(14, 13) = _tmp473 * _tmp475 + _tmp474 * _tmp476 + _tmp689; + _hessian(15, 13) = _tmp473 * _tmp480 + _tmp474 * _tmp481 + _tmp697; + _hessian(16, 13) = _tmp473 * _tmp485 + _tmp474 * _tmp486 + _tmp698; + _hessian(17, 13) = _tmp473 * _tmp490 + _tmp474 * _tmp491 + _tmp699; + _hessian(18, 13) = _tmp473 * _tmp496 + _tmp474 * _tmp497 + _tmp700; + _hessian(19, 13) = _tmp473 * _tmp502 + _tmp474 * _tmp503 + _tmp701; + _hessian(20, 13) = _tmp473 * _tmp508 + _tmp474 * _tmp509 + _tmp702; + _hessian(21, 13) = _tmp473 * _tmp564 + _tmp474 * _tmp565 + _tmp703; + _hessian(22, 13) = _tmp473 * _tmp605 + _tmp474 * _tmp606 + _tmp704; + _hessian(23, 13) = _tmp473 * _tmp644 + _tmp474 * _tmp645 + _tmp705; + _hessian(0, 14) = 0; + _hessian(1, 14) = 0; + _hessian(2, 14) = 0; + _hessian(3, 14) = 0; + _hessian(4, 14) = 0; + _hessian(5, 14) = 0; + _hessian(6, 14) = 0; + _hessian(7, 14) = 0; + _hessian(8, 14) = 0; + _hessian(9, 14) = 0; + _hessian(10, 14) = 0; + _hessian(11, 14) = 0; + _hessian(12, 14) = 0; + _hessian(13, 14) = 0; + _hessian(14, 14) = std::pow(_tmp475, Scalar(2)) + std::pow(_tmp476, Scalar(2)) + _tmp707; + _hessian(15, 14) = _tmp475 * _tmp480 + _tmp476 * _tmp481 + _tmp714; + _hessian(16, 14) = _tmp475 * _tmp485 + _tmp476 * _tmp486 + _tmp715; + _hessian(17, 14) = _tmp475 * _tmp490 + _tmp476 * _tmp491 + _tmp716; + _hessian(18, 14) = _tmp475 * _tmp496 + _tmp476 * _tmp497 + _tmp717; + _hessian(19, 14) = _tmp475 * _tmp502 + _tmp476 * _tmp503 + _tmp718; + _hessian(20, 14) = _tmp475 * _tmp508 + _tmp476 * _tmp509 + _tmp719; + _hessian(21, 14) = _tmp475 * _tmp564 + _tmp476 * _tmp565 + _tmp720; + _hessian(22, 14) = _tmp475 * _tmp605 + _tmp476 * _tmp606 + _tmp721; + _hessian(23, 14) = _tmp475 * _tmp644 + _tmp476 * _tmp645 + _tmp722; + _hessian(0, 15) = 0; + _hessian(1, 15) = 0; + _hessian(2, 15) = 0; + _hessian(3, 15) = 0; + _hessian(4, 15) = 0; + _hessian(5, 15) = 0; + _hessian(6, 15) = 0; + _hessian(7, 15) = 0; + _hessian(8, 15) = 0; + _hessian(9, 15) = 0; + _hessian(10, 15) = 0; + _hessian(11, 15) = 0; + _hessian(12, 15) = 0; + _hessian(13, 15) = 0; + _hessian(14, 15) = 0; + _hessian(15, 15) = std::pow(_tmp477, Scalar(2)) + std::pow(_tmp478, Scalar(2)) + + std::pow(_tmp479, Scalar(2)) + std::pow(_tmp480, Scalar(2)) + + std::pow(_tmp481, Scalar(2)) + _tmp724; + _hessian(16, 15) = _tmp477 * _tmp482 + _tmp478 * _tmp483 + _tmp479 * _tmp484 + + _tmp480 * _tmp485 + _tmp481 * _tmp486 + _tmp726; + _hessian(17, 15) = _tmp477 * _tmp487 + _tmp478 * _tmp488 + _tmp479 * _tmp489 + + _tmp480 * _tmp490 + _tmp481 * _tmp491 + _tmp728; + _hessian(18, 15) = _tmp477 * _tmp493 + _tmp478 * _tmp494 + _tmp479 * _tmp495 + + _tmp480 * _tmp496 + _tmp481 * _tmp497 - _tmp734; + _hessian(19, 15) = _tmp477 * _tmp499 + _tmp478 * _tmp500 + _tmp479 * _tmp501 + + _tmp480 * _tmp502 + _tmp481 * _tmp503 - _tmp736; + _hessian(20, 15) = _tmp477 * _tmp505 + _tmp478 * _tmp506 + _tmp479 * _tmp507 + + _tmp480 * _tmp508 + _tmp481 * _tmp509 - _tmp737; + _hessian(21, 15) = _tmp477 * _tmp560 + _tmp478 * _tmp561 + _tmp479 * _tmp563 + + _tmp480 * _tmp564 + _tmp481 * _tmp565 + _tmp738; + _hessian(22, 15) = _tmp477 * _tmp601 + _tmp478 * _tmp602 + _tmp479 * _tmp604 + + _tmp480 * _tmp605 + _tmp481 * _tmp606 + _tmp739; + _hessian(23, 15) = _tmp477 * _tmp640 + _tmp478 * _tmp641 + _tmp479 * _tmp643 + + _tmp480 * _tmp644 + _tmp481 * _tmp645 + _tmp740; + _hessian(0, 16) = 0; + _hessian(1, 16) = 0; + _hessian(2, 16) = 0; + _hessian(3, 16) = 0; + _hessian(4, 16) = 0; + _hessian(5, 16) = 0; + _hessian(6, 16) = 0; + _hessian(7, 16) = 0; + _hessian(8, 16) = 0; + _hessian(9, 16) = 0; + _hessian(10, 16) = 0; + _hessian(11, 16) = 0; + _hessian(12, 16) = 0; + _hessian(13, 16) = 0; + _hessian(14, 16) = 0; + _hessian(15, 16) = 0; + _hessian(16, 16) = std::pow(_tmp482, Scalar(2)) + std::pow(_tmp483, Scalar(2)) + + std::pow(_tmp484, Scalar(2)) + std::pow(_tmp485, Scalar(2)) + + std::pow(_tmp486, Scalar(2)) + _tmp741; + _hessian(17, 16) = _tmp482 * _tmp487 + _tmp483 * _tmp488 + _tmp484 * _tmp489 + + _tmp485 * _tmp490 + _tmp486 * _tmp491 + _tmp742; + _hessian(18, 16) = _tmp482 * _tmp493 + _tmp483 * _tmp494 + _tmp484 * _tmp495 + + _tmp485 * _tmp496 + _tmp486 * _tmp497 - _tmp747; + _hessian(19, 16) = _tmp482 * _tmp499 + _tmp483 * _tmp500 + _tmp484 * _tmp501 + + _tmp485 * _tmp502 + _tmp486 * _tmp503 - _tmp748; + _hessian(20, 16) = _tmp482 * _tmp505 + _tmp483 * _tmp506 + _tmp484 * _tmp507 + + _tmp485 * _tmp508 + _tmp486 * _tmp509 - _tmp749; + _hessian(21, 16) = _tmp482 * _tmp560 + _tmp483 * _tmp561 + _tmp484 * _tmp563 + + _tmp485 * _tmp564 + _tmp486 * _tmp565 + _tmp750; + _hessian(22, 16) = _tmp482 * _tmp601 + _tmp483 * _tmp602 + _tmp484 * _tmp604 + + _tmp485 * _tmp605 + _tmp486 * _tmp606 + _tmp751; + _hessian(23, 16) = _tmp482 * _tmp640 + _tmp483 * _tmp641 + _tmp484 * _tmp643 + + _tmp485 * _tmp644 + _tmp486 * _tmp645 + _tmp752; + _hessian(0, 17) = 0; + _hessian(1, 17) = 0; + _hessian(2, 17) = 0; + _hessian(3, 17) = 0; + _hessian(4, 17) = 0; + _hessian(5, 17) = 0; + _hessian(6, 17) = 0; + _hessian(7, 17) = 0; + _hessian(8, 17) = 0; + _hessian(9, 17) = 0; + _hessian(10, 17) = 0; + _hessian(11, 17) = 0; + _hessian(12, 17) = 0; + _hessian(13, 17) = 0; + _hessian(14, 17) = 0; + _hessian(15, 17) = 0; + _hessian(16, 17) = 0; + _hessian(17, 17) = std::pow(_tmp487, Scalar(2)) + std::pow(_tmp488, Scalar(2)) + + std::pow(_tmp489, Scalar(2)) + std::pow(_tmp490, Scalar(2)) + + std::pow(_tmp491, Scalar(2)) + _tmp753; + _hessian(18, 17) = _tmp487 * _tmp493 + _tmp488 * _tmp494 + _tmp489 * _tmp495 + + _tmp490 * _tmp496 + _tmp491 * _tmp497 - _tmp757; + _hessian(19, 17) = _tmp487 * _tmp499 + _tmp488 * _tmp500 + _tmp489 * _tmp501 + + _tmp490 * _tmp502 + _tmp491 * _tmp503 - _tmp758; + _hessian(20, 17) = _tmp487 * _tmp505 + _tmp488 * _tmp506 + _tmp489 * _tmp507 + + _tmp490 * _tmp508 + _tmp491 * _tmp509 - _tmp760; + _hessian(21, 17) = _tmp487 * _tmp560 + _tmp488 * _tmp561 + _tmp489 * _tmp563 + + _tmp490 * _tmp564 + _tmp491 * _tmp565 + _tmp761; + _hessian(22, 17) = _tmp487 * _tmp601 + _tmp488 * _tmp602 + _tmp489 * _tmp604 + + _tmp490 * _tmp605 + _tmp491 * _tmp606 + _tmp762; + _hessian(23, 17) = _tmp487 * _tmp640 + _tmp488 * _tmp641 + _tmp489 * _tmp643 + + _tmp490 * _tmp644 + _tmp491 * _tmp645 + _tmp763; + _hessian(0, 18) = 0; + _hessian(1, 18) = 0; + _hessian(2, 18) = 0; + _hessian(3, 18) = 0; + _hessian(4, 18) = 0; + _hessian(5, 18) = 0; + _hessian(6, 18) = 0; + _hessian(7, 18) = 0; + _hessian(8, 18) = 0; + _hessian(9, 18) = 0; + _hessian(10, 18) = 0; + _hessian(11, 18) = 0; + _hessian(12, 18) = 0; + _hessian(13, 18) = 0; + _hessian(14, 18) = 0; + _hessian(15, 18) = 0; + _hessian(16, 18) = 0; + _hessian(17, 18) = 0; + _hessian(18, 18) = std::pow(Dv_D_accel_bias(0, 0), Scalar(2)) * _tmp723 + + std::pow(_tmp493, Scalar(2)) + std::pow(_tmp494, Scalar(2)) + + std::pow(_tmp495, Scalar(2)) + std::pow(_tmp496, Scalar(2)) + + std::pow(_tmp497, Scalar(2)); + _hessian(19, 18) = Dv_D_accel_bias(0, 0) * _tmp735 + _tmp493 * _tmp499 + _tmp494 * _tmp500 + + _tmp495 * _tmp501 + _tmp496 * _tmp502 + _tmp497 * _tmp503; + _hessian(20, 18) = Dv_D_accel_bias(0, 0) * _tmp759 + _tmp493 * _tmp505 + _tmp494 * _tmp506 + + _tmp495 * _tmp507 + _tmp496 * _tmp508 + _tmp497 * _tmp509; + _hessian(21, 18) = -_tmp492 * _tmp558 + _tmp493 * _tmp560 + _tmp494 * _tmp561 + + _tmp495 * _tmp563 + _tmp496 * _tmp564 + _tmp497 * _tmp565; + _hessian(22, 18) = -_tmp492 * _tmp599 + _tmp493 * _tmp601 + _tmp494 * _tmp602 + + _tmp495 * _tmp604 + _tmp496 * _tmp605 + _tmp497 * _tmp606; + _hessian(23, 18) = -_tmp492 * _tmp638 + _tmp493 * _tmp640 + _tmp494 * _tmp641 + + _tmp495 * _tmp643 + _tmp496 * _tmp644 + _tmp497 * _tmp645; + _hessian(0, 19) = 0; + _hessian(1, 19) = 0; + _hessian(2, 19) = 0; + _hessian(3, 19) = 0; + _hessian(4, 19) = 0; + _hessian(5, 19) = 0; + _hessian(6, 19) = 0; + _hessian(7, 19) = 0; + _hessian(8, 19) = 0; + _hessian(9, 19) = 0; + _hessian(10, 19) = 0; + _hessian(11, 19) = 0; + _hessian(12, 19) = 0; + _hessian(13, 19) = 0; + _hessian(14, 19) = 0; + _hessian(15, 19) = 0; + _hessian(16, 19) = 0; + _hessian(17, 19) = 0; + _hessian(18, 19) = 0; + _hessian(19, 19) = std::pow(Dv_D_accel_bias(0, 1), Scalar(2)) * _tmp723 + + std::pow(_tmp499, Scalar(2)) + std::pow(_tmp500, Scalar(2)) + + std::pow(_tmp501, Scalar(2)) + std::pow(_tmp502, Scalar(2)) + + std::pow(_tmp503, Scalar(2)); + _hessian(20, 19) = Dv_D_accel_bias(0, 2) * _tmp735 + _tmp499 * _tmp505 + _tmp500 * _tmp506 + + _tmp501 * _tmp507 + _tmp502 * _tmp508 + _tmp503 * _tmp509; + _hessian(21, 19) = -_tmp498 * _tmp558 + _tmp499 * _tmp560 + _tmp500 * _tmp561 + + _tmp501 * _tmp563 + _tmp502 * _tmp564 + _tmp503 * _tmp565; + _hessian(22, 19) = -_tmp498 * _tmp599 + _tmp499 * _tmp601 + _tmp500 * _tmp602 + + _tmp501 * _tmp604 + _tmp502 * _tmp605 + _tmp503 * _tmp606; + _hessian(23, 19) = -_tmp498 * _tmp638 + _tmp499 * _tmp640 + _tmp500 * _tmp641 + + _tmp501 * _tmp643 + _tmp502 * _tmp644 + _tmp503 * _tmp645; + _hessian(0, 20) = 0; + _hessian(1, 20) = 0; + _hessian(2, 20) = 0; + _hessian(3, 20) = 0; + _hessian(4, 20) = 0; + _hessian(5, 20) = 0; + _hessian(6, 20) = 0; + _hessian(7, 20) = 0; + _hessian(8, 20) = 0; + _hessian(9, 20) = 0; + _hessian(10, 20) = 0; + _hessian(11, 20) = 0; + _hessian(12, 20) = 0; + _hessian(13, 20) = 0; + _hessian(14, 20) = 0; + _hessian(15, 20) = 0; + _hessian(16, 20) = 0; + _hessian(17, 20) = 0; + _hessian(18, 20) = 0; + _hessian(19, 20) = 0; + _hessian(20, 20) = std::pow(Dv_D_accel_bias(0, 2), Scalar(2)) * _tmp723 + + std::pow(_tmp505, Scalar(2)) + std::pow(_tmp506, Scalar(2)) + + std::pow(_tmp507, Scalar(2)) + std::pow(_tmp508, Scalar(2)) + + std::pow(_tmp509, Scalar(2)); + _hessian(21, 20) = -_tmp504 * _tmp558 + _tmp505 * _tmp560 + _tmp506 * _tmp561 + + _tmp507 * _tmp563 + _tmp508 * _tmp564 + _tmp509 * _tmp565; + _hessian(22, 20) = -_tmp504 * _tmp599 + _tmp505 * _tmp601 + _tmp506 * _tmp602 + + _tmp507 * _tmp604 + _tmp508 * _tmp605 + _tmp509 * _tmp606; + _hessian(23, 20) = -_tmp504 * _tmp638 + _tmp505 * _tmp640 + _tmp506 * _tmp641 + + _tmp507 * _tmp643 + _tmp508 * _tmp644 + _tmp509 * _tmp645; + _hessian(0, 21) = 0; + _hessian(1, 21) = 0; + _hessian(2, 21) = 0; + _hessian(3, 21) = 0; + _hessian(4, 21) = 0; + _hessian(5, 21) = 0; + _hessian(6, 21) = 0; + _hessian(7, 21) = 0; + _hessian(8, 21) = 0; + _hessian(9, 21) = 0; + _hessian(10, 21) = 0; + _hessian(11, 21) = 0; + _hessian(12, 21) = 0; + _hessian(13, 21) = 0; + _hessian(14, 21) = 0; + _hessian(15, 21) = 0; + _hessian(16, 21) = 0; + _hessian(17, 21) = 0; + _hessian(18, 21) = 0; + _hessian(19, 21) = 0; + _hessian(20, 21) = 0; + _hessian(21, 21) = + std::pow(_tmp549, Scalar(2)) + std::pow(_tmp552, Scalar(2)) + std::pow(_tmp557, Scalar(2)) + + std::pow(_tmp558, Scalar(2)) + std::pow(_tmp560, Scalar(2)) + std::pow(_tmp561, Scalar(2)) + + std::pow(_tmp563, Scalar(2)) + std::pow(_tmp564, Scalar(2)) + std::pow(_tmp565, Scalar(2)); + _hessian(22, 21) = _tmp549 * _tmp589 + _tmp552 * _tmp592 + _tmp557 * _tmp598 + + _tmp558 * _tmp599 + _tmp560 * _tmp601 + _tmp561 * _tmp602 + + _tmp563 * _tmp604 + _tmp564 * _tmp605 + _tmp565 * _tmp606; + _hessian(23, 21) = _tmp549 * _tmp628 + _tmp552 * _tmp631 + _tmp557 * _tmp637 + + _tmp558 * _tmp638 + _tmp560 * _tmp640 + _tmp561 * _tmp641 + + _tmp563 * _tmp643 + _tmp564 * _tmp644 + _tmp565 * _tmp645; + _hessian(0, 22) = 0; + _hessian(1, 22) = 0; + _hessian(2, 22) = 0; + _hessian(3, 22) = 0; + _hessian(4, 22) = 0; + _hessian(5, 22) = 0; + _hessian(6, 22) = 0; + _hessian(7, 22) = 0; + _hessian(8, 22) = 0; + _hessian(9, 22) = 0; + _hessian(10, 22) = 0; + _hessian(11, 22) = 0; + _hessian(12, 22) = 0; + _hessian(13, 22) = 0; + _hessian(14, 22) = 0; + _hessian(15, 22) = 0; + _hessian(16, 22) = 0; + _hessian(17, 22) = 0; + _hessian(18, 22) = 0; + _hessian(19, 22) = 0; + _hessian(20, 22) = 0; + _hessian(21, 22) = 0; + _hessian(22, 22) = + std::pow(_tmp589, Scalar(2)) + std::pow(_tmp592, Scalar(2)) + std::pow(_tmp598, Scalar(2)) + + std::pow(_tmp599, Scalar(2)) + std::pow(_tmp601, Scalar(2)) + std::pow(_tmp602, Scalar(2)) + + std::pow(_tmp604, Scalar(2)) + std::pow(_tmp605, Scalar(2)) + std::pow(_tmp606, Scalar(2)); + _hessian(23, 22) = _tmp589 * _tmp628 + _tmp592 * _tmp631 + _tmp598 * _tmp637 + + _tmp599 * _tmp638 + _tmp601 * _tmp640 + _tmp602 * _tmp641 + + _tmp604 * _tmp643 + _tmp605 * _tmp644 + _tmp606 * _tmp645; + _hessian(0, 23) = 0; + _hessian(1, 23) = 0; + _hessian(2, 23) = 0; + _hessian(3, 23) = 0; + _hessian(4, 23) = 0; + _hessian(5, 23) = 0; + _hessian(6, 23) = 0; + _hessian(7, 23) = 0; + _hessian(8, 23) = 0; + _hessian(9, 23) = 0; + _hessian(10, 23) = 0; + _hessian(11, 23) = 0; + _hessian(12, 23) = 0; + _hessian(13, 23) = 0; + _hessian(14, 23) = 0; + _hessian(15, 23) = 0; + _hessian(16, 23) = 0; + _hessian(17, 23) = 0; + _hessian(18, 23) = 0; + _hessian(19, 23) = 0; + _hessian(20, 23) = 0; + _hessian(21, 23) = 0; + _hessian(22, 23) = 0; + _hessian(23, 23) = + std::pow(_tmp628, Scalar(2)) + std::pow(_tmp631, Scalar(2)) + std::pow(_tmp637, Scalar(2)) + + std::pow(_tmp638, Scalar(2)) + std::pow(_tmp640, Scalar(2)) + std::pow(_tmp641, Scalar(2)) + + std::pow(_tmp643, Scalar(2)) + std::pow(_tmp644, Scalar(2)) + std::pow(_tmp645, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp101 * _tmp195 + _tmp112 * _tmp206 + _tmp119 * _tmp215 + _tmp127 * _tmp220 + + _tmp130 * _tmp225 + _tmp132 * _tmp230 + _tmp178 * _tmp63 + _tmp183 * _tmp71 + + _tmp191 * _tmp78; + _rhs(1, 0) = _tmp101 * _tmp263 + _tmp112 * _tmp265 + _tmp119 * _tmp269 + _tmp127 * _tmp272 + + _tmp130 * _tmp273 + _tmp132 * _tmp274 + _tmp244 * _tmp63 + _tmp248 * _tmp71 + + _tmp254 * _tmp78; + _rhs(2, 0) = _tmp101 * _tmp295 + _tmp112 * _tmp301 + _tmp119 * _tmp302 + _tmp127 * _tmp304 + + _tmp130 * _tmp306 + _tmp132 * _tmp307 + _tmp283 * _tmp63 + _tmp286 * _tmp71 + + _tmp292 * _tmp78; + _rhs(3, 0) = _tmp130 * _tmp311 + _tmp132 * _tmp315 - _tmp764; + _rhs(4, 0) = _tmp130 * _tmp319 + _tmp132 * _tmp323 - _tmp765; + _rhs(5, 0) = _tmp130 * _tmp327 + _tmp132 * _tmp331 - _tmp766; + _rhs(6, 0) = _tmp112 * _tmp335 + _tmp119 * _tmp339 + _tmp127 * _tmp343 + _tmp130 * _tmp347 + + _tmp132 * _tmp351 - _tmp767; + _rhs(7, 0) = _tmp112 * _tmp355 + _tmp119 * _tmp359 + _tmp127 * _tmp363 + _tmp130 * _tmp367 + + _tmp132 * _tmp371 - _tmp768; + _rhs(8, 0) = _tmp112 * _tmp375 + _tmp119 * _tmp379 + _tmp127 * _tmp383 + _tmp130 * _tmp387 + + _tmp132 * _tmp391 - _tmp769; + _rhs(9, 0) = _tmp101 * _tmp422 + _tmp112 * _tmp423 + _tmp119 * _tmp425 + _tmp127 * _tmp426 + + _tmp130 * _tmp427 + _tmp132 * _tmp429 + _tmp401 * _tmp63 + _tmp409 * _tmp71 + + _tmp420 * _tmp78; + _rhs(10, 0) = _tmp101 * _tmp444 + _tmp112 * _tmp446 + _tmp119 * _tmp447 + _tmp127 * _tmp449 + + _tmp130 * _tmp450 + _tmp132 * _tmp451 + _tmp434 * _tmp63 + _tmp437 * _tmp71 + + _tmp443 * _tmp78; + _rhs(11, 0) = _tmp101 * _tmp463 + _tmp112 * _tmp465 + _tmp119 * _tmp466 + _tmp127 * _tmp468 + + _tmp130 * _tmp469 + _tmp132 * _tmp470 + _tmp455 * _tmp63 + _tmp459 * _tmp71 + + _tmp462 * _tmp78; + _rhs(12, 0) = _tmp130 * _tmp471 + _tmp132 * _tmp472 + _tmp764; + _rhs(13, 0) = _tmp130 * _tmp473 + _tmp132 * _tmp474 + _tmp765; + _rhs(14, 0) = _tmp130 * _tmp475 + _tmp132 * _tmp476 + _tmp766; + _rhs(15, 0) = _tmp112 * _tmp477 + _tmp119 * _tmp478 + _tmp127 * _tmp479 + _tmp130 * _tmp480 + + _tmp132 * _tmp481 + _tmp767; + _rhs(16, 0) = _tmp112 * _tmp482 + _tmp119 * _tmp483 + _tmp127 * _tmp484 + _tmp130 * _tmp485 + + _tmp132 * _tmp486 + _tmp768; + _rhs(17, 0) = _tmp112 * _tmp487 + _tmp119 * _tmp488 + _tmp127 * _tmp489 + _tmp130 * _tmp490 + + _tmp132 * _tmp491 + _tmp769; + _rhs(18, 0) = -_tmp101 * _tmp492 + _tmp112 * _tmp493 + _tmp119 * _tmp494 + _tmp127 * _tmp495 + + _tmp130 * _tmp496 + _tmp132 * _tmp497; + _rhs(19, 0) = -_tmp101 * _tmp498 + _tmp112 * _tmp499 + _tmp119 * _tmp500 + _tmp127 * _tmp501 + + _tmp130 * _tmp502 + _tmp132 * _tmp503; + _rhs(20, 0) = -_tmp101 * _tmp504 + _tmp112 * _tmp505 + _tmp119 * _tmp506 + _tmp127 * _tmp507 + + _tmp130 * _tmp508 + _tmp132 * _tmp509; + _rhs(21, 0) = _tmp101 * _tmp558 + _tmp112 * _tmp560 + _tmp119 * _tmp561 + _tmp127 * _tmp563 + + _tmp130 * _tmp564 + _tmp132 * _tmp565 + _tmp549 * _tmp63 + _tmp552 * _tmp71 + + _tmp557 * _tmp78; + _rhs(22, 0) = _tmp101 * _tmp599 + _tmp112 * _tmp601 + _tmp119 * _tmp602 + _tmp127 * _tmp604 + + _tmp130 * _tmp605 + _tmp132 * _tmp606 + _tmp589 * _tmp63 + _tmp592 * _tmp71 + + _tmp598 * _tmp78; + _rhs(23, 0) = _tmp101 * _tmp638 + _tmp112 * _tmp640 + _tmp119 * _tmp641 + _tmp127 * _tmp643 + + _tmp130 * _tmp644 + _tmp132 * _tmp645 + _tmp628 * _tmp63 + _tmp631 * _tmp71 + + _tmp637 * _tmp78; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/symforce/geo/matrix.py b/symforce/geo/matrix.py index d00699edd..70927bfdf 100644 --- a/symforce/geo/matrix.py +++ b/symforce/geo/matrix.py @@ -487,6 +487,23 @@ def transpose(self) -> Matrix: """ return Matrix(self.mat.transpose()) + def lower_triangle(self: MatrixT) -> MatrixT: + """ + Returns the lower triangle (including diagonal) of self + + self must be square + """ + rows, cols = self.shape + if rows != cols: + raise ValueError( + f"Attempted to take lower triangle of non-square matrix (found shape {self.shape})" + ) + + lt = self.__class__() + for k in range(rows): + lt[k, : k + 1] = self[k, : k + 1] + return lt + def reshape(self, rows: int, cols: int) -> Matrix: return Matrix(self.mat.reshape(rows, cols)) diff --git a/symforce/pybind/CMakeLists.txt b/symforce/pybind/CMakeLists.txt index 2034b5ec9..085b3d765 100644 --- a/symforce/pybind/CMakeLists.txt +++ b/symforce/pybind/CMakeLists.txt @@ -3,6 +3,10 @@ # This source code is under the Apache 2.0 license found in the LICENSE file. # ---------------------------------------------------------------------------- +# ============================================================================== +# Third Party Dependencies +# ============================================================================== + include(FetchContent) find_package(pybind11 QUIET) @@ -21,6 +25,13 @@ else() message(STATUS "pybind11 found") endif() +# ============================================================================== +# SymForce Targets +# ============================================================================== + +# ------------------------------------------------------------------------------ +# cc_sym + pybind11_add_module( cc_sym SHARED diff --git a/symforce/slam/CMakeLists.txt b/symforce/slam/CMakeLists.txt new file mode 100644 index 000000000..2b1b8c0e8 --- /dev/null +++ b/symforce/slam/CMakeLists.txt @@ -0,0 +1,24 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +# ============================================================================== +# SymForce Targets +# ============================================================================== + +file(GLOB_RECURSE SYMFORCE_SLAM_SOURCES CONFIGURE_DEPENDS *.cc **/*.cc) +file(GLOB_RECURSE SYMFORCE_SLAM_HEADERS CONFIGURE_DEPENDS *.h **/*.h *.tcc **/*.tcc) + +add_library( + symforce_slam + ${SYMFORCE_LIBRARY_TYPE} + ${SYMFORCE_SLAM_SOURCES} + ${SYMFORCE_SLAM_HEADERS} +) +target_compile_options(symforce_slam PRIVATE ${SYMFORCE_COMPILE_OPTIONS}) +target_link_libraries(symforce_slam + symforce_gen + symforce_opt + ${SYMFORCE_EIGEN_TARGET} +) diff --git a/symforce/slam/__init__.py b/symforce/slam/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/symforce/slam/imu_preintegration/__init__.py b/symforce/slam/imu_preintegration/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/symforce/slam/imu_preintegration/generate.py b/symforce/slam/imu_preintegration/generate.py new file mode 100644 index 000000000..3d8b7fe24 --- /dev/null +++ b/symforce/slam/imu_preintegration/generate.py @@ -0,0 +1,73 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import functools + +from symforce import codegen +from symforce import typing as T +from symforce.slam.imu_preintegration.manifold_symbolic import imu_manifold_preintegration_update +from symforce.slam.imu_preintegration.manifold_symbolic import internal_imu_residual + + +def generate_manifold_imu_preintegration( + config: codegen.CodegenConfig, output_dir: T.Openable +) -> None: + """ + Generate the on-manifold IMU preintegration update and residual functions. + """ + + update_output_names = [ + "new_DR", + "new_Dv", + "new_Dp", + "new_covariance", + "new_DR_D_gyro_bias", + "new_Dv_D_accel_bias", + "new_Dv_D_gyro_bias", + "new_Dp_D_accel_bias", + "new_Dp_D_gyro_bias", + ] + + codegen_update = codegen.Codegen.function( + functools.partial(imu_manifold_preintegration_update, use_handwritten_derivatives=True), + config=config, + output_names=update_output_names, + ) + codegen_update.generate_function(output_dir=output_dir, skip_directory_nesting=True) + + codegen_update_auto_derivative = codegen.Codegen.function( + functools.partial(imu_manifold_preintegration_update, use_handwritten_derivatives=False), + name="imu_manifold_preintegration_update_auto_derivative", + docstring=""" + Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to + help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output. + Since this function is more expensive, there is no reason to use it instead. + """ + + ( + imu_manifold_preintegration_update.__doc__ + if imu_manifold_preintegration_update.__doc__ is not None + else "" + ), + config=config, + output_names=update_output_names, + ) + codegen_update_auto_derivative.generate_function( + output_dir=output_dir, skip_directory_nesting=True + ) + + codegen_residual = codegen.Codegen.function( + internal_imu_residual, + config=config, + ).with_linearization( + which_args=[ + "pose_i", + "vel_i", + "pose_j", + "vel_j", + "accel_bias_i", + "gyro_bias_i", + ] + ) + codegen_residual.generate_function(output_dir=output_dir, skip_directory_nesting=True) diff --git a/symforce/slam/imu_preintegration/imu_factor.cc b/symforce/slam/imu_preintegration/imu_factor.cc new file mode 100644 index 000000000..6ff8fd25a --- /dev/null +++ b/symforce/slam/imu_preintegration/imu_factor.cc @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#include "./imu_factor.h" + +#include + +#include + +#include "preintegrated_imu_measurements.h" + +namespace sym { + +template +ImuFactor::ImuFactor(const ImuPreintegrator& preintegrator) + : preintegrated_measurements_{preintegrator.PreintegratedMeasurements()}, + // NOTE(brad, chao): llt then inverse is 2x faster than inverse then llt + sqrt_info_{preintegrator.Covariance().llt().matrixL().solve( + Eigen::Matrix::Identity())} {} + +template +sym::Factor ImuFactor::Factor(const std::vector& keys_to_func) const { + const auto begin = keys_to_func.begin(); + // NOTE(brad): *this is copied. Keys to optimize happen to be first 6 keys to func + return sym::Factor::Hessian(*this, keys_to_func, std::vector(begin, begin + 6)); +} + +template +void ImuFactor::operator()( + const sym::Pose3& pose_i, const Eigen::Matrix& vel_i, + const sym::Pose3& pose_j, const Eigen::Matrix& vel_j, + const Eigen::Matrix& accel_bias_i, const Eigen::Matrix& gyro_bias_i, + const Eigen::Matrix& gravity, const Scalar epsilon, + Eigen::Matrix* const res, Eigen::Matrix* const jacobian, + Eigen::Matrix* const hessian, Eigen::Matrix* const rhs) const { + InternalImuFactor( + pose_i, vel_i, pose_j, vel_j, accel_bias_i, gyro_bias_i, preintegrated_measurements_.DR, + preintegrated_measurements_.Dv, preintegrated_measurements_.Dp, sqrt_info_, + preintegrated_measurements_.DR_D_gyro_bias, preintegrated_measurements_.Dv_D_accel_bias, + preintegrated_measurements_.Dv_D_gyro_bias, preintegrated_measurements_.Dp_D_accel_bias, + preintegrated_measurements_.Dp_D_gyro_bias, preintegrated_measurements_.accel_bias, + preintegrated_measurements_.gyro_bias, gravity, preintegrated_measurements_.integrated_dt, + epsilon, + // outputs + res, jacobian, hessian, rhs); +} + +} // namespace sym + +template class sym::ImuFactor; +template class sym::ImuFactor; diff --git a/symforce/slam/imu_preintegration/imu_factor.h b/symforce/slam/imu_preintegration/imu_factor.h new file mode 100644 index 000000000..589432c45 --- /dev/null +++ b/symforce/slam/imu_preintegration/imu_factor.h @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include + +#include +#include +#include + +#include "./imu_preintegrator.h" +#include "./preintegrated_imu_measurements.h" + +namespace sym { + +/** + * A factor for using on-manifold IMU preintegration in a SymForce optimization problem. By + * on-manifold, it is meant that the angular velocity measurements are composed as rotations + * rather than tangent-space approximations. + * + * Assumes IMU bias is constant over the preintegration window. Does not recompute the + * preintegrated measurements when the IMU bias estimate changes during optimization, but rather + * uses a first order approximation linearized at the IMU biases given during preintegration. + * + * The gravity argument should be [0, 0, -g] (where g is 9.8, assuming your world frame is + * gravity aligned so that the -z direction points towards the Earth) unless your IMU reads 0 + * acceleration while stationary, in which case it should be [0, 0, 0]. + * + * More generally, the gravity argument should yield the true acceleration when added to the + * accelerometer measurement (after the measurement has been converted to the world frame). + * + * Is a functor so as to store the preintegrated IMU measurements between two times. + * The residual computed is the local-coordinate difference between the final state (pose + * and velocity) and the state you would expect given the initial state and the IMU measurements. + * + * Based heavily on the on manifold ImuFactor found in GTSAM and on the paper: + * + * Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, + * “IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation”, + * Robotics: Science and Systems (RSS), 2015. + * + * Example Usage: + * + * enum Var : char { + * POSE = 'p', // Pose3d + * VELOCITY = 'v', // Vector3d + * ACCEL_BIAS = 'A', // Vector3d + * GYRO_BIAS = 'G', // Vector3d + * GRAVITY = 'g', // Vector3d + * EPSILON = 'e' // Scalar + * }; + * + * struct ImuMeasurement { + * Eigen::Vector3d acceleration; + * Eigen::Vector3d angular_velocity; + * double duration; + * }; + * + * int main() { + * // Dummy function declarations + * std::vector GetMeasurementsBetween(double start_time, double end_time); + * std::vector GetKeyFrameTimes(); + * Eigen::Vector3d GetAccelBiasEstimate(double time); + * Eigen::Vector3d GetGyroBiasEstimate(double time); + * void AppendOtherFactors(std::vector & factors); + * + * // Example accelerometer and gyroscope covariances + * const Eigen::Vector3d accel_cov = Eigen::Vector3d::Constant(1e-5); + * const Eigen::Vector3d gyro_cov = Eigen::Vector3d::Constant(1e-5); + * + * std::vector factors; + * + * // GetKeyFrameTimes assumed to return at least 2 times + * std::vector key_frame_times = GetKeyFrameTimes(); + * + * // Integrating Imu measurements between keyframes, creating an ImuFactor for each + * for (int i = 0; i < static_cast(key_frame_times.size()) - 1; i++) { + * const double start_time = key_frame_times[i]; + * const double end_time = key_frame_times[i + 1]; + * + * const std::vector measurements = GetMeasurementsBetween(start_time, + * end_time); + * + * // ImuPreintegrator should be initialized with the best guesses for the IMU biases + * sym::ImuPreintegrator integrator(GetAccelBiasEstimate(start_time), + * GetGyroBiasEstimate(start_time)); + * for (const ImuMeasurement& meas : measurements) { + * integrator.IntegrateMeasurement(meas.acceleration, meas.angular_velocity, accel_cov, + * gyro_cov, meas.duration); + * } + * + * factors.push_back(sym::ImuFactor(integrator) + * .Factor({{Var::POSE, i}, + * {Var::VELOCITY, i}, + * {Var::POSE, i + 1}, + * {Var::VELOCITY, i + 1}, + * {Var::ACCEL_BIAS, i}, + * {Var::GYRO_BIAS, i}, + * {Var::GRAVITY}, + * {Var::EPSILON}})); + * } + * + * // Adding any other factors there might be for the problem + * AppendOtherFactors(factors); + * + * sym::Optimizerd optimizer(sym::DefaultOptimizerParams(), factors); + * + * // Build Values + * sym::Valuesd values; + * for (int i = 0; i < key_frame_times.size(); i++) { + * values.Set({Var::POSE, i}, sym::Pose3d()); + * values.Set({Var::VELOCITY, i}, Eigen::Vector3d::Zero()); + * } + * for (int i = 0; i < key_frame_times.size() - 1; i++) { + * values.Set({Var::ACCEL_BIAS, i}, GetAccelBiasEstimate(key_frame_times[i])); + * values.Set({Var::GYRO_BIAS, i}, GetGyroBiasEstimate(key_frame_times[i])); + * } + * // gravity should point towards the direction of acceleration + * values.Set({Var::GRAVITY}, Eigen::Vector3d(0.0, 0.0, -9.8)); + * values.Set({Var::EPSILON}, sym::kDefaultEpsilond); + * // Initialize any other items in values ... + * + * optimizer.Optimize(&values); + * } + */ +template +class ImuFactor { + private: + PreintegratedImuMeasurements preintegrated_measurements_; + Eigen::Matrix sqrt_info_; + + public: + /** + * Construct an ImuFactor connecting two states from the (preintegrated) imu measurements + * between them. + */ + ImuFactor(const ImuPreintegrator& preintegrator); + + /** + * Construct a Factor object that can be passed to an Optimizer object given the keys to + * be passed to the function. + * + * Convenience method to avoid manually specifying which arguments are optimized. + */ + sym::Factor Factor(const std::vector& keys_to_func) const; + + /** + * Calculate a between factor on the product manifold of the pose and velocity where the prior + * is calculated from the preintegrated IMU measurements. + * + * Time step i is the time of the first IMU measurement of the interval. + * Time step j is the time after the last IMU measurement of the interval. + * + * Args: + * pose_i: Pose at time step i (world_T_body) + * vel_i: Velocity at time step i (world frame) + * pose_j: Pose at time step j (world_T_body) + * vel_j: Velocity at time step j (world frame) + * accel_bias_i: The bias of the accelerometer measurements between timesteps i and j + * gyro_bias_i: The bias of the gyroscope measurements between timesteps i and j + * gravity: Acceleration due to gravity (in the same frame as pose_x and vel_x), + * i.e., the vector which when added to the accelerometer measurements + * gives the true acceleration (up to bias and noise) of the IMU. + * epsilon: epsilon used for numerical stability + * + * Outputs: + * res: The 9dof whitened local coordinate difference between predicted and estimated state + * jacobian: (9x24) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), + * accel_bias_i (3), gyro_bias_i (3) + * hessian: (24x24) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), + * vel_j (3), accel_bias_i (3), gyro_bias_i (3) + * rhs: (24x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), + * accel_bias_i (3), gyro_bias_i (3) + */ + void operator()(const sym::Pose3& pose_i, const Eigen::Matrix& vel_i, + const sym::Pose3& pose_j, const Eigen::Matrix& vel_j, + const Eigen::Matrix& accel_bias_i, + const Eigen::Matrix& gyro_bias_i, + const Eigen::Matrix& gravity, const Scalar epsilon, + Eigen::Matrix* const res = nullptr, + Eigen::Matrix* const jacobian = nullptr, + Eigen::Matrix* const hessian = nullptr, + Eigen::Matrix* const rhs = nullptr) const; +}; + +using ImuFactord = ImuFactor; +using ImuFactorf = ImuFactor; + +} // namespace sym + +extern template class sym::ImuFactor; +extern template class sym::ImuFactor; diff --git a/symforce/slam/imu_preintegration/imu_preintegrator.cc b/symforce/slam/imu_preintegration/imu_preintegrator.cc new file mode 100644 index 000000000..1bc3eb050 --- /dev/null +++ b/symforce/slam/imu_preintegration/imu_preintegrator.cc @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#include "./imu_preintegrator.h" + +#include + +namespace sym { + +template +ImuPreintegrator::ImuPreintegrator(const Vector3& accel_bias, const Vector3& gyro_bias) + : preintegrated_measurements_(accel_bias, gyro_bias), covariance_{Matrix99::Zero()} {} + +template +void ImuPreintegrator::IntegrateMeasurement(const Vector3& measured_accel, + const Vector3& measured_gyro, + const Vector3& accel_cov, + const Vector3& gyro_cov, const Scalar dt, + const Scalar epsilon) { + Rot3 new_DR; + Vector3 new_Dv; + Vector3 new_Dp; + Matrix99 new_covariance; + Matrix33 new_DR_D_gyro_bias; + Matrix33 new_Dv_D_accel_bias; + Matrix33 new_Dv_D_gyro_bias; + Matrix33 new_Dp_D_accel_bias; + Matrix33 new_Dp_D_gyro_bias; + ImuManifoldPreintegrationUpdate( + preintegrated_measurements_.DR, preintegrated_measurements_.Dv, + preintegrated_measurements_.Dp, covariance_, preintegrated_measurements_.DR_D_gyro_bias, + preintegrated_measurements_.Dv_D_accel_bias, preintegrated_measurements_.Dv_D_gyro_bias, + preintegrated_measurements_.Dp_D_accel_bias, preintegrated_measurements_.Dp_D_gyro_bias, + preintegrated_measurements_.accel_bias, preintegrated_measurements_.gyro_bias, accel_cov, + gyro_cov, measured_accel, measured_gyro, dt, epsilon, + // outputs + &new_DR, &new_Dv, &new_Dp, &new_covariance, &new_DR_D_gyro_bias, &new_Dv_D_accel_bias, + &new_Dv_D_gyro_bias, &new_Dp_D_accel_bias, &new_Dp_D_gyro_bias); + + preintegrated_measurements_.DR = new_DR; + preintegrated_measurements_.Dv = new_Dv; + preintegrated_measurements_.Dp = new_Dp; + preintegrated_measurements_.DR_D_gyro_bias = new_DR_D_gyro_bias; + preintegrated_measurements_.Dv_D_accel_bias = new_Dv_D_accel_bias; + preintegrated_measurements_.Dv_D_gyro_bias = new_Dv_D_gyro_bias; + preintegrated_measurements_.Dp_D_accel_bias = new_Dp_D_accel_bias; + preintegrated_measurements_.Dp_D_gyro_bias = new_Dp_D_gyro_bias; + covariance_ = new_covariance; + + preintegrated_measurements_.integrated_dt += dt; +} + +template +const PreintegratedImuMeasurements& ImuPreintegrator::PreintegratedMeasurements() + const { + return preintegrated_measurements_; +} + +template +const typename ImuPreintegrator::Matrix99& ImuPreintegrator::Covariance() const { + return covariance_; +} + +} // namespace sym + +template class sym::ImuPreintegrator; +template class sym::ImuPreintegrator; diff --git a/symforce/slam/imu_preintegration/imu_preintegrator.h b/symforce/slam/imu_preintegration/imu_preintegrator.h new file mode 100644 index 000000000..561550457 --- /dev/null +++ b/symforce/slam/imu_preintegration/imu_preintegrator.h @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include + +#include "./preintegrated_imu_measurements.h" + +namespace sym { + +/** + * Class to on-manifold preintegrate IMU measurements for usage in a SymForce optimization + * problem. + * + * For usage, see the doc-string of ImuFactor in imu_factor.h + */ +template +class ImuPreintegrator { + public: + using Vector3 = typename PreintegratedImuMeasurements::Vector3; + using Matrix33 = typename PreintegratedImuMeasurements::Matrix33; + using Matrix99 = Eigen::Matrix; + + private: + PreintegratedImuMeasurements preintegrated_measurements_; + Matrix99 covariance_; // covariance of [DR, Dv, Dp] in local coordinates of mean + + public: + /** + * Initialize with given accel_bias and gyro_bias + */ + ImuPreintegrator(const Vector3& accel_bias, const Vector3& gyro_bias); + + /** + * Integrate a new measurement. + * + * Args: + * measured_accel is the next accelerometer measurement after the last integrated measurement + * measured_gyro is the next gyroscope measurement after the last integrated measurement + * accel_cov is the covariance of the accelerometer measurement (represented by its diagonal + * entries) [(radians / time)^2 * time] + * gyro_cov is the covariance of the gyroscope measurement (represented by its diagonal + * entries) [(radians / time)^2 * time] + * dt is the time span over which these measurements were made + * + * Postcondition: + * If the orientation, velocity, and position were R0, v0, and p0 at the start of the first + * integrated IMU measurements, and Rf, vf, and pf are the corresponding values at the end of + * the measurement described by the arguments, DT is the total time covered by the integrated + * measurements, and g is the vector of acceleration due to gravity, then + * - pim.DR -> R0.inverse() * Rf + * - pim.Dv -> R0.inverse() * (vf - v0 - g * DT) + * - pim.Dp -> R0.inverse() * (pf - p0 - v0 * DT - 0.5 * g * DT^2) + * - pim.DX_D_accel_bias -> the derivative of DX wrt the accel bias linearized at pim.accel_bias + * - pim.DX_D_gyro_bias -> the derivative of DX wrt the gyro bias linearized at pim.gyro_bias + * - pim.accel_bias -> unchanged + * - pim.gyro_bias -> unchanged + * - pim.integrated_dt -> DT + * - covariance -> the covariance [DR, Dv, Dp] in local coordinates of mean + */ + void IntegrateMeasurement(const Vector3& measured_accel, const Vector3& measured_gyro, + const Vector3& accel_cov, const Vector3& gyro_cov, const Scalar dt, + const Scalar epsilon = kDefaultEpsilon); + + const PreintegratedImuMeasurements& PreintegratedMeasurements() const; + + const Matrix99& Covariance() const; +}; + +using ImuPreintegratord = ImuPreintegrator; +using ImuPreintegratorf = ImuPreintegrator; + +} // namespace sym + +extern template class sym::ImuPreintegrator; +extern template class sym::ImuPreintegrator; diff --git a/symforce/slam/imu_preintegration/manifold_symbolic.py b/symforce/slam/imu_preintegration/manifold_symbolic.py new file mode 100644 index 000000000..1c629b1df --- /dev/null +++ b/symforce/slam/imu_preintegration/manifold_symbolic.py @@ -0,0 +1,349 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import symforce.symbolic as sf +from symforce import typing as T +from symforce.jacobian_helpers import tangent_jacobians + + +def internal_imu_residual( + pose_i: sf.Pose3, + vel_i: sf.V3, + pose_j: sf.Pose3, + vel_j: sf.V3, + accel_bias_i: sf.V3, + gyro_bias_i: sf.V3, + # Preintegrated measurements: state + DR: sf.Rot3, + Dv: sf.V3, + Dp: sf.V3, + # Other pre-integrated quantities + sqrt_info: sf.M99, + DR_D_gyro_bias: sf.M33, + Dv_D_accel_bias: sf.M33, + Dv_D_gyro_bias: sf.M33, + Dp_D_accel_bias: sf.M33, + Dp_D_gyro_bias: sf.M33, + # other + accel_bias_hat: sf.V3, + gyro_bias_hat: sf.V3, + gravity: sf.V3, + dt: T.Scalar, + epsilon: T.Scalar, +) -> sf.V9: + """ + An internal helper function to linearize the difference between the orientation, velocity, and + position at one time step and those of another time step. The expected difference is calculated + from the preintegrated IMU measurements between those time steps. + + NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use + the one found in symforce/slam/imu_preintegration/imu_factor.h. + + Time step i is the time of the first IMU measurement of the interval. + Time step j is the time after the last IMU measurement of the interval. + + Assumes sqrt_info is lower triangular and only reads the lower triangle. + + Args: + pose_i (sf.Pose3): Pose at time step i (world_T_body) + vel_i (sf.V3): Velocity at time step i (world frame) + pose_j (sf.Pose3): Pose at time step j (world_T_body) + vel_j (sf.V3): Velocity at time step j (world frame) + accel_bias_i (sf.V3): The accelerometer bias between timesteps i and j + gyro_bias_i (sf.V3): The gyroscope bias between timesteps i and j + DR (sf.Rot3): Preintegrated estimate for pose_i.R.inverse() * pose_j.R + Dv (sf.V3): Preintegrated estimate for vel_j - vel_i in the body frame at timestep i + Dp (sf.V3): Preintegrated estimate for position change (before velocity and gravity + corrections) in the body frame at timestep i: + R_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2), + where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i + sqrt_info (sf.M99): sqrt info matrix of DR('s tangent space), Dv, Dp + DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat + Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat + Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat + Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat + Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat + accel_bias_hat (sf.V3): The accelerometer bias used during preintegration + gyro_bias_hat (sf.V3): The gyroscope bias used during preintegration + gravity (sf.V3): Acceleration due to gravity (in the same frame as pose_x and vel_x), + i.e., the vector which when added to the accelerometer measurements gives the + true acceleration (up to bias and noise) of the IMU. + dt (T.Scalar): The time between timestep i and j: t_j - t_i + epsilon (T.Scalar): epsilon used for numerical stability + + Outputs: + res: 9dof product residual between the orientations, then velocities, then positions. + """ + delta_gyro_bias = gyro_bias_i - gyro_bias_hat + delta_accel_bias = accel_bias_i - accel_bias_hat + + # Correct preintegrated measurements for updated bias estimates + corrected_DR = DR * sf.Rot3.from_tangent( + (DR_D_gyro_bias * delta_gyro_bias).to_storage(), epsilon + ) + corrected_Dv = Dv + Dv_D_gyro_bias * delta_gyro_bias + Dv_D_accel_bias * delta_accel_bias + corrected_Dp = Dp + Dp_D_gyro_bias * delta_gyro_bias + Dp_D_accel_bias * delta_accel_bias + + res_R = (corrected_DR.inverse() * pose_i.R.inverse() * pose_j.R).to_tangent(epsilon) + res_v = pose_i.R.inverse() * (vel_j - vel_i - gravity * dt) - corrected_Dv + res_p = ( + pose_i.R.inverse() * (pose_j.t - pose_i.t - vel_i * dt - sf.S(1) / 2 * gravity * dt ** 2) + - corrected_Dp + ) + + res = sf.V9(*res_R, *res_v, *res_p) + + return sf.M91(sqrt_info.lower_triangle() * res) + + +def right_jacobian(tangent: sf.V3, epsilon: T.Scalar) -> sf.M33: + """ + The right jacobian J(v) is d/du Log(Exp(v)^{-1} Exp(v + u)), i.e., a matrix such that + Exp(v + dv) ~= Exp(v) Exp(J(v) dv), where v and u are tangent vectors of SO(3). + + Returns J(tangent). + """ + # NOTE(brad): We use sqrt(epsilon) to ensure cos(norm) is far enough from 1 to mitigate + # the effects of catastrophic cancellation in 1 - cos(norm) below. + norm = tangent.norm(sf.sqrt(epsilon)) + tangent_hat = sf.Matrix.skew_symmetric(tangent) + out = sf.M33.eye() + out -= ((1 - sf.cos(norm)) / (norm ** 2)) * tangent_hat + out += ((norm - sf.sin(norm)) / (norm ** 3)) * (tangent_hat * tangent_hat) + return out + + +def handwritten_new_state_D_state_gyro_accel( + DR: sf.Rot3, corrected_gyro: sf.V3, corrected_accel: sf.V3, dt: T.Scalar, epsilon: T.Scalar +) -> T.Tuple[sf.M99, sf.M93, sf.M93]: + """ + Calculates the derivatives of the new state (meaning the DR, Dv, and Dp) w.r.t. the previous state, + gyroscope measurement, and the accelerometer mesaurement. + + + Args: + DR (sf.Rot3): Preintegrated DR of the previous state + corrected_gyro (sf.V3): gyroscope measurment corrected for IMU bias + corrected_accel (sf.V3): accelerometer measurement corrected for IMU bias + dt (T.Scalar): Time difference between the previous time step and the new time step + epsilon (T.Scalar): epsilon for numerical stability + + Returns: + T.Tuple[sf.M99, sf.M93, sf.M93]: new_state_D_old_state, new_state_D_gyro_measurement, new_state_D_accel_measurement + """ + # calculate new_state_D_old_state + DR_update = sf.Rot3.from_tangent(corrected_gyro * dt, epsilon) + new_DR_D_old_DR = DR_update.inverse().to_rotation_matrix() + new_DR_D_old_Dv = sf.M33.zero() + new_DR_D_old_Dp = sf.M33.zero() + + new_Dv_D_old_DR = -DR.to_rotation_matrix() * sf.Matrix.skew_symmetric(corrected_accel * dt) + new_Dv_D_old_Dv = sf.M33.eye() + new_Dv_D_old_Dp = sf.M33.zero() + + new_Dp_D_old_DR = ( + (-DR.to_rotation_matrix() * sf.Matrix.skew_symmetric(corrected_accel * dt)) * dt / 2 + ) + new_Dp_D_old_Dv = dt * sf.M33.eye() + new_Dp_D_old_Dp = sf.M33.eye() + + new_state_D_old_state = sf.Matrix.block_matrix( + [ + [new_DR_D_old_DR, new_DR_D_old_Dv, new_DR_D_old_Dp], + [new_Dv_D_old_DR, new_Dv_D_old_Dv, new_Dv_D_old_Dp], + [new_Dp_D_old_DR, new_Dp_D_old_Dv, new_Dp_D_old_Dp], + ] + ) + + # calculate new_D_gyro + new_DR_D_gyro = right_jacobian(corrected_gyro * dt, epsilon) * dt + new_DvDp_D_gyro = sf.M63.zero() + + new_state_D_gyro = sf.Matrix.block_matrix( + [ + [new_DR_D_gyro], + [new_DvDp_D_gyro], + ] + ) + + # calculate new_D_accel + new_DR_D_accel = sf.M33.zero() + new_Dv_D_accel = DR.to_rotation_matrix() * dt + new_Dp_D_accel = DR.to_rotation_matrix() * (dt * dt / 2) + + new_state_D_accel = sf.Matrix.block_matrix( + [ + [new_DR_D_accel], + [new_Dv_D_accel], + [new_Dp_D_accel], + ] + ) + + return sf.M99(new_state_D_old_state), sf.M93(new_state_D_gyro), sf.M93(new_state_D_accel) + + +def imu_manifold_preintegration_update( + # Initial state + DR: sf.Rot3, + Dv: sf.V3, + Dp: sf.V3, + covariance: sf.M99, + DR_D_gyro_bias: sf.M33, + Dv_D_accel_bias: sf.M33, + Dv_D_gyro_bias: sf.M33, + Dp_D_accel_bias: sf.M33, + Dp_D_gyro_bias: sf.M33, + # Biases and noise model + accel_bias: sf.V3, + gyro_bias: sf.V3, + accel_cov_diagonal: sf.V3, + gyro_cov_diagonal: sf.V3, + # Measurement + accel_measurement: sf.V3, + gyro_measurement: sf.V3, + dt: T.Scalar, + # Singularity handling, + epsilon: T.Scalar, + # Configuration + use_handwritten_derivatives: bool = True, +) -> T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: + """ + An internal helper function to update a set of preintegrated IMU measurements with a new pair of + gyroscope and accelerometer measurements. Returns the updated preintegrated measurements. + + NOTE: If you are interested in this function, you should likely be using the + IntegrateMeasurement method of the PreintegratedImuMeasurements class located in + symforce/slam/imu_preintegration/preintegrated_imu_measurements.h. + + When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the + derivatives w.r.t. to bias should all be 0. + + Args: + DR (sf.Rot3): Preintegrated change in orientation + Dv (sf.V3): Preintegrated change in velocity + Dp (sf.V3): Preintegrated change in position + covariance (sf.M99): Covariance [DR, Dv, Dp] in local coordinates of mean + DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias + Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias + Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias + Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias + Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias + accel_bias (sf.V3): Initial guess for the accelerometer measurement bias + gyro_bias (sf.V3): Initial guess for the gyroscope measurement bias + accel_cov_diagonal (sf.M33): The diagonal of the covariance of the accelerometer measurement + gyro_cov_diagonal (sf.M33): The diagonal of the covariance of the gyroscope measurement + accel_measurement (sf.V3): The accelerometer measurement + gyro_measurement (sf.V3): The gyroscope measurement + dt (T.Scalar): The time between the new measurements and the last + epsilon (T.Scalar): epsilon for numerical stability + + Returns: + T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: + new_DR, + new_Dv, + new_Dp, + new_covariance, + new_DR_D_gyro_bias, + new_Dv_D_accel_bias, + new_Dv_D_gyro_bias, + new_Dp_D_accel_bias + new_Dp_D_gyro_bias, + """ + # Correct for IMU bias + corrected_accel = accel_measurement - accel_bias + corrected_gyro = gyro_measurement - gyro_bias + + # Integrate the state + new_DR, new_Dv, new_Dp = integrate_state( + DR, + Dv, + Dp, + accel=corrected_accel, + gyro=corrected_gyro, + dt=dt, + epsilon=epsilon, + ) + + # NOTE(Brad): Both ways of calculating the derivatives of new_state should be the same. + if not use_handwritten_derivatives: + # Definitely correct, but not very amenable to CSE + def new_state_D(wrt_variables: T.List[T.Any]) -> sf.Matrix: + return sf.Matrix.block_matrix( + [ + tangent_jacobians(new_DR, wrt_variables), + tangent_jacobians(new_Dv, wrt_variables), + tangent_jacobians(new_Dp, wrt_variables), + ] + ) + + new_state_D_state = new_state_D([DR, Dv, Dp]) + new_state_D_gyro_bias = new_state_D([gyro_bias]) + new_state_D_accel_bias = new_state_D([accel_bias]) + + new_state_D_gyro = new_state_D([gyro_measurement]) + new_state_D_accel = new_state_D([accel_measurement]) + else: + # Handwritten derivatives, reduces op count by ~20% + ( + new_state_D_state, + new_state_D_gyro, + new_state_D_accel, + ) = handwritten_new_state_D_state_gyro_accel( + DR, corrected_gyro, corrected_accel, dt, epsilon + ) + new_state_D_gyro_bias = -new_state_D_gyro + new_state_D_accel_bias = -new_state_D_accel + + new_covariance = new_state_D_state * covariance * new_state_D_state.T + new_covariance += new_state_D_gyro * sf.M.diag(gyro_cov_diagonal / dt) * new_state_D_gyro.T + new_covariance += new_state_D_accel * sf.M.diag(accel_cov_diagonal / dt) * new_state_D_accel.T + + state_D_bias = sf.Matrix.block_matrix( + [ + [DR_D_gyro_bias, sf.M33.zero()], + [Dv_D_gyro_bias, Dv_D_accel_bias], + [Dp_D_gyro_bias, Dp_D_accel_bias], + ] + ) + + new_state_D_bias = sf.Matrix.block_matrix([[new_state_D_gyro_bias, new_state_D_accel_bias]]) + + new_state_D_bias = new_state_D_state * state_D_bias + new_state_D_bias + + return ( + new_DR, + new_Dv, + new_Dp, + sf.M99(new_covariance), + sf.M33(new_state_D_bias[0:3, 0:3]), # new_DR_D_gyro_bias + sf.M33(new_state_D_bias[3:6, 3:6]), # new_Dv_D_accel_bias + sf.M33(new_state_D_bias[3:6, 0:3]), # new_Dv_D_gyro_bias + sf.M33(new_state_D_bias[6:9, 3:6]), # new_Dp_D_accel_bias + sf.M33(new_state_D_bias[6:9, 0:3]), # new_Dp_D_gyro_bias + ) + + +def integrate_state( + # state + DR: sf.Rot3, + Dv: sf.V3, + Dp: sf.V3, + accel: sf.V3, + gyro: sf.V3, + dt: T.Scalar, + epsilon: T.Scalar, +) -> T.Tuple[sf.Rot3, sf.V3, sf.V3]: + """ + Given the old preintegrated state and the new IMU measurements, calculates the + new preintegrated state. + + Returns: + T.Tuple[sf.Rot3, sf.V3, sf.V3]: (new_DR, new_Dv, new_Dp) + """ + new_DR = DR * sf.Rot3.from_tangent(gyro * dt, epsilon) + new_Dv = Dv + DR * accel * dt + new_Dp = Dp + Dv * dt + DR * accel * dt ** 2 / 2 + + return new_DR, new_Dv, new_Dp diff --git a/symforce/slam/imu_preintegration/preintegrated_imu_measurements.cc b/symforce/slam/imu_preintegration/preintegrated_imu_measurements.cc new file mode 100644 index 000000000..9821fa3cd --- /dev/null +++ b/symforce/slam/imu_preintegration/preintegrated_imu_measurements.cc @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#include "./preintegrated_imu_measurements.h" + +namespace sym { + +template +PreintegratedImuMeasurements::PreintegratedImuMeasurements(const Vector3& accel_bias, + const Vector3& gyro_bias) + : DR(), + Dv{Vector3::Zero()}, + Dp{Vector3::Zero()}, + DR_D_gyro_bias{Matrix33::Zero()}, + Dv_D_accel_bias{Matrix33::Zero()}, + Dv_D_gyro_bias{Matrix33::Zero()}, + Dp_D_accel_bias{Matrix33::Zero()}, + Dp_D_gyro_bias{Matrix33::Zero()}, + accel_bias{accel_bias}, + gyro_bias{gyro_bias}, + integrated_dt{0.0} {} + +} // namespace sym + +template struct sym::PreintegratedImuMeasurements; +template struct sym::PreintegratedImuMeasurements; diff --git a/symforce/slam/imu_preintegration/preintegrated_imu_measurements.h b/symforce/slam/imu_preintegration/preintegrated_imu_measurements.h new file mode 100644 index 000000000..a619ed4d5 --- /dev/null +++ b/symforce/slam/imu_preintegration/preintegrated_imu_measurements.h @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Struct of Preintegrated IMU Measurements (not including the covariance of change in + * orientation, velocity, and position). + */ +template +struct PreintegratedImuMeasurements { + using Vector3 = Eigen::Matrix; + using Matrix33 = Eigen::Matrix; + + // The rotation that occured over the measurement period; i.e., maps the coordinates of a vector + // in the body frame of the end of the measurement period to the coordinates of the vector in the + // body frame at the start of the measurement period. + sym::Rot3 DR; + + // The velocity change that occured over the measurement period in the body frame of the + // initial measurement (assuming 0 acceleration due to gravity) + Vector3 Dv; + + // The position change that occured over the measurement period in the body frame of the + // initial measurement (assuming 0 acceleration due to gravity and 0 initial velocity) + Vector3 Dp; + + // Derivatives of DR/Dv/Dp w.r.t. the gyroscope/accelerometer bias linearized at the values + // of gyro_bias and accel_bias + Matrix33 DR_D_gyro_bias; + Matrix33 Dv_D_accel_bias; + Matrix33 Dv_D_gyro_bias; + Matrix33 Dp_D_accel_bias; + Matrix33 Dp_D_gyro_bias; + + // The assumed accelometer bias used during preintegration + Vector3 accel_bias; + + // The assumed gyroscope bias used during preintegration + Vector3 gyro_bias; + + // The elapsed time of the measurement period + Scalar integrated_dt; + + /** + * Initialize instance struct with accel_bias and gyro_bias and all other values + * zeroed out (scalars, vectors, and matrices) or set to the identity (DR). + */ + PreintegratedImuMeasurements(const Vector3& accel_bias, const Vector3& gyro_bias); +}; + +using PreintegratedImuMeasurementsd = PreintegratedImuMeasurements; +using PreintegratedImuMeasurementsf = PreintegratedImuMeasurements; + +} // namespace sym + +extern template struct sym::PreintegratedImuMeasurements; +extern template struct sym::PreintegratedImuMeasurements; diff --git a/test/geo_matrix_test.py b/test/geo_matrix_test.py index 9d8de83ed..de3f0fa64 100644 --- a/test/geo_matrix_test.py +++ b/test/geo_matrix_test.py @@ -280,6 +280,23 @@ def test_transpose(self) -> None: self.assertEqual(sf.M12() * sf.M21(), sf.M11()) self.assertEqual(sf.M21() * sf.M12(), sf.M22()) + def test_lower_triangle(self) -> None: + """ + Tests: + Matrix.lower_triangle + """ + m11 = sf.M11(1) + self.assertEqual(m11, m11.lower_triangle()) + + m22 = sf.M22([[1, 2], [3, 4]]) + self.assertEqual(sf.M22([[1, 0], [3, 4]]), m22.lower_triangle()) + + m33 = sf.Matrix([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) + self.assertEqual(sf.M33([[1, 0, 0], [4, 5, 0], [7, 8, 9]]), m33.lower_triangle()) + + self.assertRaises(ValueError, lambda: sf.M45().lower_triangle()) + self.assertRaises(ValueError, lambda: sf.M54().lower_triangle()) + def test_row_col(self) -> None: """ Tests: diff --git a/test/symforce_gen_codegen_test.py b/test/symforce_gen_codegen_test.py index 86b9fbf02..9d9bda525 100644 --- a/test/symforce_gen_codegen_test.py +++ b/test/symforce_gen_codegen_test.py @@ -26,6 +26,7 @@ from symforce.codegen import slam_factors_codegen from symforce.codegen import sym_util_package_codegen from symforce.codegen import template_util +from symforce.slam.imu_preintegration.generate import generate_manifold_imu_preintegration from symforce.test_util import TestCase from symforce.test_util import symengine_only @@ -150,6 +151,10 @@ def test_gen_package_codegen_cpp(self) -> None: # Prior factors, between factors, and SLAM factors for C++. geo_factors_codegen.generate(output_dir / "sym") slam_factors_codegen.generate(output_dir / "sym") + generate_manifold_imu_preintegration( + config=config, + output_dir=output_dir / "sym" / "factors" / "internal", + ) # Generate typedefs.h sym_util_package_codegen.generate(config=config, output_dir=output_dir)