From cf820425c8392ab7530644d9c676bc57b88427b1 Mon Sep 17 00:00:00 2001 From: bradley-solliday-skydio Date: Thu, 19 Jan 2023 18:13:58 -0800 Subject: [PATCH] Regenerate geo factors with sqrt_info variants Topic: sqrt_info_variants --- gen/cpp/sym/factors/between_factor_matrix31.h | 180 +--- .../between_factor_matrix31_diagonal.h | 110 ++ .../between_factor_matrix31_isotropic.h | 109 ++ .../between_factor_matrix31_square.h | 178 ++++ gen/cpp/sym/factors/between_factor_pose2.h | 223 +--- .../between_factor_pose2_diagonal.h | 190 ++++ .../between_factor_pose2_isotropic.h | 175 ++++ .../between_factor_pose2_square.h | 221 ++++ gen/cpp/sym/factors/between_factor_pose3.h | 960 +----------------- .../between_factor_pose3_diagonal.h | 526 ++++++++++ .../between_factor_pose3_isotropic.h | 516 ++++++++++ .../between_factor_pose3_square.h | 958 +++++++++++++++++ .../factors/between_factor_pose3_position.h | 314 +----- .../between_factor_pose3_position_diagonal.h | 282 +++++ .../between_factor_pose3_position_isotropic.h | 280 +++++ .../between_factor_pose3_position_square.h | 312 ++++++ .../factors/between_factor_pose3_rotation.h | 411 +------- .../between_factor_pose3_rotation_diagonal.h | 327 ++++++ .../between_factor_pose3_rotation_isotropic.h | 325 ++++++ .../between_factor_pose3_rotation_square.h | 409 ++++++++ gen/cpp/sym/factors/between_factor_rot2.h | 113 +-- .../between_factor_rot2_diagonal.h | 111 ++ .../between_factor_rot2_isotropic.h | 111 ++ .../between_factor_rot2_square.h | 111 ++ gen/cpp/sym/factors/between_factor_rot3.h | 407 +------- .../between_factor_rot3_diagonal.h | 312 ++++++ .../between_factor_rot3_isotropic.h | 306 ++++++ .../between_factor_rot3_square.h | 405 ++++++++ gen/cpp/sym/factors/prior_factor_matrix31.h | 109 +- .../prior_factor_matrix31_diagonal.h | 93 ++ .../prior_factor_matrix31_isotropic.h | 90 ++ .../prior_factor_matrix31_square.h | 107 ++ gen/cpp/sym/factors/prior_factor_pose2.h | 122 +-- .../prior_factor_pose2_diagonal.h | 103 ++ .../prior_factor_pose2_isotropic.h | 101 ++ .../prior_factor_pose2_square.h | 120 +++ gen/cpp/sym/factors/prior_factor_pose3.h | 395 +------ .../prior_factor_pose3_diagonal.h | 200 ++++ .../prior_factor_pose3_isotropic.h | 190 ++++ .../prior_factor_pose3_square.h | 393 +++++++ .../sym/factors/prior_factor_pose3_position.h | 123 +-- .../prior_factor_pose3_position_diagonal.h | 99 ++ .../prior_factor_pose3_position_isotropic.h | 97 ++ .../prior_factor_pose3_position_square.h | 121 +++ .../sym/factors/prior_factor_pose3_rotation.h | 245 +---- .../prior_factor_pose3_rotation_diagonal.h | 190 ++++ .../prior_factor_pose3_rotation_isotropic.h | 186 ++++ .../prior_factor_pose3_rotation_square.h | 243 +++++ gen/cpp/sym/factors/prior_factor_rot2.h | 89 +- .../prior_factor_rot2_diagonal.h | 87 ++ .../prior_factor_rot2_isotropic.h | 87 ++ .../prior_factor_rot2_square.h | 87 ++ gen/cpp/sym/factors/prior_factor_rot3.h | 240 +---- .../prior_factor_rot3_diagonal.h | 180 ++++ .../prior_factor_rot3_isotropic.h | 175 ++++ .../prior_factor_rot3_square.h | 238 +++++ 56 files changed, 9503 insertions(+), 3889 deletions(-) create mode 100644 gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_square.h create mode 100644 gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_square.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_square.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_square.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_square.h create mode 100644 gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_square.h create mode 100644 gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_diagonal.h create mode 100644 gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_isotropic.h create mode 100644 gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_square.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_diagonal.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_isotropic.h create mode 100644 gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_square.h diff --git a/gen/cpp/sym/factors/between_factor_matrix31.h b/gen/cpp/sym/factors/between_factor_matrix31.h index dbca47b83..5df781eea 100644 --- a/gen/cpp/sym/factors/between_factor_matrix31.h +++ b/gen/cpp/sym/factors/between_factor_matrix31.h @@ -1,177 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_T_b. - * - * In vector space terms that would be: - * (b - a) - a_T_b - * - * In lie group terms: - * local_coordinates(a_T_b, between(a, b)) - * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x6) jacobian of res wrt args a (3), b (3) - * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) - * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) - */ -template -void BetweenFactorMatrix31(const Eigen::Matrix& a, - const Eigen::Matrix& b, - const Eigen::Matrix& a_T_b, - const Eigen::Matrix& sqrt_info, 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: 102 - - // Unused inputs - (void)epsilon; - - // Input arrays - - // Intermediate terms (42) - const Scalar _tmp0 = -a(1, 0) - a_T_b(1, 0) + b(1, 0); - const Scalar _tmp1 = -a(2, 0) - a_T_b(2, 0) + b(2, 0); - const Scalar _tmp2 = -a(0, 0) - a_T_b(0, 0) + b(0, 0); - const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp2 * sqrt_info(0, 0); - const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp2 * sqrt_info(1, 0); - const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp2 * sqrt_info(2, 0); - const Scalar _tmp6 = std::pow(sqrt_info(0, 0), Scalar(2)); - const Scalar _tmp7 = std::pow(sqrt_info(2, 0), Scalar(2)); - const Scalar _tmp8 = std::pow(sqrt_info(1, 0), Scalar(2)); - const Scalar _tmp9 = _tmp6 + _tmp7 + _tmp8; - const Scalar _tmp10 = sqrt_info(0, 0) * sqrt_info(0, 1); - const Scalar _tmp11 = sqrt_info(2, 0) * sqrt_info(2, 1); - const Scalar _tmp12 = sqrt_info(1, 0) * sqrt_info(1, 1); - const Scalar _tmp13 = _tmp10 + _tmp11 + _tmp12; - const Scalar _tmp14 = sqrt_info(0, 0) * sqrt_info(0, 2); - const Scalar _tmp15 = sqrt_info(2, 0) * sqrt_info(2, 2); - const Scalar _tmp16 = sqrt_info(1, 0) * sqrt_info(1, 2); - const Scalar _tmp17 = _tmp14 + _tmp15 + _tmp16; - const Scalar _tmp18 = -_tmp10 - _tmp11 - _tmp12; - const Scalar _tmp19 = -_tmp14 - _tmp15 - _tmp16; - const Scalar _tmp20 = std::pow(sqrt_info(0, 1), Scalar(2)); - const Scalar _tmp21 = std::pow(sqrt_info(2, 1), Scalar(2)); - const Scalar _tmp22 = std::pow(sqrt_info(1, 1), Scalar(2)); - const Scalar _tmp23 = _tmp20 + _tmp21 + _tmp22; - const Scalar _tmp24 = sqrt_info(0, 1) * sqrt_info(0, 2); - const Scalar _tmp25 = sqrt_info(2, 1) * sqrt_info(2, 2); - const Scalar _tmp26 = sqrt_info(1, 1) * sqrt_info(1, 2); - const Scalar _tmp27 = _tmp24 + _tmp25 + _tmp26; - const Scalar _tmp28 = -_tmp24 - _tmp25 - _tmp26; - const Scalar _tmp29 = std::pow(sqrt_info(0, 2), Scalar(2)); - const Scalar _tmp30 = std::pow(sqrt_info(2, 2), Scalar(2)); - const Scalar _tmp31 = std::pow(sqrt_info(1, 2), Scalar(2)); - const Scalar _tmp32 = _tmp29 + _tmp30 + _tmp31; - const Scalar _tmp33 = _tmp5 * sqrt_info(2, 0); - const Scalar _tmp34 = _tmp3 * sqrt_info(0, 0); - const Scalar _tmp35 = _tmp4 * sqrt_info(1, 0); - const Scalar _tmp36 = _tmp5 * sqrt_info(2, 1); - const Scalar _tmp37 = _tmp3 * sqrt_info(0, 1); - const Scalar _tmp38 = _tmp4 * sqrt_info(1, 1); - const Scalar _tmp39 = _tmp5 * sqrt_info(2, 2); - const Scalar _tmp40 = _tmp3 * sqrt_info(0, 2); - const Scalar _tmp41 = _tmp4 * sqrt_info(1, 2); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp3; - _res(1, 0) = _tmp4; - _res(2, 0) = _tmp5; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = -sqrt_info(0, 0); - _jacobian(1, 0) = -sqrt_info(1, 0); - _jacobian(2, 0) = -sqrt_info(2, 0); - _jacobian(0, 1) = -sqrt_info(0, 1); - _jacobian(1, 1) = -sqrt_info(1, 1); - _jacobian(2, 1) = -sqrt_info(2, 1); - _jacobian(0, 2) = -sqrt_info(0, 2); - _jacobian(1, 2) = -sqrt_info(1, 2); - _jacobian(2, 2) = -sqrt_info(2, 2); - _jacobian(0, 3) = sqrt_info(0, 0); - _jacobian(1, 3) = sqrt_info(1, 0); - _jacobian(2, 3) = sqrt_info(2, 0); - _jacobian(0, 4) = sqrt_info(0, 1); - _jacobian(1, 4) = sqrt_info(1, 1); - _jacobian(2, 4) = sqrt_info(2, 1); - _jacobian(0, 5) = sqrt_info(0, 2); - _jacobian(1, 5) = sqrt_info(1, 2); - _jacobian(2, 5) = sqrt_info(2, 2); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = _tmp9; - _hessian(1, 0) = _tmp13; - _hessian(2, 0) = _tmp17; - _hessian(3, 0) = -_tmp6 - _tmp7 - _tmp8; - _hessian(4, 0) = _tmp18; - _hessian(5, 0) = _tmp19; - _hessian(0, 1) = 0; - _hessian(1, 1) = _tmp23; - _hessian(2, 1) = _tmp27; - _hessian(3, 1) = _tmp18; - _hessian(4, 1) = -_tmp20 - _tmp21 - _tmp22; - _hessian(5, 1) = _tmp28; - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = _tmp32; - _hessian(3, 2) = _tmp19; - _hessian(4, 2) = _tmp28; - _hessian(5, 2) = -_tmp29 - _tmp30 - _tmp31; - _hessian(0, 3) = 0; - _hessian(1, 3) = 0; - _hessian(2, 3) = 0; - _hessian(3, 3) = _tmp9; - _hessian(4, 3) = _tmp13; - _hessian(5, 3) = _tmp17; - _hessian(0, 4) = 0; - _hessian(1, 4) = 0; - _hessian(2, 4) = 0; - _hessian(3, 4) = 0; - _hessian(4, 4) = _tmp23; - _hessian(5, 4) = _tmp27; - _hessian(0, 5) = 0; - _hessian(1, 5) = 0; - _hessian(2, 5) = 0; - _hessian(3, 5) = 0; - _hessian(4, 5) = 0; - _hessian(5, 5) = _tmp32; - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = -_tmp33 - _tmp34 - _tmp35; - _rhs(1, 0) = -_tmp36 - _tmp37 - _tmp38; - _rhs(2, 0) = -_tmp39 - _tmp40 - _tmp41; - _rhs(3, 0) = _tmp33 + _tmp34 + _tmp35; - _rhs(4, 0) = _tmp36 + _tmp37 + _tmp38; - _rhs(5, 0) = _tmp39 + _tmp40 + _tmp41; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_matrix31/between_factor_matrix31_diagonal.h" +#include "./between_factor_matrix31/between_factor_matrix31_isotropic.h" +#include "./between_factor_matrix31/between_factor_matrix31_square.h" diff --git a/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_diagonal.h b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_diagonal.h new file mode 100644 index 000000000..c6ea6b183 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_diagonal.h @@ -0,0 +1,110 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorMatrix31(const Eigen::Matrix& a, + const Eigen::Matrix& b, + const Eigen::Matrix& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 24 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = -a(0, 0) - a_T_b(0, 0) + b(0, 0); + const Scalar _tmp1 = -a(1, 0) - a_T_b(1, 0) + b(1, 0); + const Scalar _tmp2 = -a(2, 0) - a_T_b(2, 0) + b(2, 0); + const Scalar _tmp3 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp4 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp5 = std::pow(sqrt_info(2, 0), Scalar(2)); + const Scalar _tmp6 = _tmp0 * _tmp3; + const Scalar _tmp7 = _tmp1 * _tmp4; + const Scalar _tmp8 = _tmp2 * _tmp5; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info(0, 0); + _res(1, 0) = _tmp1 * sqrt_info(1, 0); + _res(2, 0) = _tmp2 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = -sqrt_info(0, 0); + _jacobian(1, 1) = -sqrt_info(1, 0); + _jacobian(2, 2) = -sqrt_info(2, 0); + _jacobian(0, 3) = sqrt_info(0, 0); + _jacobian(1, 4) = sqrt_info(1, 0); + _jacobian(2, 5) = sqrt_info(2, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp3; + _hessian(3, 0) = -_tmp3; + _hessian(1, 1) = _tmp4; + _hessian(4, 1) = -_tmp4; + _hessian(2, 2) = _tmp5; + _hessian(5, 2) = -_tmp5; + _hessian(3, 3) = _tmp3; + _hessian(4, 4) = _tmp4; + _hessian(5, 5) = _tmp5; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = -_tmp6; + _rhs(1, 0) = -_tmp7; + _rhs(2, 0) = -_tmp8; + _rhs(3, 0) = _tmp6; + _rhs(4, 0) = _tmp7; + _rhs(5, 0) = _tmp8; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_isotropic.h b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_isotropic.h new file mode 100644 index 000000000..99e565abd --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_isotropic.h @@ -0,0 +1,109 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorMatrix31(const Eigen::Matrix& a, + const Eigen::Matrix& b, + const Eigen::Matrix& a_T_b, const Scalar sqrt_info, + 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: 18 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = -a(0, 0) - a_T_b(0, 0) + b(0, 0); + const Scalar _tmp1 = -a(1, 0) - a_T_b(1, 0) + b(1, 0); + const Scalar _tmp2 = -a(2, 0) - a_T_b(2, 0) + b(2, 0); + const Scalar _tmp3 = -sqrt_info; + const Scalar _tmp4 = std::pow(sqrt_info, Scalar(2)); + const Scalar _tmp5 = -_tmp4; + const Scalar _tmp6 = _tmp0 * _tmp4; + const Scalar _tmp7 = _tmp1 * _tmp4; + const Scalar _tmp8 = _tmp2 * _tmp4; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info; + _res(1, 0) = _tmp1 * sqrt_info; + _res(2, 0) = _tmp2 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = _tmp3; + _jacobian(1, 1) = _tmp3; + _jacobian(2, 2) = _tmp3; + _jacobian(0, 3) = sqrt_info; + _jacobian(1, 4) = sqrt_info; + _jacobian(2, 5) = sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp4; + _hessian(3, 0) = _tmp5; + _hessian(1, 1) = _tmp4; + _hessian(4, 1) = _tmp5; + _hessian(2, 2) = _tmp4; + _hessian(5, 2) = _tmp5; + _hessian(3, 3) = _tmp4; + _hessian(4, 4) = _tmp4; + _hessian(5, 5) = _tmp4; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = -_tmp6; + _rhs(1, 0) = -_tmp7; + _rhs(2, 0) = -_tmp8; + _rhs(3, 0) = _tmp6; + _rhs(4, 0) = _tmp7; + _rhs(5, 0) = _tmp8; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_square.h b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_square.h new file mode 100644 index 000000000..5306eebff --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_matrix31/between_factor_matrix31_square.h @@ -0,0 +1,178 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorMatrix31(const Eigen::Matrix& a, + const Eigen::Matrix& b, + const Eigen::Matrix& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 102 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (42) + const Scalar _tmp0 = -a(1, 0) - a_T_b(1, 0) + b(1, 0); + const Scalar _tmp1 = -a(2, 0) - a_T_b(2, 0) + b(2, 0); + const Scalar _tmp2 = -a(0, 0) - a_T_b(0, 0) + b(0, 0); + const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp2 * sqrt_info(0, 0); + const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp2 * sqrt_info(1, 0); + const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp2 * sqrt_info(2, 0); + const Scalar _tmp6 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp7 = std::pow(sqrt_info(2, 0), Scalar(2)); + const Scalar _tmp8 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp9 = _tmp6 + _tmp7 + _tmp8; + const Scalar _tmp10 = sqrt_info(0, 0) * sqrt_info(0, 1); + const Scalar _tmp11 = sqrt_info(2, 0) * sqrt_info(2, 1); + const Scalar _tmp12 = sqrt_info(1, 0) * sqrt_info(1, 1); + const Scalar _tmp13 = _tmp10 + _tmp11 + _tmp12; + const Scalar _tmp14 = sqrt_info(0, 0) * sqrt_info(0, 2); + const Scalar _tmp15 = sqrt_info(2, 0) * sqrt_info(2, 2); + const Scalar _tmp16 = sqrt_info(1, 0) * sqrt_info(1, 2); + const Scalar _tmp17 = _tmp14 + _tmp15 + _tmp16; + const Scalar _tmp18 = -_tmp10 - _tmp11 - _tmp12; + const Scalar _tmp19 = -_tmp14 - _tmp15 - _tmp16; + const Scalar _tmp20 = std::pow(sqrt_info(0, 1), Scalar(2)); + const Scalar _tmp21 = std::pow(sqrt_info(2, 1), Scalar(2)); + const Scalar _tmp22 = std::pow(sqrt_info(1, 1), Scalar(2)); + const Scalar _tmp23 = _tmp20 + _tmp21 + _tmp22; + const Scalar _tmp24 = sqrt_info(0, 1) * sqrt_info(0, 2); + const Scalar _tmp25 = sqrt_info(2, 1) * sqrt_info(2, 2); + const Scalar _tmp26 = sqrt_info(1, 1) * sqrt_info(1, 2); + const Scalar _tmp27 = _tmp24 + _tmp25 + _tmp26; + const Scalar _tmp28 = -_tmp24 - _tmp25 - _tmp26; + const Scalar _tmp29 = std::pow(sqrt_info(0, 2), Scalar(2)); + const Scalar _tmp30 = std::pow(sqrt_info(2, 2), Scalar(2)); + const Scalar _tmp31 = std::pow(sqrt_info(1, 2), Scalar(2)); + const Scalar _tmp32 = _tmp29 + _tmp30 + _tmp31; + const Scalar _tmp33 = _tmp5 * sqrt_info(2, 0); + const Scalar _tmp34 = _tmp3 * sqrt_info(0, 0); + const Scalar _tmp35 = _tmp4 * sqrt_info(1, 0); + const Scalar _tmp36 = _tmp5 * sqrt_info(2, 1); + const Scalar _tmp37 = _tmp3 * sqrt_info(0, 1); + const Scalar _tmp38 = _tmp4 * sqrt_info(1, 1); + const Scalar _tmp39 = _tmp5 * sqrt_info(2, 2); + const Scalar _tmp40 = _tmp3 * sqrt_info(0, 2); + const Scalar _tmp41 = _tmp4 * sqrt_info(1, 2); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp3; + _res(1, 0) = _tmp4; + _res(2, 0) = _tmp5; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = -sqrt_info(0, 0); + _jacobian(1, 0) = -sqrt_info(1, 0); + _jacobian(2, 0) = -sqrt_info(2, 0); + _jacobian(0, 1) = -sqrt_info(0, 1); + _jacobian(1, 1) = -sqrt_info(1, 1); + _jacobian(2, 1) = -sqrt_info(2, 1); + _jacobian(0, 2) = -sqrt_info(0, 2); + _jacobian(1, 2) = -sqrt_info(1, 2); + _jacobian(2, 2) = -sqrt_info(2, 2); + _jacobian(0, 3) = sqrt_info(0, 0); + _jacobian(1, 3) = sqrt_info(1, 0); + _jacobian(2, 3) = sqrt_info(2, 0); + _jacobian(0, 4) = sqrt_info(0, 1); + _jacobian(1, 4) = sqrt_info(1, 1); + _jacobian(2, 4) = sqrt_info(2, 1); + _jacobian(0, 5) = sqrt_info(0, 2); + _jacobian(1, 5) = sqrt_info(1, 2); + _jacobian(2, 5) = sqrt_info(2, 2); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = _tmp9; + _hessian(1, 0) = _tmp13; + _hessian(2, 0) = _tmp17; + _hessian(3, 0) = -_tmp6 - _tmp7 - _tmp8; + _hessian(4, 0) = _tmp18; + _hessian(5, 0) = _tmp19; + _hessian(0, 1) = 0; + _hessian(1, 1) = _tmp23; + _hessian(2, 1) = _tmp27; + _hessian(3, 1) = _tmp18; + _hessian(4, 1) = -_tmp20 - _tmp21 - _tmp22; + _hessian(5, 1) = _tmp28; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = _tmp32; + _hessian(3, 2) = _tmp19; + _hessian(4, 2) = _tmp28; + _hessian(5, 2) = -_tmp29 - _tmp30 - _tmp31; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = _tmp9; + _hessian(4, 3) = _tmp13; + _hessian(5, 3) = _tmp17; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = _tmp23; + _hessian(5, 4) = _tmp27; + _hessian(0, 5) = 0; + _hessian(1, 5) = 0; + _hessian(2, 5) = 0; + _hessian(3, 5) = 0; + _hessian(4, 5) = 0; + _hessian(5, 5) = _tmp32; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = -_tmp33 - _tmp34 - _tmp35; + _rhs(1, 0) = -_tmp36 - _tmp37 - _tmp38; + _rhs(2, 0) = -_tmp39 - _tmp40 - _tmp41; + _rhs(3, 0) = _tmp33 + _tmp34 + _tmp35; + _rhs(4, 0) = _tmp36 + _tmp37 + _tmp38; + _rhs(5, 0) = _tmp39 + _tmp40 + _tmp41; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose2.h b/gen/cpp/sym/factors/between_factor_pose2.h index dc76bc5e4..75dabfd57 100644 --- a/gen/cpp/sym/factors/between_factor_pose2.h +++ b/gen/cpp/sym/factors/between_factor_pose2.h @@ -1,220 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_T_b. - * - * In vector space terms that would be: - * (b - a) - a_T_b - * - * In lie group terms: - * local_coordinates(a_T_b, between(a, b)) - * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x6) jacobian of res wrt args a (3), b (3) - * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) - * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) - */ -template -void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b, - const sym::Pose2& a_T_b, - const Eigen::Matrix& sqrt_info, 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: 265 - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - const Eigen::Matrix& _a_T_b = a_T_b.Data(); - - // Intermediate terms (68) - const Scalar _tmp0 = _a[0] * _a[3]; - const Scalar _tmp1 = _a[1] * _a[2]; - const Scalar _tmp2 = _a[0] * _b[3] - _a[1] * _b[2]; - const Scalar _tmp3 = -_a_T_b[3] - _tmp0 + _tmp1 + _tmp2; - const Scalar _tmp4 = _a[0] * _a[2] + _a[1] * _a[3]; - const Scalar _tmp5 = _a[1] * _b[3]; - const Scalar _tmp6 = _a[0] * _b[2]; - const Scalar _tmp7 = -_a_T_b[2] - _tmp4 + _tmp5 + _tmp6; - const Scalar _tmp8 = _a[1] * _b[1]; - const Scalar _tmp9 = _a[0] * _b[0]; - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = _a_T_b[1] * _tmp10; - const Scalar _tmp12 = _a[1] * _b[0]; - const Scalar _tmp13 = _a[0] * _b[1]; - const Scalar _tmp14 = -_tmp12 + _tmp13; - const Scalar _tmp15 = _a_T_b[0] * _tmp14; - const Scalar _tmp16 = -_tmp11 + _tmp15; - const Scalar _tmp17 = _a_T_b[0] * _tmp10; - const Scalar _tmp18 = _a_T_b[1] * _tmp14; - const Scalar _tmp19 = _tmp17 + _tmp18; - const Scalar _tmp20 = _tmp19 + epsilon * ((((_tmp19) > 0) - ((_tmp19) < 0)) + Scalar(0.5)); - const Scalar _tmp21 = std::atan2(_tmp16, _tmp20); - const Scalar _tmp22 = - _tmp21 * sqrt_info(0, 0) + _tmp3 * sqrt_info(0, 2) + _tmp7 * sqrt_info(0, 1); - const Scalar _tmp23 = - _tmp21 * sqrt_info(1, 0) + _tmp3 * sqrt_info(1, 2) + _tmp7 * sqrt_info(1, 1); - const Scalar _tmp24 = - _tmp21 * sqrt_info(2, 0) + _tmp3 * sqrt_info(2, 2) + _tmp7 * sqrt_info(2, 1); - const Scalar _tmp25 = -_tmp8 - _tmp9; - const Scalar _tmp26 = Scalar(1.0) / (_tmp20); - const Scalar _tmp27 = std::pow(_tmp20, Scalar(2)); - const Scalar _tmp28 = _tmp16 / _tmp27; - const Scalar _tmp29 = std::pow(_tmp16, Scalar(2)) + _tmp27; - const Scalar _tmp30 = _tmp27 / _tmp29; - const Scalar _tmp31 = - _tmp30 * (_tmp26 * (_a_T_b[0] * _tmp25 - _tmp18) - _tmp28 * (_a_T_b[1] * _tmp25 + _tmp15)); - const Scalar _tmp32 = -_tmp0 + _tmp1 + _tmp2; - const Scalar _tmp33 = _tmp4 - _tmp5 - _tmp6; - const Scalar _tmp34 = - _tmp31 * sqrt_info(0, 0) + _tmp32 * sqrt_info(0, 1) + _tmp33 * sqrt_info(0, 2); - const Scalar _tmp35 = - _tmp31 * sqrt_info(1, 0) + _tmp32 * sqrt_info(1, 1) + _tmp33 * sqrt_info(1, 2); - const Scalar _tmp36 = - _tmp31 * sqrt_info(2, 0) + _tmp32 * sqrt_info(2, 1) + _tmp33 * sqrt_info(2, 2); - const Scalar _tmp37 = _a[0] * sqrt_info(0, 1); - const Scalar _tmp38 = _a[1] * sqrt_info(0, 2); - const Scalar _tmp39 = -_tmp37 + _tmp38; - const Scalar _tmp40 = _a[1] * sqrt_info(1, 2); - const Scalar _tmp41 = _a[0] * sqrt_info(1, 1); - const Scalar _tmp42 = _tmp40 - _tmp41; - const Scalar _tmp43 = _a[1] * sqrt_info(2, 2); - const Scalar _tmp44 = _a[0] * sqrt_info(2, 1); - const Scalar _tmp45 = _tmp43 - _tmp44; - const Scalar _tmp46 = _a[0] * sqrt_info(0, 2); - const Scalar _tmp47 = _a[1] * sqrt_info(0, 1); - const Scalar _tmp48 = -_tmp46 - _tmp47; - const Scalar _tmp49 = _a[1] * sqrt_info(1, 1); - const Scalar _tmp50 = _a[0] * sqrt_info(1, 2); - const Scalar _tmp51 = -_tmp49 - _tmp50; - const Scalar _tmp52 = _a[1] * sqrt_info(2, 1); - const Scalar _tmp53 = _a[0] * sqrt_info(2, 2); - const Scalar _tmp54 = -_tmp52 - _tmp53; - const Scalar _tmp55 = _tmp12 - _tmp13; - const Scalar _tmp56 = - _tmp26 * (-_a_T_b[1] * _tmp55 + _tmp17) - _tmp28 * (_a_T_b[0] * _tmp55 + _tmp11); - const Scalar _tmp57 = _tmp30 * _tmp56; - const Scalar _tmp58 = _tmp57 * sqrt_info(0, 0); - const Scalar _tmp59 = _tmp57 * sqrt_info(1, 0); - const Scalar _tmp60 = _tmp57 * sqrt_info(2, 0); - const Scalar _tmp61 = _tmp37 - _tmp38; - const Scalar _tmp62 = -_tmp40 + _tmp41; - const Scalar _tmp63 = -_tmp43 + _tmp44; - const Scalar _tmp64 = _tmp46 + _tmp47; - const Scalar _tmp65 = _tmp49 + _tmp50; - const Scalar _tmp66 = _tmp52 + _tmp53; - const Scalar _tmp67 = - std::pow(_tmp20, Scalar(4)) * std::pow(_tmp56, Scalar(2)) / std::pow(_tmp29, Scalar(2)); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp22; - _res(1, 0) = _tmp23; - _res(2, 0) = _tmp24; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp34; - _jacobian(1, 0) = _tmp35; - _jacobian(2, 0) = _tmp36; - _jacobian(0, 1) = _tmp39; - _jacobian(1, 1) = _tmp42; - _jacobian(2, 1) = _tmp45; - _jacobian(0, 2) = _tmp48; - _jacobian(1, 2) = _tmp51; - _jacobian(2, 2) = _tmp54; - _jacobian(0, 3) = _tmp58; - _jacobian(1, 3) = _tmp59; - _jacobian(2, 3) = _tmp60; - _jacobian(0, 4) = _tmp61; - _jacobian(1, 4) = _tmp62; - _jacobian(2, 4) = _tmp63; - _jacobian(0, 5) = _tmp64; - _jacobian(1, 5) = _tmp65; - _jacobian(2, 5) = _tmp66; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = - std::pow(_tmp34, Scalar(2)) + std::pow(_tmp35, Scalar(2)) + std::pow(_tmp36, Scalar(2)); - _hessian(1, 0) = _tmp34 * _tmp39 + _tmp35 * _tmp42 + _tmp36 * _tmp45; - _hessian(2, 0) = _tmp34 * _tmp48 + _tmp35 * _tmp51 + _tmp36 * _tmp54; - _hessian(3, 0) = _tmp34 * _tmp58 + _tmp35 * _tmp59 + _tmp36 * _tmp60; - _hessian(4, 0) = _tmp34 * _tmp61 + _tmp35 * _tmp62 + _tmp36 * _tmp63; - _hessian(5, 0) = _tmp34 * _tmp64 + _tmp35 * _tmp65 + _tmp36 * _tmp66; - _hessian(0, 1) = 0; - _hessian(1, 1) = - std::pow(_tmp39, Scalar(2)) + std::pow(_tmp42, Scalar(2)) + std::pow(_tmp45, Scalar(2)); - _hessian(2, 1) = _tmp39 * _tmp48 + _tmp42 * _tmp51 + _tmp45 * _tmp54; - _hessian(3, 1) = _tmp39 * _tmp58 + _tmp42 * _tmp59 + _tmp45 * _tmp60; - _hessian(4, 1) = _tmp39 * _tmp61 + _tmp42 * _tmp62 + _tmp45 * _tmp63; - _hessian(5, 1) = _tmp39 * _tmp64 + _tmp42 * _tmp65 + _tmp45 * _tmp66; - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = - std::pow(_tmp48, Scalar(2)) + std::pow(_tmp51, Scalar(2)) + std::pow(_tmp54, Scalar(2)); - _hessian(3, 2) = _tmp48 * _tmp58 + _tmp51 * _tmp59 + _tmp54 * _tmp60; - _hessian(4, 2) = _tmp48 * _tmp61 + _tmp51 * _tmp62 + _tmp54 * _tmp63; - _hessian(5, 2) = _tmp48 * _tmp64 + _tmp51 * _tmp65 + _tmp54 * _tmp66; - _hessian(0, 3) = 0; - _hessian(1, 3) = 0; - _hessian(2, 3) = 0; - _hessian(3, 3) = _tmp67 * std::pow(sqrt_info(0, 0), Scalar(2)) + - _tmp67 * std::pow(sqrt_info(1, 0), Scalar(2)) + - _tmp67 * std::pow(sqrt_info(2, 0), Scalar(2)); - _hessian(4, 3) = _tmp58 * _tmp61 + _tmp59 * _tmp62 + _tmp60 * _tmp63; - _hessian(5, 3) = _tmp58 * _tmp64 + _tmp59 * _tmp65 + _tmp60 * _tmp66; - _hessian(0, 4) = 0; - _hessian(1, 4) = 0; - _hessian(2, 4) = 0; - _hessian(3, 4) = 0; - _hessian(4, 4) = - std::pow(_tmp61, Scalar(2)) + std::pow(_tmp62, Scalar(2)) + std::pow(_tmp63, Scalar(2)); - _hessian(5, 4) = _tmp61 * _tmp64 + _tmp62 * _tmp65 + _tmp63 * _tmp66; - _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(_tmp64, Scalar(2)) + std::pow(_tmp65, Scalar(2)) + std::pow(_tmp66, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp22 * _tmp34 + _tmp23 * _tmp35 + _tmp24 * _tmp36; - _rhs(1, 0) = _tmp22 * _tmp39 + _tmp23 * _tmp42 + _tmp24 * _tmp45; - _rhs(2, 0) = _tmp22 * _tmp48 + _tmp23 * _tmp51 + _tmp24 * _tmp54; - _rhs(3, 0) = _tmp22 * _tmp58 + _tmp23 * _tmp59 + _tmp24 * _tmp60; - _rhs(4, 0) = _tmp22 * _tmp61 + _tmp23 * _tmp62 + _tmp24 * _tmp63; - _rhs(5, 0) = _tmp22 * _tmp64 + _tmp23 * _tmp65 + _tmp24 * _tmp66; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_pose2/between_factor_pose2_diagonal.h" +#include "./between_factor_pose2/between_factor_pose2_isotropic.h" +#include "./between_factor_pose2/between_factor_pose2_square.h" diff --git a/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_diagonal.h b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_diagonal.h new file mode 100644 index 000000000..faf6fe9f7 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_diagonal.h @@ -0,0 +1,190 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b, + const sym::Pose2& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 143 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (67) + const Scalar _tmp0 = _a[1] * _b[1]; + const Scalar _tmp1 = _a[0] * _b[0]; + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = _a_T_b[1] * _tmp2; + const Scalar _tmp4 = _a[1] * _b[0]; + const Scalar _tmp5 = _a[0] * _b[1]; + const Scalar _tmp6 = -_tmp4 + _tmp5; + const Scalar _tmp7 = _a_T_b[0] * _tmp6; + const Scalar _tmp8 = -_tmp3 + _tmp7; + const Scalar _tmp9 = _a_T_b[0] * _tmp2; + const Scalar _tmp10 = _a_T_b[1] * _tmp6; + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); + const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); + const Scalar _tmp14 = _a[0] * _a[2] + _a[1] * _a[3]; + const Scalar _tmp15 = _a[1] * _b[3]; + const Scalar _tmp16 = _a[0] * _b[2]; + const Scalar _tmp17 = -_a_T_b[2] - _tmp14 + _tmp15 + _tmp16; + const Scalar _tmp18 = _a[0] * _a[3]; + const Scalar _tmp19 = _a[1] * _a[2]; + const Scalar _tmp20 = _a[0] * _b[3] - _a[1] * _b[2]; + const Scalar _tmp21 = -_a_T_b[3] - _tmp18 + _tmp19 + _tmp20; + const Scalar _tmp22 = -_tmp0 - _tmp1; + const Scalar _tmp23 = Scalar(1.0) / (_tmp12); + const Scalar _tmp24 = std::pow(_tmp12, Scalar(2)); + const Scalar _tmp25 = _tmp8 / _tmp24; + const Scalar _tmp26 = + _tmp23 * (_a_T_b[0] * _tmp22 - _tmp10) - _tmp25 * (_a_T_b[1] * _tmp22 + _tmp7); + const Scalar _tmp27 = _tmp24 + std::pow(_tmp8, Scalar(2)); + const Scalar _tmp28 = _tmp24 / _tmp27; + const Scalar _tmp29 = _tmp28 * sqrt_info(0, 0); + const Scalar _tmp30 = -_tmp18 + _tmp19 + _tmp20; + const Scalar _tmp31 = _tmp14 - _tmp15 - _tmp16; + const Scalar _tmp32 = _a[0] * sqrt_info(1, 0); + const Scalar _tmp33 = _a[1] * sqrt_info(2, 0); + const Scalar _tmp34 = _a[1] * sqrt_info(1, 0); + const Scalar _tmp35 = _a[0] * sqrt_info(2, 0); + const Scalar _tmp36 = _tmp4 - _tmp5; + const Scalar _tmp37 = + _tmp23 * (-_a_T_b[1] * _tmp36 + _tmp9) - _tmp25 * (_a_T_b[0] * _tmp36 + _tmp3); + const Scalar _tmp38 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp39 = std::pow(_tmp12, Scalar(4)) * _tmp38 / std::pow(_tmp27, Scalar(2)); + const Scalar _tmp40 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp41 = std::pow(sqrt_info(2, 0), Scalar(2)); + const Scalar _tmp42 = _a[0] * _tmp40; + const Scalar _tmp43 = _tmp30 * _tmp42; + const Scalar _tmp44 = _tmp31 * _tmp41; + const Scalar _tmp45 = _a[1] * _tmp44; + const Scalar _tmp46 = _tmp30 * _tmp40; + const Scalar _tmp47 = _a[1] * _tmp46; + const Scalar _tmp48 = _a[0] * _tmp44; + const Scalar _tmp49 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp50 = _tmp41 * _tmp49; + const Scalar _tmp51 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp52 = _tmp40 * _tmp51; + const Scalar _tmp53 = _tmp50 + _tmp52; + const Scalar _tmp54 = _a[0] * _a[1] * _tmp41; + const Scalar _tmp55 = _a[1] * _tmp42; + const Scalar _tmp56 = -_tmp54 + _tmp55; + const Scalar _tmp57 = _tmp54 - _tmp55; + const Scalar _tmp58 = _tmp40 * _tmp49; + const Scalar _tmp59 = _tmp41 * _tmp51; + const Scalar _tmp60 = _tmp58 + _tmp59; + const Scalar _tmp61 = _tmp13 * _tmp28 * _tmp38; + const Scalar _tmp62 = _tmp21 * _tmp41; + const Scalar _tmp63 = _tmp17 * _tmp42; + const Scalar _tmp64 = _a[1] * _tmp62; + const Scalar _tmp65 = _a[0] * _tmp62; + const Scalar _tmp66 = _a[1] * _tmp17 * _tmp40; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp13 * sqrt_info(0, 0); + _res(1, 0) = _tmp17 * sqrt_info(1, 0); + _res(2, 0) = _tmp21 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp26 * _tmp29; + _jacobian(1, 0) = _tmp30 * sqrt_info(1, 0); + _jacobian(2, 0) = _tmp31 * sqrt_info(2, 0); + _jacobian(0, 1) = 0; + _jacobian(1, 1) = -_tmp32; + _jacobian(2, 1) = _tmp33; + _jacobian(0, 2) = 0; + _jacobian(1, 2) = -_tmp34; + _jacobian(2, 2) = -_tmp35; + _jacobian(0, 3) = _tmp29 * _tmp37; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = _tmp32; + _jacobian(2, 4) = -_tmp33; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = _tmp34; + _jacobian(2, 5) = _tmp35; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp26, Scalar(2)) * _tmp39 + std::pow(_tmp30, Scalar(2)) * _tmp40 + + std::pow(_tmp31, Scalar(2)) * _tmp41; + _hessian(1, 0) = -_tmp43 + _tmp45; + _hessian(2, 0) = -_tmp47 - _tmp48; + _hessian(3, 0) = _tmp26 * _tmp37 * _tmp39; + _hessian(4, 0) = _tmp43 - _tmp45; + _hessian(5, 0) = _tmp47 + _tmp48; + _hessian(1, 1) = _tmp53; + _hessian(2, 1) = _tmp56; + _hessian(4, 1) = -_tmp50 - _tmp52; + _hessian(5, 1) = _tmp57; + _hessian(2, 2) = _tmp60; + _hessian(4, 2) = _tmp57; + _hessian(5, 2) = -_tmp58 - _tmp59; + _hessian(3, 3) = std::pow(_tmp37, Scalar(2)) * _tmp39; + _hessian(4, 4) = _tmp53; + _hessian(5, 4) = _tmp56; + _hessian(5, 5) = _tmp60; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp17 * _tmp46 + _tmp26 * _tmp61 + _tmp31 * _tmp62; + _rhs(1, 0) = -_tmp63 + _tmp64; + _rhs(2, 0) = -_tmp65 - _tmp66; + _rhs(3, 0) = _tmp37 * _tmp61; + _rhs(4, 0) = _tmp63 - _tmp64; + _rhs(5, 0) = _tmp65 + _tmp66; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_isotropic.h b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_isotropic.h new file mode 100644 index 000000000..b82715ec8 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_isotropic.h @@ -0,0 +1,175 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b, + const sym::Pose2& a_T_b, const Scalar sqrt_info, + 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: 126 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (57) + const Scalar _tmp0 = _a[1] * _b[1]; + const Scalar _tmp1 = _a[0] * _b[0]; + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = _a_T_b[1] * _tmp2; + const Scalar _tmp4 = _a[1] * _b[0]; + const Scalar _tmp5 = _a[0] * _b[1]; + const Scalar _tmp6 = -_tmp4 + _tmp5; + const Scalar _tmp7 = _a_T_b[0] * _tmp6; + const Scalar _tmp8 = -_tmp3 + _tmp7; + const Scalar _tmp9 = _a_T_b[0] * _tmp2; + const Scalar _tmp10 = _a_T_b[1] * _tmp6; + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); + const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); + const Scalar _tmp14 = _a[0] * _a[2] + _a[1] * _a[3]; + const Scalar _tmp15 = _a[1] * _b[3]; + const Scalar _tmp16 = _a[0] * _b[2]; + const Scalar _tmp17 = -_a_T_b[2] - _tmp14 + _tmp15 + _tmp16; + const Scalar _tmp18 = _a[0] * _a[3]; + const Scalar _tmp19 = _a[1] * _a[2]; + const Scalar _tmp20 = _a[0] * _b[3] - _a[1] * _b[2]; + const Scalar _tmp21 = -_a_T_b[3] - _tmp18 + _tmp19 + _tmp20; + const Scalar _tmp22 = -_tmp0 - _tmp1; + const Scalar _tmp23 = Scalar(1.0) / (_tmp12); + const Scalar _tmp24 = std::pow(_tmp12, Scalar(2)); + const Scalar _tmp25 = _tmp8 / _tmp24; + const Scalar _tmp26 = + _tmp23 * (_a_T_b[0] * _tmp22 - _tmp10) - _tmp25 * (_a_T_b[1] * _tmp22 + _tmp7); + const Scalar _tmp27 = _tmp24 + std::pow(_tmp8, Scalar(2)); + const Scalar _tmp28 = _tmp24 / _tmp27; + const Scalar _tmp29 = _tmp28 * sqrt_info; + const Scalar _tmp30 = -_tmp18 + _tmp19 + _tmp20; + const Scalar _tmp31 = _tmp14 - _tmp15 - _tmp16; + const Scalar _tmp32 = _a[0] * sqrt_info; + const Scalar _tmp33 = -_tmp32; + const Scalar _tmp34 = _a[1] * sqrt_info; + const Scalar _tmp35 = -_tmp34; + const Scalar _tmp36 = _tmp4 - _tmp5; + const Scalar _tmp37 = + _tmp23 * (-_a_T_b[1] * _tmp36 + _tmp9) - _tmp25 * (_a_T_b[0] * _tmp36 + _tmp3); + const Scalar _tmp38 = std::pow(sqrt_info, Scalar(2)); + const Scalar _tmp39 = std::pow(_tmp12, Scalar(4)) * _tmp38 / std::pow(_tmp27, Scalar(2)); + const Scalar _tmp40 = _tmp30 * _tmp38; + const Scalar _tmp41 = _a[0] * _tmp40; + const Scalar _tmp42 = _tmp31 * _tmp38; + const Scalar _tmp43 = _a[1] * _tmp42; + const Scalar _tmp44 = _a[1] * _tmp40; + const Scalar _tmp45 = _a[0] * _tmp42; + const Scalar _tmp46 = std::pow(_a[0], Scalar(2)) * _tmp38; + const Scalar _tmp47 = std::pow(_a[1], Scalar(2)) * _tmp38; + const Scalar _tmp48 = _tmp46 + _tmp47; + const Scalar _tmp49 = -_tmp46 - _tmp47; + const Scalar _tmp50 = _tmp13 * _tmp28 * _tmp38; + const Scalar _tmp51 = _a[1] * _tmp38; + const Scalar _tmp52 = _tmp21 * _tmp51; + const Scalar _tmp53 = _a[0] * _tmp38; + const Scalar _tmp54 = _tmp17 * _tmp53; + const Scalar _tmp55 = _tmp17 * _tmp51; + const Scalar _tmp56 = _tmp21 * _tmp53; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp13 * sqrt_info; + _res(1, 0) = _tmp17 * sqrt_info; + _res(2, 0) = _tmp21 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp26 * _tmp29; + _jacobian(1, 0) = _tmp30 * sqrt_info; + _jacobian(2, 0) = _tmp31 * sqrt_info; + _jacobian(0, 1) = 0; + _jacobian(1, 1) = _tmp33; + _jacobian(2, 1) = _tmp34; + _jacobian(0, 2) = 0; + _jacobian(1, 2) = _tmp35; + _jacobian(2, 2) = _tmp33; + _jacobian(0, 3) = _tmp29 * _tmp37; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = _tmp32; + _jacobian(2, 4) = _tmp35; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = _tmp34; + _jacobian(2, 5) = _tmp32; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp26, Scalar(2)) * _tmp39 + std::pow(_tmp30, Scalar(2)) * _tmp38 + + std::pow(_tmp31, Scalar(2)) * _tmp38; + _hessian(1, 0) = -_tmp41 + _tmp43; + _hessian(2, 0) = -_tmp44 - _tmp45; + _hessian(3, 0) = _tmp26 * _tmp37 * _tmp39; + _hessian(4, 0) = _tmp41 - _tmp43; + _hessian(5, 0) = _tmp44 + _tmp45; + _hessian(1, 1) = _tmp48; + _hessian(4, 1) = _tmp49; + _hessian(2, 2) = _tmp48; + _hessian(5, 2) = _tmp49; + _hessian(3, 3) = std::pow(_tmp37, Scalar(2)) * _tmp39; + _hessian(4, 4) = _tmp48; + _hessian(5, 5) = _tmp48; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp17 * _tmp40 + _tmp21 * _tmp42 + _tmp26 * _tmp50; + _rhs(1, 0) = _tmp52 - _tmp54; + _rhs(2, 0) = -_tmp55 - _tmp56; + _rhs(3, 0) = _tmp37 * _tmp50; + _rhs(4, 0) = -_tmp52 + _tmp54; + _rhs(5, 0) = _tmp55 + _tmp56; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_square.h b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_square.h new file mode 100644 index 000000000..f5d27cbf9 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose2/between_factor_pose2_square.h @@ -0,0 +1,221 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b, + const sym::Pose2& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 265 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (68) + const Scalar _tmp0 = _a[0] * _a[3]; + const Scalar _tmp1 = _a[1] * _a[2]; + const Scalar _tmp2 = _a[0] * _b[3] - _a[1] * _b[2]; + const Scalar _tmp3 = -_a_T_b[3] - _tmp0 + _tmp1 + _tmp2; + const Scalar _tmp4 = _a[0] * _a[2] + _a[1] * _a[3]; + const Scalar _tmp5 = _a[1] * _b[3]; + const Scalar _tmp6 = _a[0] * _b[2]; + const Scalar _tmp7 = -_a_T_b[2] - _tmp4 + _tmp5 + _tmp6; + const Scalar _tmp8 = _a[1] * _b[1]; + const Scalar _tmp9 = _a[0] * _b[0]; + const Scalar _tmp10 = _tmp8 + _tmp9; + const Scalar _tmp11 = _a_T_b[1] * _tmp10; + const Scalar _tmp12 = _a[1] * _b[0]; + const Scalar _tmp13 = _a[0] * _b[1]; + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _a_T_b[0] * _tmp14; + const Scalar _tmp16 = -_tmp11 + _tmp15; + const Scalar _tmp17 = _a_T_b[0] * _tmp10; + const Scalar _tmp18 = _a_T_b[1] * _tmp14; + const Scalar _tmp19 = _tmp17 + _tmp18; + const Scalar _tmp20 = _tmp19 + epsilon * ((((_tmp19) > 0) - ((_tmp19) < 0)) + Scalar(0.5)); + const Scalar _tmp21 = std::atan2(_tmp16, _tmp20); + const Scalar _tmp22 = + _tmp21 * sqrt_info(0, 0) + _tmp3 * sqrt_info(0, 2) + _tmp7 * sqrt_info(0, 1); + const Scalar _tmp23 = + _tmp21 * sqrt_info(1, 0) + _tmp3 * sqrt_info(1, 2) + _tmp7 * sqrt_info(1, 1); + const Scalar _tmp24 = + _tmp21 * sqrt_info(2, 0) + _tmp3 * sqrt_info(2, 2) + _tmp7 * sqrt_info(2, 1); + const Scalar _tmp25 = -_tmp8 - _tmp9; + const Scalar _tmp26 = Scalar(1.0) / (_tmp20); + const Scalar _tmp27 = std::pow(_tmp20, Scalar(2)); + const Scalar _tmp28 = _tmp16 / _tmp27; + const Scalar _tmp29 = std::pow(_tmp16, Scalar(2)) + _tmp27; + const Scalar _tmp30 = _tmp27 / _tmp29; + const Scalar _tmp31 = + _tmp30 * (_tmp26 * (_a_T_b[0] * _tmp25 - _tmp18) - _tmp28 * (_a_T_b[1] * _tmp25 + _tmp15)); + const Scalar _tmp32 = -_tmp0 + _tmp1 + _tmp2; + const Scalar _tmp33 = _tmp4 - _tmp5 - _tmp6; + const Scalar _tmp34 = + _tmp31 * sqrt_info(0, 0) + _tmp32 * sqrt_info(0, 1) + _tmp33 * sqrt_info(0, 2); + const Scalar _tmp35 = + _tmp31 * sqrt_info(1, 0) + _tmp32 * sqrt_info(1, 1) + _tmp33 * sqrt_info(1, 2); + const Scalar _tmp36 = + _tmp31 * sqrt_info(2, 0) + _tmp32 * sqrt_info(2, 1) + _tmp33 * sqrt_info(2, 2); + const Scalar _tmp37 = _a[0] * sqrt_info(0, 1); + const Scalar _tmp38 = _a[1] * sqrt_info(0, 2); + const Scalar _tmp39 = -_tmp37 + _tmp38; + const Scalar _tmp40 = _a[1] * sqrt_info(1, 2); + const Scalar _tmp41 = _a[0] * sqrt_info(1, 1); + const Scalar _tmp42 = _tmp40 - _tmp41; + const Scalar _tmp43 = _a[1] * sqrt_info(2, 2); + const Scalar _tmp44 = _a[0] * sqrt_info(2, 1); + const Scalar _tmp45 = _tmp43 - _tmp44; + const Scalar _tmp46 = _a[0] * sqrt_info(0, 2); + const Scalar _tmp47 = _a[1] * sqrt_info(0, 1); + const Scalar _tmp48 = -_tmp46 - _tmp47; + const Scalar _tmp49 = _a[1] * sqrt_info(1, 1); + const Scalar _tmp50 = _a[0] * sqrt_info(1, 2); + const Scalar _tmp51 = -_tmp49 - _tmp50; + const Scalar _tmp52 = _a[1] * sqrt_info(2, 1); + const Scalar _tmp53 = _a[0] * sqrt_info(2, 2); + const Scalar _tmp54 = -_tmp52 - _tmp53; + const Scalar _tmp55 = _tmp12 - _tmp13; + const Scalar _tmp56 = + _tmp26 * (-_a_T_b[1] * _tmp55 + _tmp17) - _tmp28 * (_a_T_b[0] * _tmp55 + _tmp11); + const Scalar _tmp57 = _tmp30 * _tmp56; + const Scalar _tmp58 = _tmp57 * sqrt_info(0, 0); + const Scalar _tmp59 = _tmp57 * sqrt_info(1, 0); + const Scalar _tmp60 = _tmp57 * sqrt_info(2, 0); + const Scalar _tmp61 = _tmp37 - _tmp38; + const Scalar _tmp62 = -_tmp40 + _tmp41; + const Scalar _tmp63 = -_tmp43 + _tmp44; + const Scalar _tmp64 = _tmp46 + _tmp47; + const Scalar _tmp65 = _tmp49 + _tmp50; + const Scalar _tmp66 = _tmp52 + _tmp53; + const Scalar _tmp67 = + std::pow(_tmp20, Scalar(4)) * std::pow(_tmp56, Scalar(2)) / std::pow(_tmp29, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp22; + _res(1, 0) = _tmp23; + _res(2, 0) = _tmp24; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp34; + _jacobian(1, 0) = _tmp35; + _jacobian(2, 0) = _tmp36; + _jacobian(0, 1) = _tmp39; + _jacobian(1, 1) = _tmp42; + _jacobian(2, 1) = _tmp45; + _jacobian(0, 2) = _tmp48; + _jacobian(1, 2) = _tmp51; + _jacobian(2, 2) = _tmp54; + _jacobian(0, 3) = _tmp58; + _jacobian(1, 3) = _tmp59; + _jacobian(2, 3) = _tmp60; + _jacobian(0, 4) = _tmp61; + _jacobian(1, 4) = _tmp62; + _jacobian(2, 4) = _tmp63; + _jacobian(0, 5) = _tmp64; + _jacobian(1, 5) = _tmp65; + _jacobian(2, 5) = _tmp66; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp34, Scalar(2)) + std::pow(_tmp35, Scalar(2)) + std::pow(_tmp36, Scalar(2)); + _hessian(1, 0) = _tmp34 * _tmp39 + _tmp35 * _tmp42 + _tmp36 * _tmp45; + _hessian(2, 0) = _tmp34 * _tmp48 + _tmp35 * _tmp51 + _tmp36 * _tmp54; + _hessian(3, 0) = _tmp34 * _tmp58 + _tmp35 * _tmp59 + _tmp36 * _tmp60; + _hessian(4, 0) = _tmp34 * _tmp61 + _tmp35 * _tmp62 + _tmp36 * _tmp63; + _hessian(5, 0) = _tmp34 * _tmp64 + _tmp35 * _tmp65 + _tmp36 * _tmp66; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp39, Scalar(2)) + std::pow(_tmp42, Scalar(2)) + std::pow(_tmp45, Scalar(2)); + _hessian(2, 1) = _tmp39 * _tmp48 + _tmp42 * _tmp51 + _tmp45 * _tmp54; + _hessian(3, 1) = _tmp39 * _tmp58 + _tmp42 * _tmp59 + _tmp45 * _tmp60; + _hessian(4, 1) = _tmp39 * _tmp61 + _tmp42 * _tmp62 + _tmp45 * _tmp63; + _hessian(5, 1) = _tmp39 * _tmp64 + _tmp42 * _tmp65 + _tmp45 * _tmp66; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp48, Scalar(2)) + std::pow(_tmp51, Scalar(2)) + std::pow(_tmp54, Scalar(2)); + _hessian(3, 2) = _tmp48 * _tmp58 + _tmp51 * _tmp59 + _tmp54 * _tmp60; + _hessian(4, 2) = _tmp48 * _tmp61 + _tmp51 * _tmp62 + _tmp54 * _tmp63; + _hessian(5, 2) = _tmp48 * _tmp64 + _tmp51 * _tmp65 + _tmp54 * _tmp66; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = _tmp67 * std::pow(sqrt_info(0, 0), Scalar(2)) + + _tmp67 * std::pow(sqrt_info(1, 0), Scalar(2)) + + _tmp67 * std::pow(sqrt_info(2, 0), Scalar(2)); + _hessian(4, 3) = _tmp58 * _tmp61 + _tmp59 * _tmp62 + _tmp60 * _tmp63; + _hessian(5, 3) = _tmp58 * _tmp64 + _tmp59 * _tmp65 + _tmp60 * _tmp66; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = + std::pow(_tmp61, Scalar(2)) + std::pow(_tmp62, Scalar(2)) + std::pow(_tmp63, Scalar(2)); + _hessian(5, 4) = _tmp61 * _tmp64 + _tmp62 * _tmp65 + _tmp63 * _tmp66; + _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(_tmp64, Scalar(2)) + std::pow(_tmp65, Scalar(2)) + std::pow(_tmp66, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp22 * _tmp34 + _tmp23 * _tmp35 + _tmp24 * _tmp36; + _rhs(1, 0) = _tmp22 * _tmp39 + _tmp23 * _tmp42 + _tmp24 * _tmp45; + _rhs(2, 0) = _tmp22 * _tmp48 + _tmp23 * _tmp51 + _tmp24 * _tmp54; + _rhs(3, 0) = _tmp22 * _tmp58 + _tmp23 * _tmp59 + _tmp24 * _tmp60; + _rhs(4, 0) = _tmp22 * _tmp61 + _tmp23 * _tmp62 + _tmp24 * _tmp63; + _rhs(5, 0) = _tmp22 * _tmp64 + _tmp23 * _tmp65 + _tmp24 * _tmp66; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3.h b/gen/cpp/sym/factors/between_factor_pose3.h index dc6997c02..3f858e2c4 100644 --- a/gen/cpp/sym/factors/between_factor_pose3.h +++ b/gen/cpp/sym/factors/between_factor_pose3.h @@ -1,957 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_T_b. - * - * In vector space terms that would be: - * (b - a) - a_T_b - * - * In lie group terms: - * local_coordinates(a_T_b, between(a, b)) - * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (6x12) jacobian of res wrt args a (6), b (6) - * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) - * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) - */ -template -void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b, - const sym::Pose3& a_T_b, - const Eigen::Matrix& sqrt_info, 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: 2386 - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - const Eigen::Matrix& _a_T_b = a_T_b.Data(); - - // Intermediate terms (369) - const Scalar _tmp0 = std::pow(_a[0], Scalar(2)); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = -_tmp1; - const Scalar _tmp3 = std::pow(_a[2], Scalar(2)); - const Scalar _tmp4 = 2 * _tmp3; - const Scalar _tmp5 = 1 - _tmp4; - const Scalar _tmp6 = _tmp2 + _tmp5; - const Scalar _tmp7 = 2 * _a[1]; - const Scalar _tmp8 = _a[2] * _tmp7; - const Scalar _tmp9 = 2 * _a[3]; - const Scalar _tmp10 = _a[0] * _tmp9; - const Scalar _tmp11 = _tmp10 + _tmp8; - const Scalar _tmp12 = _a[6] * _tmp11; - const Scalar _tmp13 = _a[0] * _tmp7; - const Scalar _tmp14 = _a[2] * _tmp9; - const Scalar _tmp15 = -_tmp14; - const Scalar _tmp16 = _tmp13 + _tmp15; - const Scalar _tmp17 = _a[4] * _tmp16; - const Scalar _tmp18 = _b[4] * _tmp16 + _b[6] * _tmp11; - const Scalar _tmp19 = -_a[5] * _tmp6 - _a_T_b[5] + _b[5] * _tmp6 - _tmp12 - _tmp17 + _tmp18; - const Scalar _tmp20 = std::pow(_a[1], Scalar(2)); - const Scalar _tmp21 = 2 * _tmp20; - const Scalar _tmp22 = -_tmp21; - const Scalar _tmp23 = _tmp22 + _tmp5; - const Scalar _tmp24 = 2 * _a[0] * _a[2]; - const Scalar _tmp25 = _a[3] * _tmp7; - const Scalar _tmp26 = -_tmp25; - const Scalar _tmp27 = _tmp24 + _tmp26; - const Scalar _tmp28 = _a[6] * _tmp27; - const Scalar _tmp29 = _tmp13 + _tmp14; - const Scalar _tmp30 = _a[5] * _tmp29; - const Scalar _tmp31 = _b[5] * _tmp29 + _b[6] * _tmp27; - const Scalar _tmp32 = -_a[4] * _tmp23 - _a_T_b[4] + _b[4] * _tmp23 - _tmp28 - _tmp30 + _tmp31; - const Scalar _tmp33 = _a[0] * _b[1]; - const Scalar _tmp34 = _a[2] * _b[3]; - const Scalar _tmp35 = _a[3] * _b[2]; - const Scalar _tmp36 = _a[1] * _b[0]; - const Scalar _tmp37 = -_tmp33 - _tmp34 + _tmp35 + _tmp36; - const Scalar _tmp38 = _a[2] * _b[1]; - const Scalar _tmp39 = _a[0] * _b[3]; - const Scalar _tmp40 = _a[1] * _b[2]; - const Scalar _tmp41 = _a[3] * _b[0]; - const Scalar _tmp42 = _tmp38 - _tmp39 - _tmp40 + _tmp41; - const Scalar _tmp43 = _a[3] * _b[1]; - const Scalar _tmp44 = _a[1] * _b[3]; - const Scalar _tmp45 = _a[0] * _b[2]; - const Scalar _tmp46 = _a[2] * _b[0]; - const Scalar _tmp47 = _tmp43 - _tmp44 + _tmp45 - _tmp46; - const Scalar _tmp48 = _a[1] * _b[1]; - const Scalar _tmp49 = _a[2] * _b[2]; - const Scalar _tmp50 = _a[0] * _b[0]; - const Scalar _tmp51 = _a[3] * _b[3]; - const Scalar _tmp52 = _tmp48 + _tmp49 + _tmp50 + _tmp51; - const Scalar _tmp53 = - -_a_T_b[0] * _tmp47 + _a_T_b[1] * _tmp42 - _a_T_b[2] * _tmp52 + _a_T_b[3] * _tmp37; - const Scalar _tmp54 = _a_T_b[2] * _tmp37; - const Scalar _tmp55 = _a_T_b[0] * _tmp42; - const Scalar _tmp56 = _a_T_b[1] * _tmp47; - const Scalar _tmp57 = -_tmp54 - _tmp55 - _tmp56; - const Scalar _tmp58 = _a_T_b[3] * _tmp52; - const Scalar _tmp59 = - 2 * std::min(0, (((-_tmp57 + _tmp58) > 0) - ((-_tmp57 + _tmp58) < 0))) + 1; - const Scalar _tmp60 = 2 * _tmp59; - const Scalar _tmp61 = 1 - epsilon; - const Scalar _tmp62 = std::min(_tmp61, std::fabs(_tmp57 - _tmp58)); - const Scalar _tmp63 = std::acos(_tmp62) / std::sqrt(Scalar(1 - std::pow(_tmp62, Scalar(2)))); - const Scalar _tmp64 = _tmp60 * _tmp63; - const Scalar _tmp65 = _tmp53 * _tmp64; - const Scalar _tmp66 = - -_a_T_b[0] * _tmp52 - _a_T_b[1] * _tmp37 + _a_T_b[2] * _tmp47 + _a_T_b[3] * _tmp42; - const Scalar _tmp67 = _tmp66 * sqrt_info(0, 0); - const Scalar _tmp68 = - _a_T_b[0] * _tmp37 - _a_T_b[1] * _tmp52 - _a_T_b[2] * _tmp42 + _a_T_b[3] * _tmp47; - const Scalar _tmp69 = _tmp63 * _tmp68; - const Scalar _tmp70 = _tmp60 * _tmp69; - const Scalar _tmp71 = _tmp2 + _tmp22 + 1; - const Scalar _tmp72 = -_tmp10; - const Scalar _tmp73 = _tmp72 + _tmp8; - const Scalar _tmp74 = _a[5] * _tmp73; - const Scalar _tmp75 = _tmp24 + _tmp25; - const Scalar _tmp76 = _a[4] * _tmp75; - const Scalar _tmp77 = _b[4] * _tmp75 + _b[5] * _tmp73; - const Scalar _tmp78 = -_a[6] * _tmp71 - _a_T_b[6] + _b[6] * _tmp71 - _tmp74 - _tmp76 + _tmp77; - const Scalar _tmp79 = _tmp19 * sqrt_info(0, 4) + _tmp32 * sqrt_info(0, 3) + _tmp64 * _tmp67 + - _tmp65 * sqrt_info(0, 2) + _tmp70 * sqrt_info(0, 1) + - _tmp78 * sqrt_info(0, 5); - const Scalar _tmp80 = _tmp64 * _tmp66; - const Scalar _tmp81 = _tmp60 * sqrt_info(1, 1); - const Scalar _tmp82 = _tmp19 * sqrt_info(1, 4) + _tmp32 * sqrt_info(1, 3) + - _tmp65 * sqrt_info(1, 2) + _tmp69 * _tmp81 + _tmp78 * sqrt_info(1, 5) + - _tmp80 * sqrt_info(1, 0); - const Scalar _tmp83 = _tmp19 * sqrt_info(2, 4) + _tmp32 * sqrt_info(2, 3) + - _tmp65 * sqrt_info(2, 2) + _tmp70 * sqrt_info(2, 1) + - _tmp78 * sqrt_info(2, 5) + _tmp80 * sqrt_info(2, 0); - const Scalar _tmp84 = _tmp19 * sqrt_info(3, 4) + _tmp32 * sqrt_info(3, 3) + - _tmp65 * sqrt_info(3, 2) + _tmp70 * sqrt_info(3, 1) + - _tmp78 * sqrt_info(3, 5) + _tmp80 * sqrt_info(3, 0); - const Scalar _tmp85 = _tmp19 * sqrt_info(4, 4) + _tmp32 * sqrt_info(4, 3) + - _tmp65 * sqrt_info(4, 2) + _tmp70 * sqrt_info(4, 1) + - _tmp78 * sqrt_info(4, 5) + _tmp80 * sqrt_info(4, 0); - const Scalar _tmp86 = _tmp60 * sqrt_info(5, 1); - const Scalar _tmp87 = _tmp19 * sqrt_info(5, 4) + _tmp32 * sqrt_info(5, 3) + - _tmp65 * sqrt_info(5, 2) + _tmp69 * _tmp86 + _tmp78 * sqrt_info(5, 5) + - _tmp80 * sqrt_info(5, 0); - const Scalar _tmp88 = (Scalar(1) / Scalar(2)) * _tmp48; - const Scalar _tmp89 = (Scalar(1) / Scalar(2)) * _tmp51; - const Scalar _tmp90 = (Scalar(1) / Scalar(2)) * _tmp49; - const Scalar _tmp91 = (Scalar(1) / Scalar(2)) * _tmp50; - const Scalar _tmp92 = -_tmp88 - _tmp89 - _tmp90 - _tmp91; - const Scalar _tmp93 = _a_T_b[0] * _tmp92; - const Scalar _tmp94 = (Scalar(1) / Scalar(2)) * _tmp38; - const Scalar _tmp95 = (Scalar(1) / Scalar(2)) * _tmp39; - const Scalar _tmp96 = (Scalar(1) / Scalar(2)) * _tmp40; - const Scalar _tmp97 = (Scalar(1) / Scalar(2)) * _tmp41; - const Scalar _tmp98 = _tmp94 - _tmp95 - _tmp96 + _tmp97; - const Scalar _tmp99 = _a_T_b[3] * _tmp98; - const Scalar _tmp100 = (Scalar(1) / Scalar(2)) * _tmp33; - const Scalar _tmp101 = (Scalar(1) / Scalar(2)) * _tmp34; - const Scalar _tmp102 = (Scalar(1) / Scalar(2)) * _tmp35; - const Scalar _tmp103 = (Scalar(1) / Scalar(2)) * _tmp36; - const Scalar _tmp104 = -_tmp100 - _tmp101 + _tmp102 + _tmp103; - const Scalar _tmp105 = _a_T_b[1] * _tmp104; - const Scalar _tmp106 = (Scalar(1) / Scalar(2)) * _tmp43; - const Scalar _tmp107 = (Scalar(1) / Scalar(2)) * _tmp44; - const Scalar _tmp108 = (Scalar(1) / Scalar(2)) * _tmp45; - const Scalar _tmp109 = (Scalar(1) / Scalar(2)) * _tmp46; - const Scalar _tmp110 = -_tmp106 + _tmp107 - _tmp108 + _tmp109; - const Scalar _tmp111 = _a_T_b[2] * _tmp110; - const Scalar _tmp112 = _tmp105 + _tmp111; - const Scalar _tmp113 = _tmp112 + _tmp93 + _tmp99; - const Scalar _tmp114 = _tmp54 + _tmp55 + _tmp56 + _tmp58; - const Scalar _tmp115 = std::fabs(_tmp114); - const Scalar _tmp116 = std::min(_tmp115, _tmp61); - const Scalar _tmp117 = 1 - std::pow(_tmp116, Scalar(2)); - const Scalar _tmp118 = _tmp59 * ((((-_tmp115 + _tmp61) > 0) - ((-_tmp115 + _tmp61) < 0)) + 1) * - (((_tmp114) > 0) - ((_tmp114) < 0)); - const Scalar _tmp119 = _tmp118 / _tmp117; - const Scalar _tmp120 = _tmp119 * _tmp68; - const Scalar _tmp121 = _tmp113 * _tmp120; - const Scalar _tmp122 = -_a_T_b[0] * _tmp104; - const Scalar _tmp123 = _a_T_b[1] * _tmp92; - const Scalar _tmp124 = _a_T_b[3] * _tmp110; - const Scalar _tmp125 = _a_T_b[2] * _tmp98; - const Scalar _tmp126 = _tmp122 + _tmp123 + _tmp124 - _tmp125; - const Scalar _tmp127 = std::acos(_tmp116); - const Scalar _tmp128 = _tmp127 / std::sqrt(_tmp117); - const Scalar _tmp129 = _tmp128 * _tmp60; - const Scalar _tmp130 = _tmp126 * _tmp129; - const Scalar _tmp131 = _tmp119 * _tmp67; - const Scalar _tmp132 = _tmp116 * _tmp118 * _tmp127 / (_tmp117 * std::sqrt(_tmp117)); - const Scalar _tmp133 = _tmp113 * _tmp132; - const Scalar _tmp134 = _tmp133 * _tmp53; - const Scalar _tmp135 = _a_T_b[3] * _tmp92; - const Scalar _tmp136 = _a_T_b[0] * _tmp98; - const Scalar _tmp137 = _a_T_b[2] * _tmp104; - const Scalar _tmp138 = -_a_T_b[1] * _tmp110; - const Scalar _tmp139 = _tmp137 + _tmp138; - const Scalar _tmp140 = _tmp135 - _tmp136 + _tmp139; - const Scalar _tmp141 = _tmp129 * sqrt_info(0, 0); - const Scalar _tmp142 = _tmp68 * sqrt_info(0, 1); - const Scalar _tmp143 = -_a_T_b[1] * _tmp98; - const Scalar _tmp144 = _a_T_b[2] * _tmp92; - const Scalar _tmp145 = _a_T_b[3] * _tmp104; - const Scalar _tmp146 = _a_T_b[0] * _tmp110; - const Scalar _tmp147 = _tmp145 + _tmp146; - const Scalar _tmp148 = _tmp128 * (_tmp143 - _tmp144 + _tmp147); - const Scalar _tmp149 = _tmp148 * _tmp60; - const Scalar _tmp150 = -_tmp20; - const Scalar _tmp151 = std::pow(_a[3], Scalar(2)); - const Scalar _tmp152 = _tmp150 + _tmp151; - const Scalar _tmp153 = -_tmp0; - const Scalar _tmp154 = _tmp153 + _tmp3; - const Scalar _tmp155 = _tmp152 + _tmp154; - const Scalar _tmp156 = -_a[6] * _tmp155 + _b[6] * _tmp155 - _tmp74 - _tmp76 + _tmp77; - const Scalar _tmp157 = _tmp119 * _tmp53; - const Scalar _tmp158 = _tmp113 * _tmp157; - const Scalar _tmp159 = -_tmp151; - const Scalar _tmp160 = _tmp0 + _tmp150 + _tmp159 + _tmp3; - const Scalar _tmp161 = -_tmp8; - const Scalar _tmp162 = _tmp161 + _tmp72; - const Scalar _tmp163 = -_tmp13; - const Scalar _tmp164 = _tmp14 + _tmp163; - const Scalar _tmp165 = -_a[4] * _tmp164 - _a[5] * _tmp160 - _a[6] * _tmp162 + _b[4] * _tmp164 + - _b[5] * _tmp160 + _b[6] * _tmp162; - const Scalar _tmp166 = -_tmp113 * _tmp131 - _tmp121 * sqrt_info(0, 1) + - _tmp130 * sqrt_info(0, 2) + _tmp133 * _tmp142 + _tmp133 * _tmp67 + - _tmp134 * sqrt_info(0, 2) + _tmp140 * _tmp141 + _tmp149 * sqrt_info(0, 1) + - _tmp156 * sqrt_info(0, 4) - _tmp158 * sqrt_info(0, 2) + - _tmp165 * sqrt_info(0, 5); - const Scalar _tmp167 = _tmp129 * sqrt_info(1, 2); - const Scalar _tmp168 = _tmp119 * _tmp66; - const Scalar _tmp169 = _tmp113 * _tmp168; - const Scalar _tmp170 = _tmp53 * sqrt_info(1, 2); - const Scalar _tmp171 = _tmp129 * _tmp140; - const Scalar _tmp172 = _tmp133 * _tmp68; - const Scalar _tmp173 = _tmp133 * _tmp66; - const Scalar _tmp174 = -_tmp121 * sqrt_info(1, 1) + _tmp126 * _tmp167 + _tmp133 * _tmp170 + - _tmp148 * _tmp81 + _tmp156 * sqrt_info(1, 4) - _tmp158 * sqrt_info(1, 2) + - _tmp165 * sqrt_info(1, 5) - _tmp169 * sqrt_info(1, 0) + - _tmp171 * sqrt_info(1, 0) + _tmp172 * sqrt_info(1, 1) + - _tmp173 * sqrt_info(1, 0); - const Scalar _tmp175 = _tmp120 * sqrt_info(2, 1); - const Scalar _tmp176 = - -_tmp113 * _tmp175 + _tmp130 * sqrt_info(2, 2) + _tmp134 * sqrt_info(2, 2) + - _tmp149 * sqrt_info(2, 1) + _tmp156 * sqrt_info(2, 4) - _tmp158 * sqrt_info(2, 2) + - _tmp165 * sqrt_info(2, 5) - _tmp169 * sqrt_info(2, 0) + _tmp171 * sqrt_info(2, 0) + - _tmp172 * sqrt_info(2, 1) + _tmp173 * sqrt_info(2, 0); - const Scalar _tmp177 = _tmp68 * sqrt_info(3, 1); - const Scalar _tmp178 = -_tmp121 * sqrt_info(3, 1) + _tmp130 * sqrt_info(3, 2) + - _tmp133 * _tmp177 + _tmp134 * sqrt_info(3, 2) + _tmp149 * sqrt_info(3, 1) + - _tmp156 * sqrt_info(3, 4) - _tmp158 * sqrt_info(3, 2) + - _tmp165 * sqrt_info(3, 5) - _tmp169 * sqrt_info(3, 0) + - _tmp171 * sqrt_info(3, 0) + _tmp173 * sqrt_info(3, 0); - const Scalar _tmp179 = _tmp53 * sqrt_info(4, 2); - const Scalar _tmp180 = -_tmp121 * sqrt_info(4, 1) + _tmp130 * sqrt_info(4, 2) + - _tmp133 * _tmp179 + _tmp149 * sqrt_info(4, 1) + _tmp156 * sqrt_info(4, 4) - - _tmp158 * sqrt_info(4, 2) + _tmp165 * sqrt_info(4, 5) - - _tmp169 * sqrt_info(4, 0) + _tmp171 * sqrt_info(4, 0) + - _tmp172 * sqrt_info(4, 1) + _tmp173 * sqrt_info(4, 0); - const Scalar _tmp181 = _tmp53 * sqrt_info(5, 2); - const Scalar _tmp182 = -_tmp121 * sqrt_info(5, 1) + _tmp130 * sqrt_info(5, 2) + - _tmp133 * _tmp181 + _tmp148 * _tmp86 + _tmp156 * sqrt_info(5, 4) - - _tmp158 * sqrt_info(5, 2) + _tmp165 * sqrt_info(5, 5) - - _tmp169 * sqrt_info(5, 0) + _tmp171 * sqrt_info(5, 0) + - _tmp172 * sqrt_info(5, 1) + _tmp173 * sqrt_info(5, 0); - const Scalar _tmp183 = _tmp106 - _tmp107 + _tmp108 - _tmp109; - const Scalar _tmp184 = _a_T_b[3] * _tmp183; - const Scalar _tmp185 = _tmp100 + _tmp101 - _tmp102 - _tmp103; - const Scalar _tmp186 = _a_T_b[0] * _tmp185; - const Scalar _tmp187 = _tmp125 + _tmp186; - const Scalar _tmp188 = _tmp123 + _tmp184 + _tmp187; - const Scalar _tmp189 = -_a_T_b[2] * _tmp183; - const Scalar _tmp190 = _a_T_b[1] * _tmp185; - const Scalar _tmp191 = _tmp190 + _tmp99; - const Scalar _tmp192 = _tmp189 + _tmp191 - _tmp93; - const Scalar _tmp193 = _tmp129 * _tmp192; - const Scalar _tmp194 = _a_T_b[3] * _tmp185; - const Scalar _tmp195 = _a_T_b[0] * _tmp183; - const Scalar _tmp196 = _tmp143 + _tmp144 + _tmp194 - _tmp195; - const Scalar _tmp197 = _tmp132 * _tmp188; - const Scalar _tmp198 = _tmp53 * sqrt_info(0, 2); - const Scalar _tmp199 = _tmp157 * _tmp188; - const Scalar _tmp200 = _tmp120 * _tmp188; - const Scalar _tmp201 = _a_T_b[1] * _tmp183; - const Scalar _tmp202 = -_a_T_b[2] * _tmp185; - const Scalar _tmp203 = _tmp136 + _tmp202; - const Scalar _tmp204 = _tmp135 - _tmp201 + _tmp203; - const Scalar _tmp205 = _tmp129 * sqrt_info(0, 1); - const Scalar _tmp206 = -_tmp3; - const Scalar _tmp207 = _tmp0 + _tmp206; - const Scalar _tmp208 = _tmp159 + _tmp20; - const Scalar _tmp209 = _tmp207 + _tmp208; - const Scalar _tmp210 = _tmp10 + _tmp161; - const Scalar _tmp211 = -_tmp24; - const Scalar _tmp212 = _tmp211 + _tmp26; - const Scalar _tmp213 = -_a[4] * _tmp212 - _a[5] * _tmp210 - _a[6] * _tmp209 + _b[4] * _tmp212 + - _b[5] * _tmp210 + _b[6] * _tmp209; - const Scalar _tmp214 = _tmp152 + _tmp207; - const Scalar _tmp215 = -_a[4] * _tmp214 + _b[4] * _tmp214 - _tmp28 - _tmp30 + _tmp31; - const Scalar _tmp216 = -_tmp131 * _tmp188 + _tmp141 * _tmp196 + _tmp142 * _tmp197 + - _tmp193 * sqrt_info(0, 2) + _tmp197 * _tmp198 + _tmp197 * _tmp67 - - _tmp199 * sqrt_info(0, 2) - _tmp200 * sqrt_info(0, 1) + _tmp204 * _tmp205 + - _tmp213 * sqrt_info(0, 3) + _tmp215 * sqrt_info(0, 5); - const Scalar _tmp217 = _tmp168 * _tmp188; - const Scalar _tmp218 = _tmp129 * _tmp196; - const Scalar _tmp219 = _tmp197 * _tmp66; - const Scalar _tmp220 = _tmp197 * _tmp68; - const Scalar _tmp221 = _tmp129 * _tmp204; - const Scalar _tmp222 = _tmp167 * _tmp192 + _tmp170 * _tmp197 - _tmp199 * sqrt_info(1, 2) - - _tmp200 * sqrt_info(1, 1) + _tmp213 * sqrt_info(1, 3) + - _tmp215 * sqrt_info(1, 5) - _tmp217 * sqrt_info(1, 0) + - _tmp218 * sqrt_info(1, 0) + _tmp219 * sqrt_info(1, 0) + - _tmp220 * sqrt_info(1, 1) + _tmp221 * sqrt_info(1, 1); - const Scalar _tmp223 = _tmp53 * sqrt_info(2, 2); - const Scalar _tmp224 = -_tmp175 * _tmp188 + _tmp193 * sqrt_info(2, 2) + _tmp197 * _tmp223 - - _tmp199 * sqrt_info(2, 2) + _tmp213 * sqrt_info(2, 3) + - _tmp215 * sqrt_info(2, 5) - _tmp217 * sqrt_info(2, 0) + - _tmp218 * sqrt_info(2, 0) + _tmp219 * sqrt_info(2, 0) + - _tmp220 * sqrt_info(2, 1) + _tmp221 * sqrt_info(2, 1); - const Scalar _tmp225 = _tmp53 * sqrt_info(3, 2); - const Scalar _tmp226 = _tmp177 * _tmp197 + _tmp193 * sqrt_info(3, 2) + _tmp197 * _tmp225 - - _tmp199 * sqrt_info(3, 2) - _tmp200 * sqrt_info(3, 1) + - _tmp213 * sqrt_info(3, 3) + _tmp215 * sqrt_info(3, 5) - - _tmp217 * sqrt_info(3, 0) + _tmp218 * sqrt_info(3, 0) + - _tmp219 * sqrt_info(3, 0) + _tmp221 * sqrt_info(3, 1); - const Scalar _tmp227 = _tmp179 * _tmp197 + _tmp193 * sqrt_info(4, 2) - _tmp199 * sqrt_info(4, 2) - - _tmp200 * sqrt_info(4, 1) + _tmp213 * sqrt_info(4, 3) + - _tmp215 * sqrt_info(4, 5) - _tmp217 * sqrt_info(4, 0) + - _tmp218 * sqrt_info(4, 0) + _tmp219 * sqrt_info(4, 0) + - _tmp220 * sqrt_info(4, 1) + _tmp221 * sqrt_info(4, 1); - const Scalar _tmp228 = _tmp128 * _tmp86; - const Scalar _tmp229 = _tmp181 * _tmp197 + _tmp193 * sqrt_info(5, 2) - _tmp199 * sqrt_info(5, 2) - - _tmp200 * sqrt_info(5, 1) + _tmp204 * _tmp228 + _tmp213 * sqrt_info(5, 3) + - _tmp215 * sqrt_info(5, 5) - _tmp217 * sqrt_info(5, 0) + - _tmp218 * sqrt_info(5, 0) + _tmp219 * sqrt_info(5, 0) + - _tmp220 * sqrt_info(5, 1); - const Scalar _tmp230 = -_tmp94 + _tmp95 + _tmp96 - _tmp97; - const Scalar _tmp231 = _a_T_b[1] * _tmp230; - const Scalar _tmp232 = _tmp195 + _tmp231; - const Scalar _tmp233 = _tmp144 + _tmp145 + _tmp232; - const Scalar _tmp234 = _tmp120 * _tmp233; - const Scalar _tmp235 = _tmp157 * _tmp233; - const Scalar _tmp236 = _tmp132 * _tmp233; - const Scalar _tmp237 = _a_T_b[3] * _tmp230; - const Scalar _tmp238 = -_tmp105 + _tmp189 + _tmp237 + _tmp93; - const Scalar _tmp239 = _a_T_b[2] * _tmp230; - const Scalar _tmp240 = _tmp184 + _tmp239; - const Scalar _tmp241 = _tmp129 * (_tmp122 - _tmp123 + _tmp240); - const Scalar _tmp242 = -_a_T_b[0] * _tmp230; - const Scalar _tmp243 = _tmp135 - _tmp137 + _tmp201 + _tmp242; - const Scalar _tmp244 = _tmp129 * _tmp243; - const Scalar _tmp245 = _tmp151 + _tmp153 + _tmp20 + _tmp206; - const Scalar _tmp246 = -_a[5] * _tmp245 + _b[5] * _tmp245 - _tmp12 - _tmp17 + _tmp18; - const Scalar _tmp247 = _tmp154 + _tmp208; - const Scalar _tmp248 = _tmp211 + _tmp25; - const Scalar _tmp249 = _tmp15 + _tmp163; - const Scalar _tmp250 = -_a[4] * _tmp247 - _a[5] * _tmp249 - _a[6] * _tmp248 + _b[4] * _tmp247 + - _b[5] * _tmp249 + _b[6] * _tmp248; - const Scalar _tmp251 = -_tmp131 * _tmp233 + _tmp142 * _tmp236 + _tmp198 * _tmp236 + - _tmp205 * _tmp238 - _tmp234 * sqrt_info(0, 1) - _tmp235 * sqrt_info(0, 2) + - _tmp236 * _tmp67 + _tmp241 * sqrt_info(0, 0) + _tmp244 * sqrt_info(0, 2) + - _tmp246 * sqrt_info(0, 3) + _tmp250 * sqrt_info(0, 4); - const Scalar _tmp252 = _tmp168 * _tmp233; - const Scalar _tmp253 = _tmp129 * _tmp238; - const Scalar _tmp254 = _tmp236 * _tmp68; - const Scalar _tmp255 = _tmp236 * _tmp66; - const Scalar _tmp256 = _tmp167 * _tmp243 + _tmp170 * _tmp236 - _tmp234 * sqrt_info(1, 1) - - _tmp235 * sqrt_info(1, 2) + _tmp241 * sqrt_info(1, 0) + - _tmp246 * sqrt_info(1, 3) + _tmp250 * sqrt_info(1, 4) - - _tmp252 * sqrt_info(1, 0) + _tmp253 * sqrt_info(1, 1) + - _tmp254 * sqrt_info(1, 1) + _tmp255 * sqrt_info(1, 0); - const Scalar _tmp257 = _tmp223 * _tmp236 - _tmp234 * sqrt_info(2, 1) - _tmp235 * sqrt_info(2, 2) + - _tmp241 * sqrt_info(2, 0) + _tmp244 * sqrt_info(2, 2) + - _tmp246 * sqrt_info(2, 3) + _tmp250 * sqrt_info(2, 4) - - _tmp252 * sqrt_info(2, 0) + _tmp253 * sqrt_info(2, 1) + - _tmp254 * sqrt_info(2, 1) + _tmp255 * sqrt_info(2, 0); - const Scalar _tmp258 = _tmp225 * _tmp236 - _tmp234 * sqrt_info(3, 1) - _tmp235 * sqrt_info(3, 2) + - _tmp241 * sqrt_info(3, 0) + _tmp244 * sqrt_info(3, 2) + - _tmp246 * sqrt_info(3, 3) + _tmp250 * sqrt_info(3, 4) - - _tmp252 * sqrt_info(3, 0) + _tmp253 * sqrt_info(3, 1) + - _tmp254 * sqrt_info(3, 1) + _tmp255 * sqrt_info(3, 0); - const Scalar _tmp259 = _tmp179 * _tmp236 - _tmp234 * sqrt_info(4, 1) - _tmp235 * sqrt_info(4, 2) + - _tmp241 * sqrt_info(4, 0) + _tmp244 * sqrt_info(4, 2) + - _tmp246 * sqrt_info(4, 3) + _tmp250 * sqrt_info(4, 4) - - _tmp252 * sqrt_info(4, 0) + _tmp253 * sqrt_info(4, 1) + - _tmp254 * sqrt_info(4, 1) + _tmp255 * sqrt_info(4, 0); - const Scalar _tmp260 = _tmp181 * _tmp236 + _tmp228 * _tmp238 - _tmp234 * sqrt_info(5, 1) - - _tmp235 * sqrt_info(5, 2) + _tmp241 * sqrt_info(5, 0) + - _tmp244 * sqrt_info(5, 2) + _tmp246 * sqrt_info(5, 3) + - _tmp250 * sqrt_info(5, 4) - _tmp252 * sqrt_info(5, 0) + - _tmp254 * sqrt_info(5, 1) + _tmp255 * sqrt_info(5, 0); - const Scalar _tmp261 = _tmp4 - 1; - const Scalar _tmp262 = _tmp21 + _tmp261; - const Scalar _tmp263 = - _tmp164 * sqrt_info(0, 4) + _tmp212 * sqrt_info(0, 5) + _tmp262 * sqrt_info(0, 3); - const Scalar _tmp264 = - _tmp164 * sqrt_info(1, 4) + _tmp212 * sqrt_info(1, 5) + _tmp262 * sqrt_info(1, 3); - const Scalar _tmp265 = - _tmp164 * sqrt_info(2, 4) + _tmp212 * sqrt_info(2, 5) + _tmp262 * sqrt_info(2, 3); - const Scalar _tmp266 = - _tmp164 * sqrt_info(3, 4) + _tmp212 * sqrt_info(3, 5) + _tmp262 * sqrt_info(3, 3); - const Scalar _tmp267 = - _tmp164 * sqrt_info(4, 4) + _tmp212 * sqrt_info(4, 5) + _tmp262 * sqrt_info(4, 3); - const Scalar _tmp268 = - _tmp164 * sqrt_info(5, 4) + _tmp212 * sqrt_info(5, 5) + _tmp262 * sqrt_info(5, 3); - const Scalar _tmp269 = _tmp1 + _tmp261; - const Scalar _tmp270 = - _tmp210 * sqrt_info(0, 5) + _tmp249 * sqrt_info(0, 3) + _tmp269 * sqrt_info(0, 4); - const Scalar _tmp271 = - _tmp210 * sqrt_info(1, 5) + _tmp249 * sqrt_info(1, 3) + _tmp269 * sqrt_info(1, 4); - const Scalar _tmp272 = - _tmp210 * sqrt_info(2, 5) + _tmp249 * sqrt_info(2, 3) + _tmp269 * sqrt_info(2, 4); - const Scalar _tmp273 = - _tmp210 * sqrt_info(3, 5) + _tmp249 * sqrt_info(3, 3) + _tmp269 * sqrt_info(3, 4); - const Scalar _tmp274 = - _tmp210 * sqrt_info(4, 5) + _tmp249 * sqrt_info(4, 3) + _tmp269 * sqrt_info(4, 4); - const Scalar _tmp275 = - _tmp210 * sqrt_info(5, 5) + _tmp249 * sqrt_info(5, 3) + _tmp269 * sqrt_info(5, 4); - const Scalar _tmp276 = _tmp1 + _tmp21 - 1; - const Scalar _tmp277 = - _tmp162 * sqrt_info(0, 4) + _tmp248 * sqrt_info(0, 3) + _tmp276 * sqrt_info(0, 5); - const Scalar _tmp278 = - _tmp162 * sqrt_info(1, 4) + _tmp248 * sqrt_info(1, 3) + _tmp276 * sqrt_info(1, 5); - const Scalar _tmp279 = - _tmp162 * sqrt_info(2, 4) + _tmp248 * sqrt_info(2, 3) + _tmp276 * sqrt_info(2, 5); - const Scalar _tmp280 = - _tmp162 * sqrt_info(3, 4) + _tmp248 * sqrt_info(3, 3) + _tmp276 * sqrt_info(3, 5); - const Scalar _tmp281 = - _tmp162 * sqrt_info(4, 4) + _tmp248 * sqrt_info(4, 3) + _tmp276 * sqrt_info(4, 5); - const Scalar _tmp282 = - _tmp162 * sqrt_info(5, 4) + _tmp248 * sqrt_info(5, 3) + _tmp276 * sqrt_info(5, 5); - const Scalar _tmp283 = _tmp88 + _tmp89 + _tmp90 + _tmp91; - const Scalar _tmp284 = _a_T_b[0] * _tmp283; - const Scalar _tmp285 = _tmp237 + _tmp284; - const Scalar _tmp286 = _tmp112 + _tmp285; - const Scalar _tmp287 = _tmp132 * _tmp286; - const Scalar _tmp288 = _tmp157 * _tmp286; - const Scalar _tmp289 = _tmp120 * _tmp286; - const Scalar _tmp290 = _a_T_b[3] * _tmp283; - const Scalar _tmp291 = _tmp242 + _tmp290; - const Scalar _tmp292 = _tmp139 + _tmp291; - const Scalar _tmp293 = _a_T_b[1] * _tmp283; - const Scalar _tmp294 = _tmp124 + _tmp293; - const Scalar _tmp295 = _tmp122 - _tmp239 + _tmp294; - const Scalar _tmp296 = _tmp129 * _tmp295; - const Scalar _tmp297 = _a_T_b[2] * _tmp283; - const Scalar _tmp298 = _tmp147 - _tmp231 - _tmp297; - const Scalar _tmp299 = -_tmp131 * _tmp286 + _tmp141 * _tmp292 + _tmp142 * _tmp287 + - _tmp198 * _tmp287 + _tmp205 * _tmp298 + _tmp287 * _tmp67 - - _tmp288 * sqrt_info(0, 2) - _tmp289 * sqrt_info(0, 1) + - _tmp296 * sqrt_info(0, 2); - const Scalar _tmp300 = _tmp287 * _tmp66; - const Scalar _tmp301 = _tmp287 * _tmp68; - const Scalar _tmp302 = _tmp168 * _tmp286; - const Scalar _tmp303 = _tmp129 * _tmp292; - const Scalar _tmp304 = _tmp129 * _tmp298; - const Scalar _tmp305 = _tmp167 * _tmp295 + _tmp170 * _tmp287 - _tmp288 * sqrt_info(1, 2) - - _tmp289 * sqrt_info(1, 1) + _tmp300 * sqrt_info(1, 0) + - _tmp301 * sqrt_info(1, 1) - _tmp302 * sqrt_info(1, 0) + - _tmp303 * sqrt_info(1, 0) + _tmp304 * sqrt_info(1, 1); - const Scalar _tmp306 = -_tmp175 * _tmp286 + _tmp223 * _tmp287 - _tmp288 * sqrt_info(2, 2) + - _tmp296 * sqrt_info(2, 2) + _tmp300 * sqrt_info(2, 0) + - _tmp301 * sqrt_info(2, 1) - _tmp302 * sqrt_info(2, 0) + - _tmp303 * sqrt_info(2, 0) + _tmp304 * sqrt_info(2, 1); - const Scalar _tmp307 = _tmp177 * _tmp287 + _tmp225 * _tmp287 - _tmp288 * sqrt_info(3, 2) - - _tmp289 * sqrt_info(3, 1) + _tmp296 * sqrt_info(3, 2) + - _tmp300 * sqrt_info(3, 0) - _tmp302 * sqrt_info(3, 0) + - _tmp303 * sqrt_info(3, 0) + _tmp304 * sqrt_info(3, 1); - const Scalar _tmp308 = _tmp179 * _tmp287 - _tmp288 * sqrt_info(4, 2) - _tmp289 * sqrt_info(4, 1) + - _tmp296 * sqrt_info(4, 2) + _tmp300 * sqrt_info(4, 0) + - _tmp301 * sqrt_info(4, 1) - _tmp302 * sqrt_info(4, 0) + - _tmp303 * sqrt_info(4, 0) + _tmp304 * sqrt_info(4, 1); - const Scalar _tmp309 = _tmp181 * _tmp287 + _tmp228 * _tmp298 - _tmp288 * sqrt_info(5, 2) - - _tmp289 * sqrt_info(5, 1) + _tmp296 * sqrt_info(5, 2) + - _tmp300 * sqrt_info(5, 0) + _tmp301 * sqrt_info(5, 1) - - _tmp302 * sqrt_info(5, 0) + _tmp303 * sqrt_info(5, 0); - const Scalar _tmp310 = -_tmp111 + _tmp191 - _tmp284; - const Scalar _tmp311 = _tmp129 * _tmp310; - const Scalar _tmp312 = _tmp187 + _tmp294; - const Scalar _tmp313 = _tmp132 * _tmp312; - const Scalar _tmp314 = _tmp120 * _tmp312; - const Scalar _tmp315 = _tmp194 + _tmp297; - const Scalar _tmp316 = _tmp143 - _tmp146 + _tmp315; - const Scalar _tmp317 = _tmp138 + _tmp203 + _tmp290; - const Scalar _tmp318 = _tmp157 * _tmp312; - const Scalar _tmp319 = -_tmp131 * _tmp312 + _tmp141 * _tmp316 + _tmp142 * _tmp313 + - _tmp198 * _tmp313 + _tmp205 * _tmp317 + _tmp311 * sqrt_info(0, 2) + - _tmp313 * _tmp67 - _tmp314 * sqrt_info(0, 1) - _tmp318 * sqrt_info(0, 2); - const Scalar _tmp320 = _tmp313 * _tmp68; - const Scalar _tmp321 = _tmp129 * _tmp316; - const Scalar _tmp322 = _tmp168 * _tmp312; - const Scalar _tmp323 = _tmp129 * _tmp317; - const Scalar _tmp324 = _tmp313 * _tmp66; - const Scalar _tmp325 = _tmp167 * _tmp310 + _tmp170 * _tmp313 - _tmp314 * sqrt_info(1, 1) - - _tmp318 * sqrt_info(1, 2) + _tmp320 * sqrt_info(1, 1) + - _tmp321 * sqrt_info(1, 0) - _tmp322 * sqrt_info(1, 0) + - _tmp323 * sqrt_info(1, 1) + _tmp324 * sqrt_info(1, 0); - const Scalar _tmp326 = _tmp129 * sqrt_info(2, 0); - const Scalar _tmp327 = -_tmp175 * _tmp312 + _tmp223 * _tmp313 + _tmp311 * sqrt_info(2, 2) + - _tmp316 * _tmp326 - _tmp318 * sqrt_info(2, 2) + _tmp320 * sqrt_info(2, 1) - - _tmp322 * sqrt_info(2, 0) + _tmp323 * sqrt_info(2, 1) + - _tmp324 * sqrt_info(2, 0); - const Scalar _tmp328 = _tmp177 * _tmp313 + _tmp225 * _tmp313 + _tmp311 * sqrt_info(3, 2) - - _tmp314 * sqrt_info(3, 1) - _tmp318 * sqrt_info(3, 2) + - _tmp321 * sqrt_info(3, 0) - _tmp322 * sqrt_info(3, 0) + - _tmp323 * sqrt_info(3, 1) + _tmp324 * sqrt_info(3, 0); - const Scalar _tmp329 = _tmp179 * _tmp313 + _tmp311 * sqrt_info(4, 2) - _tmp314 * sqrt_info(4, 1) - - _tmp318 * sqrt_info(4, 2) + _tmp320 * sqrt_info(4, 1) + - _tmp321 * sqrt_info(4, 0) - _tmp322 * sqrt_info(4, 0) + - _tmp323 * sqrt_info(4, 1) + _tmp324 * sqrt_info(4, 0); - const Scalar _tmp330 = _tmp181 * _tmp313 + _tmp228 * _tmp317 + _tmp311 * sqrt_info(5, 2) - - _tmp314 * sqrt_info(5, 1) - _tmp318 * sqrt_info(5, 2) + - _tmp320 * sqrt_info(5, 1) + _tmp321 * sqrt_info(5, 0) - - _tmp322 * sqrt_info(5, 0) + _tmp324 * sqrt_info(5, 0); - const Scalar _tmp331 = _tmp232 + _tmp315; - const Scalar _tmp332 = _tmp132 * _tmp331; - const Scalar _tmp333 = _tmp332 * _tmp53; - const Scalar _tmp334 = _tmp120 * _tmp331; - const Scalar _tmp335 = _tmp189 - _tmp190 + _tmp285; - const Scalar _tmp336 = _tmp201 + _tmp202 + _tmp291; - const Scalar _tmp337 = _tmp129 * _tmp336; - const Scalar _tmp338 = -_tmp186 + _tmp240 - _tmp293; - const Scalar _tmp339 = _tmp157 * _tmp331; - const Scalar _tmp340 = -_tmp131 * _tmp331 + _tmp141 * _tmp338 + _tmp142 * _tmp332 + - _tmp205 * _tmp335 + _tmp332 * _tmp67 + _tmp333 * sqrt_info(0, 2) - - _tmp334 * sqrt_info(0, 1) + _tmp337 * sqrt_info(0, 2) - - _tmp339 * sqrt_info(0, 2); - const Scalar _tmp341 = _tmp168 * _tmp331; - const Scalar _tmp342 = _tmp129 * _tmp335; - const Scalar _tmp343 = _tmp129 * _tmp338; - const Scalar _tmp344 = _tmp332 * _tmp68; - const Scalar _tmp345 = _tmp332 * _tmp66; - const Scalar _tmp346 = _tmp167 * _tmp336 + _tmp170 * _tmp332 - _tmp334 * sqrt_info(1, 1) - - _tmp339 * sqrt_info(1, 2) - _tmp341 * sqrt_info(1, 0) + - _tmp342 * sqrt_info(1, 1) + _tmp343 * sqrt_info(1, 0) + - _tmp344 * sqrt_info(1, 1) + _tmp345 * sqrt_info(1, 0); - const Scalar _tmp347 = -_tmp175 * _tmp331 + _tmp326 * _tmp338 + _tmp333 * sqrt_info(2, 2) + - _tmp337 * sqrt_info(2, 2) - _tmp339 * sqrt_info(2, 2) - - _tmp341 * sqrt_info(2, 0) + _tmp342 * sqrt_info(2, 1) + - _tmp344 * sqrt_info(2, 1) + _tmp345 * sqrt_info(2, 0); - const Scalar _tmp348 = _tmp177 * _tmp332 + _tmp333 * sqrt_info(3, 2) - _tmp334 * sqrt_info(3, 1) + - _tmp337 * sqrt_info(3, 2) - _tmp339 * sqrt_info(3, 2) - - _tmp341 * sqrt_info(3, 0) + _tmp342 * sqrt_info(3, 1) + - _tmp343 * sqrt_info(3, 0) + _tmp345 * sqrt_info(3, 0); - const Scalar _tmp349 = - _tmp333 * sqrt_info(4, 2) - _tmp334 * sqrt_info(4, 1) + _tmp337 * sqrt_info(4, 2) - - _tmp339 * sqrt_info(4, 2) - _tmp341 * sqrt_info(4, 0) + _tmp342 * sqrt_info(4, 1) + - _tmp343 * sqrt_info(4, 0) + _tmp344 * sqrt_info(4, 1) + _tmp345 * sqrt_info(4, 0); - const Scalar _tmp350 = _tmp181 * _tmp332 + _tmp228 * _tmp335 - _tmp334 * sqrt_info(5, 1) + - _tmp337 * sqrt_info(5, 2) - _tmp339 * sqrt_info(5, 2) - - _tmp341 * sqrt_info(5, 0) + _tmp343 * sqrt_info(5, 0) + - _tmp344 * sqrt_info(5, 1) + _tmp345 * sqrt_info(5, 0); - const Scalar _tmp351 = - _tmp16 * sqrt_info(0, 4) + _tmp23 * sqrt_info(0, 3) + _tmp75 * sqrt_info(0, 5); - const Scalar _tmp352 = - _tmp16 * sqrt_info(1, 4) + _tmp23 * sqrt_info(1, 3) + _tmp75 * sqrt_info(1, 5); - const Scalar _tmp353 = - _tmp16 * sqrt_info(2, 4) + _tmp23 * sqrt_info(2, 3) + _tmp75 * sqrt_info(2, 5); - const Scalar _tmp354 = - _tmp16 * sqrt_info(3, 4) + _tmp23 * sqrt_info(3, 3) + _tmp75 * sqrt_info(3, 5); - const Scalar _tmp355 = - _tmp16 * sqrt_info(4, 4) + _tmp23 * sqrt_info(4, 3) + _tmp75 * sqrt_info(4, 5); - const Scalar _tmp356 = - _tmp16 * sqrt_info(5, 4) + _tmp23 * sqrt_info(5, 3) + _tmp75 * sqrt_info(5, 5); - const Scalar _tmp357 = - _tmp29 * sqrt_info(0, 3) + _tmp6 * sqrt_info(0, 4) + _tmp73 * sqrt_info(0, 5); - const Scalar _tmp358 = - _tmp29 * sqrt_info(1, 3) + _tmp6 * sqrt_info(1, 4) + _tmp73 * sqrt_info(1, 5); - const Scalar _tmp359 = - _tmp29 * sqrt_info(2, 3) + _tmp6 * sqrt_info(2, 4) + _tmp73 * sqrt_info(2, 5); - const Scalar _tmp360 = - _tmp29 * sqrt_info(3, 3) + _tmp6 * sqrt_info(3, 4) + _tmp73 * sqrt_info(3, 5); - const Scalar _tmp361 = - _tmp29 * sqrt_info(4, 3) + _tmp6 * sqrt_info(4, 4) + _tmp73 * sqrt_info(4, 5); - const Scalar _tmp362 = - _tmp29 * sqrt_info(5, 3) + _tmp6 * sqrt_info(5, 4) + _tmp73 * sqrt_info(5, 5); - const Scalar _tmp363 = - _tmp11 * sqrt_info(0, 4) + _tmp27 * sqrt_info(0, 3) + _tmp71 * sqrt_info(0, 5); - const Scalar _tmp364 = - _tmp11 * sqrt_info(1, 4) + _tmp27 * sqrt_info(1, 3) + _tmp71 * sqrt_info(1, 5); - const Scalar _tmp365 = - _tmp11 * sqrt_info(2, 4) + _tmp27 * sqrt_info(2, 3) + _tmp71 * sqrt_info(2, 5); - const Scalar _tmp366 = - _tmp11 * sqrt_info(3, 4) + _tmp27 * sqrt_info(3, 3) + _tmp71 * sqrt_info(3, 5); - const Scalar _tmp367 = - _tmp11 * sqrt_info(4, 4) + _tmp27 * sqrt_info(4, 3) + _tmp71 * sqrt_info(4, 5); - const Scalar _tmp368 = - _tmp11 * sqrt_info(5, 4) + _tmp27 * sqrt_info(5, 3) + _tmp71 * sqrt_info(5, 5); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp79; - _res(1, 0) = _tmp82; - _res(2, 0) = _tmp83; - _res(3, 0) = _tmp84; - _res(4, 0) = _tmp85; - _res(5, 0) = _tmp87; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp166; - _jacobian(1, 0) = _tmp174; - _jacobian(2, 0) = _tmp176; - _jacobian(3, 0) = _tmp178; - _jacobian(4, 0) = _tmp180; - _jacobian(5, 0) = _tmp182; - _jacobian(0, 1) = _tmp216; - _jacobian(1, 1) = _tmp222; - _jacobian(2, 1) = _tmp224; - _jacobian(3, 1) = _tmp226; - _jacobian(4, 1) = _tmp227; - _jacobian(5, 1) = _tmp229; - _jacobian(0, 2) = _tmp251; - _jacobian(1, 2) = _tmp256; - _jacobian(2, 2) = _tmp257; - _jacobian(3, 2) = _tmp258; - _jacobian(4, 2) = _tmp259; - _jacobian(5, 2) = _tmp260; - _jacobian(0, 3) = _tmp263; - _jacobian(1, 3) = _tmp264; - _jacobian(2, 3) = _tmp265; - _jacobian(3, 3) = _tmp266; - _jacobian(4, 3) = _tmp267; - _jacobian(5, 3) = _tmp268; - _jacobian(0, 4) = _tmp270; - _jacobian(1, 4) = _tmp271; - _jacobian(2, 4) = _tmp272; - _jacobian(3, 4) = _tmp273; - _jacobian(4, 4) = _tmp274; - _jacobian(5, 4) = _tmp275; - _jacobian(0, 5) = _tmp277; - _jacobian(1, 5) = _tmp278; - _jacobian(2, 5) = _tmp279; - _jacobian(3, 5) = _tmp280; - _jacobian(4, 5) = _tmp281; - _jacobian(5, 5) = _tmp282; - _jacobian(0, 6) = _tmp299; - _jacobian(1, 6) = _tmp305; - _jacobian(2, 6) = _tmp306; - _jacobian(3, 6) = _tmp307; - _jacobian(4, 6) = _tmp308; - _jacobian(5, 6) = _tmp309; - _jacobian(0, 7) = _tmp319; - _jacobian(1, 7) = _tmp325; - _jacobian(2, 7) = _tmp327; - _jacobian(3, 7) = _tmp328; - _jacobian(4, 7) = _tmp329; - _jacobian(5, 7) = _tmp330; - _jacobian(0, 8) = _tmp340; - _jacobian(1, 8) = _tmp346; - _jacobian(2, 8) = _tmp347; - _jacobian(3, 8) = _tmp348; - _jacobian(4, 8) = _tmp349; - _jacobian(5, 8) = _tmp350; - _jacobian(0, 9) = _tmp351; - _jacobian(1, 9) = _tmp352; - _jacobian(2, 9) = _tmp353; - _jacobian(3, 9) = _tmp354; - _jacobian(4, 9) = _tmp355; - _jacobian(5, 9) = _tmp356; - _jacobian(0, 10) = _tmp357; - _jacobian(1, 10) = _tmp358; - _jacobian(2, 10) = _tmp359; - _jacobian(3, 10) = _tmp360; - _jacobian(4, 10) = _tmp361; - _jacobian(5, 10) = _tmp362; - _jacobian(0, 11) = _tmp363; - _jacobian(1, 11) = _tmp364; - _jacobian(2, 11) = _tmp365; - _jacobian(3, 11) = _tmp366; - _jacobian(4, 11) = _tmp367; - _jacobian(5, 11) = _tmp368; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = std::pow(_tmp166, Scalar(2)) + std::pow(_tmp174, Scalar(2)) + - std::pow(_tmp176, Scalar(2)) + std::pow(_tmp178, Scalar(2)) + - std::pow(_tmp180, Scalar(2)) + std::pow(_tmp182, Scalar(2)); - _hessian(1, 0) = _tmp166 * _tmp216 + _tmp174 * _tmp222 + _tmp176 * _tmp224 + _tmp178 * _tmp226 + - _tmp180 * _tmp227 + _tmp182 * _tmp229; - _hessian(2, 0) = _tmp166 * _tmp251 + _tmp174 * _tmp256 + _tmp176 * _tmp257 + _tmp178 * _tmp258 + - _tmp180 * _tmp259 + _tmp182 * _tmp260; - _hessian(3, 0) = _tmp166 * _tmp263 + _tmp174 * _tmp264 + _tmp176 * _tmp265 + _tmp178 * _tmp266 + - _tmp180 * _tmp267 + _tmp182 * _tmp268; - _hessian(4, 0) = _tmp166 * _tmp270 + _tmp174 * _tmp271 + _tmp176 * _tmp272 + _tmp178 * _tmp273 + - _tmp180 * _tmp274 + _tmp182 * _tmp275; - _hessian(5, 0) = _tmp166 * _tmp277 + _tmp174 * _tmp278 + _tmp176 * _tmp279 + _tmp178 * _tmp280 + - _tmp180 * _tmp281 + _tmp182 * _tmp282; - _hessian(6, 0) = _tmp166 * _tmp299 + _tmp174 * _tmp305 + _tmp176 * _tmp306 + _tmp178 * _tmp307 + - _tmp180 * _tmp308 + _tmp182 * _tmp309; - _hessian(7, 0) = _tmp166 * _tmp319 + _tmp174 * _tmp325 + _tmp176 * _tmp327 + _tmp178 * _tmp328 + - _tmp180 * _tmp329 + _tmp182 * _tmp330; - _hessian(8, 0) = _tmp166 * _tmp340 + _tmp174 * _tmp346 + _tmp176 * _tmp347 + _tmp178 * _tmp348 + - _tmp180 * _tmp349 + _tmp182 * _tmp350; - _hessian(9, 0) = _tmp166 * _tmp351 + _tmp174 * _tmp352 + _tmp176 * _tmp353 + _tmp178 * _tmp354 + - _tmp180 * _tmp355 + _tmp182 * _tmp356; - _hessian(10, 0) = _tmp166 * _tmp357 + _tmp174 * _tmp358 + _tmp176 * _tmp359 + - _tmp178 * _tmp360 + _tmp180 * _tmp361 + _tmp182 * _tmp362; - _hessian(11, 0) = _tmp166 * _tmp363 + _tmp174 * _tmp364 + _tmp176 * _tmp365 + - _tmp178 * _tmp366 + _tmp180 * _tmp367 + _tmp182 * _tmp368; - _hessian(0, 1) = 0; - _hessian(1, 1) = std::pow(_tmp216, Scalar(2)) + std::pow(_tmp222, Scalar(2)) + - std::pow(_tmp224, Scalar(2)) + std::pow(_tmp226, Scalar(2)) + - std::pow(_tmp227, Scalar(2)) + std::pow(_tmp229, Scalar(2)); - _hessian(2, 1) = _tmp216 * _tmp251 + _tmp222 * _tmp256 + _tmp224 * _tmp257 + _tmp226 * _tmp258 + - _tmp227 * _tmp259 + _tmp229 * _tmp260; - _hessian(3, 1) = _tmp216 * _tmp263 + _tmp222 * _tmp264 + _tmp224 * _tmp265 + _tmp226 * _tmp266 + - _tmp227 * _tmp267 + _tmp229 * _tmp268; - _hessian(4, 1) = _tmp216 * _tmp270 + _tmp222 * _tmp271 + _tmp224 * _tmp272 + _tmp226 * _tmp273 + - _tmp227 * _tmp274 + _tmp229 * _tmp275; - _hessian(5, 1) = _tmp216 * _tmp277 + _tmp222 * _tmp278 + _tmp224 * _tmp279 + _tmp226 * _tmp280 + - _tmp227 * _tmp281 + _tmp229 * _tmp282; - _hessian(6, 1) = _tmp216 * _tmp299 + _tmp222 * _tmp305 + _tmp224 * _tmp306 + _tmp226 * _tmp307 + - _tmp227 * _tmp308 + _tmp229 * _tmp309; - _hessian(7, 1) = _tmp216 * _tmp319 + _tmp222 * _tmp325 + _tmp224 * _tmp327 + _tmp226 * _tmp328 + - _tmp227 * _tmp329 + _tmp229 * _tmp330; - _hessian(8, 1) = _tmp216 * _tmp340 + _tmp222 * _tmp346 + _tmp224 * _tmp347 + _tmp226 * _tmp348 + - _tmp227 * _tmp349 + _tmp229 * _tmp350; - _hessian(9, 1) = _tmp216 * _tmp351 + _tmp222 * _tmp352 + _tmp224 * _tmp353 + _tmp226 * _tmp354 + - _tmp227 * _tmp355 + _tmp229 * _tmp356; - _hessian(10, 1) = _tmp216 * _tmp357 + _tmp222 * _tmp358 + _tmp224 * _tmp359 + - _tmp226 * _tmp360 + _tmp227 * _tmp361 + _tmp229 * _tmp362; - _hessian(11, 1) = _tmp216 * _tmp363 + _tmp222 * _tmp364 + _tmp224 * _tmp365 + - _tmp226 * _tmp366 + _tmp227 * _tmp367 + _tmp229 * _tmp368; - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = std::pow(_tmp251, Scalar(2)) + std::pow(_tmp256, Scalar(2)) + - std::pow(_tmp257, Scalar(2)) + std::pow(_tmp258, Scalar(2)) + - std::pow(_tmp259, Scalar(2)) + std::pow(_tmp260, Scalar(2)); - _hessian(3, 2) = _tmp251 * _tmp263 + _tmp256 * _tmp264 + _tmp257 * _tmp265 + _tmp258 * _tmp266 + - _tmp259 * _tmp267 + _tmp260 * _tmp268; - _hessian(4, 2) = _tmp251 * _tmp270 + _tmp256 * _tmp271 + _tmp257 * _tmp272 + _tmp258 * _tmp273 + - _tmp259 * _tmp274 + _tmp260 * _tmp275; - _hessian(5, 2) = _tmp251 * _tmp277 + _tmp256 * _tmp278 + _tmp257 * _tmp279 + _tmp258 * _tmp280 + - _tmp259 * _tmp281 + _tmp260 * _tmp282; - _hessian(6, 2) = _tmp251 * _tmp299 + _tmp256 * _tmp305 + _tmp257 * _tmp306 + _tmp258 * _tmp307 + - _tmp259 * _tmp308 + _tmp260 * _tmp309; - _hessian(7, 2) = _tmp251 * _tmp319 + _tmp256 * _tmp325 + _tmp257 * _tmp327 + _tmp258 * _tmp328 + - _tmp259 * _tmp329 + _tmp260 * _tmp330; - _hessian(8, 2) = _tmp251 * _tmp340 + _tmp256 * _tmp346 + _tmp257 * _tmp347 + _tmp258 * _tmp348 + - _tmp259 * _tmp349 + _tmp260 * _tmp350; - _hessian(9, 2) = _tmp251 * _tmp351 + _tmp256 * _tmp352 + _tmp257 * _tmp353 + _tmp258 * _tmp354 + - _tmp259 * _tmp355 + _tmp260 * _tmp356; - _hessian(10, 2) = _tmp251 * _tmp357 + _tmp256 * _tmp358 + _tmp257 * _tmp359 + - _tmp258 * _tmp360 + _tmp259 * _tmp361 + _tmp260 * _tmp362; - _hessian(11, 2) = _tmp251 * _tmp363 + _tmp256 * _tmp364 + _tmp257 * _tmp365 + - _tmp258 * _tmp366 + _tmp259 * _tmp367 + _tmp260 * _tmp368; - _hessian(0, 3) = 0; - _hessian(1, 3) = 0; - _hessian(2, 3) = 0; - _hessian(3, 3) = std::pow(_tmp263, Scalar(2)) + std::pow(_tmp264, Scalar(2)) + - std::pow(_tmp265, Scalar(2)) + std::pow(_tmp266, Scalar(2)) + - std::pow(_tmp267, Scalar(2)) + std::pow(_tmp268, Scalar(2)); - _hessian(4, 3) = _tmp263 * _tmp270 + _tmp264 * _tmp271 + _tmp265 * _tmp272 + _tmp266 * _tmp273 + - _tmp267 * _tmp274 + _tmp268 * _tmp275; - _hessian(5, 3) = _tmp263 * _tmp277 + _tmp264 * _tmp278 + _tmp265 * _tmp279 + _tmp266 * _tmp280 + - _tmp267 * _tmp281 + _tmp268 * _tmp282; - _hessian(6, 3) = _tmp263 * _tmp299 + _tmp264 * _tmp305 + _tmp265 * _tmp306 + _tmp266 * _tmp307 + - _tmp267 * _tmp308 + _tmp268 * _tmp309; - _hessian(7, 3) = _tmp263 * _tmp319 + _tmp264 * _tmp325 + _tmp265 * _tmp327 + _tmp266 * _tmp328 + - _tmp267 * _tmp329 + _tmp268 * _tmp330; - _hessian(8, 3) = _tmp263 * _tmp340 + _tmp264 * _tmp346 + _tmp265 * _tmp347 + _tmp266 * _tmp348 + - _tmp267 * _tmp349 + _tmp268 * _tmp350; - _hessian(9, 3) = _tmp263 * _tmp351 + _tmp264 * _tmp352 + _tmp265 * _tmp353 + _tmp266 * _tmp354 + - _tmp267 * _tmp355 + _tmp268 * _tmp356; - _hessian(10, 3) = _tmp263 * _tmp357 + _tmp264 * _tmp358 + _tmp265 * _tmp359 + - _tmp266 * _tmp360 + _tmp267 * _tmp361 + _tmp268 * _tmp362; - _hessian(11, 3) = _tmp263 * _tmp363 + _tmp264 * _tmp364 + _tmp265 * _tmp365 + - _tmp266 * _tmp366 + _tmp267 * _tmp367 + _tmp268 * _tmp368; - _hessian(0, 4) = 0; - _hessian(1, 4) = 0; - _hessian(2, 4) = 0; - _hessian(3, 4) = 0; - _hessian(4, 4) = std::pow(_tmp270, Scalar(2)) + std::pow(_tmp271, Scalar(2)) + - std::pow(_tmp272, Scalar(2)) + std::pow(_tmp273, Scalar(2)) + - std::pow(_tmp274, Scalar(2)) + std::pow(_tmp275, Scalar(2)); - _hessian(5, 4) = _tmp270 * _tmp277 + _tmp271 * _tmp278 + _tmp272 * _tmp279 + _tmp273 * _tmp280 + - _tmp274 * _tmp281 + _tmp275 * _tmp282; - _hessian(6, 4) = _tmp270 * _tmp299 + _tmp271 * _tmp305 + _tmp272 * _tmp306 + _tmp273 * _tmp307 + - _tmp274 * _tmp308 + _tmp275 * _tmp309; - _hessian(7, 4) = _tmp270 * _tmp319 + _tmp271 * _tmp325 + _tmp272 * _tmp327 + _tmp273 * _tmp328 + - _tmp274 * _tmp329 + _tmp275 * _tmp330; - _hessian(8, 4) = _tmp270 * _tmp340 + _tmp271 * _tmp346 + _tmp272 * _tmp347 + _tmp273 * _tmp348 + - _tmp274 * _tmp349 + _tmp275 * _tmp350; - _hessian(9, 4) = _tmp270 * _tmp351 + _tmp271 * _tmp352 + _tmp272 * _tmp353 + _tmp273 * _tmp354 + - _tmp274 * _tmp355 + _tmp275 * _tmp356; - _hessian(10, 4) = _tmp270 * _tmp357 + _tmp271 * _tmp358 + _tmp272 * _tmp359 + - _tmp273 * _tmp360 + _tmp274 * _tmp361 + _tmp275 * _tmp362; - _hessian(11, 4) = _tmp270 * _tmp363 + _tmp271 * _tmp364 + _tmp272 * _tmp365 + - _tmp273 * _tmp366 + _tmp274 * _tmp367 + _tmp275 * _tmp368; - _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(_tmp277, Scalar(2)) + std::pow(_tmp278, Scalar(2)) + - std::pow(_tmp279, Scalar(2)) + std::pow(_tmp280, Scalar(2)) + - std::pow(_tmp281, Scalar(2)) + std::pow(_tmp282, Scalar(2)); - _hessian(6, 5) = _tmp277 * _tmp299 + _tmp278 * _tmp305 + _tmp279 * _tmp306 + _tmp280 * _tmp307 + - _tmp281 * _tmp308 + _tmp282 * _tmp309; - _hessian(7, 5) = _tmp277 * _tmp319 + _tmp278 * _tmp325 + _tmp279 * _tmp327 + _tmp280 * _tmp328 + - _tmp281 * _tmp329 + _tmp282 * _tmp330; - _hessian(8, 5) = _tmp277 * _tmp340 + _tmp278 * _tmp346 + _tmp279 * _tmp347 + _tmp280 * _tmp348 + - _tmp281 * _tmp349 + _tmp282 * _tmp350; - _hessian(9, 5) = _tmp277 * _tmp351 + _tmp278 * _tmp352 + _tmp279 * _tmp353 + _tmp280 * _tmp354 + - _tmp281 * _tmp355 + _tmp282 * _tmp356; - _hessian(10, 5) = _tmp277 * _tmp357 + _tmp278 * _tmp358 + _tmp279 * _tmp359 + - _tmp280 * _tmp360 + _tmp281 * _tmp361 + _tmp282 * _tmp362; - _hessian(11, 5) = _tmp277 * _tmp363 + _tmp278 * _tmp364 + _tmp279 * _tmp365 + - _tmp280 * _tmp366 + _tmp281 * _tmp367 + _tmp282 * _tmp368; - _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(_tmp299, Scalar(2)) + std::pow(_tmp305, Scalar(2)) + - std::pow(_tmp306, Scalar(2)) + std::pow(_tmp307, Scalar(2)) + - std::pow(_tmp308, Scalar(2)) + std::pow(_tmp309, Scalar(2)); - _hessian(7, 6) = _tmp299 * _tmp319 + _tmp305 * _tmp325 + _tmp306 * _tmp327 + _tmp307 * _tmp328 + - _tmp308 * _tmp329 + _tmp309 * _tmp330; - _hessian(8, 6) = _tmp299 * _tmp340 + _tmp305 * _tmp346 + _tmp306 * _tmp347 + _tmp307 * _tmp348 + - _tmp308 * _tmp349 + _tmp309 * _tmp350; - _hessian(9, 6) = _tmp299 * _tmp351 + _tmp305 * _tmp352 + _tmp306 * _tmp353 + _tmp307 * _tmp354 + - _tmp308 * _tmp355 + _tmp309 * _tmp356; - _hessian(10, 6) = _tmp299 * _tmp357 + _tmp305 * _tmp358 + _tmp306 * _tmp359 + - _tmp307 * _tmp360 + _tmp308 * _tmp361 + _tmp309 * _tmp362; - _hessian(11, 6) = _tmp299 * _tmp363 + _tmp305 * _tmp364 + _tmp306 * _tmp365 + - _tmp307 * _tmp366 + _tmp308 * _tmp367 + _tmp309 * _tmp368; - _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(_tmp319, Scalar(2)) + std::pow(_tmp325, Scalar(2)) + - std::pow(_tmp327, Scalar(2)) + std::pow(_tmp328, Scalar(2)) + - std::pow(_tmp329, Scalar(2)) + std::pow(_tmp330, Scalar(2)); - _hessian(8, 7) = _tmp319 * _tmp340 + _tmp325 * _tmp346 + _tmp327 * _tmp347 + _tmp328 * _tmp348 + - _tmp329 * _tmp349 + _tmp330 * _tmp350; - _hessian(9, 7) = _tmp319 * _tmp351 + _tmp325 * _tmp352 + _tmp327 * _tmp353 + _tmp328 * _tmp354 + - _tmp329 * _tmp355 + _tmp330 * _tmp356; - _hessian(10, 7) = _tmp319 * _tmp357 + _tmp325 * _tmp358 + _tmp327 * _tmp359 + - _tmp328 * _tmp360 + _tmp329 * _tmp361 + _tmp330 * _tmp362; - _hessian(11, 7) = _tmp319 * _tmp363 + _tmp325 * _tmp364 + _tmp327 * _tmp365 + - _tmp328 * _tmp366 + _tmp329 * _tmp367 + _tmp330 * _tmp368; - _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(_tmp340, Scalar(2)) + std::pow(_tmp346, Scalar(2)) + - std::pow(_tmp347, Scalar(2)) + std::pow(_tmp348, Scalar(2)) + - std::pow(_tmp349, Scalar(2)) + std::pow(_tmp350, Scalar(2)); - _hessian(9, 8) = _tmp340 * _tmp351 + _tmp346 * _tmp352 + _tmp347 * _tmp353 + _tmp348 * _tmp354 + - _tmp349 * _tmp355 + _tmp350 * _tmp356; - _hessian(10, 8) = _tmp340 * _tmp357 + _tmp346 * _tmp358 + _tmp347 * _tmp359 + - _tmp348 * _tmp360 + _tmp349 * _tmp361 + _tmp350 * _tmp362; - _hessian(11, 8) = _tmp340 * _tmp363 + _tmp346 * _tmp364 + _tmp347 * _tmp365 + - _tmp348 * _tmp366 + _tmp349 * _tmp367 + _tmp350 * _tmp368; - _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(_tmp351, Scalar(2)) + std::pow(_tmp352, Scalar(2)) + - std::pow(_tmp353, Scalar(2)) + std::pow(_tmp354, Scalar(2)) + - std::pow(_tmp355, Scalar(2)) + std::pow(_tmp356, Scalar(2)); - _hessian(10, 9) = _tmp351 * _tmp357 + _tmp352 * _tmp358 + _tmp353 * _tmp359 + - _tmp354 * _tmp360 + _tmp355 * _tmp361 + _tmp356 * _tmp362; - _hessian(11, 9) = _tmp351 * _tmp363 + _tmp352 * _tmp364 + _tmp353 * _tmp365 + - _tmp354 * _tmp366 + _tmp355 * _tmp367 + _tmp356 * _tmp368; - _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(_tmp357, Scalar(2)) + std::pow(_tmp358, Scalar(2)) + - std::pow(_tmp359, Scalar(2)) + std::pow(_tmp360, Scalar(2)) + - std::pow(_tmp361, Scalar(2)) + std::pow(_tmp362, Scalar(2)); - _hessian(11, 10) = _tmp357 * _tmp363 + _tmp358 * _tmp364 + _tmp359 * _tmp365 + - _tmp360 * _tmp366 + _tmp361 * _tmp367 + _tmp362 * _tmp368; - _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(_tmp363, Scalar(2)) + std::pow(_tmp364, Scalar(2)) + - std::pow(_tmp365, Scalar(2)) + std::pow(_tmp366, Scalar(2)) + - std::pow(_tmp367, Scalar(2)) + std::pow(_tmp368, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp166 * _tmp79 + _tmp174 * _tmp82 + _tmp176 * _tmp83 + _tmp178 * _tmp84 + - _tmp180 * _tmp85 + _tmp182 * _tmp87; - _rhs(1, 0) = _tmp216 * _tmp79 + _tmp222 * _tmp82 + _tmp224 * _tmp83 + _tmp226 * _tmp84 + - _tmp227 * _tmp85 + _tmp229 * _tmp87; - _rhs(2, 0) = _tmp251 * _tmp79 + _tmp256 * _tmp82 + _tmp257 * _tmp83 + _tmp258 * _tmp84 + - _tmp259 * _tmp85 + _tmp260 * _tmp87; - _rhs(3, 0) = _tmp263 * _tmp79 + _tmp264 * _tmp82 + _tmp265 * _tmp83 + _tmp266 * _tmp84 + - _tmp267 * _tmp85 + _tmp268 * _tmp87; - _rhs(4, 0) = _tmp270 * _tmp79 + _tmp271 * _tmp82 + _tmp272 * _tmp83 + _tmp273 * _tmp84 + - _tmp274 * _tmp85 + _tmp275 * _tmp87; - _rhs(5, 0) = _tmp277 * _tmp79 + _tmp278 * _tmp82 + _tmp279 * _tmp83 + _tmp280 * _tmp84 + - _tmp281 * _tmp85 + _tmp282 * _tmp87; - _rhs(6, 0) = _tmp299 * _tmp79 + _tmp305 * _tmp82 + _tmp306 * _tmp83 + _tmp307 * _tmp84 + - _tmp308 * _tmp85 + _tmp309 * _tmp87; - _rhs(7, 0) = _tmp319 * _tmp79 + _tmp325 * _tmp82 + _tmp327 * _tmp83 + _tmp328 * _tmp84 + - _tmp329 * _tmp85 + _tmp330 * _tmp87; - _rhs(8, 0) = _tmp340 * _tmp79 + _tmp346 * _tmp82 + _tmp347 * _tmp83 + _tmp348 * _tmp84 + - _tmp349 * _tmp85 + _tmp350 * _tmp87; - _rhs(9, 0) = _tmp351 * _tmp79 + _tmp352 * _tmp82 + _tmp353 * _tmp83 + _tmp354 * _tmp84 + - _tmp355 * _tmp85 + _tmp356 * _tmp87; - _rhs(10, 0) = _tmp357 * _tmp79 + _tmp358 * _tmp82 + _tmp359 * _tmp83 + _tmp360 * _tmp84 + - _tmp361 * _tmp85 + _tmp362 * _tmp87; - _rhs(11, 0) = _tmp363 * _tmp79 + _tmp364 * _tmp82 + _tmp365 * _tmp83 + _tmp366 * _tmp84 + - _tmp367 * _tmp85 + _tmp368 * _tmp87; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_pose3/between_factor_pose3_diagonal.h" +#include "./between_factor_pose3/between_factor_pose3_isotropic.h" +#include "./between_factor_pose3/between_factor_pose3_square.h" diff --git a/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_diagonal.h b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_diagonal.h new file mode 100644 index 000000000..945da257f --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_diagonal.h @@ -0,0 +1,526 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b, + const sym::Pose3& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 926 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (252) + const Scalar _tmp0 = _a[0] * _b[1]; + const Scalar _tmp1 = _a[2] * _b[3]; + const Scalar _tmp2 = _a[3] * _b[2]; + const Scalar _tmp3 = _a[1] * _b[0]; + const Scalar _tmp4 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a_T_b[2] * _tmp4; + const Scalar _tmp6 = _a[2] * _b[1]; + const Scalar _tmp7 = _a[0] * _b[3]; + const Scalar _tmp8 = _a[1] * _b[2]; + const Scalar _tmp9 = _a[3] * _b[0]; + const Scalar _tmp10 = _tmp6 - _tmp7 - _tmp8 + _tmp9; + const Scalar _tmp11 = _a_T_b[0] * _tmp10; + const Scalar _tmp12 = _a[3] * _b[1]; + const Scalar _tmp13 = _a[1] * _b[3]; + const Scalar _tmp14 = _a[0] * _b[2]; + const Scalar _tmp15 = _a[2] * _b[0]; + const Scalar _tmp16 = _tmp12 - _tmp13 + _tmp14 - _tmp15; + const Scalar _tmp17 = _a_T_b[1] * _tmp16; + const Scalar _tmp18 = -_tmp11 - _tmp17 - _tmp5; + const Scalar _tmp19 = _a[1] * _b[1]; + const Scalar _tmp20 = _a[2] * _b[2]; + const Scalar _tmp21 = _a[0] * _b[0]; + const Scalar _tmp22 = _a[3] * _b[3]; + const Scalar _tmp23 = _tmp19 + _tmp20 + _tmp21 + _tmp22; + const Scalar _tmp24 = _a_T_b[3] * _tmp23; + const Scalar _tmp25 = + 2 * std::min(0, (((-_tmp18 + _tmp24) > 0) - ((-_tmp18 + _tmp24) < 0))) + 1; + const Scalar _tmp26 = 2 * _tmp25; + const Scalar _tmp27 = 1 - epsilon; + const Scalar _tmp28 = std::min(_tmp27, std::fabs(_tmp18 - _tmp24)); + const Scalar _tmp29 = std::acos(_tmp28) / std::sqrt(Scalar(1 - std::pow(_tmp28, Scalar(2)))); + const Scalar _tmp30 = _tmp26 * _tmp29; + const Scalar _tmp31 = sqrt_info(0, 0) * (-_a_T_b[0] * _tmp23 - _a_T_b[1] * _tmp4 + + _a_T_b[2] * _tmp16 + _a_T_b[3] * _tmp10); + const Scalar _tmp32 = _tmp30 * _tmp31; + const Scalar _tmp33 = sqrt_info(1, 0) * (_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp23 - + _a_T_b[2] * _tmp10 + _a_T_b[3] * _tmp16); + const Scalar _tmp34 = _tmp30 * _tmp33; + const Scalar _tmp35 = + -_a_T_b[0] * _tmp16 + _a_T_b[1] * _tmp10 - _a_T_b[2] * _tmp23 + _a_T_b[3] * _tmp4; + const Scalar _tmp36 = _tmp26 * sqrt_info(2, 0); + const Scalar _tmp37 = _tmp29 * _tmp35 * _tmp36; + const Scalar _tmp38 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp39 = 2 * _tmp38; + const Scalar _tmp40 = -_tmp39; + const Scalar _tmp41 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp42 = 2 * _tmp41; + const Scalar _tmp43 = -_tmp42; + const Scalar _tmp44 = _tmp40 + _tmp43 + 1; + const Scalar _tmp45 = 2 * _a[0] * _a[2]; + const Scalar _tmp46 = 2 * _a[3]; + const Scalar _tmp47 = _a[1] * _tmp46; + const Scalar _tmp48 = -_tmp47; + const Scalar _tmp49 = _tmp45 + _tmp48; + const Scalar _tmp50 = _a[6] * _tmp49; + const Scalar _tmp51 = 2 * _a[1]; + const Scalar _tmp52 = _a[0] * _tmp51; + const Scalar _tmp53 = _a[2] * _tmp46; + const Scalar _tmp54 = _tmp52 + _tmp53; + const Scalar _tmp55 = _a[5] * _tmp54; + const Scalar _tmp56 = _b[5] * _tmp54 + _b[6] * _tmp49; + const Scalar _tmp57 = -_a[4] * _tmp44 - _a_T_b[4] + _b[4] * _tmp44 - _tmp50 - _tmp55 + _tmp56; + const Scalar _tmp58 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp59 = 2 * _tmp58; + const Scalar _tmp60 = 1 - _tmp59; + const Scalar _tmp61 = _tmp40 + _tmp60; + const Scalar _tmp62 = _a[2] * _tmp51; + const Scalar _tmp63 = _a[0] * _tmp46; + const Scalar _tmp64 = _tmp62 + _tmp63; + const Scalar _tmp65 = _a[6] * _tmp64; + const Scalar _tmp66 = -_tmp53; + const Scalar _tmp67 = _tmp52 + _tmp66; + const Scalar _tmp68 = _a[4] * _tmp67; + const Scalar _tmp69 = _b[4] * _tmp67 + _b[6] * _tmp64; + const Scalar _tmp70 = -_a[5] * _tmp61 - _a_T_b[5] + _b[5] * _tmp61 - _tmp65 - _tmp68 + _tmp69; + const Scalar _tmp71 = _tmp43 + _tmp60; + const Scalar _tmp72 = -_tmp63; + const Scalar _tmp73 = _tmp62 + _tmp72; + const Scalar _tmp74 = _a[5] * _tmp73; + const Scalar _tmp75 = _tmp45 + _tmp47; + const Scalar _tmp76 = _a[4] * _tmp75; + const Scalar _tmp77 = _b[4] * _tmp75 + _b[5] * _tmp73; + const Scalar _tmp78 = -_a[6] * _tmp71 - _a_T_b[6] + _b[6] * _tmp71 - _tmp74 - _tmp76 + _tmp77; + const Scalar _tmp79 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp80 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp81 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp82 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp83 = -_tmp79 - _tmp80 + _tmp81 + _tmp82; + const Scalar _tmp84 = _a_T_b[2] * _tmp83; + const Scalar _tmp85 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp86 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp87 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp88 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp89 = -_tmp85 - _tmp86 - _tmp87 - _tmp88; + const Scalar _tmp90 = _a_T_b[3] * _tmp89; + const Scalar _tmp91 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp92 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp93 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp94 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp95 = -_tmp91 + _tmp92 - _tmp93 + _tmp94; + const Scalar _tmp96 = -_a_T_b[1] * _tmp95; + const Scalar _tmp97 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp98 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp99 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp100 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp101 = _tmp100 + _tmp97 - _tmp98 - _tmp99; + const Scalar _tmp102 = _a_T_b[0] * _tmp101; + const Scalar _tmp103 = _tmp11 + _tmp17 + _tmp24 + _tmp5; + const Scalar _tmp104 = std::fabs(_tmp103); + const Scalar _tmp105 = std::min(_tmp104, _tmp27); + const Scalar _tmp106 = 1 - std::pow(_tmp105, Scalar(2)); + const Scalar _tmp107 = std::acos(_tmp105); + const Scalar _tmp108 = _tmp107 / std::sqrt(_tmp106); + const Scalar _tmp109 = _tmp108 * _tmp26; + const Scalar _tmp110 = _tmp109 * sqrt_info(0, 0); + const Scalar _tmp111 = _a_T_b[0] * _tmp89; + const Scalar _tmp112 = _a_T_b[3] * _tmp101; + const Scalar _tmp113 = _a_T_b[1] * _tmp83; + const Scalar _tmp114 = _a_T_b[2] * _tmp95; + const Scalar _tmp115 = _tmp113 + _tmp114; + const Scalar _tmp116 = _tmp111 + _tmp112 + _tmp115; + const Scalar _tmp117 = _tmp25 * ((((-_tmp104 + _tmp27) > 0) - ((-_tmp104 + _tmp27) < 0)) + 1) * + (((_tmp103) > 0) - ((_tmp103) < 0)); + const Scalar _tmp118 = _tmp117 / _tmp106; + const Scalar _tmp119 = _tmp116 * _tmp118; + const Scalar _tmp120 = _tmp105 * _tmp107 * _tmp117 / (_tmp106 * std::sqrt(_tmp106)); + const Scalar _tmp121 = _tmp120 * _tmp31; + const Scalar _tmp122 = + _tmp110 * (-_tmp102 + _tmp84 + _tmp90 + _tmp96) + _tmp116 * _tmp121 - _tmp119 * _tmp31; + const Scalar _tmp123 = _tmp120 * _tmp33; + const Scalar _tmp124 = -_a_T_b[1] * _tmp101; + const Scalar _tmp125 = _a_T_b[2] * _tmp89; + const Scalar _tmp126 = _a_T_b[3] * _tmp83; + const Scalar _tmp127 = _a_T_b[0] * _tmp95; + const Scalar _tmp128 = _tmp126 + _tmp127; + const Scalar _tmp129 = _tmp109 * sqrt_info(1, 0); + const Scalar _tmp130 = _tmp118 * _tmp33; + const Scalar _tmp131 = + _tmp116 * _tmp123 - _tmp116 * _tmp130 + _tmp129 * (_tmp124 - _tmp125 + _tmp128); + const Scalar _tmp132 = -_a_T_b[0] * _tmp83; + const Scalar _tmp133 = _a_T_b[1] * _tmp89; + const Scalar _tmp134 = _a_T_b[3] * _tmp95; + const Scalar _tmp135 = _a_T_b[2] * _tmp101; + const Scalar _tmp136 = _tmp108 * _tmp36; + const Scalar _tmp137 = _tmp35 * sqrt_info(2, 0); + const Scalar _tmp138 = _tmp120 * _tmp137; + const Scalar _tmp139 = + _tmp116 * _tmp138 - _tmp119 * _tmp137 + _tmp136 * (_tmp132 + _tmp133 + _tmp134 - _tmp135); + const Scalar _tmp140 = -_tmp58; + const Scalar _tmp141 = -_tmp41; + const Scalar _tmp142 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp143 = _tmp140 + _tmp141 + _tmp142 + _tmp38; + const Scalar _tmp144 = -_a[6] * _tmp143 + _b[6] * _tmp143 - _tmp74 - _tmp76 + _tmp77; + const Scalar _tmp145 = _tmp141 + _tmp58; + const Scalar _tmp146 = -_tmp142; + const Scalar _tmp147 = _tmp146 + _tmp38; + const Scalar _tmp148 = _tmp145 + _tmp147; + const Scalar _tmp149 = -_tmp62; + const Scalar _tmp150 = _tmp149 + _tmp72; + const Scalar _tmp151 = -_tmp52; + const Scalar _tmp152 = _tmp151 + _tmp53; + const Scalar _tmp153 = -_a[4] * _tmp152 - _a[5] * _tmp148 - _a[6] * _tmp150 + _b[4] * _tmp152 + + _b[5] * _tmp148 + _b[6] * _tmp150; + const Scalar _tmp154 = _tmp91 - _tmp92 + _tmp93 - _tmp94; + const Scalar _tmp155 = _a_T_b[3] * _tmp154; + const Scalar _tmp156 = _tmp79 + _tmp80 - _tmp81 - _tmp82; + const Scalar _tmp157 = _a_T_b[0] * _tmp156; + const Scalar _tmp158 = _tmp135 + _tmp157; + const Scalar _tmp159 = _tmp133 + _tmp155 + _tmp158; + const Scalar _tmp160 = _a_T_b[3] * _tmp156; + const Scalar _tmp161 = _a_T_b[0] * _tmp154; + const Scalar _tmp162 = _tmp118 * _tmp31; + const Scalar _tmp163 = + _tmp110 * (_tmp124 + _tmp125 + _tmp160 - _tmp161) + _tmp121 * _tmp159 - _tmp159 * _tmp162; + const Scalar _tmp164 = _a_T_b[1] * _tmp154; + const Scalar _tmp165 = -_a_T_b[2] * _tmp156; + const Scalar _tmp166 = _tmp102 + _tmp165; + const Scalar _tmp167 = + _tmp123 * _tmp159 + _tmp129 * (-_tmp164 + _tmp166 + _tmp90) - _tmp130 * _tmp159; + const Scalar _tmp168 = _tmp118 * _tmp137; + const Scalar _tmp169 = -_a_T_b[2] * _tmp154; + const Scalar _tmp170 = _a_T_b[1] * _tmp156; + const Scalar _tmp171 = _tmp112 + _tmp170; + const Scalar _tmp172 = + _tmp136 * (-_tmp111 + _tmp169 + _tmp171) + _tmp138 * _tmp159 - _tmp159 * _tmp168; + const Scalar _tmp173 = -_tmp38; + const Scalar _tmp174 = _tmp146 + _tmp173 + _tmp41 + _tmp58; + const Scalar _tmp175 = _tmp149 + _tmp63; + const Scalar _tmp176 = -_tmp45; + const Scalar _tmp177 = _tmp176 + _tmp48; + const Scalar _tmp178 = -_a[4] * _tmp177 - _a[5] * _tmp175 - _a[6] * _tmp174 + _b[4] * _tmp177 + + _b[5] * _tmp175 + _b[6] * _tmp174; + const Scalar _tmp179 = _tmp142 + _tmp173; + const Scalar _tmp180 = _tmp145 + _tmp179; + const Scalar _tmp181 = -_a[4] * _tmp180 + _b[4] * _tmp180 - _tmp50 - _tmp55 + _tmp56; + const Scalar _tmp182 = -_tmp100 - _tmp97 + _tmp98 + _tmp99; + const Scalar _tmp183 = _a_T_b[1] * _tmp182; + const Scalar _tmp184 = _tmp161 + _tmp183; + const Scalar _tmp185 = _tmp125 + _tmp126 + _tmp184; + const Scalar _tmp186 = _a_T_b[2] * _tmp182; + const Scalar _tmp187 = _tmp155 + _tmp186; + const Scalar _tmp188 = + _tmp110 * (_tmp132 - _tmp133 + _tmp187) + _tmp121 * _tmp185 - _tmp162 * _tmp185; + const Scalar _tmp189 = _a_T_b[3] * _tmp182; + const Scalar _tmp190 = + _tmp123 * _tmp185 + _tmp129 * (_tmp111 - _tmp113 + _tmp169 + _tmp189) - _tmp130 * _tmp185; + const Scalar _tmp191 = -_a_T_b[0] * _tmp182; + const Scalar _tmp192 = _tmp164 + _tmp191; + const Scalar _tmp193 = + _tmp136 * (_tmp192 - _tmp84 + _tmp90) + _tmp138 * _tmp185 - _tmp168 * _tmp185; + const Scalar _tmp194 = _tmp140 + _tmp41; + const Scalar _tmp195 = _tmp179 + _tmp194; + const Scalar _tmp196 = -_a[5] * _tmp195 + _b[5] * _tmp195 - _tmp65 - _tmp68 + _tmp69; + const Scalar _tmp197 = _tmp147 + _tmp194; + const Scalar _tmp198 = _tmp176 + _tmp47; + const Scalar _tmp199 = _tmp151 + _tmp66; + const Scalar _tmp200 = -_a[4] * _tmp197 - _a[5] * _tmp199 - _a[6] * _tmp198 + _b[4] * _tmp197 + + _b[5] * _tmp199 + _b[6] * _tmp198; + const Scalar _tmp201 = _tmp39 + _tmp42 - 1; + const Scalar _tmp202 = _tmp59 - 1; + const Scalar _tmp203 = _tmp202 + _tmp39; + const Scalar _tmp204 = _tmp202 + _tmp42; + const Scalar _tmp205 = _tmp85 + _tmp86 + _tmp87 + _tmp88; + const Scalar _tmp206 = _a_T_b[3] * _tmp205; + const Scalar _tmp207 = _tmp206 + _tmp96; + const Scalar _tmp208 = _a_T_b[0] * _tmp205; + const Scalar _tmp209 = _tmp189 + _tmp208; + const Scalar _tmp210 = _tmp115 + _tmp209; + const Scalar _tmp211 = + _tmp110 * (_tmp191 + _tmp207 + _tmp84) + _tmp121 * _tmp210 - _tmp162 * _tmp210; + const Scalar _tmp212 = _a_T_b[2] * _tmp205; + const Scalar _tmp213 = + _tmp123 * _tmp210 + _tmp129 * (_tmp128 - _tmp183 - _tmp212) - _tmp130 * _tmp210; + const Scalar _tmp214 = _a_T_b[1] * _tmp205; + const Scalar _tmp215 = _tmp134 + _tmp214; + const Scalar _tmp216 = + _tmp136 * (_tmp132 - _tmp186 + _tmp215) + _tmp138 * _tmp210 - _tmp168 * _tmp210; + const Scalar _tmp217 = _tmp160 + _tmp212; + const Scalar _tmp218 = _tmp158 + _tmp215; + const Scalar _tmp219 = _tmp118 * _tmp218; + const Scalar _tmp220 = + _tmp110 * (_tmp124 - _tmp127 + _tmp217) + _tmp121 * _tmp218 - _tmp219 * _tmp31; + const Scalar _tmp221 = _tmp123 * _tmp218 + _tmp129 * (_tmp166 + _tmp207) - _tmp219 * _tmp33; + const Scalar _tmp222 = + _tmp136 * (-_tmp114 + _tmp171 - _tmp208) - _tmp137 * _tmp219 + _tmp138 * _tmp218; + const Scalar _tmp223 = _tmp184 + _tmp217; + const Scalar _tmp224 = _tmp118 * _tmp223; + const Scalar _tmp225 = + _tmp110 * (-_tmp157 + _tmp187 - _tmp214) + _tmp121 * _tmp223 - _tmp224 * _tmp31; + const Scalar _tmp226 = + _tmp123 * _tmp223 + _tmp129 * (_tmp169 - _tmp170 + _tmp209) - _tmp130 * _tmp223; + const Scalar _tmp227 = + _tmp136 * (_tmp165 + _tmp192 + _tmp206) - _tmp137 * _tmp224 + _tmp138 * _tmp223; + const Scalar _tmp228 = std::pow(sqrt_info(4, 0), Scalar(2)); + const Scalar _tmp229 = std::pow(sqrt_info(5, 0), Scalar(2)); + const Scalar _tmp230 = _tmp153 * _tmp229; + const Scalar _tmp231 = _tmp144 * _tmp228; + const Scalar _tmp232 = _tmp204 * _tmp229; + const Scalar _tmp233 = _tmp229 * _tmp71; + const Scalar _tmp234 = std::pow(sqrt_info(3, 0), Scalar(2)); + const Scalar _tmp235 = _tmp196 * _tmp234; + const Scalar _tmp236 = _tmp178 * _tmp234; + const Scalar _tmp237 = _tmp181 * _tmp229; + const Scalar _tmp238 = _tmp200 * _tmp228; + const Scalar _tmp239 = _tmp177 * _tmp229; + const Scalar _tmp240 = _tmp203 * _tmp228; + const Scalar _tmp241 = _tmp201 * _tmp234; + const Scalar _tmp242 = _tmp150 * _tmp228; + const Scalar _tmp243 = _tmp228 * _tmp67; + const Scalar _tmp244 = _tmp228 * _tmp61; + const Scalar _tmp245 = _tmp199 * _tmp234; + const Scalar _tmp246 = _tmp175 * _tmp229; + const Scalar _tmp247 = _tmp234 * _tmp44; + const Scalar _tmp248 = _tmp234 * _tmp54; + const Scalar _tmp249 = _tmp229 * _tmp78; + const Scalar _tmp250 = _tmp228 * _tmp70; + const Scalar _tmp251 = _tmp234 * _tmp57; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp32; + _res(1, 0) = _tmp34; + _res(2, 0) = _tmp37; + _res(3, 0) = _tmp57 * sqrt_info(3, 0); + _res(4, 0) = _tmp70 * sqrt_info(4, 0); + _res(5, 0) = _tmp78 * sqrt_info(5, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp122; + _jacobian(1, 0) = _tmp131; + _jacobian(2, 0) = _tmp139; + _jacobian(3, 0) = 0; + _jacobian(4, 0) = _tmp144 * sqrt_info(4, 0); + _jacobian(5, 0) = _tmp153 * sqrt_info(5, 0); + _jacobian(0, 1) = _tmp163; + _jacobian(1, 1) = _tmp167; + _jacobian(2, 1) = _tmp172; + _jacobian(3, 1) = _tmp178 * sqrt_info(3, 0); + _jacobian(4, 1) = 0; + _jacobian(5, 1) = _tmp181 * sqrt_info(5, 0); + _jacobian(0, 2) = _tmp188; + _jacobian(1, 2) = _tmp190; + _jacobian(2, 2) = _tmp193; + _jacobian(3, 2) = _tmp196 * sqrt_info(3, 0); + _jacobian(4, 2) = _tmp200 * sqrt_info(4, 0); + _jacobian(5, 2) = 0; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(3, 3) = _tmp201 * sqrt_info(3, 0); + _jacobian(4, 3) = _tmp152 * sqrt_info(4, 0); + _jacobian(5, 3) = _tmp177 * sqrt_info(5, 0); + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(3, 4) = _tmp199 * sqrt_info(3, 0); + _jacobian(4, 4) = _tmp203 * sqrt_info(4, 0); + _jacobian(5, 4) = _tmp175 * sqrt_info(5, 0); + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(3, 5) = _tmp198 * sqrt_info(3, 0); + _jacobian(4, 5) = _tmp150 * sqrt_info(4, 0); + _jacobian(5, 5) = _tmp204 * sqrt_info(5, 0); + _jacobian(0, 6) = _tmp211; + _jacobian(1, 6) = _tmp213; + _jacobian(2, 6) = _tmp216; + _jacobian(3, 6) = 0; + _jacobian(4, 6) = 0; + _jacobian(5, 6) = 0; + _jacobian(0, 7) = _tmp220; + _jacobian(1, 7) = _tmp221; + _jacobian(2, 7) = _tmp222; + _jacobian(3, 7) = 0; + _jacobian(4, 7) = 0; + _jacobian(5, 7) = 0; + _jacobian(0, 8) = _tmp225; + _jacobian(1, 8) = _tmp226; + _jacobian(2, 8) = _tmp227; + _jacobian(3, 8) = 0; + _jacobian(4, 8) = 0; + _jacobian(5, 8) = 0; + _jacobian(0, 9) = 0; + _jacobian(1, 9) = 0; + _jacobian(2, 9) = 0; + _jacobian(3, 9) = _tmp44 * sqrt_info(3, 0); + _jacobian(4, 9) = _tmp67 * sqrt_info(4, 0); + _jacobian(5, 9) = _tmp75 * sqrt_info(5, 0); + _jacobian(0, 10) = 0; + _jacobian(1, 10) = 0; + _jacobian(2, 10) = 0; + _jacobian(3, 10) = _tmp54 * sqrt_info(3, 0); + _jacobian(4, 10) = _tmp61 * sqrt_info(4, 0); + _jacobian(5, 10) = _tmp73 * sqrt_info(5, 0); + _jacobian(0, 11) = 0; + _jacobian(1, 11) = 0; + _jacobian(2, 11) = 0; + _jacobian(3, 11) = _tmp49 * sqrt_info(3, 0); + _jacobian(4, 11) = _tmp64 * sqrt_info(4, 0); + _jacobian(5, 11) = _tmp71 * sqrt_info(5, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp122, Scalar(2)) + std::pow(_tmp131, Scalar(2)) + + std::pow(_tmp139, Scalar(2)) + std::pow(_tmp144, Scalar(2)) * _tmp228 + + std::pow(_tmp153, Scalar(2)) * _tmp229; + _hessian(1, 0) = _tmp122 * _tmp163 + _tmp131 * _tmp167 + _tmp139 * _tmp172 + _tmp181 * _tmp230; + _hessian(2, 0) = _tmp122 * _tmp188 + _tmp131 * _tmp190 + _tmp139 * _tmp193 + _tmp200 * _tmp231; + _hessian(3, 0) = _tmp152 * _tmp231 + _tmp177 * _tmp230; + _hessian(4, 0) = _tmp175 * _tmp230 + _tmp203 * _tmp231; + _hessian(5, 0) = _tmp150 * _tmp231 + _tmp153 * _tmp232; + _hessian(6, 0) = _tmp122 * _tmp211 + _tmp131 * _tmp213 + _tmp139 * _tmp216; + _hessian(7, 0) = _tmp122 * _tmp220 + _tmp131 * _tmp221 + _tmp139 * _tmp222; + _hessian(8, 0) = _tmp122 * _tmp225 + _tmp131 * _tmp226 + _tmp139 * _tmp227; + _hessian(9, 0) = _tmp230 * _tmp75 + _tmp231 * _tmp67; + _hessian(10, 0) = _tmp230 * _tmp73 + _tmp231 * _tmp61; + _hessian(11, 0) = _tmp153 * _tmp233 + _tmp231 * _tmp64; + _hessian(1, 1) = std::pow(_tmp163, Scalar(2)) + std::pow(_tmp167, Scalar(2)) + + std::pow(_tmp172, Scalar(2)) + std::pow(_tmp178, Scalar(2)) * _tmp234 + + std::pow(_tmp181, Scalar(2)) * _tmp229; + _hessian(2, 1) = _tmp163 * _tmp188 + _tmp167 * _tmp190 + _tmp172 * _tmp193 + _tmp178 * _tmp235; + _hessian(3, 1) = _tmp177 * _tmp237 + _tmp201 * _tmp236; + _hessian(4, 1) = _tmp175 * _tmp237 + _tmp199 * _tmp236; + _hessian(5, 1) = _tmp181 * _tmp232 + _tmp198 * _tmp236; + _hessian(6, 1) = _tmp163 * _tmp211 + _tmp167 * _tmp213 + _tmp172 * _tmp216; + _hessian(7, 1) = _tmp163 * _tmp220 + _tmp167 * _tmp221 + _tmp172 * _tmp222; + _hessian(8, 1) = _tmp163 * _tmp225 + _tmp167 * _tmp226 + _tmp172 * _tmp227; + _hessian(9, 1) = _tmp236 * _tmp44 + _tmp237 * _tmp75; + _hessian(10, 1) = _tmp236 * _tmp54 + _tmp237 * _tmp73; + _hessian(11, 1) = _tmp181 * _tmp233 + _tmp236 * _tmp49; + _hessian(2, 2) = std::pow(_tmp188, Scalar(2)) + std::pow(_tmp190, Scalar(2)) + + std::pow(_tmp193, Scalar(2)) + std::pow(_tmp196, Scalar(2)) * _tmp234 + + std::pow(_tmp200, Scalar(2)) * _tmp228; + _hessian(3, 2) = _tmp152 * _tmp238 + _tmp201 * _tmp235; + _hessian(4, 2) = _tmp199 * _tmp235 + _tmp203 * _tmp238; + _hessian(5, 2) = _tmp150 * _tmp238 + _tmp198 * _tmp235; + _hessian(6, 2) = _tmp188 * _tmp211 + _tmp190 * _tmp213 + _tmp193 * _tmp216; + _hessian(7, 2) = _tmp188 * _tmp220 + _tmp190 * _tmp221 + _tmp193 * _tmp222; + _hessian(8, 2) = _tmp188 * _tmp225 + _tmp190 * _tmp226 + _tmp193 * _tmp227; + _hessian(9, 2) = _tmp235 * _tmp44 + _tmp238 * _tmp67; + _hessian(10, 2) = _tmp235 * _tmp54 + _tmp238 * _tmp61; + _hessian(11, 2) = _tmp235 * _tmp49 + _tmp238 * _tmp64; + _hessian(3, 3) = std::pow(_tmp152, Scalar(2)) * _tmp228 + + std::pow(_tmp177, Scalar(2)) * _tmp229 + + std::pow(_tmp201, Scalar(2)) * _tmp234; + _hessian(4, 3) = _tmp152 * _tmp240 + _tmp175 * _tmp239 + _tmp199 * _tmp241; + _hessian(5, 3) = _tmp152 * _tmp242 + _tmp177 * _tmp232 + _tmp198 * _tmp241; + _hessian(9, 3) = _tmp152 * _tmp243 + _tmp239 * _tmp75 + _tmp241 * _tmp44; + _hessian(10, 3) = _tmp152 * _tmp244 + _tmp239 * _tmp73 + _tmp241 * _tmp54; + _hessian(11, 3) = _tmp152 * _tmp228 * _tmp64 + _tmp177 * _tmp233 + _tmp241 * _tmp49; + _hessian(4, 4) = std::pow(_tmp175, Scalar(2)) * _tmp229 + + std::pow(_tmp199, Scalar(2)) * _tmp234 + + std::pow(_tmp203, Scalar(2)) * _tmp228; + _hessian(5, 4) = _tmp150 * _tmp240 + _tmp175 * _tmp232 + _tmp198 * _tmp245; + _hessian(9, 4) = _tmp199 * _tmp247 + _tmp240 * _tmp67 + _tmp246 * _tmp75; + _hessian(10, 4) = _tmp240 * _tmp61 + _tmp245 * _tmp54 + _tmp246 * _tmp73; + _hessian(11, 4) = _tmp175 * _tmp233 + _tmp240 * _tmp64 + _tmp245 * _tmp49; + _hessian(5, 5) = std::pow(_tmp150, Scalar(2)) * _tmp228 + + std::pow(_tmp198, Scalar(2)) * _tmp234 + + std::pow(_tmp204, Scalar(2)) * _tmp229; + _hessian(9, 5) = _tmp150 * _tmp243 + _tmp198 * _tmp247 + _tmp232 * _tmp75; + _hessian(10, 5) = _tmp150 * _tmp244 + _tmp198 * _tmp248 + _tmp232 * _tmp73; + _hessian(11, 5) = _tmp198 * _tmp234 * _tmp49 + _tmp232 * _tmp71 + _tmp242 * _tmp64; + _hessian(6, 6) = + std::pow(_tmp211, Scalar(2)) + std::pow(_tmp213, Scalar(2)) + std::pow(_tmp216, Scalar(2)); + _hessian(7, 6) = _tmp211 * _tmp220 + _tmp213 * _tmp221 + _tmp216 * _tmp222; + _hessian(8, 6) = _tmp211 * _tmp225 + _tmp213 * _tmp226 + _tmp216 * _tmp227; + _hessian(7, 7) = + std::pow(_tmp220, Scalar(2)) + std::pow(_tmp221, Scalar(2)) + std::pow(_tmp222, Scalar(2)); + _hessian(8, 7) = _tmp220 * _tmp225 + _tmp221 * _tmp226 + _tmp222 * _tmp227; + _hessian(8, 8) = + std::pow(_tmp225, Scalar(2)) + std::pow(_tmp226, Scalar(2)) + std::pow(_tmp227, Scalar(2)); + _hessian(9, 9) = _tmp228 * std::pow(_tmp67, Scalar(2)) + _tmp229 * std::pow(_tmp75, Scalar(2)) + + _tmp234 * std::pow(_tmp44, Scalar(2)); + _hessian(10, 9) = _tmp229 * _tmp73 * _tmp75 + _tmp244 * _tmp67 + _tmp247 * _tmp54; + _hessian(11, 9) = _tmp233 * _tmp75 + _tmp243 * _tmp64 + _tmp247 * _tmp49; + _hessian(10, 10) = _tmp228 * std::pow(_tmp61, Scalar(2)) + + _tmp229 * std::pow(_tmp73, Scalar(2)) + + _tmp234 * std::pow(_tmp54, Scalar(2)); + _hessian(11, 10) = _tmp233 * _tmp73 + _tmp244 * _tmp64 + _tmp248 * _tmp49; + _hessian(11, 11) = _tmp228 * std::pow(_tmp64, Scalar(2)) + + _tmp229 * std::pow(_tmp71, Scalar(2)) + + _tmp234 * std::pow(_tmp49, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp122 * _tmp32 + _tmp131 * _tmp34 + _tmp139 * _tmp37 + _tmp144 * _tmp250 + + _tmp153 * _tmp249; + _rhs(1, 0) = _tmp163 * _tmp32 + _tmp167 * _tmp34 + _tmp172 * _tmp37 + _tmp178 * _tmp251 + + _tmp181 * _tmp249; + _rhs(2, 0) = _tmp188 * _tmp32 + _tmp190 * _tmp34 + _tmp193 * _tmp37 + _tmp200 * _tmp250 + + _tmp235 * _tmp57; + _rhs(3, 0) = _tmp152 * _tmp250 + _tmp177 * _tmp249 + _tmp201 * _tmp251; + _rhs(4, 0) = _tmp175 * _tmp249 + _tmp199 * _tmp251 + _tmp203 * _tmp250; + _rhs(5, 0) = _tmp150 * _tmp250 + _tmp198 * _tmp251 + _tmp232 * _tmp78; + _rhs(6, 0) = _tmp211 * _tmp32 + _tmp213 * _tmp34 + _tmp216 * _tmp37; + _rhs(7, 0) = _tmp220 * _tmp32 + _tmp221 * _tmp34 + _tmp222 * _tmp37; + _rhs(8, 0) = _tmp225 * _tmp32 + _tmp226 * _tmp34 + _tmp227 * _tmp37; + _rhs(9, 0) = _tmp249 * _tmp75 + _tmp250 * _tmp67 + _tmp251 * _tmp44; + _rhs(10, 0) = _tmp249 * _tmp73 + _tmp250 * _tmp61 + _tmp251 * _tmp54; + _rhs(11, 0) = _tmp233 * _tmp78 + _tmp250 * _tmp64 + _tmp251 * _tmp49; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_isotropic.h b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_isotropic.h new file mode 100644 index 000000000..28cf4b4fd --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_isotropic.h @@ -0,0 +1,516 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b, + const sym::Pose3& a_T_b, const Scalar sqrt_info, + 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: 915 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (241) + const Scalar _tmp0 = _a[0] * _b[1]; + const Scalar _tmp1 = _a[2] * _b[3]; + const Scalar _tmp2 = _a[3] * _b[2]; + const Scalar _tmp3 = _a[1] * _b[0]; + const Scalar _tmp4 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a[2] * _b[1]; + const Scalar _tmp6 = _a[0] * _b[3]; + const Scalar _tmp7 = _a[1] * _b[2]; + const Scalar _tmp8 = _a[3] * _b[0]; + const Scalar _tmp9 = _tmp5 - _tmp6 - _tmp7 + _tmp8; + const Scalar _tmp10 = _a[3] * _b[1]; + const Scalar _tmp11 = _a[1] * _b[3]; + const Scalar _tmp12 = _a[0] * _b[2]; + const Scalar _tmp13 = _a[2] * _b[0]; + const Scalar _tmp14 = _tmp10 - _tmp11 + _tmp12 - _tmp13; + const Scalar _tmp15 = _a[1] * _b[1]; + const Scalar _tmp16 = _a[2] * _b[2]; + const Scalar _tmp17 = _a[0] * _b[0]; + const Scalar _tmp18 = _a[3] * _b[3]; + const Scalar _tmp19 = _tmp15 + _tmp16 + _tmp17 + _tmp18; + const Scalar _tmp20 = + -_a_T_b[0] * _tmp19 - _a_T_b[1] * _tmp4 + _a_T_b[2] * _tmp14 + _a_T_b[3] * _tmp9; + const Scalar _tmp21 = _a_T_b[2] * _tmp4; + const Scalar _tmp22 = _a_T_b[0] * _tmp9; + const Scalar _tmp23 = _a_T_b[1] * _tmp14; + const Scalar _tmp24 = -_tmp21 - _tmp22 - _tmp23; + const Scalar _tmp25 = _a_T_b[3] * _tmp19; + const Scalar _tmp26 = 1 - epsilon; + const Scalar _tmp27 = std::min(_tmp26, std::fabs(_tmp24 - _tmp25)); + const Scalar _tmp28 = + sqrt_info * + (2 * std::min(0, (((-_tmp24 + _tmp25) > 0) - ((-_tmp24 + _tmp25) < 0))) + 1); + const Scalar _tmp29 = 2 * _tmp28; + const Scalar _tmp30 = + _tmp29 * std::acos(_tmp27) / std::sqrt(Scalar(1 - std::pow(_tmp27, Scalar(2)))); + const Scalar _tmp31 = _tmp20 * _tmp30; + const Scalar _tmp32 = + _a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp19 - _a_T_b[2] * _tmp9 + _a_T_b[3] * _tmp14; + const Scalar _tmp33 = _tmp30 * _tmp32; + const Scalar _tmp34 = + -_a_T_b[0] * _tmp14 + _a_T_b[1] * _tmp9 - _a_T_b[2] * _tmp19 + _a_T_b[3] * _tmp4; + const Scalar _tmp35 = _tmp30 * _tmp34; + const Scalar _tmp36 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp37 = 2 * _tmp36; + const Scalar _tmp38 = -_tmp37; + const Scalar _tmp39 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp40 = 2 * _tmp39; + const Scalar _tmp41 = -_tmp40; + const Scalar _tmp42 = _tmp38 + _tmp41 + 1; + const Scalar _tmp43 = 2 * _a[0]; + const Scalar _tmp44 = _a[2] * _tmp43; + const Scalar _tmp45 = 2 * _a[1] * _a[3]; + const Scalar _tmp46 = -_tmp45; + const Scalar _tmp47 = _tmp44 + _tmp46; + const Scalar _tmp48 = _a[6] * _tmp47; + const Scalar _tmp49 = _a[1] * _tmp43; + const Scalar _tmp50 = 2 * _a[2]; + const Scalar _tmp51 = _a[3] * _tmp50; + const Scalar _tmp52 = _tmp49 + _tmp51; + const Scalar _tmp53 = _a[5] * _tmp52; + const Scalar _tmp54 = _b[5] * _tmp52 + _b[6] * _tmp47; + const Scalar _tmp55 = -_a[4] * _tmp42 - _a_T_b[4] + _b[4] * _tmp42 - _tmp48 - _tmp53 + _tmp54; + const Scalar _tmp56 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp57 = 2 * _tmp56; + const Scalar _tmp58 = 1 - _tmp57; + const Scalar _tmp59 = _tmp38 + _tmp58; + const Scalar _tmp60 = _a[1] * _tmp50; + const Scalar _tmp61 = _a[3] * _tmp43; + const Scalar _tmp62 = _tmp60 + _tmp61; + const Scalar _tmp63 = _a[6] * _tmp62; + const Scalar _tmp64 = -_tmp51; + const Scalar _tmp65 = _tmp49 + _tmp64; + const Scalar _tmp66 = _a[4] * _tmp65; + const Scalar _tmp67 = _b[4] * _tmp65 + _b[6] * _tmp62; + const Scalar _tmp68 = -_a[5] * _tmp59 - _a_T_b[5] + _b[5] * _tmp59 - _tmp63 - _tmp66 + _tmp67; + const Scalar _tmp69 = _tmp41 + _tmp58; + const Scalar _tmp70 = -_tmp61; + const Scalar _tmp71 = _tmp60 + _tmp70; + const Scalar _tmp72 = _a[5] * _tmp71; + const Scalar _tmp73 = _tmp44 + _tmp45; + const Scalar _tmp74 = _a[4] * _tmp73; + const Scalar _tmp75 = _b[4] * _tmp73 + _b[5] * _tmp71; + const Scalar _tmp76 = -_a[6] * _tmp69 - _a_T_b[6] + _b[6] * _tmp69 - _tmp72 - _tmp74 + _tmp75; + const Scalar _tmp77 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp78 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp79 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp80 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp81 = _tmp77 - _tmp78 - _tmp79 + _tmp80; + const Scalar _tmp82 = _a_T_b[3] * _tmp81; + const Scalar _tmp83 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp84 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp85 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp86 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp87 = -_tmp83 - _tmp84 - _tmp85 - _tmp86; + const Scalar _tmp88 = _a_T_b[0] * _tmp87; + const Scalar _tmp89 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp90 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp91 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp92 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp93 = -_tmp89 - _tmp90 + _tmp91 + _tmp92; + const Scalar _tmp94 = _a_T_b[1] * _tmp93; + const Scalar _tmp95 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp96 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp97 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp98 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp99 = -_tmp95 + _tmp96 - _tmp97 + _tmp98; + const Scalar _tmp100 = _a_T_b[2] * _tmp99; + const Scalar _tmp101 = _tmp100 + _tmp94; + const Scalar _tmp102 = _tmp101 + _tmp82 + _tmp88; + const Scalar _tmp103 = _tmp21 + _tmp22 + _tmp23 + _tmp25; + const Scalar _tmp104 = std::fabs(_tmp103); + const Scalar _tmp105 = std::min(_tmp104, _tmp26); + const Scalar _tmp106 = 1 - std::pow(_tmp105, Scalar(2)); + const Scalar _tmp107 = _tmp28 * ((((-_tmp104 + _tmp26) > 0) - ((-_tmp104 + _tmp26) < 0)) + 1) * + (((_tmp103) > 0) - ((_tmp103) < 0)); + const Scalar _tmp108 = _tmp107 / _tmp106; + const Scalar _tmp109 = _tmp108 * _tmp20; + const Scalar _tmp110 = std::acos(_tmp105); + const Scalar _tmp111 = _tmp105 * _tmp107 * _tmp110 / (_tmp106 * std::sqrt(_tmp106)); + const Scalar _tmp112 = _tmp111 * _tmp20; + const Scalar _tmp113 = _a_T_b[2] * _tmp93; + const Scalar _tmp114 = _a_T_b[3] * _tmp87; + const Scalar _tmp115 = -_a_T_b[1] * _tmp99; + const Scalar _tmp116 = _a_T_b[0] * _tmp81; + const Scalar _tmp117 = _tmp110 * _tmp29 / std::sqrt(_tmp106); + const Scalar _tmp118 = + -_tmp102 * _tmp109 + _tmp102 * _tmp112 + _tmp117 * (_tmp113 + _tmp114 + _tmp115 - _tmp116); + const Scalar _tmp119 = -_a_T_b[1] * _tmp81; + const Scalar _tmp120 = _a_T_b[2] * _tmp87; + const Scalar _tmp121 = _a_T_b[3] * _tmp93; + const Scalar _tmp122 = _a_T_b[0] * _tmp99; + const Scalar _tmp123 = _tmp121 + _tmp122; + const Scalar _tmp124 = _tmp108 * _tmp32; + const Scalar _tmp125 = _tmp111 * _tmp32; + const Scalar _tmp126 = + -_tmp102 * _tmp124 + _tmp102 * _tmp125 + _tmp117 * (_tmp119 - _tmp120 + _tmp123); + const Scalar _tmp127 = _tmp111 * _tmp34; + const Scalar _tmp128 = -_a_T_b[0] * _tmp93; + const Scalar _tmp129 = _a_T_b[1] * _tmp87; + const Scalar _tmp130 = _a_T_b[3] * _tmp99; + const Scalar _tmp131 = _a_T_b[2] * _tmp81; + const Scalar _tmp132 = _tmp108 * _tmp34; + const Scalar _tmp133 = + _tmp102 * _tmp127 - _tmp102 * _tmp132 + _tmp117 * (_tmp128 + _tmp129 + _tmp130 - _tmp131); + const Scalar _tmp134 = -_tmp56; + const Scalar _tmp135 = -_tmp39; + const Scalar _tmp136 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp137 = _tmp134 + _tmp135 + _tmp136 + _tmp36; + const Scalar _tmp138 = -_a[6] * _tmp137 + _b[6] * _tmp137 - _tmp72 - _tmp74 + _tmp75; + const Scalar _tmp139 = _tmp135 + _tmp56; + const Scalar _tmp140 = -_tmp136; + const Scalar _tmp141 = _tmp140 + _tmp36; + const Scalar _tmp142 = _tmp139 + _tmp141; + const Scalar _tmp143 = -_tmp60; + const Scalar _tmp144 = _tmp143 + _tmp70; + const Scalar _tmp145 = -_tmp49; + const Scalar _tmp146 = _tmp145 + _tmp51; + const Scalar _tmp147 = -_a[4] * _tmp146 - _a[5] * _tmp142 - _a[6] * _tmp144 + _b[4] * _tmp146 + + _b[5] * _tmp142 + _b[6] * _tmp144; + const Scalar _tmp148 = _tmp95 - _tmp96 + _tmp97 - _tmp98; + const Scalar _tmp149 = _a_T_b[3] * _tmp148; + const Scalar _tmp150 = _tmp89 + _tmp90 - _tmp91 - _tmp92; + const Scalar _tmp151 = _a_T_b[0] * _tmp150; + const Scalar _tmp152 = _tmp131 + _tmp151; + const Scalar _tmp153 = _tmp129 + _tmp149 + _tmp152; + const Scalar _tmp154 = _a_T_b[0] * _tmp148; + const Scalar _tmp155 = _a_T_b[3] * _tmp150; + const Scalar _tmp156 = _tmp119 + _tmp155; + const Scalar _tmp157 = + -_tmp109 * _tmp153 + _tmp112 * _tmp153 + _tmp117 * (_tmp120 - _tmp154 + _tmp156); + const Scalar _tmp158 = _a_T_b[1] * _tmp148; + const Scalar _tmp159 = -_a_T_b[2] * _tmp150; + const Scalar _tmp160 = _tmp116 + _tmp159; + const Scalar _tmp161 = + _tmp117 * (_tmp114 - _tmp158 + _tmp160) - _tmp124 * _tmp153 + _tmp125 * _tmp153; + const Scalar _tmp162 = -_a_T_b[2] * _tmp148; + const Scalar _tmp163 = _a_T_b[1] * _tmp150; + const Scalar _tmp164 = _tmp163 + _tmp82; + const Scalar _tmp165 = + _tmp117 * (_tmp162 + _tmp164 - _tmp88) + _tmp127 * _tmp153 - _tmp132 * _tmp153; + const Scalar _tmp166 = -_tmp36; + const Scalar _tmp167 = _tmp140 + _tmp166 + _tmp39 + _tmp56; + const Scalar _tmp168 = _tmp143 + _tmp61; + const Scalar _tmp169 = -_tmp44; + const Scalar _tmp170 = _tmp169 + _tmp46; + const Scalar _tmp171 = -_a[4] * _tmp170 - _a[5] * _tmp168 - _a[6] * _tmp167 + _b[4] * _tmp170 + + _b[5] * _tmp168 + _b[6] * _tmp167; + const Scalar _tmp172 = _tmp136 + _tmp166; + const Scalar _tmp173 = _tmp139 + _tmp172; + const Scalar _tmp174 = -_a[4] * _tmp173 + _b[4] * _tmp173 - _tmp48 - _tmp53 + _tmp54; + const Scalar _tmp175 = -_tmp77 + _tmp78 + _tmp79 - _tmp80; + const Scalar _tmp176 = _a_T_b[1] * _tmp175; + const Scalar _tmp177 = _tmp154 + _tmp176; + const Scalar _tmp178 = _tmp120 + _tmp121 + _tmp177; + const Scalar _tmp179 = _a_T_b[2] * _tmp175; + const Scalar _tmp180 = _tmp149 + _tmp179; + const Scalar _tmp181 = + -_tmp109 * _tmp178 + _tmp112 * _tmp178 + _tmp117 * (_tmp128 - _tmp129 + _tmp180); + const Scalar _tmp182 = _a_T_b[3] * _tmp175; + const Scalar _tmp183 = _tmp162 + _tmp182; + const Scalar _tmp184 = + _tmp117 * (_tmp183 + _tmp88 - _tmp94) - _tmp124 * _tmp178 + _tmp125 * _tmp178; + const Scalar _tmp185 = -_a_T_b[0] * _tmp175; + const Scalar _tmp186 = _tmp158 + _tmp185; + const Scalar _tmp187 = + _tmp117 * (-_tmp113 + _tmp114 + _tmp186) + _tmp127 * _tmp178 - _tmp132 * _tmp178; + const Scalar _tmp188 = _tmp134 + _tmp39; + const Scalar _tmp189 = _tmp172 + _tmp188; + const Scalar _tmp190 = -_a[5] * _tmp189 + _b[5] * _tmp189 - _tmp63 - _tmp66 + _tmp67; + const Scalar _tmp191 = _tmp141 + _tmp188; + const Scalar _tmp192 = _tmp169 + _tmp45; + const Scalar _tmp193 = _tmp145 + _tmp64; + const Scalar _tmp194 = -_a[4] * _tmp191 - _a[5] * _tmp193 - _a[6] * _tmp192 + _b[4] * _tmp191 + + _b[5] * _tmp193 + _b[6] * _tmp192; + const Scalar _tmp195 = _tmp37 + _tmp40 - 1; + const Scalar _tmp196 = _tmp57 - 1; + const Scalar _tmp197 = _tmp196 + _tmp37; + const Scalar _tmp198 = _tmp196 + _tmp40; + const Scalar _tmp199 = _tmp83 + _tmp84 + _tmp85 + _tmp86; + const Scalar _tmp200 = _a_T_b[3] * _tmp199; + const Scalar _tmp201 = _tmp115 + _tmp200; + const Scalar _tmp202 = _a_T_b[0] * _tmp199; + const Scalar _tmp203 = _tmp101 + _tmp182 + _tmp202; + const Scalar _tmp204 = + -_tmp109 * _tmp203 + _tmp112 * _tmp203 + _tmp117 * (_tmp113 + _tmp185 + _tmp201); + const Scalar _tmp205 = _a_T_b[2] * _tmp199; + const Scalar _tmp206 = + _tmp117 * (_tmp123 - _tmp176 - _tmp205) - _tmp124 * _tmp203 + _tmp125 * _tmp203; + const Scalar _tmp207 = _a_T_b[1] * _tmp199; + const Scalar _tmp208 = _tmp130 + _tmp207; + const Scalar _tmp209 = + _tmp117 * (_tmp128 - _tmp179 + _tmp208) + _tmp127 * _tmp203 - _tmp132 * _tmp203; + const Scalar _tmp210 = _tmp152 + _tmp208; + const Scalar _tmp211 = _tmp108 * _tmp210; + const Scalar _tmp212 = + _tmp112 * _tmp210 + _tmp117 * (-_tmp122 + _tmp156 + _tmp205) - _tmp20 * _tmp211; + const Scalar _tmp213 = _tmp117 * (_tmp160 + _tmp201) + _tmp125 * _tmp210 - _tmp211 * _tmp32; + const Scalar _tmp214 = + _tmp117 * (-_tmp100 + _tmp164 - _tmp202) + _tmp127 * _tmp210 - _tmp211 * _tmp34; + const Scalar _tmp215 = _tmp155 + _tmp177 + _tmp205; + const Scalar _tmp216 = + -_tmp109 * _tmp215 + _tmp112 * _tmp215 + _tmp117 * (-_tmp151 + _tmp180 - _tmp207); + const Scalar _tmp217 = + _tmp117 * (-_tmp163 + _tmp183 + _tmp202) - _tmp124 * _tmp215 + _tmp125 * _tmp215; + const Scalar _tmp218 = + _tmp117 * (_tmp159 + _tmp186 + _tmp200) + _tmp127 * _tmp215 - _tmp132 * _tmp215; + const Scalar _tmp219 = std::pow(sqrt_info, Scalar(2)); + const Scalar _tmp220 = _tmp147 * _tmp219; + const Scalar _tmp221 = _tmp138 * _tmp219; + const Scalar _tmp222 = _tmp146 * _tmp219; + const Scalar _tmp223 = _tmp168 * _tmp219; + const Scalar _tmp224 = _tmp144 * _tmp219; + const Scalar _tmp225 = _tmp219 * _tmp65; + const Scalar _tmp226 = _tmp219 * _tmp59; + const Scalar _tmp227 = _tmp219 * _tmp71; + const Scalar _tmp228 = _tmp219 * _tmp62; + const Scalar _tmp229 = _tmp190 * _tmp219; + const Scalar _tmp230 = _tmp171 * _tmp219; + const Scalar _tmp231 = _tmp174 * _tmp219; + const Scalar _tmp232 = _tmp195 * _tmp219; + const Scalar _tmp233 = _tmp219 * _tmp73; + const Scalar _tmp234 = _tmp219 * _tmp69; + const Scalar _tmp235 = _tmp192 * _tmp219; + const Scalar _tmp236 = _tmp219 * _tmp52; + const Scalar _tmp237 = _tmp219 * _tmp47; + const Scalar _tmp238 = _tmp219 * _tmp68; + const Scalar _tmp239 = _tmp219 * _tmp76; + const Scalar _tmp240 = _tmp219 * _tmp55; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp31; + _res(1, 0) = _tmp33; + _res(2, 0) = _tmp35; + _res(3, 0) = _tmp55 * sqrt_info; + _res(4, 0) = _tmp68 * sqrt_info; + _res(5, 0) = _tmp76 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp118; + _jacobian(1, 0) = _tmp126; + _jacobian(2, 0) = _tmp133; + _jacobian(3, 0) = 0; + _jacobian(4, 0) = _tmp138 * sqrt_info; + _jacobian(5, 0) = _tmp147 * sqrt_info; + _jacobian(0, 1) = _tmp157; + _jacobian(1, 1) = _tmp161; + _jacobian(2, 1) = _tmp165; + _jacobian(3, 1) = _tmp171 * sqrt_info; + _jacobian(4, 1) = 0; + _jacobian(5, 1) = _tmp174 * sqrt_info; + _jacobian(0, 2) = _tmp181; + _jacobian(1, 2) = _tmp184; + _jacobian(2, 2) = _tmp187; + _jacobian(3, 2) = _tmp190 * sqrt_info; + _jacobian(4, 2) = _tmp194 * sqrt_info; + _jacobian(5, 2) = 0; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(3, 3) = _tmp195 * sqrt_info; + _jacobian(4, 3) = _tmp146 * sqrt_info; + _jacobian(5, 3) = _tmp170 * sqrt_info; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(3, 4) = _tmp193 * sqrt_info; + _jacobian(4, 4) = _tmp197 * sqrt_info; + _jacobian(5, 4) = _tmp168 * sqrt_info; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(3, 5) = _tmp192 * sqrt_info; + _jacobian(4, 5) = _tmp144 * sqrt_info; + _jacobian(5, 5) = _tmp198 * sqrt_info; + _jacobian(0, 6) = _tmp204; + _jacobian(1, 6) = _tmp206; + _jacobian(2, 6) = _tmp209; + _jacobian(3, 6) = 0; + _jacobian(4, 6) = 0; + _jacobian(5, 6) = 0; + _jacobian(0, 7) = _tmp212; + _jacobian(1, 7) = _tmp213; + _jacobian(2, 7) = _tmp214; + _jacobian(3, 7) = 0; + _jacobian(4, 7) = 0; + _jacobian(5, 7) = 0; + _jacobian(0, 8) = _tmp216; + _jacobian(1, 8) = _tmp217; + _jacobian(2, 8) = _tmp218; + _jacobian(3, 8) = 0; + _jacobian(4, 8) = 0; + _jacobian(5, 8) = 0; + _jacobian(0, 9) = 0; + _jacobian(1, 9) = 0; + _jacobian(2, 9) = 0; + _jacobian(3, 9) = _tmp42 * sqrt_info; + _jacobian(4, 9) = _tmp65 * sqrt_info; + _jacobian(5, 9) = _tmp73 * sqrt_info; + _jacobian(0, 10) = 0; + _jacobian(1, 10) = 0; + _jacobian(2, 10) = 0; + _jacobian(3, 10) = _tmp52 * sqrt_info; + _jacobian(4, 10) = _tmp59 * sqrt_info; + _jacobian(5, 10) = _tmp71 * sqrt_info; + _jacobian(0, 11) = 0; + _jacobian(1, 11) = 0; + _jacobian(2, 11) = 0; + _jacobian(3, 11) = _tmp47 * sqrt_info; + _jacobian(4, 11) = _tmp62 * sqrt_info; + _jacobian(5, 11) = _tmp69 * sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp118, Scalar(2)) + std::pow(_tmp126, Scalar(2)) + + std::pow(_tmp133, Scalar(2)) + std::pow(_tmp138, Scalar(2)) * _tmp219 + + std::pow(_tmp147, Scalar(2)) * _tmp219; + _hessian(1, 0) = _tmp118 * _tmp157 + _tmp126 * _tmp161 + _tmp133 * _tmp165 + _tmp174 * _tmp220; + _hessian(2, 0) = _tmp118 * _tmp181 + _tmp126 * _tmp184 + _tmp133 * _tmp187 + _tmp194 * _tmp221; + _hessian(3, 0) = _tmp138 * _tmp222 + _tmp170 * _tmp220; + _hessian(4, 0) = _tmp147 * _tmp223 + _tmp197 * _tmp221; + _hessian(5, 0) = _tmp138 * _tmp224 + _tmp198 * _tmp220; + _hessian(6, 0) = _tmp118 * _tmp204 + _tmp126 * _tmp206 + _tmp133 * _tmp209; + _hessian(7, 0) = _tmp118 * _tmp212 + _tmp126 * _tmp213 + _tmp133 * _tmp214; + _hessian(8, 0) = _tmp118 * _tmp216 + _tmp126 * _tmp217 + _tmp133 * _tmp218; + _hessian(9, 0) = _tmp138 * _tmp225 + _tmp220 * _tmp73; + _hessian(10, 0) = _tmp138 * _tmp226 + _tmp147 * _tmp227; + _hessian(11, 0) = _tmp138 * _tmp228 + _tmp220 * _tmp69; + _hessian(1, 1) = std::pow(_tmp157, Scalar(2)) + std::pow(_tmp161, Scalar(2)) + + std::pow(_tmp165, Scalar(2)) + std::pow(_tmp171, Scalar(2)) * _tmp219 + + std::pow(_tmp174, Scalar(2)) * _tmp219; + _hessian(2, 1) = _tmp157 * _tmp181 + _tmp161 * _tmp184 + _tmp165 * _tmp187 + _tmp171 * _tmp229; + _hessian(3, 1) = _tmp170 * _tmp231 + _tmp195 * _tmp230; + _hessian(4, 1) = _tmp174 * _tmp223 + _tmp193 * _tmp230; + _hessian(5, 1) = _tmp192 * _tmp230 + _tmp198 * _tmp231; + _hessian(6, 1) = _tmp157 * _tmp204 + _tmp161 * _tmp206 + _tmp165 * _tmp209; + _hessian(7, 1) = _tmp157 * _tmp212 + _tmp161 * _tmp213 + _tmp165 * _tmp214; + _hessian(8, 1) = _tmp157 * _tmp216 + _tmp161 * _tmp217 + _tmp165 * _tmp218; + _hessian(9, 1) = _tmp230 * _tmp42 + _tmp231 * _tmp73; + _hessian(10, 1) = _tmp174 * _tmp227 + _tmp230 * _tmp52; + _hessian(11, 1) = _tmp230 * _tmp47 + _tmp231 * _tmp69; + _hessian(2, 2) = std::pow(_tmp181, Scalar(2)) + std::pow(_tmp184, Scalar(2)) + + std::pow(_tmp187, Scalar(2)) + std::pow(_tmp190, Scalar(2)) * _tmp219 + + std::pow(_tmp194, Scalar(2)) * _tmp219; + _hessian(3, 2) = _tmp194 * _tmp222 + _tmp195 * _tmp229; + _hessian(4, 2) = _tmp193 * _tmp229 + _tmp194 * _tmp197 * _tmp219; + _hessian(5, 2) = _tmp192 * _tmp229 + _tmp194 * _tmp224; + _hessian(6, 2) = _tmp181 * _tmp204 + _tmp184 * _tmp206 + _tmp187 * _tmp209; + _hessian(7, 2) = _tmp181 * _tmp212 + _tmp184 * _tmp213 + _tmp187 * _tmp214; + _hessian(8, 2) = _tmp181 * _tmp216 + _tmp184 * _tmp217 + _tmp187 * _tmp218; + _hessian(9, 2) = _tmp194 * _tmp225 + _tmp229 * _tmp42; + _hessian(10, 2) = _tmp194 * _tmp226 + _tmp229 * _tmp52; + _hessian(11, 2) = _tmp194 * _tmp228 + _tmp229 * _tmp47; + _hessian(3, 3) = std::pow(_tmp146, Scalar(2)) * _tmp219 + + std::pow(_tmp170, Scalar(2)) * _tmp219 + + std::pow(_tmp195, Scalar(2)) * _tmp219; + _hessian(4, 3) = _tmp170 * _tmp223 + _tmp193 * _tmp232 + _tmp197 * _tmp222; + _hessian(5, 3) = _tmp144 * _tmp222 + _tmp170 * _tmp198 * _tmp219 + _tmp192 * _tmp232; + _hessian(9, 3) = _tmp170 * _tmp233 + _tmp222 * _tmp65 + _tmp232 * _tmp42; + _hessian(10, 3) = _tmp170 * _tmp227 + _tmp222 * _tmp59 + _tmp232 * _tmp52; + _hessian(11, 3) = _tmp170 * _tmp234 + _tmp222 * _tmp62 + _tmp232 * _tmp47; + _hessian(4, 4) = std::pow(_tmp168, Scalar(2)) * _tmp219 + + std::pow(_tmp193, Scalar(2)) * _tmp219 + + std::pow(_tmp197, Scalar(2)) * _tmp219; + _hessian(5, 4) = _tmp193 * _tmp235 + _tmp197 * _tmp224 + _tmp198 * _tmp223; + _hessian(9, 4) = _tmp193 * _tmp219 * _tmp42 + _tmp197 * _tmp225 + _tmp223 * _tmp73; + _hessian(10, 4) = _tmp168 * _tmp227 + _tmp193 * _tmp236 + _tmp197 * _tmp226; + _hessian(11, 4) = _tmp193 * _tmp237 + _tmp197 * _tmp228 + _tmp223 * _tmp69; + _hessian(5, 5) = std::pow(_tmp144, Scalar(2)) * _tmp219 + + std::pow(_tmp192, Scalar(2)) * _tmp219 + + std::pow(_tmp198, Scalar(2)) * _tmp219; + _hessian(9, 5) = _tmp198 * _tmp233 + _tmp224 * _tmp65 + _tmp235 * _tmp42; + _hessian(10, 5) = _tmp192 * _tmp236 + _tmp198 * _tmp227 + _tmp224 * _tmp59; + _hessian(11, 5) = _tmp192 * _tmp237 + _tmp198 * _tmp234 + _tmp224 * _tmp62; + _hessian(6, 6) = + std::pow(_tmp204, Scalar(2)) + std::pow(_tmp206, Scalar(2)) + std::pow(_tmp209, Scalar(2)); + _hessian(7, 6) = _tmp204 * _tmp212 + _tmp206 * _tmp213 + _tmp209 * _tmp214; + _hessian(8, 6) = _tmp204 * _tmp216 + _tmp206 * _tmp217 + _tmp209 * _tmp218; + _hessian(7, 7) = + std::pow(_tmp212, Scalar(2)) + std::pow(_tmp213, Scalar(2)) + std::pow(_tmp214, Scalar(2)); + _hessian(8, 7) = _tmp212 * _tmp216 + _tmp213 * _tmp217 + _tmp214 * _tmp218; + _hessian(8, 8) = + std::pow(_tmp216, Scalar(2)) + std::pow(_tmp217, Scalar(2)) + std::pow(_tmp218, Scalar(2)); + _hessian(9, 9) = _tmp219 * std::pow(_tmp42, Scalar(2)) + _tmp219 * std::pow(_tmp65, Scalar(2)) + + _tmp219 * std::pow(_tmp73, Scalar(2)); + _hessian(10, 9) = _tmp225 * _tmp59 + _tmp227 * _tmp73 + _tmp236 * _tmp42; + _hessian(11, 9) = _tmp225 * _tmp62 + _tmp234 * _tmp73 + _tmp237 * _tmp42; + _hessian(10, 10) = _tmp219 * std::pow(_tmp52, Scalar(2)) + + _tmp219 * std::pow(_tmp59, Scalar(2)) + + _tmp219 * std::pow(_tmp71, Scalar(2)); + _hessian(11, 10) = _tmp227 * _tmp69 + _tmp228 * _tmp59 + _tmp237 * _tmp52; + _hessian(11, 11) = _tmp219 * std::pow(_tmp47, Scalar(2)) + + _tmp219 * std::pow(_tmp62, Scalar(2)) + + _tmp219 * std::pow(_tmp69, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp118 * _tmp31 + _tmp126 * _tmp33 + _tmp133 * _tmp35 + _tmp138 * _tmp238 + + _tmp220 * _tmp76; + _rhs(1, 0) = _tmp157 * _tmp31 + _tmp161 * _tmp33 + _tmp165 * _tmp35 + _tmp230 * _tmp55 + + _tmp231 * _tmp76; + _rhs(2, 0) = _tmp181 * _tmp31 + _tmp184 * _tmp33 + _tmp187 * _tmp35 + _tmp194 * _tmp238 + + _tmp229 * _tmp55; + _rhs(3, 0) = _tmp170 * _tmp239 + _tmp195 * _tmp240 + _tmp222 * _tmp68; + _rhs(4, 0) = _tmp193 * _tmp240 + _tmp197 * _tmp238 + _tmp223 * _tmp76; + _rhs(5, 0) = _tmp192 * _tmp240 + _tmp198 * _tmp239 + _tmp224 * _tmp68; + _rhs(6, 0) = _tmp204 * _tmp31 + _tmp206 * _tmp33 + _tmp209 * _tmp35; + _rhs(7, 0) = _tmp212 * _tmp31 + _tmp213 * _tmp33 + _tmp214 * _tmp35; + _rhs(8, 0) = _tmp216 * _tmp31 + _tmp217 * _tmp33 + _tmp218 * _tmp35; + _rhs(9, 0) = _tmp225 * _tmp68 + _tmp239 * _tmp73 + _tmp240 * _tmp42; + _rhs(10, 0) = _tmp227 * _tmp76 + _tmp238 * _tmp59 + _tmp240 * _tmp52; + _rhs(11, 0) = _tmp238 * _tmp62 + _tmp239 * _tmp69 + _tmp240 * _tmp47; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_square.h b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_square.h new file mode 100644 index 000000000..30f7d8a37 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3/between_factor_pose3_square.h @@ -0,0 +1,958 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b, + const sym::Pose3& a_T_b, + const Eigen::Matrix& sqrt_info, 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: 2386 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (369) + const Scalar _tmp0 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = -_tmp1; + const Scalar _tmp3 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = 1 - _tmp4; + const Scalar _tmp6 = _tmp2 + _tmp5; + const Scalar _tmp7 = 2 * _a[1]; + const Scalar _tmp8 = _a[2] * _tmp7; + const Scalar _tmp9 = 2 * _a[3]; + const Scalar _tmp10 = _a[0] * _tmp9; + const Scalar _tmp11 = _tmp10 + _tmp8; + const Scalar _tmp12 = _a[6] * _tmp11; + const Scalar _tmp13 = _a[0] * _tmp7; + const Scalar _tmp14 = _a[2] * _tmp9; + const Scalar _tmp15 = -_tmp14; + const Scalar _tmp16 = _tmp13 + _tmp15; + const Scalar _tmp17 = _a[4] * _tmp16; + const Scalar _tmp18 = _b[4] * _tmp16 + _b[6] * _tmp11; + const Scalar _tmp19 = -_a[5] * _tmp6 - _a_T_b[5] + _b[5] * _tmp6 - _tmp12 - _tmp17 + _tmp18; + const Scalar _tmp20 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp21 = 2 * _tmp20; + const Scalar _tmp22 = -_tmp21; + const Scalar _tmp23 = _tmp22 + _tmp5; + const Scalar _tmp24 = 2 * _a[0] * _a[2]; + const Scalar _tmp25 = _a[3] * _tmp7; + const Scalar _tmp26 = -_tmp25; + const Scalar _tmp27 = _tmp24 + _tmp26; + const Scalar _tmp28 = _a[6] * _tmp27; + const Scalar _tmp29 = _tmp13 + _tmp14; + const Scalar _tmp30 = _a[5] * _tmp29; + const Scalar _tmp31 = _b[5] * _tmp29 + _b[6] * _tmp27; + const Scalar _tmp32 = -_a[4] * _tmp23 - _a_T_b[4] + _b[4] * _tmp23 - _tmp28 - _tmp30 + _tmp31; + const Scalar _tmp33 = _a[0] * _b[1]; + const Scalar _tmp34 = _a[2] * _b[3]; + const Scalar _tmp35 = _a[3] * _b[2]; + const Scalar _tmp36 = _a[1] * _b[0]; + const Scalar _tmp37 = -_tmp33 - _tmp34 + _tmp35 + _tmp36; + const Scalar _tmp38 = _a[2] * _b[1]; + const Scalar _tmp39 = _a[0] * _b[3]; + const Scalar _tmp40 = _a[1] * _b[2]; + const Scalar _tmp41 = _a[3] * _b[0]; + const Scalar _tmp42 = _tmp38 - _tmp39 - _tmp40 + _tmp41; + const Scalar _tmp43 = _a[3] * _b[1]; + const Scalar _tmp44 = _a[1] * _b[3]; + const Scalar _tmp45 = _a[0] * _b[2]; + const Scalar _tmp46 = _a[2] * _b[0]; + const Scalar _tmp47 = _tmp43 - _tmp44 + _tmp45 - _tmp46; + const Scalar _tmp48 = _a[1] * _b[1]; + const Scalar _tmp49 = _a[2] * _b[2]; + const Scalar _tmp50 = _a[0] * _b[0]; + const Scalar _tmp51 = _a[3] * _b[3]; + const Scalar _tmp52 = _tmp48 + _tmp49 + _tmp50 + _tmp51; + const Scalar _tmp53 = + -_a_T_b[0] * _tmp47 + _a_T_b[1] * _tmp42 - _a_T_b[2] * _tmp52 + _a_T_b[3] * _tmp37; + const Scalar _tmp54 = _a_T_b[2] * _tmp37; + const Scalar _tmp55 = _a_T_b[0] * _tmp42; + const Scalar _tmp56 = _a_T_b[1] * _tmp47; + const Scalar _tmp57 = -_tmp54 - _tmp55 - _tmp56; + const Scalar _tmp58 = _a_T_b[3] * _tmp52; + const Scalar _tmp59 = + 2 * std::min(0, (((-_tmp57 + _tmp58) > 0) - ((-_tmp57 + _tmp58) < 0))) + 1; + const Scalar _tmp60 = 2 * _tmp59; + const Scalar _tmp61 = 1 - epsilon; + const Scalar _tmp62 = std::min(_tmp61, std::fabs(_tmp57 - _tmp58)); + const Scalar _tmp63 = std::acos(_tmp62) / std::sqrt(Scalar(1 - std::pow(_tmp62, Scalar(2)))); + const Scalar _tmp64 = _tmp60 * _tmp63; + const Scalar _tmp65 = _tmp53 * _tmp64; + const Scalar _tmp66 = + -_a_T_b[0] * _tmp52 - _a_T_b[1] * _tmp37 + _a_T_b[2] * _tmp47 + _a_T_b[3] * _tmp42; + const Scalar _tmp67 = _tmp66 * sqrt_info(0, 0); + const Scalar _tmp68 = + _a_T_b[0] * _tmp37 - _a_T_b[1] * _tmp52 - _a_T_b[2] * _tmp42 + _a_T_b[3] * _tmp47; + const Scalar _tmp69 = _tmp63 * _tmp68; + const Scalar _tmp70 = _tmp60 * _tmp69; + const Scalar _tmp71 = _tmp2 + _tmp22 + 1; + const Scalar _tmp72 = -_tmp10; + const Scalar _tmp73 = _tmp72 + _tmp8; + const Scalar _tmp74 = _a[5] * _tmp73; + const Scalar _tmp75 = _tmp24 + _tmp25; + const Scalar _tmp76 = _a[4] * _tmp75; + const Scalar _tmp77 = _b[4] * _tmp75 + _b[5] * _tmp73; + const Scalar _tmp78 = -_a[6] * _tmp71 - _a_T_b[6] + _b[6] * _tmp71 - _tmp74 - _tmp76 + _tmp77; + const Scalar _tmp79 = _tmp19 * sqrt_info(0, 4) + _tmp32 * sqrt_info(0, 3) + _tmp64 * _tmp67 + + _tmp65 * sqrt_info(0, 2) + _tmp70 * sqrt_info(0, 1) + + _tmp78 * sqrt_info(0, 5); + const Scalar _tmp80 = _tmp64 * _tmp66; + const Scalar _tmp81 = _tmp60 * sqrt_info(1, 1); + const Scalar _tmp82 = _tmp19 * sqrt_info(1, 4) + _tmp32 * sqrt_info(1, 3) + + _tmp65 * sqrt_info(1, 2) + _tmp69 * _tmp81 + _tmp78 * sqrt_info(1, 5) + + _tmp80 * sqrt_info(1, 0); + const Scalar _tmp83 = _tmp19 * sqrt_info(2, 4) + _tmp32 * sqrt_info(2, 3) + + _tmp65 * sqrt_info(2, 2) + _tmp70 * sqrt_info(2, 1) + + _tmp78 * sqrt_info(2, 5) + _tmp80 * sqrt_info(2, 0); + const Scalar _tmp84 = _tmp19 * sqrt_info(3, 4) + _tmp32 * sqrt_info(3, 3) + + _tmp65 * sqrt_info(3, 2) + _tmp70 * sqrt_info(3, 1) + + _tmp78 * sqrt_info(3, 5) + _tmp80 * sqrt_info(3, 0); + const Scalar _tmp85 = _tmp19 * sqrt_info(4, 4) + _tmp32 * sqrt_info(4, 3) + + _tmp65 * sqrt_info(4, 2) + _tmp70 * sqrt_info(4, 1) + + _tmp78 * sqrt_info(4, 5) + _tmp80 * sqrt_info(4, 0); + const Scalar _tmp86 = _tmp60 * sqrt_info(5, 1); + const Scalar _tmp87 = _tmp19 * sqrt_info(5, 4) + _tmp32 * sqrt_info(5, 3) + + _tmp65 * sqrt_info(5, 2) + _tmp69 * _tmp86 + _tmp78 * sqrt_info(5, 5) + + _tmp80 * sqrt_info(5, 0); + const Scalar _tmp88 = (Scalar(1) / Scalar(2)) * _tmp48; + const Scalar _tmp89 = (Scalar(1) / Scalar(2)) * _tmp51; + const Scalar _tmp90 = (Scalar(1) / Scalar(2)) * _tmp49; + const Scalar _tmp91 = (Scalar(1) / Scalar(2)) * _tmp50; + const Scalar _tmp92 = -_tmp88 - _tmp89 - _tmp90 - _tmp91; + const Scalar _tmp93 = _a_T_b[0] * _tmp92; + const Scalar _tmp94 = (Scalar(1) / Scalar(2)) * _tmp38; + const Scalar _tmp95 = (Scalar(1) / Scalar(2)) * _tmp39; + const Scalar _tmp96 = (Scalar(1) / Scalar(2)) * _tmp40; + const Scalar _tmp97 = (Scalar(1) / Scalar(2)) * _tmp41; + const Scalar _tmp98 = _tmp94 - _tmp95 - _tmp96 + _tmp97; + const Scalar _tmp99 = _a_T_b[3] * _tmp98; + const Scalar _tmp100 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp101 = (Scalar(1) / Scalar(2)) * _tmp34; + const Scalar _tmp102 = (Scalar(1) / Scalar(2)) * _tmp35; + const Scalar _tmp103 = (Scalar(1) / Scalar(2)) * _tmp36; + const Scalar _tmp104 = -_tmp100 - _tmp101 + _tmp102 + _tmp103; + const Scalar _tmp105 = _a_T_b[1] * _tmp104; + const Scalar _tmp106 = (Scalar(1) / Scalar(2)) * _tmp43; + const Scalar _tmp107 = (Scalar(1) / Scalar(2)) * _tmp44; + const Scalar _tmp108 = (Scalar(1) / Scalar(2)) * _tmp45; + const Scalar _tmp109 = (Scalar(1) / Scalar(2)) * _tmp46; + const Scalar _tmp110 = -_tmp106 + _tmp107 - _tmp108 + _tmp109; + const Scalar _tmp111 = _a_T_b[2] * _tmp110; + const Scalar _tmp112 = _tmp105 + _tmp111; + const Scalar _tmp113 = _tmp112 + _tmp93 + _tmp99; + const Scalar _tmp114 = _tmp54 + _tmp55 + _tmp56 + _tmp58; + const Scalar _tmp115 = std::fabs(_tmp114); + const Scalar _tmp116 = std::min(_tmp115, _tmp61); + const Scalar _tmp117 = 1 - std::pow(_tmp116, Scalar(2)); + const Scalar _tmp118 = _tmp59 * ((((-_tmp115 + _tmp61) > 0) - ((-_tmp115 + _tmp61) < 0)) + 1) * + (((_tmp114) > 0) - ((_tmp114) < 0)); + const Scalar _tmp119 = _tmp118 / _tmp117; + const Scalar _tmp120 = _tmp119 * _tmp68; + const Scalar _tmp121 = _tmp113 * _tmp120; + const Scalar _tmp122 = -_a_T_b[0] * _tmp104; + const Scalar _tmp123 = _a_T_b[1] * _tmp92; + const Scalar _tmp124 = _a_T_b[3] * _tmp110; + const Scalar _tmp125 = _a_T_b[2] * _tmp98; + const Scalar _tmp126 = _tmp122 + _tmp123 + _tmp124 - _tmp125; + const Scalar _tmp127 = std::acos(_tmp116); + const Scalar _tmp128 = _tmp127 / std::sqrt(_tmp117); + const Scalar _tmp129 = _tmp128 * _tmp60; + const Scalar _tmp130 = _tmp126 * _tmp129; + const Scalar _tmp131 = _tmp119 * _tmp67; + const Scalar _tmp132 = _tmp116 * _tmp118 * _tmp127 / (_tmp117 * std::sqrt(_tmp117)); + const Scalar _tmp133 = _tmp113 * _tmp132; + const Scalar _tmp134 = _tmp133 * _tmp53; + const Scalar _tmp135 = _a_T_b[3] * _tmp92; + const Scalar _tmp136 = _a_T_b[0] * _tmp98; + const Scalar _tmp137 = _a_T_b[2] * _tmp104; + const Scalar _tmp138 = -_a_T_b[1] * _tmp110; + const Scalar _tmp139 = _tmp137 + _tmp138; + const Scalar _tmp140 = _tmp135 - _tmp136 + _tmp139; + const Scalar _tmp141 = _tmp129 * sqrt_info(0, 0); + const Scalar _tmp142 = _tmp68 * sqrt_info(0, 1); + const Scalar _tmp143 = -_a_T_b[1] * _tmp98; + const Scalar _tmp144 = _a_T_b[2] * _tmp92; + const Scalar _tmp145 = _a_T_b[3] * _tmp104; + const Scalar _tmp146 = _a_T_b[0] * _tmp110; + const Scalar _tmp147 = _tmp145 + _tmp146; + const Scalar _tmp148 = _tmp128 * (_tmp143 - _tmp144 + _tmp147); + const Scalar _tmp149 = _tmp148 * _tmp60; + const Scalar _tmp150 = -_tmp20; + const Scalar _tmp151 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp152 = _tmp150 + _tmp151; + const Scalar _tmp153 = -_tmp0; + const Scalar _tmp154 = _tmp153 + _tmp3; + const Scalar _tmp155 = _tmp152 + _tmp154; + const Scalar _tmp156 = -_a[6] * _tmp155 + _b[6] * _tmp155 - _tmp74 - _tmp76 + _tmp77; + const Scalar _tmp157 = _tmp119 * _tmp53; + const Scalar _tmp158 = _tmp113 * _tmp157; + const Scalar _tmp159 = -_tmp151; + const Scalar _tmp160 = _tmp0 + _tmp150 + _tmp159 + _tmp3; + const Scalar _tmp161 = -_tmp8; + const Scalar _tmp162 = _tmp161 + _tmp72; + const Scalar _tmp163 = -_tmp13; + const Scalar _tmp164 = _tmp14 + _tmp163; + const Scalar _tmp165 = -_a[4] * _tmp164 - _a[5] * _tmp160 - _a[6] * _tmp162 + _b[4] * _tmp164 + + _b[5] * _tmp160 + _b[6] * _tmp162; + const Scalar _tmp166 = -_tmp113 * _tmp131 - _tmp121 * sqrt_info(0, 1) + + _tmp130 * sqrt_info(0, 2) + _tmp133 * _tmp142 + _tmp133 * _tmp67 + + _tmp134 * sqrt_info(0, 2) + _tmp140 * _tmp141 + _tmp149 * sqrt_info(0, 1) + + _tmp156 * sqrt_info(0, 4) - _tmp158 * sqrt_info(0, 2) + + _tmp165 * sqrt_info(0, 5); + const Scalar _tmp167 = _tmp129 * sqrt_info(1, 2); + const Scalar _tmp168 = _tmp119 * _tmp66; + const Scalar _tmp169 = _tmp113 * _tmp168; + const Scalar _tmp170 = _tmp53 * sqrt_info(1, 2); + const Scalar _tmp171 = _tmp129 * _tmp140; + const Scalar _tmp172 = _tmp133 * _tmp68; + const Scalar _tmp173 = _tmp133 * _tmp66; + const Scalar _tmp174 = -_tmp121 * sqrt_info(1, 1) + _tmp126 * _tmp167 + _tmp133 * _tmp170 + + _tmp148 * _tmp81 + _tmp156 * sqrt_info(1, 4) - _tmp158 * sqrt_info(1, 2) + + _tmp165 * sqrt_info(1, 5) - _tmp169 * sqrt_info(1, 0) + + _tmp171 * sqrt_info(1, 0) + _tmp172 * sqrt_info(1, 1) + + _tmp173 * sqrt_info(1, 0); + const Scalar _tmp175 = _tmp120 * sqrt_info(2, 1); + const Scalar _tmp176 = + -_tmp113 * _tmp175 + _tmp130 * sqrt_info(2, 2) + _tmp134 * sqrt_info(2, 2) + + _tmp149 * sqrt_info(2, 1) + _tmp156 * sqrt_info(2, 4) - _tmp158 * sqrt_info(2, 2) + + _tmp165 * sqrt_info(2, 5) - _tmp169 * sqrt_info(2, 0) + _tmp171 * sqrt_info(2, 0) + + _tmp172 * sqrt_info(2, 1) + _tmp173 * sqrt_info(2, 0); + const Scalar _tmp177 = _tmp68 * sqrt_info(3, 1); + const Scalar _tmp178 = -_tmp121 * sqrt_info(3, 1) + _tmp130 * sqrt_info(3, 2) + + _tmp133 * _tmp177 + _tmp134 * sqrt_info(3, 2) + _tmp149 * sqrt_info(3, 1) + + _tmp156 * sqrt_info(3, 4) - _tmp158 * sqrt_info(3, 2) + + _tmp165 * sqrt_info(3, 5) - _tmp169 * sqrt_info(3, 0) + + _tmp171 * sqrt_info(3, 0) + _tmp173 * sqrt_info(3, 0); + const Scalar _tmp179 = _tmp53 * sqrt_info(4, 2); + const Scalar _tmp180 = -_tmp121 * sqrt_info(4, 1) + _tmp130 * sqrt_info(4, 2) + + _tmp133 * _tmp179 + _tmp149 * sqrt_info(4, 1) + _tmp156 * sqrt_info(4, 4) - + _tmp158 * sqrt_info(4, 2) + _tmp165 * sqrt_info(4, 5) - + _tmp169 * sqrt_info(4, 0) + _tmp171 * sqrt_info(4, 0) + + _tmp172 * sqrt_info(4, 1) + _tmp173 * sqrt_info(4, 0); + const Scalar _tmp181 = _tmp53 * sqrt_info(5, 2); + const Scalar _tmp182 = -_tmp121 * sqrt_info(5, 1) + _tmp130 * sqrt_info(5, 2) + + _tmp133 * _tmp181 + _tmp148 * _tmp86 + _tmp156 * sqrt_info(5, 4) - + _tmp158 * sqrt_info(5, 2) + _tmp165 * sqrt_info(5, 5) - + _tmp169 * sqrt_info(5, 0) + _tmp171 * sqrt_info(5, 0) + + _tmp172 * sqrt_info(5, 1) + _tmp173 * sqrt_info(5, 0); + const Scalar _tmp183 = _tmp106 - _tmp107 + _tmp108 - _tmp109; + const Scalar _tmp184 = _a_T_b[3] * _tmp183; + const Scalar _tmp185 = _tmp100 + _tmp101 - _tmp102 - _tmp103; + const Scalar _tmp186 = _a_T_b[0] * _tmp185; + const Scalar _tmp187 = _tmp125 + _tmp186; + const Scalar _tmp188 = _tmp123 + _tmp184 + _tmp187; + const Scalar _tmp189 = -_a_T_b[2] * _tmp183; + const Scalar _tmp190 = _a_T_b[1] * _tmp185; + const Scalar _tmp191 = _tmp190 + _tmp99; + const Scalar _tmp192 = _tmp189 + _tmp191 - _tmp93; + const Scalar _tmp193 = _tmp129 * _tmp192; + const Scalar _tmp194 = _a_T_b[3] * _tmp185; + const Scalar _tmp195 = _a_T_b[0] * _tmp183; + const Scalar _tmp196 = _tmp143 + _tmp144 + _tmp194 - _tmp195; + const Scalar _tmp197 = _tmp132 * _tmp188; + const Scalar _tmp198 = _tmp53 * sqrt_info(0, 2); + const Scalar _tmp199 = _tmp157 * _tmp188; + const Scalar _tmp200 = _tmp120 * _tmp188; + const Scalar _tmp201 = _a_T_b[1] * _tmp183; + const Scalar _tmp202 = -_a_T_b[2] * _tmp185; + const Scalar _tmp203 = _tmp136 + _tmp202; + const Scalar _tmp204 = _tmp135 - _tmp201 + _tmp203; + const Scalar _tmp205 = _tmp129 * sqrt_info(0, 1); + const Scalar _tmp206 = -_tmp3; + const Scalar _tmp207 = _tmp0 + _tmp206; + const Scalar _tmp208 = _tmp159 + _tmp20; + const Scalar _tmp209 = _tmp207 + _tmp208; + const Scalar _tmp210 = _tmp10 + _tmp161; + const Scalar _tmp211 = -_tmp24; + const Scalar _tmp212 = _tmp211 + _tmp26; + const Scalar _tmp213 = -_a[4] * _tmp212 - _a[5] * _tmp210 - _a[6] * _tmp209 + _b[4] * _tmp212 + + _b[5] * _tmp210 + _b[6] * _tmp209; + const Scalar _tmp214 = _tmp152 + _tmp207; + const Scalar _tmp215 = -_a[4] * _tmp214 + _b[4] * _tmp214 - _tmp28 - _tmp30 + _tmp31; + const Scalar _tmp216 = -_tmp131 * _tmp188 + _tmp141 * _tmp196 + _tmp142 * _tmp197 + + _tmp193 * sqrt_info(0, 2) + _tmp197 * _tmp198 + _tmp197 * _tmp67 - + _tmp199 * sqrt_info(0, 2) - _tmp200 * sqrt_info(0, 1) + _tmp204 * _tmp205 + + _tmp213 * sqrt_info(0, 3) + _tmp215 * sqrt_info(0, 5); + const Scalar _tmp217 = _tmp168 * _tmp188; + const Scalar _tmp218 = _tmp129 * _tmp196; + const Scalar _tmp219 = _tmp197 * _tmp66; + const Scalar _tmp220 = _tmp197 * _tmp68; + const Scalar _tmp221 = _tmp129 * _tmp204; + const Scalar _tmp222 = _tmp167 * _tmp192 + _tmp170 * _tmp197 - _tmp199 * sqrt_info(1, 2) - + _tmp200 * sqrt_info(1, 1) + _tmp213 * sqrt_info(1, 3) + + _tmp215 * sqrt_info(1, 5) - _tmp217 * sqrt_info(1, 0) + + _tmp218 * sqrt_info(1, 0) + _tmp219 * sqrt_info(1, 0) + + _tmp220 * sqrt_info(1, 1) + _tmp221 * sqrt_info(1, 1); + const Scalar _tmp223 = _tmp53 * sqrt_info(2, 2); + const Scalar _tmp224 = -_tmp175 * _tmp188 + _tmp193 * sqrt_info(2, 2) + _tmp197 * _tmp223 - + _tmp199 * sqrt_info(2, 2) + _tmp213 * sqrt_info(2, 3) + + _tmp215 * sqrt_info(2, 5) - _tmp217 * sqrt_info(2, 0) + + _tmp218 * sqrt_info(2, 0) + _tmp219 * sqrt_info(2, 0) + + _tmp220 * sqrt_info(2, 1) + _tmp221 * sqrt_info(2, 1); + const Scalar _tmp225 = _tmp53 * sqrt_info(3, 2); + const Scalar _tmp226 = _tmp177 * _tmp197 + _tmp193 * sqrt_info(3, 2) + _tmp197 * _tmp225 - + _tmp199 * sqrt_info(3, 2) - _tmp200 * sqrt_info(3, 1) + + _tmp213 * sqrt_info(3, 3) + _tmp215 * sqrt_info(3, 5) - + _tmp217 * sqrt_info(3, 0) + _tmp218 * sqrt_info(3, 0) + + _tmp219 * sqrt_info(3, 0) + _tmp221 * sqrt_info(3, 1); + const Scalar _tmp227 = _tmp179 * _tmp197 + _tmp193 * sqrt_info(4, 2) - _tmp199 * sqrt_info(4, 2) - + _tmp200 * sqrt_info(4, 1) + _tmp213 * sqrt_info(4, 3) + + _tmp215 * sqrt_info(4, 5) - _tmp217 * sqrt_info(4, 0) + + _tmp218 * sqrt_info(4, 0) + _tmp219 * sqrt_info(4, 0) + + _tmp220 * sqrt_info(4, 1) + _tmp221 * sqrt_info(4, 1); + const Scalar _tmp228 = _tmp128 * _tmp86; + const Scalar _tmp229 = _tmp181 * _tmp197 + _tmp193 * sqrt_info(5, 2) - _tmp199 * sqrt_info(5, 2) - + _tmp200 * sqrt_info(5, 1) + _tmp204 * _tmp228 + _tmp213 * sqrt_info(5, 3) + + _tmp215 * sqrt_info(5, 5) - _tmp217 * sqrt_info(5, 0) + + _tmp218 * sqrt_info(5, 0) + _tmp219 * sqrt_info(5, 0) + + _tmp220 * sqrt_info(5, 1); + const Scalar _tmp230 = -_tmp94 + _tmp95 + _tmp96 - _tmp97; + const Scalar _tmp231 = _a_T_b[1] * _tmp230; + const Scalar _tmp232 = _tmp195 + _tmp231; + const Scalar _tmp233 = _tmp144 + _tmp145 + _tmp232; + const Scalar _tmp234 = _tmp120 * _tmp233; + const Scalar _tmp235 = _tmp157 * _tmp233; + const Scalar _tmp236 = _tmp132 * _tmp233; + const Scalar _tmp237 = _a_T_b[3] * _tmp230; + const Scalar _tmp238 = -_tmp105 + _tmp189 + _tmp237 + _tmp93; + const Scalar _tmp239 = _a_T_b[2] * _tmp230; + const Scalar _tmp240 = _tmp184 + _tmp239; + const Scalar _tmp241 = _tmp129 * (_tmp122 - _tmp123 + _tmp240); + const Scalar _tmp242 = -_a_T_b[0] * _tmp230; + const Scalar _tmp243 = _tmp135 - _tmp137 + _tmp201 + _tmp242; + const Scalar _tmp244 = _tmp129 * _tmp243; + const Scalar _tmp245 = _tmp151 + _tmp153 + _tmp20 + _tmp206; + const Scalar _tmp246 = -_a[5] * _tmp245 + _b[5] * _tmp245 - _tmp12 - _tmp17 + _tmp18; + const Scalar _tmp247 = _tmp154 + _tmp208; + const Scalar _tmp248 = _tmp211 + _tmp25; + const Scalar _tmp249 = _tmp15 + _tmp163; + const Scalar _tmp250 = -_a[4] * _tmp247 - _a[5] * _tmp249 - _a[6] * _tmp248 + _b[4] * _tmp247 + + _b[5] * _tmp249 + _b[6] * _tmp248; + const Scalar _tmp251 = -_tmp131 * _tmp233 + _tmp142 * _tmp236 + _tmp198 * _tmp236 + + _tmp205 * _tmp238 - _tmp234 * sqrt_info(0, 1) - _tmp235 * sqrt_info(0, 2) + + _tmp236 * _tmp67 + _tmp241 * sqrt_info(0, 0) + _tmp244 * sqrt_info(0, 2) + + _tmp246 * sqrt_info(0, 3) + _tmp250 * sqrt_info(0, 4); + const Scalar _tmp252 = _tmp168 * _tmp233; + const Scalar _tmp253 = _tmp129 * _tmp238; + const Scalar _tmp254 = _tmp236 * _tmp68; + const Scalar _tmp255 = _tmp236 * _tmp66; + const Scalar _tmp256 = _tmp167 * _tmp243 + _tmp170 * _tmp236 - _tmp234 * sqrt_info(1, 1) - + _tmp235 * sqrt_info(1, 2) + _tmp241 * sqrt_info(1, 0) + + _tmp246 * sqrt_info(1, 3) + _tmp250 * sqrt_info(1, 4) - + _tmp252 * sqrt_info(1, 0) + _tmp253 * sqrt_info(1, 1) + + _tmp254 * sqrt_info(1, 1) + _tmp255 * sqrt_info(1, 0); + const Scalar _tmp257 = _tmp223 * _tmp236 - _tmp234 * sqrt_info(2, 1) - _tmp235 * sqrt_info(2, 2) + + _tmp241 * sqrt_info(2, 0) + _tmp244 * sqrt_info(2, 2) + + _tmp246 * sqrt_info(2, 3) + _tmp250 * sqrt_info(2, 4) - + _tmp252 * sqrt_info(2, 0) + _tmp253 * sqrt_info(2, 1) + + _tmp254 * sqrt_info(2, 1) + _tmp255 * sqrt_info(2, 0); + const Scalar _tmp258 = _tmp225 * _tmp236 - _tmp234 * sqrt_info(3, 1) - _tmp235 * sqrt_info(3, 2) + + _tmp241 * sqrt_info(3, 0) + _tmp244 * sqrt_info(3, 2) + + _tmp246 * sqrt_info(3, 3) + _tmp250 * sqrt_info(3, 4) - + _tmp252 * sqrt_info(3, 0) + _tmp253 * sqrt_info(3, 1) + + _tmp254 * sqrt_info(3, 1) + _tmp255 * sqrt_info(3, 0); + const Scalar _tmp259 = _tmp179 * _tmp236 - _tmp234 * sqrt_info(4, 1) - _tmp235 * sqrt_info(4, 2) + + _tmp241 * sqrt_info(4, 0) + _tmp244 * sqrt_info(4, 2) + + _tmp246 * sqrt_info(4, 3) + _tmp250 * sqrt_info(4, 4) - + _tmp252 * sqrt_info(4, 0) + _tmp253 * sqrt_info(4, 1) + + _tmp254 * sqrt_info(4, 1) + _tmp255 * sqrt_info(4, 0); + const Scalar _tmp260 = _tmp181 * _tmp236 + _tmp228 * _tmp238 - _tmp234 * sqrt_info(5, 1) - + _tmp235 * sqrt_info(5, 2) + _tmp241 * sqrt_info(5, 0) + + _tmp244 * sqrt_info(5, 2) + _tmp246 * sqrt_info(5, 3) + + _tmp250 * sqrt_info(5, 4) - _tmp252 * sqrt_info(5, 0) + + _tmp254 * sqrt_info(5, 1) + _tmp255 * sqrt_info(5, 0); + const Scalar _tmp261 = _tmp4 - 1; + const Scalar _tmp262 = _tmp21 + _tmp261; + const Scalar _tmp263 = + _tmp164 * sqrt_info(0, 4) + _tmp212 * sqrt_info(0, 5) + _tmp262 * sqrt_info(0, 3); + const Scalar _tmp264 = + _tmp164 * sqrt_info(1, 4) + _tmp212 * sqrt_info(1, 5) + _tmp262 * sqrt_info(1, 3); + const Scalar _tmp265 = + _tmp164 * sqrt_info(2, 4) + _tmp212 * sqrt_info(2, 5) + _tmp262 * sqrt_info(2, 3); + const Scalar _tmp266 = + _tmp164 * sqrt_info(3, 4) + _tmp212 * sqrt_info(3, 5) + _tmp262 * sqrt_info(3, 3); + const Scalar _tmp267 = + _tmp164 * sqrt_info(4, 4) + _tmp212 * sqrt_info(4, 5) + _tmp262 * sqrt_info(4, 3); + const Scalar _tmp268 = + _tmp164 * sqrt_info(5, 4) + _tmp212 * sqrt_info(5, 5) + _tmp262 * sqrt_info(5, 3); + const Scalar _tmp269 = _tmp1 + _tmp261; + const Scalar _tmp270 = + _tmp210 * sqrt_info(0, 5) + _tmp249 * sqrt_info(0, 3) + _tmp269 * sqrt_info(0, 4); + const Scalar _tmp271 = + _tmp210 * sqrt_info(1, 5) + _tmp249 * sqrt_info(1, 3) + _tmp269 * sqrt_info(1, 4); + const Scalar _tmp272 = + _tmp210 * sqrt_info(2, 5) + _tmp249 * sqrt_info(2, 3) + _tmp269 * sqrt_info(2, 4); + const Scalar _tmp273 = + _tmp210 * sqrt_info(3, 5) + _tmp249 * sqrt_info(3, 3) + _tmp269 * sqrt_info(3, 4); + const Scalar _tmp274 = + _tmp210 * sqrt_info(4, 5) + _tmp249 * sqrt_info(4, 3) + _tmp269 * sqrt_info(4, 4); + const Scalar _tmp275 = + _tmp210 * sqrt_info(5, 5) + _tmp249 * sqrt_info(5, 3) + _tmp269 * sqrt_info(5, 4); + const Scalar _tmp276 = _tmp1 + _tmp21 - 1; + const Scalar _tmp277 = + _tmp162 * sqrt_info(0, 4) + _tmp248 * sqrt_info(0, 3) + _tmp276 * sqrt_info(0, 5); + const Scalar _tmp278 = + _tmp162 * sqrt_info(1, 4) + _tmp248 * sqrt_info(1, 3) + _tmp276 * sqrt_info(1, 5); + const Scalar _tmp279 = + _tmp162 * sqrt_info(2, 4) + _tmp248 * sqrt_info(2, 3) + _tmp276 * sqrt_info(2, 5); + const Scalar _tmp280 = + _tmp162 * sqrt_info(3, 4) + _tmp248 * sqrt_info(3, 3) + _tmp276 * sqrt_info(3, 5); + const Scalar _tmp281 = + _tmp162 * sqrt_info(4, 4) + _tmp248 * sqrt_info(4, 3) + _tmp276 * sqrt_info(4, 5); + const Scalar _tmp282 = + _tmp162 * sqrt_info(5, 4) + _tmp248 * sqrt_info(5, 3) + _tmp276 * sqrt_info(5, 5); + const Scalar _tmp283 = _tmp88 + _tmp89 + _tmp90 + _tmp91; + const Scalar _tmp284 = _a_T_b[0] * _tmp283; + const Scalar _tmp285 = _tmp237 + _tmp284; + const Scalar _tmp286 = _tmp112 + _tmp285; + const Scalar _tmp287 = _tmp132 * _tmp286; + const Scalar _tmp288 = _tmp157 * _tmp286; + const Scalar _tmp289 = _tmp120 * _tmp286; + const Scalar _tmp290 = _a_T_b[3] * _tmp283; + const Scalar _tmp291 = _tmp242 + _tmp290; + const Scalar _tmp292 = _tmp139 + _tmp291; + const Scalar _tmp293 = _a_T_b[1] * _tmp283; + const Scalar _tmp294 = _tmp124 + _tmp293; + const Scalar _tmp295 = _tmp122 - _tmp239 + _tmp294; + const Scalar _tmp296 = _tmp129 * _tmp295; + const Scalar _tmp297 = _a_T_b[2] * _tmp283; + const Scalar _tmp298 = _tmp147 - _tmp231 - _tmp297; + const Scalar _tmp299 = -_tmp131 * _tmp286 + _tmp141 * _tmp292 + _tmp142 * _tmp287 + + _tmp198 * _tmp287 + _tmp205 * _tmp298 + _tmp287 * _tmp67 - + _tmp288 * sqrt_info(0, 2) - _tmp289 * sqrt_info(0, 1) + + _tmp296 * sqrt_info(0, 2); + const Scalar _tmp300 = _tmp287 * _tmp66; + const Scalar _tmp301 = _tmp287 * _tmp68; + const Scalar _tmp302 = _tmp168 * _tmp286; + const Scalar _tmp303 = _tmp129 * _tmp292; + const Scalar _tmp304 = _tmp129 * _tmp298; + const Scalar _tmp305 = _tmp167 * _tmp295 + _tmp170 * _tmp287 - _tmp288 * sqrt_info(1, 2) - + _tmp289 * sqrt_info(1, 1) + _tmp300 * sqrt_info(1, 0) + + _tmp301 * sqrt_info(1, 1) - _tmp302 * sqrt_info(1, 0) + + _tmp303 * sqrt_info(1, 0) + _tmp304 * sqrt_info(1, 1); + const Scalar _tmp306 = -_tmp175 * _tmp286 + _tmp223 * _tmp287 - _tmp288 * sqrt_info(2, 2) + + _tmp296 * sqrt_info(2, 2) + _tmp300 * sqrt_info(2, 0) + + _tmp301 * sqrt_info(2, 1) - _tmp302 * sqrt_info(2, 0) + + _tmp303 * sqrt_info(2, 0) + _tmp304 * sqrt_info(2, 1); + const Scalar _tmp307 = _tmp177 * _tmp287 + _tmp225 * _tmp287 - _tmp288 * sqrt_info(3, 2) - + _tmp289 * sqrt_info(3, 1) + _tmp296 * sqrt_info(3, 2) + + _tmp300 * sqrt_info(3, 0) - _tmp302 * sqrt_info(3, 0) + + _tmp303 * sqrt_info(3, 0) + _tmp304 * sqrt_info(3, 1); + const Scalar _tmp308 = _tmp179 * _tmp287 - _tmp288 * sqrt_info(4, 2) - _tmp289 * sqrt_info(4, 1) + + _tmp296 * sqrt_info(4, 2) + _tmp300 * sqrt_info(4, 0) + + _tmp301 * sqrt_info(4, 1) - _tmp302 * sqrt_info(4, 0) + + _tmp303 * sqrt_info(4, 0) + _tmp304 * sqrt_info(4, 1); + const Scalar _tmp309 = _tmp181 * _tmp287 + _tmp228 * _tmp298 - _tmp288 * sqrt_info(5, 2) - + _tmp289 * sqrt_info(5, 1) + _tmp296 * sqrt_info(5, 2) + + _tmp300 * sqrt_info(5, 0) + _tmp301 * sqrt_info(5, 1) - + _tmp302 * sqrt_info(5, 0) + _tmp303 * sqrt_info(5, 0); + const Scalar _tmp310 = -_tmp111 + _tmp191 - _tmp284; + const Scalar _tmp311 = _tmp129 * _tmp310; + const Scalar _tmp312 = _tmp187 + _tmp294; + const Scalar _tmp313 = _tmp132 * _tmp312; + const Scalar _tmp314 = _tmp120 * _tmp312; + const Scalar _tmp315 = _tmp194 + _tmp297; + const Scalar _tmp316 = _tmp143 - _tmp146 + _tmp315; + const Scalar _tmp317 = _tmp138 + _tmp203 + _tmp290; + const Scalar _tmp318 = _tmp157 * _tmp312; + const Scalar _tmp319 = -_tmp131 * _tmp312 + _tmp141 * _tmp316 + _tmp142 * _tmp313 + + _tmp198 * _tmp313 + _tmp205 * _tmp317 + _tmp311 * sqrt_info(0, 2) + + _tmp313 * _tmp67 - _tmp314 * sqrt_info(0, 1) - _tmp318 * sqrt_info(0, 2); + const Scalar _tmp320 = _tmp313 * _tmp68; + const Scalar _tmp321 = _tmp129 * _tmp316; + const Scalar _tmp322 = _tmp168 * _tmp312; + const Scalar _tmp323 = _tmp129 * _tmp317; + const Scalar _tmp324 = _tmp313 * _tmp66; + const Scalar _tmp325 = _tmp167 * _tmp310 + _tmp170 * _tmp313 - _tmp314 * sqrt_info(1, 1) - + _tmp318 * sqrt_info(1, 2) + _tmp320 * sqrt_info(1, 1) + + _tmp321 * sqrt_info(1, 0) - _tmp322 * sqrt_info(1, 0) + + _tmp323 * sqrt_info(1, 1) + _tmp324 * sqrt_info(1, 0); + const Scalar _tmp326 = _tmp129 * sqrt_info(2, 0); + const Scalar _tmp327 = -_tmp175 * _tmp312 + _tmp223 * _tmp313 + _tmp311 * sqrt_info(2, 2) + + _tmp316 * _tmp326 - _tmp318 * sqrt_info(2, 2) + _tmp320 * sqrt_info(2, 1) - + _tmp322 * sqrt_info(2, 0) + _tmp323 * sqrt_info(2, 1) + + _tmp324 * sqrt_info(2, 0); + const Scalar _tmp328 = _tmp177 * _tmp313 + _tmp225 * _tmp313 + _tmp311 * sqrt_info(3, 2) - + _tmp314 * sqrt_info(3, 1) - _tmp318 * sqrt_info(3, 2) + + _tmp321 * sqrt_info(3, 0) - _tmp322 * sqrt_info(3, 0) + + _tmp323 * sqrt_info(3, 1) + _tmp324 * sqrt_info(3, 0); + const Scalar _tmp329 = _tmp179 * _tmp313 + _tmp311 * sqrt_info(4, 2) - _tmp314 * sqrt_info(4, 1) - + _tmp318 * sqrt_info(4, 2) + _tmp320 * sqrt_info(4, 1) + + _tmp321 * sqrt_info(4, 0) - _tmp322 * sqrt_info(4, 0) + + _tmp323 * sqrt_info(4, 1) + _tmp324 * sqrt_info(4, 0); + const Scalar _tmp330 = _tmp181 * _tmp313 + _tmp228 * _tmp317 + _tmp311 * sqrt_info(5, 2) - + _tmp314 * sqrt_info(5, 1) - _tmp318 * sqrt_info(5, 2) + + _tmp320 * sqrt_info(5, 1) + _tmp321 * sqrt_info(5, 0) - + _tmp322 * sqrt_info(5, 0) + _tmp324 * sqrt_info(5, 0); + const Scalar _tmp331 = _tmp232 + _tmp315; + const Scalar _tmp332 = _tmp132 * _tmp331; + const Scalar _tmp333 = _tmp332 * _tmp53; + const Scalar _tmp334 = _tmp120 * _tmp331; + const Scalar _tmp335 = _tmp189 - _tmp190 + _tmp285; + const Scalar _tmp336 = _tmp201 + _tmp202 + _tmp291; + const Scalar _tmp337 = _tmp129 * _tmp336; + const Scalar _tmp338 = -_tmp186 + _tmp240 - _tmp293; + const Scalar _tmp339 = _tmp157 * _tmp331; + const Scalar _tmp340 = -_tmp131 * _tmp331 + _tmp141 * _tmp338 + _tmp142 * _tmp332 + + _tmp205 * _tmp335 + _tmp332 * _tmp67 + _tmp333 * sqrt_info(0, 2) - + _tmp334 * sqrt_info(0, 1) + _tmp337 * sqrt_info(0, 2) - + _tmp339 * sqrt_info(0, 2); + const Scalar _tmp341 = _tmp168 * _tmp331; + const Scalar _tmp342 = _tmp129 * _tmp335; + const Scalar _tmp343 = _tmp129 * _tmp338; + const Scalar _tmp344 = _tmp332 * _tmp68; + const Scalar _tmp345 = _tmp332 * _tmp66; + const Scalar _tmp346 = _tmp167 * _tmp336 + _tmp170 * _tmp332 - _tmp334 * sqrt_info(1, 1) - + _tmp339 * sqrt_info(1, 2) - _tmp341 * sqrt_info(1, 0) + + _tmp342 * sqrt_info(1, 1) + _tmp343 * sqrt_info(1, 0) + + _tmp344 * sqrt_info(1, 1) + _tmp345 * sqrt_info(1, 0); + const Scalar _tmp347 = -_tmp175 * _tmp331 + _tmp326 * _tmp338 + _tmp333 * sqrt_info(2, 2) + + _tmp337 * sqrt_info(2, 2) - _tmp339 * sqrt_info(2, 2) - + _tmp341 * sqrt_info(2, 0) + _tmp342 * sqrt_info(2, 1) + + _tmp344 * sqrt_info(2, 1) + _tmp345 * sqrt_info(2, 0); + const Scalar _tmp348 = _tmp177 * _tmp332 + _tmp333 * sqrt_info(3, 2) - _tmp334 * sqrt_info(3, 1) + + _tmp337 * sqrt_info(3, 2) - _tmp339 * sqrt_info(3, 2) - + _tmp341 * sqrt_info(3, 0) + _tmp342 * sqrt_info(3, 1) + + _tmp343 * sqrt_info(3, 0) + _tmp345 * sqrt_info(3, 0); + const Scalar _tmp349 = + _tmp333 * sqrt_info(4, 2) - _tmp334 * sqrt_info(4, 1) + _tmp337 * sqrt_info(4, 2) - + _tmp339 * sqrt_info(4, 2) - _tmp341 * sqrt_info(4, 0) + _tmp342 * sqrt_info(4, 1) + + _tmp343 * sqrt_info(4, 0) + _tmp344 * sqrt_info(4, 1) + _tmp345 * sqrt_info(4, 0); + const Scalar _tmp350 = _tmp181 * _tmp332 + _tmp228 * _tmp335 - _tmp334 * sqrt_info(5, 1) + + _tmp337 * sqrt_info(5, 2) - _tmp339 * sqrt_info(5, 2) - + _tmp341 * sqrt_info(5, 0) + _tmp343 * sqrt_info(5, 0) + + _tmp344 * sqrt_info(5, 1) + _tmp345 * sqrt_info(5, 0); + const Scalar _tmp351 = + _tmp16 * sqrt_info(0, 4) + _tmp23 * sqrt_info(0, 3) + _tmp75 * sqrt_info(0, 5); + const Scalar _tmp352 = + _tmp16 * sqrt_info(1, 4) + _tmp23 * sqrt_info(1, 3) + _tmp75 * sqrt_info(1, 5); + const Scalar _tmp353 = + _tmp16 * sqrt_info(2, 4) + _tmp23 * sqrt_info(2, 3) + _tmp75 * sqrt_info(2, 5); + const Scalar _tmp354 = + _tmp16 * sqrt_info(3, 4) + _tmp23 * sqrt_info(3, 3) + _tmp75 * sqrt_info(3, 5); + const Scalar _tmp355 = + _tmp16 * sqrt_info(4, 4) + _tmp23 * sqrt_info(4, 3) + _tmp75 * sqrt_info(4, 5); + const Scalar _tmp356 = + _tmp16 * sqrt_info(5, 4) + _tmp23 * sqrt_info(5, 3) + _tmp75 * sqrt_info(5, 5); + const Scalar _tmp357 = + _tmp29 * sqrt_info(0, 3) + _tmp6 * sqrt_info(0, 4) + _tmp73 * sqrt_info(0, 5); + const Scalar _tmp358 = + _tmp29 * sqrt_info(1, 3) + _tmp6 * sqrt_info(1, 4) + _tmp73 * sqrt_info(1, 5); + const Scalar _tmp359 = + _tmp29 * sqrt_info(2, 3) + _tmp6 * sqrt_info(2, 4) + _tmp73 * sqrt_info(2, 5); + const Scalar _tmp360 = + _tmp29 * sqrt_info(3, 3) + _tmp6 * sqrt_info(3, 4) + _tmp73 * sqrt_info(3, 5); + const Scalar _tmp361 = + _tmp29 * sqrt_info(4, 3) + _tmp6 * sqrt_info(4, 4) + _tmp73 * sqrt_info(4, 5); + const Scalar _tmp362 = + _tmp29 * sqrt_info(5, 3) + _tmp6 * sqrt_info(5, 4) + _tmp73 * sqrt_info(5, 5); + const Scalar _tmp363 = + _tmp11 * sqrt_info(0, 4) + _tmp27 * sqrt_info(0, 3) + _tmp71 * sqrt_info(0, 5); + const Scalar _tmp364 = + _tmp11 * sqrt_info(1, 4) + _tmp27 * sqrt_info(1, 3) + _tmp71 * sqrt_info(1, 5); + const Scalar _tmp365 = + _tmp11 * sqrt_info(2, 4) + _tmp27 * sqrt_info(2, 3) + _tmp71 * sqrt_info(2, 5); + const Scalar _tmp366 = + _tmp11 * sqrt_info(3, 4) + _tmp27 * sqrt_info(3, 3) + _tmp71 * sqrt_info(3, 5); + const Scalar _tmp367 = + _tmp11 * sqrt_info(4, 4) + _tmp27 * sqrt_info(4, 3) + _tmp71 * sqrt_info(4, 5); + const Scalar _tmp368 = + _tmp11 * sqrt_info(5, 4) + _tmp27 * sqrt_info(5, 3) + _tmp71 * sqrt_info(5, 5); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp79; + _res(1, 0) = _tmp82; + _res(2, 0) = _tmp83; + _res(3, 0) = _tmp84; + _res(4, 0) = _tmp85; + _res(5, 0) = _tmp87; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp166; + _jacobian(1, 0) = _tmp174; + _jacobian(2, 0) = _tmp176; + _jacobian(3, 0) = _tmp178; + _jacobian(4, 0) = _tmp180; + _jacobian(5, 0) = _tmp182; + _jacobian(0, 1) = _tmp216; + _jacobian(1, 1) = _tmp222; + _jacobian(2, 1) = _tmp224; + _jacobian(3, 1) = _tmp226; + _jacobian(4, 1) = _tmp227; + _jacobian(5, 1) = _tmp229; + _jacobian(0, 2) = _tmp251; + _jacobian(1, 2) = _tmp256; + _jacobian(2, 2) = _tmp257; + _jacobian(3, 2) = _tmp258; + _jacobian(4, 2) = _tmp259; + _jacobian(5, 2) = _tmp260; + _jacobian(0, 3) = _tmp263; + _jacobian(1, 3) = _tmp264; + _jacobian(2, 3) = _tmp265; + _jacobian(3, 3) = _tmp266; + _jacobian(4, 3) = _tmp267; + _jacobian(5, 3) = _tmp268; + _jacobian(0, 4) = _tmp270; + _jacobian(1, 4) = _tmp271; + _jacobian(2, 4) = _tmp272; + _jacobian(3, 4) = _tmp273; + _jacobian(4, 4) = _tmp274; + _jacobian(5, 4) = _tmp275; + _jacobian(0, 5) = _tmp277; + _jacobian(1, 5) = _tmp278; + _jacobian(2, 5) = _tmp279; + _jacobian(3, 5) = _tmp280; + _jacobian(4, 5) = _tmp281; + _jacobian(5, 5) = _tmp282; + _jacobian(0, 6) = _tmp299; + _jacobian(1, 6) = _tmp305; + _jacobian(2, 6) = _tmp306; + _jacobian(3, 6) = _tmp307; + _jacobian(4, 6) = _tmp308; + _jacobian(5, 6) = _tmp309; + _jacobian(0, 7) = _tmp319; + _jacobian(1, 7) = _tmp325; + _jacobian(2, 7) = _tmp327; + _jacobian(3, 7) = _tmp328; + _jacobian(4, 7) = _tmp329; + _jacobian(5, 7) = _tmp330; + _jacobian(0, 8) = _tmp340; + _jacobian(1, 8) = _tmp346; + _jacobian(2, 8) = _tmp347; + _jacobian(3, 8) = _tmp348; + _jacobian(4, 8) = _tmp349; + _jacobian(5, 8) = _tmp350; + _jacobian(0, 9) = _tmp351; + _jacobian(1, 9) = _tmp352; + _jacobian(2, 9) = _tmp353; + _jacobian(3, 9) = _tmp354; + _jacobian(4, 9) = _tmp355; + _jacobian(5, 9) = _tmp356; + _jacobian(0, 10) = _tmp357; + _jacobian(1, 10) = _tmp358; + _jacobian(2, 10) = _tmp359; + _jacobian(3, 10) = _tmp360; + _jacobian(4, 10) = _tmp361; + _jacobian(5, 10) = _tmp362; + _jacobian(0, 11) = _tmp363; + _jacobian(1, 11) = _tmp364; + _jacobian(2, 11) = _tmp365; + _jacobian(3, 11) = _tmp366; + _jacobian(4, 11) = _tmp367; + _jacobian(5, 11) = _tmp368; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(_tmp166, Scalar(2)) + std::pow(_tmp174, Scalar(2)) + + std::pow(_tmp176, Scalar(2)) + std::pow(_tmp178, Scalar(2)) + + std::pow(_tmp180, Scalar(2)) + std::pow(_tmp182, Scalar(2)); + _hessian(1, 0) = _tmp166 * _tmp216 + _tmp174 * _tmp222 + _tmp176 * _tmp224 + _tmp178 * _tmp226 + + _tmp180 * _tmp227 + _tmp182 * _tmp229; + _hessian(2, 0) = _tmp166 * _tmp251 + _tmp174 * _tmp256 + _tmp176 * _tmp257 + _tmp178 * _tmp258 + + _tmp180 * _tmp259 + _tmp182 * _tmp260; + _hessian(3, 0) = _tmp166 * _tmp263 + _tmp174 * _tmp264 + _tmp176 * _tmp265 + _tmp178 * _tmp266 + + _tmp180 * _tmp267 + _tmp182 * _tmp268; + _hessian(4, 0) = _tmp166 * _tmp270 + _tmp174 * _tmp271 + _tmp176 * _tmp272 + _tmp178 * _tmp273 + + _tmp180 * _tmp274 + _tmp182 * _tmp275; + _hessian(5, 0) = _tmp166 * _tmp277 + _tmp174 * _tmp278 + _tmp176 * _tmp279 + _tmp178 * _tmp280 + + _tmp180 * _tmp281 + _tmp182 * _tmp282; + _hessian(6, 0) = _tmp166 * _tmp299 + _tmp174 * _tmp305 + _tmp176 * _tmp306 + _tmp178 * _tmp307 + + _tmp180 * _tmp308 + _tmp182 * _tmp309; + _hessian(7, 0) = _tmp166 * _tmp319 + _tmp174 * _tmp325 + _tmp176 * _tmp327 + _tmp178 * _tmp328 + + _tmp180 * _tmp329 + _tmp182 * _tmp330; + _hessian(8, 0) = _tmp166 * _tmp340 + _tmp174 * _tmp346 + _tmp176 * _tmp347 + _tmp178 * _tmp348 + + _tmp180 * _tmp349 + _tmp182 * _tmp350; + _hessian(9, 0) = _tmp166 * _tmp351 + _tmp174 * _tmp352 + _tmp176 * _tmp353 + _tmp178 * _tmp354 + + _tmp180 * _tmp355 + _tmp182 * _tmp356; + _hessian(10, 0) = _tmp166 * _tmp357 + _tmp174 * _tmp358 + _tmp176 * _tmp359 + + _tmp178 * _tmp360 + _tmp180 * _tmp361 + _tmp182 * _tmp362; + _hessian(11, 0) = _tmp166 * _tmp363 + _tmp174 * _tmp364 + _tmp176 * _tmp365 + + _tmp178 * _tmp366 + _tmp180 * _tmp367 + _tmp182 * _tmp368; + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(_tmp216, Scalar(2)) + std::pow(_tmp222, Scalar(2)) + + std::pow(_tmp224, Scalar(2)) + std::pow(_tmp226, Scalar(2)) + + std::pow(_tmp227, Scalar(2)) + std::pow(_tmp229, Scalar(2)); + _hessian(2, 1) = _tmp216 * _tmp251 + _tmp222 * _tmp256 + _tmp224 * _tmp257 + _tmp226 * _tmp258 + + _tmp227 * _tmp259 + _tmp229 * _tmp260; + _hessian(3, 1) = _tmp216 * _tmp263 + _tmp222 * _tmp264 + _tmp224 * _tmp265 + _tmp226 * _tmp266 + + _tmp227 * _tmp267 + _tmp229 * _tmp268; + _hessian(4, 1) = _tmp216 * _tmp270 + _tmp222 * _tmp271 + _tmp224 * _tmp272 + _tmp226 * _tmp273 + + _tmp227 * _tmp274 + _tmp229 * _tmp275; + _hessian(5, 1) = _tmp216 * _tmp277 + _tmp222 * _tmp278 + _tmp224 * _tmp279 + _tmp226 * _tmp280 + + _tmp227 * _tmp281 + _tmp229 * _tmp282; + _hessian(6, 1) = _tmp216 * _tmp299 + _tmp222 * _tmp305 + _tmp224 * _tmp306 + _tmp226 * _tmp307 + + _tmp227 * _tmp308 + _tmp229 * _tmp309; + _hessian(7, 1) = _tmp216 * _tmp319 + _tmp222 * _tmp325 + _tmp224 * _tmp327 + _tmp226 * _tmp328 + + _tmp227 * _tmp329 + _tmp229 * _tmp330; + _hessian(8, 1) = _tmp216 * _tmp340 + _tmp222 * _tmp346 + _tmp224 * _tmp347 + _tmp226 * _tmp348 + + _tmp227 * _tmp349 + _tmp229 * _tmp350; + _hessian(9, 1) = _tmp216 * _tmp351 + _tmp222 * _tmp352 + _tmp224 * _tmp353 + _tmp226 * _tmp354 + + _tmp227 * _tmp355 + _tmp229 * _tmp356; + _hessian(10, 1) = _tmp216 * _tmp357 + _tmp222 * _tmp358 + _tmp224 * _tmp359 + + _tmp226 * _tmp360 + _tmp227 * _tmp361 + _tmp229 * _tmp362; + _hessian(11, 1) = _tmp216 * _tmp363 + _tmp222 * _tmp364 + _tmp224 * _tmp365 + + _tmp226 * _tmp366 + _tmp227 * _tmp367 + _tmp229 * _tmp368; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = std::pow(_tmp251, Scalar(2)) + std::pow(_tmp256, Scalar(2)) + + std::pow(_tmp257, Scalar(2)) + std::pow(_tmp258, Scalar(2)) + + std::pow(_tmp259, Scalar(2)) + std::pow(_tmp260, Scalar(2)); + _hessian(3, 2) = _tmp251 * _tmp263 + _tmp256 * _tmp264 + _tmp257 * _tmp265 + _tmp258 * _tmp266 + + _tmp259 * _tmp267 + _tmp260 * _tmp268; + _hessian(4, 2) = _tmp251 * _tmp270 + _tmp256 * _tmp271 + _tmp257 * _tmp272 + _tmp258 * _tmp273 + + _tmp259 * _tmp274 + _tmp260 * _tmp275; + _hessian(5, 2) = _tmp251 * _tmp277 + _tmp256 * _tmp278 + _tmp257 * _tmp279 + _tmp258 * _tmp280 + + _tmp259 * _tmp281 + _tmp260 * _tmp282; + _hessian(6, 2) = _tmp251 * _tmp299 + _tmp256 * _tmp305 + _tmp257 * _tmp306 + _tmp258 * _tmp307 + + _tmp259 * _tmp308 + _tmp260 * _tmp309; + _hessian(7, 2) = _tmp251 * _tmp319 + _tmp256 * _tmp325 + _tmp257 * _tmp327 + _tmp258 * _tmp328 + + _tmp259 * _tmp329 + _tmp260 * _tmp330; + _hessian(8, 2) = _tmp251 * _tmp340 + _tmp256 * _tmp346 + _tmp257 * _tmp347 + _tmp258 * _tmp348 + + _tmp259 * _tmp349 + _tmp260 * _tmp350; + _hessian(9, 2) = _tmp251 * _tmp351 + _tmp256 * _tmp352 + _tmp257 * _tmp353 + _tmp258 * _tmp354 + + _tmp259 * _tmp355 + _tmp260 * _tmp356; + _hessian(10, 2) = _tmp251 * _tmp357 + _tmp256 * _tmp358 + _tmp257 * _tmp359 + + _tmp258 * _tmp360 + _tmp259 * _tmp361 + _tmp260 * _tmp362; + _hessian(11, 2) = _tmp251 * _tmp363 + _tmp256 * _tmp364 + _tmp257 * _tmp365 + + _tmp258 * _tmp366 + _tmp259 * _tmp367 + _tmp260 * _tmp368; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = std::pow(_tmp263, Scalar(2)) + std::pow(_tmp264, Scalar(2)) + + std::pow(_tmp265, Scalar(2)) + std::pow(_tmp266, Scalar(2)) + + std::pow(_tmp267, Scalar(2)) + std::pow(_tmp268, Scalar(2)); + _hessian(4, 3) = _tmp263 * _tmp270 + _tmp264 * _tmp271 + _tmp265 * _tmp272 + _tmp266 * _tmp273 + + _tmp267 * _tmp274 + _tmp268 * _tmp275; + _hessian(5, 3) = _tmp263 * _tmp277 + _tmp264 * _tmp278 + _tmp265 * _tmp279 + _tmp266 * _tmp280 + + _tmp267 * _tmp281 + _tmp268 * _tmp282; + _hessian(6, 3) = _tmp263 * _tmp299 + _tmp264 * _tmp305 + _tmp265 * _tmp306 + _tmp266 * _tmp307 + + _tmp267 * _tmp308 + _tmp268 * _tmp309; + _hessian(7, 3) = _tmp263 * _tmp319 + _tmp264 * _tmp325 + _tmp265 * _tmp327 + _tmp266 * _tmp328 + + _tmp267 * _tmp329 + _tmp268 * _tmp330; + _hessian(8, 3) = _tmp263 * _tmp340 + _tmp264 * _tmp346 + _tmp265 * _tmp347 + _tmp266 * _tmp348 + + _tmp267 * _tmp349 + _tmp268 * _tmp350; + _hessian(9, 3) = _tmp263 * _tmp351 + _tmp264 * _tmp352 + _tmp265 * _tmp353 + _tmp266 * _tmp354 + + _tmp267 * _tmp355 + _tmp268 * _tmp356; + _hessian(10, 3) = _tmp263 * _tmp357 + _tmp264 * _tmp358 + _tmp265 * _tmp359 + + _tmp266 * _tmp360 + _tmp267 * _tmp361 + _tmp268 * _tmp362; + _hessian(11, 3) = _tmp263 * _tmp363 + _tmp264 * _tmp364 + _tmp265 * _tmp365 + + _tmp266 * _tmp366 + _tmp267 * _tmp367 + _tmp268 * _tmp368; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = std::pow(_tmp270, Scalar(2)) + std::pow(_tmp271, Scalar(2)) + + std::pow(_tmp272, Scalar(2)) + std::pow(_tmp273, Scalar(2)) + + std::pow(_tmp274, Scalar(2)) + std::pow(_tmp275, Scalar(2)); + _hessian(5, 4) = _tmp270 * _tmp277 + _tmp271 * _tmp278 + _tmp272 * _tmp279 + _tmp273 * _tmp280 + + _tmp274 * _tmp281 + _tmp275 * _tmp282; + _hessian(6, 4) = _tmp270 * _tmp299 + _tmp271 * _tmp305 + _tmp272 * _tmp306 + _tmp273 * _tmp307 + + _tmp274 * _tmp308 + _tmp275 * _tmp309; + _hessian(7, 4) = _tmp270 * _tmp319 + _tmp271 * _tmp325 + _tmp272 * _tmp327 + _tmp273 * _tmp328 + + _tmp274 * _tmp329 + _tmp275 * _tmp330; + _hessian(8, 4) = _tmp270 * _tmp340 + _tmp271 * _tmp346 + _tmp272 * _tmp347 + _tmp273 * _tmp348 + + _tmp274 * _tmp349 + _tmp275 * _tmp350; + _hessian(9, 4) = _tmp270 * _tmp351 + _tmp271 * _tmp352 + _tmp272 * _tmp353 + _tmp273 * _tmp354 + + _tmp274 * _tmp355 + _tmp275 * _tmp356; + _hessian(10, 4) = _tmp270 * _tmp357 + _tmp271 * _tmp358 + _tmp272 * _tmp359 + + _tmp273 * _tmp360 + _tmp274 * _tmp361 + _tmp275 * _tmp362; + _hessian(11, 4) = _tmp270 * _tmp363 + _tmp271 * _tmp364 + _tmp272 * _tmp365 + + _tmp273 * _tmp366 + _tmp274 * _tmp367 + _tmp275 * _tmp368; + _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(_tmp277, Scalar(2)) + std::pow(_tmp278, Scalar(2)) + + std::pow(_tmp279, Scalar(2)) + std::pow(_tmp280, Scalar(2)) + + std::pow(_tmp281, Scalar(2)) + std::pow(_tmp282, Scalar(2)); + _hessian(6, 5) = _tmp277 * _tmp299 + _tmp278 * _tmp305 + _tmp279 * _tmp306 + _tmp280 * _tmp307 + + _tmp281 * _tmp308 + _tmp282 * _tmp309; + _hessian(7, 5) = _tmp277 * _tmp319 + _tmp278 * _tmp325 + _tmp279 * _tmp327 + _tmp280 * _tmp328 + + _tmp281 * _tmp329 + _tmp282 * _tmp330; + _hessian(8, 5) = _tmp277 * _tmp340 + _tmp278 * _tmp346 + _tmp279 * _tmp347 + _tmp280 * _tmp348 + + _tmp281 * _tmp349 + _tmp282 * _tmp350; + _hessian(9, 5) = _tmp277 * _tmp351 + _tmp278 * _tmp352 + _tmp279 * _tmp353 + _tmp280 * _tmp354 + + _tmp281 * _tmp355 + _tmp282 * _tmp356; + _hessian(10, 5) = _tmp277 * _tmp357 + _tmp278 * _tmp358 + _tmp279 * _tmp359 + + _tmp280 * _tmp360 + _tmp281 * _tmp361 + _tmp282 * _tmp362; + _hessian(11, 5) = _tmp277 * _tmp363 + _tmp278 * _tmp364 + _tmp279 * _tmp365 + + _tmp280 * _tmp366 + _tmp281 * _tmp367 + _tmp282 * _tmp368; + _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(_tmp299, Scalar(2)) + std::pow(_tmp305, Scalar(2)) + + std::pow(_tmp306, Scalar(2)) + std::pow(_tmp307, Scalar(2)) + + std::pow(_tmp308, Scalar(2)) + std::pow(_tmp309, Scalar(2)); + _hessian(7, 6) = _tmp299 * _tmp319 + _tmp305 * _tmp325 + _tmp306 * _tmp327 + _tmp307 * _tmp328 + + _tmp308 * _tmp329 + _tmp309 * _tmp330; + _hessian(8, 6) = _tmp299 * _tmp340 + _tmp305 * _tmp346 + _tmp306 * _tmp347 + _tmp307 * _tmp348 + + _tmp308 * _tmp349 + _tmp309 * _tmp350; + _hessian(9, 6) = _tmp299 * _tmp351 + _tmp305 * _tmp352 + _tmp306 * _tmp353 + _tmp307 * _tmp354 + + _tmp308 * _tmp355 + _tmp309 * _tmp356; + _hessian(10, 6) = _tmp299 * _tmp357 + _tmp305 * _tmp358 + _tmp306 * _tmp359 + + _tmp307 * _tmp360 + _tmp308 * _tmp361 + _tmp309 * _tmp362; + _hessian(11, 6) = _tmp299 * _tmp363 + _tmp305 * _tmp364 + _tmp306 * _tmp365 + + _tmp307 * _tmp366 + _tmp308 * _tmp367 + _tmp309 * _tmp368; + _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(_tmp319, Scalar(2)) + std::pow(_tmp325, Scalar(2)) + + std::pow(_tmp327, Scalar(2)) + std::pow(_tmp328, Scalar(2)) + + std::pow(_tmp329, Scalar(2)) + std::pow(_tmp330, Scalar(2)); + _hessian(8, 7) = _tmp319 * _tmp340 + _tmp325 * _tmp346 + _tmp327 * _tmp347 + _tmp328 * _tmp348 + + _tmp329 * _tmp349 + _tmp330 * _tmp350; + _hessian(9, 7) = _tmp319 * _tmp351 + _tmp325 * _tmp352 + _tmp327 * _tmp353 + _tmp328 * _tmp354 + + _tmp329 * _tmp355 + _tmp330 * _tmp356; + _hessian(10, 7) = _tmp319 * _tmp357 + _tmp325 * _tmp358 + _tmp327 * _tmp359 + + _tmp328 * _tmp360 + _tmp329 * _tmp361 + _tmp330 * _tmp362; + _hessian(11, 7) = _tmp319 * _tmp363 + _tmp325 * _tmp364 + _tmp327 * _tmp365 + + _tmp328 * _tmp366 + _tmp329 * _tmp367 + _tmp330 * _tmp368; + _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(_tmp340, Scalar(2)) + std::pow(_tmp346, Scalar(2)) + + std::pow(_tmp347, Scalar(2)) + std::pow(_tmp348, Scalar(2)) + + std::pow(_tmp349, Scalar(2)) + std::pow(_tmp350, Scalar(2)); + _hessian(9, 8) = _tmp340 * _tmp351 + _tmp346 * _tmp352 + _tmp347 * _tmp353 + _tmp348 * _tmp354 + + _tmp349 * _tmp355 + _tmp350 * _tmp356; + _hessian(10, 8) = _tmp340 * _tmp357 + _tmp346 * _tmp358 + _tmp347 * _tmp359 + + _tmp348 * _tmp360 + _tmp349 * _tmp361 + _tmp350 * _tmp362; + _hessian(11, 8) = _tmp340 * _tmp363 + _tmp346 * _tmp364 + _tmp347 * _tmp365 + + _tmp348 * _tmp366 + _tmp349 * _tmp367 + _tmp350 * _tmp368; + _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(_tmp351, Scalar(2)) + std::pow(_tmp352, Scalar(2)) + + std::pow(_tmp353, Scalar(2)) + std::pow(_tmp354, Scalar(2)) + + std::pow(_tmp355, Scalar(2)) + std::pow(_tmp356, Scalar(2)); + _hessian(10, 9) = _tmp351 * _tmp357 + _tmp352 * _tmp358 + _tmp353 * _tmp359 + + _tmp354 * _tmp360 + _tmp355 * _tmp361 + _tmp356 * _tmp362; + _hessian(11, 9) = _tmp351 * _tmp363 + _tmp352 * _tmp364 + _tmp353 * _tmp365 + + _tmp354 * _tmp366 + _tmp355 * _tmp367 + _tmp356 * _tmp368; + _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(_tmp357, Scalar(2)) + std::pow(_tmp358, Scalar(2)) + + std::pow(_tmp359, Scalar(2)) + std::pow(_tmp360, Scalar(2)) + + std::pow(_tmp361, Scalar(2)) + std::pow(_tmp362, Scalar(2)); + _hessian(11, 10) = _tmp357 * _tmp363 + _tmp358 * _tmp364 + _tmp359 * _tmp365 + + _tmp360 * _tmp366 + _tmp361 * _tmp367 + _tmp362 * _tmp368; + _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(_tmp363, Scalar(2)) + std::pow(_tmp364, Scalar(2)) + + std::pow(_tmp365, Scalar(2)) + std::pow(_tmp366, Scalar(2)) + + std::pow(_tmp367, Scalar(2)) + std::pow(_tmp368, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp166 * _tmp79 + _tmp174 * _tmp82 + _tmp176 * _tmp83 + _tmp178 * _tmp84 + + _tmp180 * _tmp85 + _tmp182 * _tmp87; + _rhs(1, 0) = _tmp216 * _tmp79 + _tmp222 * _tmp82 + _tmp224 * _tmp83 + _tmp226 * _tmp84 + + _tmp227 * _tmp85 + _tmp229 * _tmp87; + _rhs(2, 0) = _tmp251 * _tmp79 + _tmp256 * _tmp82 + _tmp257 * _tmp83 + _tmp258 * _tmp84 + + _tmp259 * _tmp85 + _tmp260 * _tmp87; + _rhs(3, 0) = _tmp263 * _tmp79 + _tmp264 * _tmp82 + _tmp265 * _tmp83 + _tmp266 * _tmp84 + + _tmp267 * _tmp85 + _tmp268 * _tmp87; + _rhs(4, 0) = _tmp270 * _tmp79 + _tmp271 * _tmp82 + _tmp272 * _tmp83 + _tmp273 * _tmp84 + + _tmp274 * _tmp85 + _tmp275 * _tmp87; + _rhs(5, 0) = _tmp277 * _tmp79 + _tmp278 * _tmp82 + _tmp279 * _tmp83 + _tmp280 * _tmp84 + + _tmp281 * _tmp85 + _tmp282 * _tmp87; + _rhs(6, 0) = _tmp299 * _tmp79 + _tmp305 * _tmp82 + _tmp306 * _tmp83 + _tmp307 * _tmp84 + + _tmp308 * _tmp85 + _tmp309 * _tmp87; + _rhs(7, 0) = _tmp319 * _tmp79 + _tmp325 * _tmp82 + _tmp327 * _tmp83 + _tmp328 * _tmp84 + + _tmp329 * _tmp85 + _tmp330 * _tmp87; + _rhs(8, 0) = _tmp340 * _tmp79 + _tmp346 * _tmp82 + _tmp347 * _tmp83 + _tmp348 * _tmp84 + + _tmp349 * _tmp85 + _tmp350 * _tmp87; + _rhs(9, 0) = _tmp351 * _tmp79 + _tmp352 * _tmp82 + _tmp353 * _tmp83 + _tmp354 * _tmp84 + + _tmp355 * _tmp85 + _tmp356 * _tmp87; + _rhs(10, 0) = _tmp357 * _tmp79 + _tmp358 * _tmp82 + _tmp359 * _tmp83 + _tmp360 * _tmp84 + + _tmp361 * _tmp85 + _tmp362 * _tmp87; + _rhs(11, 0) = _tmp363 * _tmp79 + _tmp364 * _tmp82 + _tmp365 * _tmp83 + _tmp366 * _tmp84 + + _tmp367 * _tmp85 + _tmp368 * _tmp87; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_position.h b/gen/cpp/sym/factors/between_factor_pose3_position.h index 96a7afc3a..04e754b76 100644 --- a/gen/cpp/sym/factors/between_factor_pose3_position.h +++ b/gen/cpp/sym/factors/between_factor_pose3_position.h @@ -1,311 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_t_b. - * - * In vector space terms that would be: - * (b - a) - a_t_b - * - * In lie group terms: - * local_coordinates(a_t_b, between(a, b)) - * to_tangent(compose(inverse(a_t_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x12) jacobian of res wrt args a (6), b (6) - * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) - * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) - */ -template -void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& b, - const Eigen::Matrix& a_t_b, - const Eigen::Matrix& sqrt_info, 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: 553 - - // Unused inputs - (void)epsilon; - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - - // Intermediate terms (105) - const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = -_tmp1; - const Scalar _tmp3 = std::pow(_a[1], Scalar(2)); - const Scalar _tmp4 = 2 * _tmp3; - const Scalar _tmp5 = -_tmp4; - const Scalar _tmp6 = _tmp2 + _tmp5 + 1; - const Scalar _tmp7 = 2 * _a[0]; - const Scalar _tmp8 = _a[2] * _tmp7; - const Scalar _tmp9 = 2 * _a[3]; - const Scalar _tmp10 = _a[1] * _tmp9; - const Scalar _tmp11 = -_tmp10; - const Scalar _tmp12 = _tmp11 + _tmp8; - const Scalar _tmp13 = _a[6] * _tmp12; - const Scalar _tmp14 = _a[1] * _tmp7; - const Scalar _tmp15 = _a[2] * _tmp9; - const Scalar _tmp16 = _tmp14 + _tmp15; - const Scalar _tmp17 = _a[5] * _tmp16; - const Scalar _tmp18 = _b[5] * _tmp16 + _b[6] * _tmp12; - const Scalar _tmp19 = -_a[4] * _tmp6 + _b[4] * _tmp6 - _tmp13 - _tmp17 + _tmp18 - a_t_b(0, 0); - const Scalar _tmp20 = std::pow(_a[0], Scalar(2)); - const Scalar _tmp21 = 2 * _tmp20; - const Scalar _tmp22 = 1 - _tmp21; - const Scalar _tmp23 = _tmp2 + _tmp22; - const Scalar _tmp24 = 2 * _a[1] * _a[2]; - const Scalar _tmp25 = _a[3] * _tmp7; - const Scalar _tmp26 = _tmp24 + _tmp25; - const Scalar _tmp27 = _a[6] * _tmp26; - const Scalar _tmp28 = -_tmp15; - const Scalar _tmp29 = _tmp14 + _tmp28; - const Scalar _tmp30 = _a[4] * _tmp29; - const Scalar _tmp31 = _b[4] * _tmp29 + _b[6] * _tmp26; - const Scalar _tmp32 = -_a[5] * _tmp23 + _b[5] * _tmp23 - _tmp27 - _tmp30 + _tmp31 - a_t_b(1, 0); - const Scalar _tmp33 = _tmp22 + _tmp5; - const Scalar _tmp34 = -_tmp25; - const Scalar _tmp35 = _tmp24 + _tmp34; - const Scalar _tmp36 = _a[5] * _tmp35; - const Scalar _tmp37 = _tmp10 + _tmp8; - const Scalar _tmp38 = _a[4] * _tmp37; - const Scalar _tmp39 = _b[4] * _tmp37 + _b[5] * _tmp35; - const Scalar _tmp40 = -_a[6] * _tmp33 + _b[6] * _tmp33 - _tmp36 - _tmp38 + _tmp39 - a_t_b(2, 0); - const Scalar _tmp41 = - _tmp19 * sqrt_info(0, 0) + _tmp32 * sqrt_info(0, 1) + _tmp40 * sqrt_info(0, 2); - const Scalar _tmp42 = - _tmp19 * sqrt_info(1, 0) + _tmp32 * sqrt_info(1, 1) + _tmp40 * sqrt_info(1, 2); - const Scalar _tmp43 = - _tmp19 * sqrt_info(2, 0) + _tmp32 * sqrt_info(2, 1) + _tmp40 * sqrt_info(2, 2); - const Scalar _tmp44 = -_tmp20; - const Scalar _tmp45 = std::pow(_a[3], Scalar(2)); - const Scalar _tmp46 = _tmp44 + _tmp45; - const Scalar _tmp47 = -_tmp3; - const Scalar _tmp48 = _tmp0 + _tmp47; - const Scalar _tmp49 = _tmp46 + _tmp48; - const Scalar _tmp50 = -_a[6] * _tmp49 + _b[6] * _tmp49 - _tmp36 - _tmp38 + _tmp39; - const Scalar _tmp51 = -_tmp45; - const Scalar _tmp52 = _tmp20 + _tmp51; - const Scalar _tmp53 = _tmp48 + _tmp52; - const Scalar _tmp54 = -_tmp24; - const Scalar _tmp55 = _tmp34 + _tmp54; - const Scalar _tmp56 = -_tmp14; - const Scalar _tmp57 = _tmp15 + _tmp56; - const Scalar _tmp58 = -_a[4] * _tmp57 - _a[5] * _tmp53 - _a[6] * _tmp55 + _b[4] * _tmp57 + - _b[5] * _tmp53 + _b[6] * _tmp55; - const Scalar _tmp59 = _tmp50 * sqrt_info(0, 1) + _tmp58 * sqrt_info(0, 2); - const Scalar _tmp60 = _tmp50 * sqrt_info(1, 1) + _tmp58 * sqrt_info(1, 2); - const Scalar _tmp61 = _tmp50 * sqrt_info(2, 1) + _tmp58 * sqrt_info(2, 2); - const Scalar _tmp62 = -_tmp0; - const Scalar _tmp63 = _tmp3 + _tmp62; - const Scalar _tmp64 = _tmp52 + _tmp63; - const Scalar _tmp65 = _tmp25 + _tmp54; - const Scalar _tmp66 = -_tmp8; - const Scalar _tmp67 = _tmp11 + _tmp66; - const Scalar _tmp68 = -_a[4] * _tmp67 - _a[5] * _tmp65 - _a[6] * _tmp64 + _b[4] * _tmp67 + - _b[5] * _tmp65 + _b[6] * _tmp64; - const Scalar _tmp69 = _tmp20 + _tmp45 + _tmp47 + _tmp62; - const Scalar _tmp70 = -_a[4] * _tmp69 + _b[4] * _tmp69 - _tmp13 - _tmp17 + _tmp18; - const Scalar _tmp71 = _tmp68 * sqrt_info(0, 0) + _tmp70 * sqrt_info(0, 2); - const Scalar _tmp72 = _tmp68 * sqrt_info(1, 0) + _tmp70 * sqrt_info(1, 2); - const Scalar _tmp73 = _tmp68 * sqrt_info(2, 0) + _tmp70 * sqrt_info(2, 2); - const Scalar _tmp74 = _tmp46 + _tmp63; - const Scalar _tmp75 = -_a[5] * _tmp74 + _b[5] * _tmp74 - _tmp27 - _tmp30 + _tmp31; - const Scalar _tmp76 = _tmp0 + _tmp3 + _tmp44 + _tmp51; - const Scalar _tmp77 = _tmp10 + _tmp66; - const Scalar _tmp78 = _tmp28 + _tmp56; - const Scalar _tmp79 = -_a[4] * _tmp76 - _a[5] * _tmp78 - _a[6] * _tmp77 + _b[4] * _tmp76 + - _b[5] * _tmp78 + _b[6] * _tmp77; - const Scalar _tmp80 = _tmp75 * sqrt_info(0, 0) + _tmp79 * sqrt_info(0, 1); - const Scalar _tmp81 = _tmp75 * sqrt_info(1, 0) + _tmp79 * sqrt_info(1, 1); - const Scalar _tmp82 = _tmp75 * sqrt_info(2, 0) + _tmp79 * sqrt_info(2, 1); - const Scalar _tmp83 = _tmp1 + _tmp4 - 1; - const Scalar _tmp84 = - _tmp57 * sqrt_info(0, 1) + _tmp67 * sqrt_info(0, 2) + _tmp83 * sqrt_info(0, 0); - const Scalar _tmp85 = - _tmp57 * sqrt_info(1, 1) + _tmp67 * sqrt_info(1, 2) + _tmp83 * sqrt_info(1, 0); - const Scalar _tmp86 = - _tmp57 * sqrt_info(2, 1) + _tmp67 * sqrt_info(2, 2) + _tmp83 * sqrt_info(2, 0); - const Scalar _tmp87 = _tmp21 - 1; - const Scalar _tmp88 = _tmp1 + _tmp87; - const Scalar _tmp89 = - _tmp65 * sqrt_info(0, 2) + _tmp78 * sqrt_info(0, 0) + _tmp88 * sqrt_info(0, 1); - const Scalar _tmp90 = - _tmp65 * sqrt_info(1, 2) + _tmp78 * sqrt_info(1, 0) + _tmp88 * sqrt_info(1, 1); - const Scalar _tmp91 = - _tmp65 * sqrt_info(2, 2) + _tmp78 * sqrt_info(2, 0) + _tmp88 * sqrt_info(2, 1); - const Scalar _tmp92 = _tmp4 + _tmp87; - const Scalar _tmp93 = - _tmp55 * sqrt_info(0, 1) + _tmp77 * sqrt_info(0, 0) + _tmp92 * sqrt_info(0, 2); - const Scalar _tmp94 = - _tmp55 * sqrt_info(1, 1) + _tmp77 * sqrt_info(1, 0) + _tmp92 * sqrt_info(1, 2); - const Scalar _tmp95 = - _tmp55 * sqrt_info(2, 1) + _tmp77 * sqrt_info(2, 0) + _tmp92 * sqrt_info(2, 2); - const Scalar _tmp96 = - _tmp29 * sqrt_info(0, 1) + _tmp37 * sqrt_info(0, 2) + _tmp6 * sqrt_info(0, 0); - const Scalar _tmp97 = - _tmp29 * sqrt_info(1, 1) + _tmp37 * sqrt_info(1, 2) + _tmp6 * sqrt_info(1, 0); - const Scalar _tmp98 = - _tmp29 * sqrt_info(2, 1) + _tmp37 * sqrt_info(2, 2) + _tmp6 * sqrt_info(2, 0); - const Scalar _tmp99 = - _tmp16 * sqrt_info(0, 0) + _tmp23 * sqrt_info(0, 1) + _tmp35 * sqrt_info(0, 2); - const Scalar _tmp100 = - _tmp16 * sqrt_info(1, 0) + _tmp23 * sqrt_info(1, 1) + _tmp35 * sqrt_info(1, 2); - const Scalar _tmp101 = - _tmp16 * sqrt_info(2, 0) + _tmp23 * sqrt_info(2, 1) + _tmp35 * sqrt_info(2, 2); - const Scalar _tmp102 = - _tmp12 * sqrt_info(0, 0) + _tmp26 * sqrt_info(0, 1) + _tmp33 * sqrt_info(0, 2); - const Scalar _tmp103 = - _tmp12 * sqrt_info(1, 0) + _tmp26 * sqrt_info(1, 1) + _tmp33 * sqrt_info(1, 2); - const Scalar _tmp104 = - _tmp12 * sqrt_info(2, 0) + _tmp26 * sqrt_info(2, 1) + _tmp33 * sqrt_info(2, 2); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp41; - _res(1, 0) = _tmp42; - _res(2, 0) = _tmp43; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp59; - _jacobian(1, 0) = _tmp60; - _jacobian(2, 0) = _tmp61; - _jacobian(0, 1) = _tmp71; - _jacobian(1, 1) = _tmp72; - _jacobian(2, 1) = _tmp73; - _jacobian(0, 2) = _tmp80; - _jacobian(1, 2) = _tmp81; - _jacobian(2, 2) = _tmp82; - _jacobian(0, 3) = _tmp84; - _jacobian(1, 3) = _tmp85; - _jacobian(2, 3) = _tmp86; - _jacobian(0, 4) = _tmp89; - _jacobian(1, 4) = _tmp90; - _jacobian(2, 4) = _tmp91; - _jacobian(0, 5) = _tmp93; - _jacobian(1, 5) = _tmp94; - _jacobian(2, 5) = _tmp95; - _jacobian(0, 6) = 0; - _jacobian(1, 6) = 0; - _jacobian(2, 6) = 0; - _jacobian(0, 7) = 0; - _jacobian(1, 7) = 0; - _jacobian(2, 7) = 0; - _jacobian(0, 8) = 0; - _jacobian(1, 8) = 0; - _jacobian(2, 8) = 0; - _jacobian(0, 9) = _tmp96; - _jacobian(1, 9) = _tmp97; - _jacobian(2, 9) = _tmp98; - _jacobian(0, 10) = _tmp99; - _jacobian(1, 10) = _tmp100; - _jacobian(2, 10) = _tmp101; - _jacobian(0, 11) = _tmp102; - _jacobian(1, 11) = _tmp103; - _jacobian(2, 11) = _tmp104; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian.setZero(); - - _hessian(0, 0) = - std::pow(_tmp59, Scalar(2)) + std::pow(_tmp60, Scalar(2)) + std::pow(_tmp61, Scalar(2)); - _hessian(1, 0) = _tmp59 * _tmp71 + _tmp60 * _tmp72 + _tmp61 * _tmp73; - _hessian(2, 0) = _tmp59 * _tmp80 + _tmp60 * _tmp81 + _tmp61 * _tmp82; - _hessian(3, 0) = _tmp59 * _tmp84 + _tmp60 * _tmp85 + _tmp61 * _tmp86; - _hessian(4, 0) = _tmp59 * _tmp89 + _tmp60 * _tmp90 + _tmp61 * _tmp91; - _hessian(5, 0) = _tmp59 * _tmp93 + _tmp60 * _tmp94 + _tmp61 * _tmp95; - _hessian(9, 0) = _tmp59 * _tmp96 + _tmp60 * _tmp97 + _tmp61 * _tmp98; - _hessian(10, 0) = _tmp100 * _tmp60 + _tmp101 * _tmp61 + _tmp59 * _tmp99; - _hessian(11, 0) = _tmp102 * _tmp59 + _tmp103 * _tmp60 + _tmp104 * _tmp61; - _hessian(1, 1) = - std::pow(_tmp71, Scalar(2)) + std::pow(_tmp72, Scalar(2)) + std::pow(_tmp73, Scalar(2)); - _hessian(2, 1) = _tmp71 * _tmp80 + _tmp72 * _tmp81 + _tmp73 * _tmp82; - _hessian(3, 1) = _tmp71 * _tmp84 + _tmp72 * _tmp85 + _tmp73 * _tmp86; - _hessian(4, 1) = _tmp71 * _tmp89 + _tmp72 * _tmp90 + _tmp73 * _tmp91; - _hessian(5, 1) = _tmp71 * _tmp93 + _tmp72 * _tmp94 + _tmp73 * _tmp95; - _hessian(9, 1) = _tmp71 * _tmp96 + _tmp72 * _tmp97 + _tmp73 * _tmp98; - _hessian(10, 1) = _tmp100 * _tmp72 + _tmp101 * _tmp73 + _tmp71 * _tmp99; - _hessian(11, 1) = _tmp102 * _tmp71 + _tmp103 * _tmp72 + _tmp104 * _tmp73; - _hessian(2, 2) = - std::pow(_tmp80, Scalar(2)) + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp82, Scalar(2)); - _hessian(3, 2) = _tmp80 * _tmp84 + _tmp81 * _tmp85 + _tmp82 * _tmp86; - _hessian(4, 2) = _tmp80 * _tmp89 + _tmp81 * _tmp90 + _tmp82 * _tmp91; - _hessian(5, 2) = _tmp80 * _tmp93 + _tmp81 * _tmp94 + _tmp82 * _tmp95; - _hessian(9, 2) = _tmp80 * _tmp96 + _tmp81 * _tmp97 + _tmp82 * _tmp98; - _hessian(10, 2) = _tmp100 * _tmp81 + _tmp101 * _tmp82 + _tmp80 * _tmp99; - _hessian(11, 2) = _tmp102 * _tmp80 + _tmp103 * _tmp81 + _tmp104 * _tmp82; - _hessian(3, 3) = - std::pow(_tmp84, Scalar(2)) + std::pow(_tmp85, Scalar(2)) + std::pow(_tmp86, Scalar(2)); - _hessian(4, 3) = _tmp84 * _tmp89 + _tmp85 * _tmp90 + _tmp86 * _tmp91; - _hessian(5, 3) = _tmp84 * _tmp93 + _tmp85 * _tmp94 + _tmp86 * _tmp95; - _hessian(9, 3) = _tmp84 * _tmp96 + _tmp85 * _tmp97 + _tmp86 * _tmp98; - _hessian(10, 3) = _tmp100 * _tmp85 + _tmp101 * _tmp86 + _tmp84 * _tmp99; - _hessian(11, 3) = _tmp102 * _tmp84 + _tmp103 * _tmp85 + _tmp104 * _tmp86; - _hessian(4, 4) = - std::pow(_tmp89, Scalar(2)) + std::pow(_tmp90, Scalar(2)) + std::pow(_tmp91, Scalar(2)); - _hessian(5, 4) = _tmp89 * _tmp93 + _tmp90 * _tmp94 + _tmp91 * _tmp95; - _hessian(9, 4) = _tmp89 * _tmp96 + _tmp90 * _tmp97 + _tmp91 * _tmp98; - _hessian(10, 4) = _tmp100 * _tmp90 + _tmp101 * _tmp91 + _tmp89 * _tmp99; - _hessian(11, 4) = _tmp102 * _tmp89 + _tmp103 * _tmp90 + _tmp104 * _tmp91; - _hessian(5, 5) = - std::pow(_tmp93, Scalar(2)) + std::pow(_tmp94, Scalar(2)) + std::pow(_tmp95, Scalar(2)); - _hessian(9, 5) = _tmp93 * _tmp96 + _tmp94 * _tmp97 + _tmp95 * _tmp98; - _hessian(10, 5) = _tmp100 * _tmp94 + _tmp101 * _tmp95 + _tmp93 * _tmp99; - _hessian(11, 5) = _tmp102 * _tmp93 + _tmp103 * _tmp94 + _tmp104 * _tmp95; - _hessian(9, 9) = - std::pow(_tmp96, Scalar(2)) + std::pow(_tmp97, Scalar(2)) + std::pow(_tmp98, Scalar(2)); - _hessian(10, 9) = _tmp100 * _tmp97 + _tmp101 * _tmp98 + _tmp96 * _tmp99; - _hessian(11, 9) = _tmp102 * _tmp96 + _tmp103 * _tmp97 + _tmp104 * _tmp98; - _hessian(10, 10) = - std::pow(_tmp100, Scalar(2)) + std::pow(_tmp101, Scalar(2)) + std::pow(_tmp99, Scalar(2)); - _hessian(11, 10) = _tmp100 * _tmp103 + _tmp101 * _tmp104 + _tmp102 * _tmp99; - _hessian(11, 11) = - std::pow(_tmp102, Scalar(2)) + std::pow(_tmp103, Scalar(2)) + std::pow(_tmp104, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp41 * _tmp59 + _tmp42 * _tmp60 + _tmp43 * _tmp61; - _rhs(1, 0) = _tmp41 * _tmp71 + _tmp42 * _tmp72 + _tmp43 * _tmp73; - _rhs(2, 0) = _tmp41 * _tmp80 + _tmp42 * _tmp81 + _tmp43 * _tmp82; - _rhs(3, 0) = _tmp41 * _tmp84 + _tmp42 * _tmp85 + _tmp43 * _tmp86; - _rhs(4, 0) = _tmp41 * _tmp89 + _tmp42 * _tmp90 + _tmp43 * _tmp91; - _rhs(5, 0) = _tmp41 * _tmp93 + _tmp42 * _tmp94 + _tmp43 * _tmp95; - _rhs(6, 0) = 0; - _rhs(7, 0) = 0; - _rhs(8, 0) = 0; - _rhs(9, 0) = _tmp41 * _tmp96 + _tmp42 * _tmp97 + _tmp43 * _tmp98; - _rhs(10, 0) = _tmp100 * _tmp42 + _tmp101 * _tmp43 + _tmp41 * _tmp99; - _rhs(11, 0) = _tmp102 * _tmp41 + _tmp103 * _tmp42 + _tmp104 * _tmp43; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_pose3_position/between_factor_pose3_position_diagonal.h" +#include "./between_factor_pose3_position/between_factor_pose3_position_isotropic.h" +#include "./between_factor_pose3_position/between_factor_pose3_position_square.h" diff --git a/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_diagonal.h b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_diagonal.h new file mode 100644 index 000000000..de271a365 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_diagonal.h @@ -0,0 +1,282 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_t_b. + * + * In vector space terms that would be: + * (b - a) - a_t_b + * + * In lie group terms: + * local_coordinates(a_t_b, between(a, b)) + * to_tangent(compose(inverse(a_t_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& b, + const Eigen::Matrix& a_t_b, + const Eigen::Matrix& sqrt_info, 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: 439 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + + // Intermediate terms (99) + const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = -_tmp1; + const Scalar _tmp3 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = -_tmp4; + const Scalar _tmp6 = _tmp2 + _tmp5 + 1; + const Scalar _tmp7 = 2 * _a[2]; + const Scalar _tmp8 = _a[0] * _tmp7; + const Scalar _tmp9 = 2 * _a[1]; + const Scalar _tmp10 = _a[3] * _tmp9; + const Scalar _tmp11 = -_tmp10; + const Scalar _tmp12 = _tmp11 + _tmp8; + const Scalar _tmp13 = _a[6] * _tmp12; + const Scalar _tmp14 = _a[0] * _tmp9; + const Scalar _tmp15 = _a[3] * _tmp7; + const Scalar _tmp16 = _tmp14 + _tmp15; + const Scalar _tmp17 = _a[5] * _tmp16; + const Scalar _tmp18 = _b[5] * _tmp16 + _b[6] * _tmp12; + const Scalar _tmp19 = -_a[4] * _tmp6 + _b[4] * _tmp6 - _tmp13 - _tmp17 + _tmp18 - a_t_b(0, 0); + const Scalar _tmp20 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp21 = 2 * _tmp20; + const Scalar _tmp22 = 1 - _tmp21; + const Scalar _tmp23 = _tmp2 + _tmp22; + const Scalar _tmp24 = _a[2] * _tmp9; + const Scalar _tmp25 = 2 * _a[0] * _a[3]; + const Scalar _tmp26 = _tmp24 + _tmp25; + const Scalar _tmp27 = _a[6] * _tmp26; + const Scalar _tmp28 = -_tmp15; + const Scalar _tmp29 = _tmp14 + _tmp28; + const Scalar _tmp30 = _a[4] * _tmp29; + const Scalar _tmp31 = _b[4] * _tmp29 + _b[6] * _tmp26; + const Scalar _tmp32 = -_a[5] * _tmp23 + _b[5] * _tmp23 - _tmp27 - _tmp30 + _tmp31 - a_t_b(1, 0); + const Scalar _tmp33 = _tmp22 + _tmp5; + const Scalar _tmp34 = -_tmp25; + const Scalar _tmp35 = _tmp24 + _tmp34; + const Scalar _tmp36 = _a[5] * _tmp35; + const Scalar _tmp37 = _tmp10 + _tmp8; + const Scalar _tmp38 = _a[4] * _tmp37; + const Scalar _tmp39 = _b[4] * _tmp37 + _b[5] * _tmp35; + const Scalar _tmp40 = -_a[6] * _tmp33 + _b[6] * _tmp33 - _tmp36 - _tmp38 + _tmp39 - a_t_b(2, 0); + const Scalar _tmp41 = -_tmp20; + const Scalar _tmp42 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp43 = _tmp41 + _tmp42; + const Scalar _tmp44 = -_tmp3; + const Scalar _tmp45 = _tmp0 + _tmp44; + const Scalar _tmp46 = _tmp43 + _tmp45; + const Scalar _tmp47 = -_a[6] * _tmp46 + _b[6] * _tmp46 - _tmp36 - _tmp38 + _tmp39; + const Scalar _tmp48 = -_tmp42; + const Scalar _tmp49 = _tmp20 + _tmp48; + const Scalar _tmp50 = _tmp45 + _tmp49; + const Scalar _tmp51 = -_tmp24; + const Scalar _tmp52 = _tmp34 + _tmp51; + const Scalar _tmp53 = -_tmp14; + const Scalar _tmp54 = _tmp15 + _tmp53; + const Scalar _tmp55 = -_a[4] * _tmp54 - _a[5] * _tmp50 - _a[6] * _tmp52 + _b[4] * _tmp54 + + _b[5] * _tmp50 + _b[6] * _tmp52; + const Scalar _tmp56 = -_tmp0; + const Scalar _tmp57 = _tmp3 + _tmp56; + const Scalar _tmp58 = _tmp49 + _tmp57; + const Scalar _tmp59 = _tmp25 + _tmp51; + const Scalar _tmp60 = -_tmp8; + const Scalar _tmp61 = _tmp11 + _tmp60; + const Scalar _tmp62 = -_a[4] * _tmp61 - _a[5] * _tmp59 - _a[6] * _tmp58 + _b[4] * _tmp61 + + _b[5] * _tmp59 + _b[6] * _tmp58; + const Scalar _tmp63 = _tmp20 + _tmp42 + _tmp44 + _tmp56; + const Scalar _tmp64 = -_a[4] * _tmp63 + _b[4] * _tmp63 - _tmp13 - _tmp17 + _tmp18; + const Scalar _tmp65 = _tmp43 + _tmp57; + const Scalar _tmp66 = -_a[5] * _tmp65 + _b[5] * _tmp65 - _tmp27 - _tmp30 + _tmp31; + const Scalar _tmp67 = _tmp0 + _tmp3 + _tmp41 + _tmp48; + const Scalar _tmp68 = _tmp10 + _tmp60; + const Scalar _tmp69 = _tmp28 + _tmp53; + const Scalar _tmp70 = -_a[4] * _tmp67 - _a[5] * _tmp69 - _a[6] * _tmp68 + _b[4] * _tmp67 + + _b[5] * _tmp69 + _b[6] * _tmp68; + const Scalar _tmp71 = _tmp1 + _tmp4 - 1; + const Scalar _tmp72 = _tmp21 - 1; + const Scalar _tmp73 = _tmp1 + _tmp72; + const Scalar _tmp74 = _tmp4 + _tmp72; + const Scalar _tmp75 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp76 = std::pow(sqrt_info(2, 0), Scalar(2)); + const Scalar _tmp77 = _tmp64 * _tmp76; + const Scalar _tmp78 = _tmp70 * _tmp75; + const Scalar _tmp79 = _tmp47 * _tmp75; + const Scalar _tmp80 = _tmp61 * _tmp76; + const Scalar _tmp81 = _tmp73 * _tmp75; + const Scalar _tmp82 = _tmp59 * _tmp76; + const Scalar _tmp83 = _tmp74 * _tmp76; + const Scalar _tmp84 = _tmp37 * _tmp76; + const Scalar _tmp85 = _tmp23 * _tmp75; + const Scalar _tmp86 = _tmp33 * _tmp76; + const Scalar _tmp87 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp88 = _tmp62 * _tmp87; + const Scalar _tmp89 = _tmp71 * _tmp87; + const Scalar _tmp90 = _tmp66 * _tmp87; + const Scalar _tmp91 = _tmp6 * _tmp87; + const Scalar _tmp92 = _tmp52 * _tmp75; + const Scalar _tmp93 = _tmp26 * _tmp75; + const Scalar _tmp94 = _tmp69 * _tmp87; + const Scalar _tmp95 = _tmp16 * _tmp87; + const Scalar _tmp96 = _tmp40 * _tmp76; + const Scalar _tmp97 = _tmp32 * _tmp75; + const Scalar _tmp98 = _tmp19 * _tmp87; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp19 * sqrt_info(0, 0); + _res(1, 0) = _tmp32 * sqrt_info(1, 0); + _res(2, 0) = _tmp40 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = 0; + _jacobian(1, 0) = _tmp47 * sqrt_info(1, 0); + _jacobian(2, 0) = _tmp55 * sqrt_info(2, 0); + _jacobian(0, 1) = _tmp62 * sqrt_info(0, 0); + _jacobian(1, 1) = 0; + _jacobian(2, 1) = _tmp64 * sqrt_info(2, 0); + _jacobian(0, 2) = _tmp66 * sqrt_info(0, 0); + _jacobian(1, 2) = _tmp70 * sqrt_info(1, 0); + _jacobian(2, 2) = 0; + _jacobian(0, 3) = _tmp71 * sqrt_info(0, 0); + _jacobian(1, 3) = _tmp54 * sqrt_info(1, 0); + _jacobian(2, 3) = _tmp61 * sqrt_info(2, 0); + _jacobian(0, 4) = _tmp69 * sqrt_info(0, 0); + _jacobian(1, 4) = _tmp73 * sqrt_info(1, 0); + _jacobian(2, 4) = _tmp59 * sqrt_info(2, 0); + _jacobian(0, 5) = _tmp68 * sqrt_info(0, 0); + _jacobian(1, 5) = _tmp52 * sqrt_info(1, 0); + _jacobian(2, 5) = _tmp74 * sqrt_info(2, 0); + _jacobian(0, 6) = 0; + _jacobian(1, 6) = 0; + _jacobian(2, 6) = 0; + _jacobian(0, 7) = 0; + _jacobian(1, 7) = 0; + _jacobian(2, 7) = 0; + _jacobian(0, 8) = 0; + _jacobian(1, 8) = 0; + _jacobian(2, 8) = 0; + _jacobian(0, 9) = _tmp6 * sqrt_info(0, 0); + _jacobian(1, 9) = _tmp29 * sqrt_info(1, 0); + _jacobian(2, 9) = _tmp37 * sqrt_info(2, 0); + _jacobian(0, 10) = _tmp16 * sqrt_info(0, 0); + _jacobian(1, 10) = _tmp23 * sqrt_info(1, 0); + _jacobian(2, 10) = _tmp35 * sqrt_info(2, 0); + _jacobian(0, 11) = _tmp12 * sqrt_info(0, 0); + _jacobian(1, 11) = _tmp26 * sqrt_info(1, 0); + _jacobian(2, 11) = _tmp33 * sqrt_info(2, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp47, Scalar(2)) * _tmp75 + std::pow(_tmp55, Scalar(2)) * _tmp76; + _hessian(1, 0) = _tmp55 * _tmp77; + _hessian(2, 0) = _tmp47 * _tmp78; + _hessian(3, 0) = _tmp54 * _tmp79 + _tmp55 * _tmp80; + _hessian(4, 0) = _tmp47 * _tmp81 + _tmp55 * _tmp82; + _hessian(5, 0) = _tmp52 * _tmp79 + _tmp55 * _tmp83; + _hessian(9, 0) = _tmp29 * _tmp79 + _tmp55 * _tmp84; + _hessian(10, 0) = _tmp35 * _tmp55 * _tmp76 + _tmp47 * _tmp85; + _hessian(11, 0) = _tmp26 * _tmp79 + _tmp55 * _tmp86; + _hessian(1, 1) = std::pow(_tmp62, Scalar(2)) * _tmp87 + std::pow(_tmp64, Scalar(2)) * _tmp76; + _hessian(2, 1) = _tmp66 * _tmp88; + _hessian(3, 1) = _tmp64 * _tmp80 + _tmp71 * _tmp88; + _hessian(4, 1) = _tmp64 * _tmp82 + _tmp69 * _tmp88; + _hessian(5, 1) = _tmp64 * _tmp83 + _tmp68 * _tmp88; + _hessian(9, 1) = _tmp6 * _tmp88 + _tmp64 * _tmp84; + _hessian(10, 1) = _tmp16 * _tmp88 + _tmp35 * _tmp77; + _hessian(11, 1) = _tmp12 * _tmp88 + _tmp64 * _tmp86; + _hessian(2, 2) = std::pow(_tmp66, Scalar(2)) * _tmp87 + std::pow(_tmp70, Scalar(2)) * _tmp75; + _hessian(3, 2) = _tmp54 * _tmp78 + _tmp66 * _tmp89; + _hessian(4, 2) = _tmp69 * _tmp90 + _tmp70 * _tmp81; + _hessian(5, 2) = _tmp52 * _tmp78 + _tmp68 * _tmp90; + _hessian(9, 2) = _tmp29 * _tmp78 + _tmp66 * _tmp91; + _hessian(10, 2) = _tmp16 * _tmp90 + _tmp70 * _tmp85; + _hessian(11, 2) = _tmp12 * _tmp90 + _tmp26 * _tmp78; + _hessian(3, 3) = std::pow(_tmp54, Scalar(2)) * _tmp75 + std::pow(_tmp61, Scalar(2)) * _tmp76 + + std::pow(_tmp71, Scalar(2)) * _tmp87; + _hessian(4, 3) = _tmp54 * _tmp81 + _tmp59 * _tmp80 + _tmp69 * _tmp89; + _hessian(5, 3) = _tmp54 * _tmp92 + _tmp61 * _tmp83 + _tmp68 * _tmp89; + _hessian(9, 3) = _tmp29 * _tmp54 * _tmp75 + _tmp37 * _tmp80 + _tmp6 * _tmp89; + _hessian(10, 3) = _tmp16 * _tmp89 + _tmp35 * _tmp80 + _tmp54 * _tmp85; + _hessian(11, 3) = _tmp12 * _tmp89 + _tmp54 * _tmp93 + _tmp61 * _tmp86; + _hessian(4, 4) = std::pow(_tmp59, Scalar(2)) * _tmp76 + std::pow(_tmp69, Scalar(2)) * _tmp87 + + std::pow(_tmp73, Scalar(2)) * _tmp75; + _hessian(5, 4) = _tmp52 * _tmp81 + _tmp59 * _tmp83 + _tmp68 * _tmp94; + _hessian(9, 4) = _tmp29 * _tmp81 + _tmp59 * _tmp84 + _tmp69 * _tmp91; + _hessian(10, 4) = _tmp23 * _tmp81 + _tmp35 * _tmp82 + _tmp69 * _tmp95; + _hessian(11, 4) = _tmp12 * _tmp94 + _tmp26 * _tmp81 + _tmp59 * _tmp86; + _hessian(5, 5) = std::pow(_tmp52, Scalar(2)) * _tmp75 + std::pow(_tmp68, Scalar(2)) * _tmp87 + + std::pow(_tmp74, Scalar(2)) * _tmp76; + _hessian(9, 5) = _tmp29 * _tmp92 + _tmp37 * _tmp83 + _tmp68 * _tmp91; + _hessian(10, 5) = _tmp35 * _tmp83 + _tmp52 * _tmp85 + _tmp68 * _tmp95; + _hessian(11, 5) = _tmp12 * _tmp68 * _tmp87 + _tmp26 * _tmp92 + _tmp74 * _tmp86; + _hessian(9, 9) = std::pow(_tmp29, Scalar(2)) * _tmp75 + std::pow(_tmp37, Scalar(2)) * _tmp76 + + std::pow(_tmp6, Scalar(2)) * _tmp87; + _hessian(10, 9) = _tmp16 * _tmp91 + _tmp29 * _tmp85 + _tmp35 * _tmp84; + _hessian(11, 9) = _tmp12 * _tmp91 + _tmp29 * _tmp93 + _tmp37 * _tmp86; + _hessian(10, 10) = std::pow(_tmp16, Scalar(2)) * _tmp87 + std::pow(_tmp23, Scalar(2)) * _tmp75 + + std::pow(_tmp35, Scalar(2)) * _tmp76; + _hessian(11, 10) = _tmp12 * _tmp95 + _tmp26 * _tmp85 + _tmp35 * _tmp86; + _hessian(11, 11) = std::pow(_tmp12, Scalar(2)) * _tmp87 + std::pow(_tmp26, Scalar(2)) * _tmp75 + + std::pow(_tmp33, Scalar(2)) * _tmp76; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp47 * _tmp97 + _tmp55 * _tmp96; + _rhs(1, 0) = _tmp19 * _tmp88 + _tmp64 * _tmp96; + _rhs(2, 0) = _tmp19 * _tmp90 + _tmp70 * _tmp97; + _rhs(3, 0) = _tmp19 * _tmp89 + _tmp54 * _tmp97 + _tmp61 * _tmp96; + _rhs(4, 0) = _tmp59 * _tmp96 + _tmp69 * _tmp98 + _tmp73 * _tmp97; + _rhs(5, 0) = _tmp40 * _tmp83 + _tmp52 * _tmp97 + _tmp68 * _tmp98; + _rhs(6, 0) = 0; + _rhs(7, 0) = 0; + _rhs(8, 0) = 0; + _rhs(9, 0) = _tmp19 * _tmp91 + _tmp29 * _tmp97 + _tmp37 * _tmp96; + _rhs(10, 0) = _tmp16 * _tmp98 + _tmp23 * _tmp97 + _tmp35 * _tmp96; + _rhs(11, 0) = _tmp12 * _tmp98 + _tmp26 * _tmp97 + _tmp40 * _tmp86; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_isotropic.h b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_isotropic.h new file mode 100644 index 000000000..402a4c5b3 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_isotropic.h @@ -0,0 +1,280 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_t_b. + * + * In vector space terms that would be: + * (b - a) - a_t_b + * + * In lie group terms: + * local_coordinates(a_t_b, between(a, b)) + * to_tangent(compose(inverse(a_t_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& b, + const Eigen::Matrix& a_t_b, const Scalar sqrt_info, + 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: 437 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + + // Intermediate terms (97) + const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = -_tmp1; + const Scalar _tmp3 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = -_tmp4; + const Scalar _tmp6 = _tmp2 + _tmp5 + 1; + const Scalar _tmp7 = 2 * _a[0] * _a[2]; + const Scalar _tmp8 = 2 * _a[1]; + const Scalar _tmp9 = _a[3] * _tmp8; + const Scalar _tmp10 = -_tmp9; + const Scalar _tmp11 = _tmp10 + _tmp7; + const Scalar _tmp12 = _a[6] * _tmp11; + const Scalar _tmp13 = _a[0] * _tmp8; + const Scalar _tmp14 = 2 * _a[3]; + const Scalar _tmp15 = _a[2] * _tmp14; + const Scalar _tmp16 = _tmp13 + _tmp15; + const Scalar _tmp17 = _a[5] * _tmp16; + const Scalar _tmp18 = _b[5] * _tmp16 + _b[6] * _tmp11; + const Scalar _tmp19 = -_a[4] * _tmp6 + _b[4] * _tmp6 - _tmp12 - _tmp17 + _tmp18 - a_t_b(0, 0); + const Scalar _tmp20 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp21 = 2 * _tmp20; + const Scalar _tmp22 = 1 - _tmp21; + const Scalar _tmp23 = _tmp2 + _tmp22; + const Scalar _tmp24 = _a[2] * _tmp8; + const Scalar _tmp25 = _a[0] * _tmp14; + const Scalar _tmp26 = _tmp24 + _tmp25; + const Scalar _tmp27 = _a[6] * _tmp26; + const Scalar _tmp28 = -_tmp15; + const Scalar _tmp29 = _tmp13 + _tmp28; + const Scalar _tmp30 = _a[4] * _tmp29; + const Scalar _tmp31 = _b[4] * _tmp29 + _b[6] * _tmp26; + const Scalar _tmp32 = -_a[5] * _tmp23 + _b[5] * _tmp23 - _tmp27 - _tmp30 + _tmp31 - a_t_b(1, 0); + const Scalar _tmp33 = _tmp22 + _tmp5; + const Scalar _tmp34 = -_tmp25; + const Scalar _tmp35 = _tmp24 + _tmp34; + const Scalar _tmp36 = _a[5] * _tmp35; + const Scalar _tmp37 = _tmp7 + _tmp9; + const Scalar _tmp38 = _a[4] * _tmp37; + const Scalar _tmp39 = _b[4] * _tmp37 + _b[5] * _tmp35; + const Scalar _tmp40 = -_a[6] * _tmp33 + _b[6] * _tmp33 - _tmp36 - _tmp38 + _tmp39 - a_t_b(2, 0); + const Scalar _tmp41 = -_tmp20; + const Scalar _tmp42 = -_tmp3; + const Scalar _tmp43 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp44 = _tmp0 + _tmp41 + _tmp42 + _tmp43; + const Scalar _tmp45 = -_a[6] * _tmp44 + _b[6] * _tmp44 - _tmp36 - _tmp38 + _tmp39; + const Scalar _tmp46 = -_tmp43; + const Scalar _tmp47 = _tmp0 + _tmp46; + const Scalar _tmp48 = _tmp20 + _tmp42; + const Scalar _tmp49 = _tmp47 + _tmp48; + const Scalar _tmp50 = -_tmp24; + const Scalar _tmp51 = _tmp34 + _tmp50; + const Scalar _tmp52 = -_tmp13; + const Scalar _tmp53 = _tmp15 + _tmp52; + const Scalar _tmp54 = -_a[4] * _tmp53 - _a[5] * _tmp49 - _a[6] * _tmp51 + _b[4] * _tmp53 + + _b[5] * _tmp49 + _b[6] * _tmp51; + const Scalar _tmp55 = -_tmp0; + const Scalar _tmp56 = _tmp20 + _tmp3 + _tmp46 + _tmp55; + const Scalar _tmp57 = _tmp25 + _tmp50; + const Scalar _tmp58 = -_tmp7; + const Scalar _tmp59 = _tmp10 + _tmp58; + const Scalar _tmp60 = -_a[4] * _tmp59 - _a[5] * _tmp57 - _a[6] * _tmp56 + _b[4] * _tmp59 + + _b[5] * _tmp57 + _b[6] * _tmp56; + const Scalar _tmp61 = _tmp43 + _tmp55; + const Scalar _tmp62 = _tmp48 + _tmp61; + const Scalar _tmp63 = -_a[4] * _tmp62 + _b[4] * _tmp62 - _tmp12 - _tmp17 + _tmp18; + const Scalar _tmp64 = _tmp3 + _tmp41; + const Scalar _tmp65 = _tmp61 + _tmp64; + const Scalar _tmp66 = -_a[5] * _tmp65 + _b[5] * _tmp65 - _tmp27 - _tmp30 + _tmp31; + const Scalar _tmp67 = _tmp47 + _tmp64; + const Scalar _tmp68 = _tmp58 + _tmp9; + const Scalar _tmp69 = _tmp28 + _tmp52; + const Scalar _tmp70 = -_a[4] * _tmp67 - _a[5] * _tmp69 - _a[6] * _tmp68 + _b[4] * _tmp67 + + _b[5] * _tmp69 + _b[6] * _tmp68; + const Scalar _tmp71 = _tmp1 + _tmp4 - 1; + const Scalar _tmp72 = _tmp21 - 1; + const Scalar _tmp73 = _tmp1 + _tmp72; + const Scalar _tmp74 = _tmp4 + _tmp72; + const Scalar _tmp75 = std::pow(sqrt_info, Scalar(2)); + const Scalar _tmp76 = _tmp63 * _tmp75; + const Scalar _tmp77 = _tmp70 * _tmp75; + const Scalar _tmp78 = _tmp54 * _tmp75; + const Scalar _tmp79 = _tmp45 * _tmp75; + const Scalar _tmp80 = _tmp57 * _tmp75; + const Scalar _tmp81 = _tmp74 * _tmp75; + const Scalar _tmp82 = _tmp51 * _tmp75; + const Scalar _tmp83 = _tmp35 * _tmp75; + const Scalar _tmp84 = _tmp33 * _tmp75; + const Scalar _tmp85 = _tmp26 * _tmp75; + const Scalar _tmp86 = _tmp66 * _tmp75; + const Scalar _tmp87 = _tmp60 * _tmp75; + const Scalar _tmp88 = _tmp73 * _tmp75; + const Scalar _tmp89 = _tmp71 * _tmp75; + const Scalar _tmp90 = _tmp23 * _tmp75; + const Scalar _tmp91 = _tmp69 * _tmp75; + const Scalar _tmp92 = _tmp6 * _tmp75; + const Scalar _tmp93 = _tmp16 * _tmp75; + const Scalar _tmp94 = _tmp40 * _tmp75; + const Scalar _tmp95 = _tmp32 * _tmp75; + const Scalar _tmp96 = _tmp19 * _tmp75; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp19 * sqrt_info; + _res(1, 0) = _tmp32 * sqrt_info; + _res(2, 0) = _tmp40 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = 0; + _jacobian(1, 0) = _tmp45 * sqrt_info; + _jacobian(2, 0) = _tmp54 * sqrt_info; + _jacobian(0, 1) = _tmp60 * sqrt_info; + _jacobian(1, 1) = 0; + _jacobian(2, 1) = _tmp63 * sqrt_info; + _jacobian(0, 2) = _tmp66 * sqrt_info; + _jacobian(1, 2) = _tmp70 * sqrt_info; + _jacobian(2, 2) = 0; + _jacobian(0, 3) = _tmp71 * sqrt_info; + _jacobian(1, 3) = _tmp53 * sqrt_info; + _jacobian(2, 3) = _tmp59 * sqrt_info; + _jacobian(0, 4) = _tmp69 * sqrt_info; + _jacobian(1, 4) = _tmp73 * sqrt_info; + _jacobian(2, 4) = _tmp57 * sqrt_info; + _jacobian(0, 5) = _tmp68 * sqrt_info; + _jacobian(1, 5) = _tmp51 * sqrt_info; + _jacobian(2, 5) = _tmp74 * sqrt_info; + _jacobian(0, 6) = 0; + _jacobian(1, 6) = 0; + _jacobian(2, 6) = 0; + _jacobian(0, 7) = 0; + _jacobian(1, 7) = 0; + _jacobian(2, 7) = 0; + _jacobian(0, 8) = 0; + _jacobian(1, 8) = 0; + _jacobian(2, 8) = 0; + _jacobian(0, 9) = _tmp6 * sqrt_info; + _jacobian(1, 9) = _tmp29 * sqrt_info; + _jacobian(2, 9) = _tmp37 * sqrt_info; + _jacobian(0, 10) = _tmp16 * sqrt_info; + _jacobian(1, 10) = _tmp23 * sqrt_info; + _jacobian(2, 10) = _tmp35 * sqrt_info; + _jacobian(0, 11) = _tmp11 * sqrt_info; + _jacobian(1, 11) = _tmp26 * sqrt_info; + _jacobian(2, 11) = _tmp33 * sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = std::pow(_tmp45, Scalar(2)) * _tmp75 + std::pow(_tmp54, Scalar(2)) * _tmp75; + _hessian(1, 0) = _tmp54 * _tmp76; + _hessian(2, 0) = _tmp45 * _tmp77; + _hessian(3, 0) = _tmp53 * _tmp79 + _tmp59 * _tmp78; + _hessian(4, 0) = _tmp54 * _tmp80 + _tmp73 * _tmp79; + _hessian(5, 0) = _tmp45 * _tmp82 + _tmp54 * _tmp81; + _hessian(9, 0) = _tmp29 * _tmp79 + _tmp37 * _tmp78; + _hessian(10, 0) = _tmp23 * _tmp79 + _tmp54 * _tmp83; + _hessian(11, 0) = _tmp45 * _tmp85 + _tmp54 * _tmp84; + _hessian(1, 1) = std::pow(_tmp60, Scalar(2)) * _tmp75 + std::pow(_tmp63, Scalar(2)) * _tmp75; + _hessian(2, 1) = _tmp60 * _tmp86; + _hessian(3, 1) = _tmp59 * _tmp76 + _tmp71 * _tmp87; + _hessian(4, 1) = _tmp63 * _tmp80 + _tmp69 * _tmp87; + _hessian(5, 1) = _tmp68 * _tmp87 + _tmp74 * _tmp76; + _hessian(9, 1) = _tmp37 * _tmp76 + _tmp6 * _tmp87; + _hessian(10, 1) = _tmp16 * _tmp87 + _tmp63 * _tmp83; + _hessian(11, 1) = _tmp11 * _tmp87 + _tmp33 * _tmp76; + _hessian(2, 2) = std::pow(_tmp66, Scalar(2)) * _tmp75 + std::pow(_tmp70, Scalar(2)) * _tmp75; + _hessian(3, 2) = _tmp53 * _tmp77 + _tmp71 * _tmp86; + _hessian(4, 2) = _tmp69 * _tmp86 + _tmp73 * _tmp77; + _hessian(5, 2) = _tmp68 * _tmp86 + _tmp70 * _tmp82; + _hessian(9, 2) = _tmp29 * _tmp77 + _tmp6 * _tmp86; + _hessian(10, 2) = _tmp16 * _tmp86 + _tmp23 * _tmp77; + _hessian(11, 2) = _tmp11 * _tmp86 + _tmp70 * _tmp85; + _hessian(3, 3) = std::pow(_tmp53, Scalar(2)) * _tmp75 + std::pow(_tmp59, Scalar(2)) * _tmp75 + + std::pow(_tmp71, Scalar(2)) * _tmp75; + _hessian(4, 3) = _tmp53 * _tmp88 + _tmp59 * _tmp80 + _tmp69 * _tmp89; + _hessian(5, 3) = _tmp53 * _tmp82 + _tmp59 * _tmp81 + _tmp68 * _tmp89; + _hessian(9, 3) = _tmp29 * _tmp53 * _tmp75 + _tmp37 * _tmp59 * _tmp75 + _tmp6 * _tmp89; + _hessian(10, 3) = _tmp16 * _tmp89 + _tmp53 * _tmp90 + _tmp59 * _tmp83; + _hessian(11, 3) = _tmp11 * _tmp89 + _tmp53 * _tmp85 + _tmp59 * _tmp84; + _hessian(4, 4) = std::pow(_tmp57, Scalar(2)) * _tmp75 + std::pow(_tmp69, Scalar(2)) * _tmp75 + + std::pow(_tmp73, Scalar(2)) * _tmp75; + _hessian(5, 4) = _tmp68 * _tmp91 + _tmp73 * _tmp82 + _tmp74 * _tmp80; + _hessian(9, 4) = _tmp29 * _tmp88 + _tmp37 * _tmp80 + _tmp69 * _tmp92; + _hessian(10, 4) = _tmp16 * _tmp91 + _tmp23 * _tmp88 + _tmp35 * _tmp80; + _hessian(11, 4) = _tmp11 * _tmp91 + _tmp33 * _tmp80 + _tmp73 * _tmp85; + _hessian(5, 5) = std::pow(_tmp51, Scalar(2)) * _tmp75 + std::pow(_tmp68, Scalar(2)) * _tmp75 + + std::pow(_tmp74, Scalar(2)) * _tmp75; + _hessian(9, 5) = _tmp29 * _tmp82 + _tmp37 * _tmp81 + _tmp68 * _tmp92; + _hessian(10, 5) = _tmp23 * _tmp82 + _tmp68 * _tmp93 + _tmp74 * _tmp83; + _hessian(11, 5) = _tmp11 * _tmp68 * _tmp75 + _tmp51 * _tmp85 + _tmp74 * _tmp84; + _hessian(9, 9) = std::pow(_tmp29, Scalar(2)) * _tmp75 + std::pow(_tmp37, Scalar(2)) * _tmp75 + + std::pow(_tmp6, Scalar(2)) * _tmp75; + _hessian(10, 9) = _tmp16 * _tmp92 + _tmp29 * _tmp90 + _tmp37 * _tmp83; + _hessian(11, 9) = _tmp11 * _tmp92 + _tmp29 * _tmp85 + _tmp37 * _tmp84; + _hessian(10, 10) = std::pow(_tmp16, Scalar(2)) * _tmp75 + std::pow(_tmp23, Scalar(2)) * _tmp75 + + std::pow(_tmp35, Scalar(2)) * _tmp75; + _hessian(11, 10) = _tmp11 * _tmp93 + _tmp23 * _tmp85 + _tmp33 * _tmp83; + _hessian(11, 11) = std::pow(_tmp11, Scalar(2)) * _tmp75 + std::pow(_tmp26, Scalar(2)) * _tmp75 + + std::pow(_tmp33, Scalar(2)) * _tmp75; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp45 * _tmp95 + _tmp54 * _tmp94; + _rhs(1, 0) = _tmp40 * _tmp76 + _tmp60 * _tmp96; + _rhs(2, 0) = _tmp66 * _tmp96 + _tmp70 * _tmp95; + _rhs(3, 0) = _tmp53 * _tmp95 + _tmp59 * _tmp94 + _tmp71 * _tmp96; + _rhs(4, 0) = _tmp40 * _tmp80 + _tmp69 * _tmp96 + _tmp73 * _tmp95; + _rhs(5, 0) = _tmp32 * _tmp82 + _tmp68 * _tmp96 + _tmp74 * _tmp94; + _rhs(6, 0) = 0; + _rhs(7, 0) = 0; + _rhs(8, 0) = 0; + _rhs(9, 0) = _tmp29 * _tmp95 + _tmp37 * _tmp94 + _tmp6 * _tmp96; + _rhs(10, 0) = _tmp16 * _tmp96 + _tmp23 * _tmp95 + _tmp40 * _tmp83; + _rhs(11, 0) = _tmp11 * _tmp96 + _tmp32 * _tmp85 + _tmp33 * _tmp94; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_square.h b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_square.h new file mode 100644 index 000000000..995b4b6ba --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_position/between_factor_pose3_position_square.h @@ -0,0 +1,312 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_t_b. + * + * In vector space terms that would be: + * (b - a) - a_t_b + * + * In lie group terms: + * local_coordinates(a_t_b, between(a, b)) + * to_tangent(compose(inverse(a_t_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& b, + const Eigen::Matrix& a_t_b, + const Eigen::Matrix& sqrt_info, 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: 553 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + + // Intermediate terms (105) + const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = -_tmp1; + const Scalar _tmp3 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = -_tmp4; + const Scalar _tmp6 = _tmp2 + _tmp5 + 1; + const Scalar _tmp7 = 2 * _a[0]; + const Scalar _tmp8 = _a[2] * _tmp7; + const Scalar _tmp9 = 2 * _a[3]; + const Scalar _tmp10 = _a[1] * _tmp9; + const Scalar _tmp11 = -_tmp10; + const Scalar _tmp12 = _tmp11 + _tmp8; + const Scalar _tmp13 = _a[6] * _tmp12; + const Scalar _tmp14 = _a[1] * _tmp7; + const Scalar _tmp15 = _a[2] * _tmp9; + const Scalar _tmp16 = _tmp14 + _tmp15; + const Scalar _tmp17 = _a[5] * _tmp16; + const Scalar _tmp18 = _b[5] * _tmp16 + _b[6] * _tmp12; + const Scalar _tmp19 = -_a[4] * _tmp6 + _b[4] * _tmp6 - _tmp13 - _tmp17 + _tmp18 - a_t_b(0, 0); + const Scalar _tmp20 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp21 = 2 * _tmp20; + const Scalar _tmp22 = 1 - _tmp21; + const Scalar _tmp23 = _tmp2 + _tmp22; + const Scalar _tmp24 = 2 * _a[1] * _a[2]; + const Scalar _tmp25 = _a[3] * _tmp7; + const Scalar _tmp26 = _tmp24 + _tmp25; + const Scalar _tmp27 = _a[6] * _tmp26; + const Scalar _tmp28 = -_tmp15; + const Scalar _tmp29 = _tmp14 + _tmp28; + const Scalar _tmp30 = _a[4] * _tmp29; + const Scalar _tmp31 = _b[4] * _tmp29 + _b[6] * _tmp26; + const Scalar _tmp32 = -_a[5] * _tmp23 + _b[5] * _tmp23 - _tmp27 - _tmp30 + _tmp31 - a_t_b(1, 0); + const Scalar _tmp33 = _tmp22 + _tmp5; + const Scalar _tmp34 = -_tmp25; + const Scalar _tmp35 = _tmp24 + _tmp34; + const Scalar _tmp36 = _a[5] * _tmp35; + const Scalar _tmp37 = _tmp10 + _tmp8; + const Scalar _tmp38 = _a[4] * _tmp37; + const Scalar _tmp39 = _b[4] * _tmp37 + _b[5] * _tmp35; + const Scalar _tmp40 = -_a[6] * _tmp33 + _b[6] * _tmp33 - _tmp36 - _tmp38 + _tmp39 - a_t_b(2, 0); + const Scalar _tmp41 = + _tmp19 * sqrt_info(0, 0) + _tmp32 * sqrt_info(0, 1) + _tmp40 * sqrt_info(0, 2); + const Scalar _tmp42 = + _tmp19 * sqrt_info(1, 0) + _tmp32 * sqrt_info(1, 1) + _tmp40 * sqrt_info(1, 2); + const Scalar _tmp43 = + _tmp19 * sqrt_info(2, 0) + _tmp32 * sqrt_info(2, 1) + _tmp40 * sqrt_info(2, 2); + const Scalar _tmp44 = -_tmp20; + const Scalar _tmp45 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp46 = _tmp44 + _tmp45; + const Scalar _tmp47 = -_tmp3; + const Scalar _tmp48 = _tmp0 + _tmp47; + const Scalar _tmp49 = _tmp46 + _tmp48; + const Scalar _tmp50 = -_a[6] * _tmp49 + _b[6] * _tmp49 - _tmp36 - _tmp38 + _tmp39; + const Scalar _tmp51 = -_tmp45; + const Scalar _tmp52 = _tmp20 + _tmp51; + const Scalar _tmp53 = _tmp48 + _tmp52; + const Scalar _tmp54 = -_tmp24; + const Scalar _tmp55 = _tmp34 + _tmp54; + const Scalar _tmp56 = -_tmp14; + const Scalar _tmp57 = _tmp15 + _tmp56; + const Scalar _tmp58 = -_a[4] * _tmp57 - _a[5] * _tmp53 - _a[6] * _tmp55 + _b[4] * _tmp57 + + _b[5] * _tmp53 + _b[6] * _tmp55; + const Scalar _tmp59 = _tmp50 * sqrt_info(0, 1) + _tmp58 * sqrt_info(0, 2); + const Scalar _tmp60 = _tmp50 * sqrt_info(1, 1) + _tmp58 * sqrt_info(1, 2); + const Scalar _tmp61 = _tmp50 * sqrt_info(2, 1) + _tmp58 * sqrt_info(2, 2); + const Scalar _tmp62 = -_tmp0; + const Scalar _tmp63 = _tmp3 + _tmp62; + const Scalar _tmp64 = _tmp52 + _tmp63; + const Scalar _tmp65 = _tmp25 + _tmp54; + const Scalar _tmp66 = -_tmp8; + const Scalar _tmp67 = _tmp11 + _tmp66; + const Scalar _tmp68 = -_a[4] * _tmp67 - _a[5] * _tmp65 - _a[6] * _tmp64 + _b[4] * _tmp67 + + _b[5] * _tmp65 + _b[6] * _tmp64; + const Scalar _tmp69 = _tmp20 + _tmp45 + _tmp47 + _tmp62; + const Scalar _tmp70 = -_a[4] * _tmp69 + _b[4] * _tmp69 - _tmp13 - _tmp17 + _tmp18; + const Scalar _tmp71 = _tmp68 * sqrt_info(0, 0) + _tmp70 * sqrt_info(0, 2); + const Scalar _tmp72 = _tmp68 * sqrt_info(1, 0) + _tmp70 * sqrt_info(1, 2); + const Scalar _tmp73 = _tmp68 * sqrt_info(2, 0) + _tmp70 * sqrt_info(2, 2); + const Scalar _tmp74 = _tmp46 + _tmp63; + const Scalar _tmp75 = -_a[5] * _tmp74 + _b[5] * _tmp74 - _tmp27 - _tmp30 + _tmp31; + const Scalar _tmp76 = _tmp0 + _tmp3 + _tmp44 + _tmp51; + const Scalar _tmp77 = _tmp10 + _tmp66; + const Scalar _tmp78 = _tmp28 + _tmp56; + const Scalar _tmp79 = -_a[4] * _tmp76 - _a[5] * _tmp78 - _a[6] * _tmp77 + _b[4] * _tmp76 + + _b[5] * _tmp78 + _b[6] * _tmp77; + const Scalar _tmp80 = _tmp75 * sqrt_info(0, 0) + _tmp79 * sqrt_info(0, 1); + const Scalar _tmp81 = _tmp75 * sqrt_info(1, 0) + _tmp79 * sqrt_info(1, 1); + const Scalar _tmp82 = _tmp75 * sqrt_info(2, 0) + _tmp79 * sqrt_info(2, 1); + const Scalar _tmp83 = _tmp1 + _tmp4 - 1; + const Scalar _tmp84 = + _tmp57 * sqrt_info(0, 1) + _tmp67 * sqrt_info(0, 2) + _tmp83 * sqrt_info(0, 0); + const Scalar _tmp85 = + _tmp57 * sqrt_info(1, 1) + _tmp67 * sqrt_info(1, 2) + _tmp83 * sqrt_info(1, 0); + const Scalar _tmp86 = + _tmp57 * sqrt_info(2, 1) + _tmp67 * sqrt_info(2, 2) + _tmp83 * sqrt_info(2, 0); + const Scalar _tmp87 = _tmp21 - 1; + const Scalar _tmp88 = _tmp1 + _tmp87; + const Scalar _tmp89 = + _tmp65 * sqrt_info(0, 2) + _tmp78 * sqrt_info(0, 0) + _tmp88 * sqrt_info(0, 1); + const Scalar _tmp90 = + _tmp65 * sqrt_info(1, 2) + _tmp78 * sqrt_info(1, 0) + _tmp88 * sqrt_info(1, 1); + const Scalar _tmp91 = + _tmp65 * sqrt_info(2, 2) + _tmp78 * sqrt_info(2, 0) + _tmp88 * sqrt_info(2, 1); + const Scalar _tmp92 = _tmp4 + _tmp87; + const Scalar _tmp93 = + _tmp55 * sqrt_info(0, 1) + _tmp77 * sqrt_info(0, 0) + _tmp92 * sqrt_info(0, 2); + const Scalar _tmp94 = + _tmp55 * sqrt_info(1, 1) + _tmp77 * sqrt_info(1, 0) + _tmp92 * sqrt_info(1, 2); + const Scalar _tmp95 = + _tmp55 * sqrt_info(2, 1) + _tmp77 * sqrt_info(2, 0) + _tmp92 * sqrt_info(2, 2); + const Scalar _tmp96 = + _tmp29 * sqrt_info(0, 1) + _tmp37 * sqrt_info(0, 2) + _tmp6 * sqrt_info(0, 0); + const Scalar _tmp97 = + _tmp29 * sqrt_info(1, 1) + _tmp37 * sqrt_info(1, 2) + _tmp6 * sqrt_info(1, 0); + const Scalar _tmp98 = + _tmp29 * sqrt_info(2, 1) + _tmp37 * sqrt_info(2, 2) + _tmp6 * sqrt_info(2, 0); + const Scalar _tmp99 = + _tmp16 * sqrt_info(0, 0) + _tmp23 * sqrt_info(0, 1) + _tmp35 * sqrt_info(0, 2); + const Scalar _tmp100 = + _tmp16 * sqrt_info(1, 0) + _tmp23 * sqrt_info(1, 1) + _tmp35 * sqrt_info(1, 2); + const Scalar _tmp101 = + _tmp16 * sqrt_info(2, 0) + _tmp23 * sqrt_info(2, 1) + _tmp35 * sqrt_info(2, 2); + const Scalar _tmp102 = + _tmp12 * sqrt_info(0, 0) + _tmp26 * sqrt_info(0, 1) + _tmp33 * sqrt_info(0, 2); + const Scalar _tmp103 = + _tmp12 * sqrt_info(1, 0) + _tmp26 * sqrt_info(1, 1) + _tmp33 * sqrt_info(1, 2); + const Scalar _tmp104 = + _tmp12 * sqrt_info(2, 0) + _tmp26 * sqrt_info(2, 1) + _tmp33 * sqrt_info(2, 2); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp41; + _res(1, 0) = _tmp42; + _res(2, 0) = _tmp43; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp59; + _jacobian(1, 0) = _tmp60; + _jacobian(2, 0) = _tmp61; + _jacobian(0, 1) = _tmp71; + _jacobian(1, 1) = _tmp72; + _jacobian(2, 1) = _tmp73; + _jacobian(0, 2) = _tmp80; + _jacobian(1, 2) = _tmp81; + _jacobian(2, 2) = _tmp82; + _jacobian(0, 3) = _tmp84; + _jacobian(1, 3) = _tmp85; + _jacobian(2, 3) = _tmp86; + _jacobian(0, 4) = _tmp89; + _jacobian(1, 4) = _tmp90; + _jacobian(2, 4) = _tmp91; + _jacobian(0, 5) = _tmp93; + _jacobian(1, 5) = _tmp94; + _jacobian(2, 5) = _tmp95; + _jacobian(0, 6) = 0; + _jacobian(1, 6) = 0; + _jacobian(2, 6) = 0; + _jacobian(0, 7) = 0; + _jacobian(1, 7) = 0; + _jacobian(2, 7) = 0; + _jacobian(0, 8) = 0; + _jacobian(1, 8) = 0; + _jacobian(2, 8) = 0; + _jacobian(0, 9) = _tmp96; + _jacobian(1, 9) = _tmp97; + _jacobian(2, 9) = _tmp98; + _jacobian(0, 10) = _tmp99; + _jacobian(1, 10) = _tmp100; + _jacobian(2, 10) = _tmp101; + _jacobian(0, 11) = _tmp102; + _jacobian(1, 11) = _tmp103; + _jacobian(2, 11) = _tmp104; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp59, Scalar(2)) + std::pow(_tmp60, Scalar(2)) + std::pow(_tmp61, Scalar(2)); + _hessian(1, 0) = _tmp59 * _tmp71 + _tmp60 * _tmp72 + _tmp61 * _tmp73; + _hessian(2, 0) = _tmp59 * _tmp80 + _tmp60 * _tmp81 + _tmp61 * _tmp82; + _hessian(3, 0) = _tmp59 * _tmp84 + _tmp60 * _tmp85 + _tmp61 * _tmp86; + _hessian(4, 0) = _tmp59 * _tmp89 + _tmp60 * _tmp90 + _tmp61 * _tmp91; + _hessian(5, 0) = _tmp59 * _tmp93 + _tmp60 * _tmp94 + _tmp61 * _tmp95; + _hessian(9, 0) = _tmp59 * _tmp96 + _tmp60 * _tmp97 + _tmp61 * _tmp98; + _hessian(10, 0) = _tmp100 * _tmp60 + _tmp101 * _tmp61 + _tmp59 * _tmp99; + _hessian(11, 0) = _tmp102 * _tmp59 + _tmp103 * _tmp60 + _tmp104 * _tmp61; + _hessian(1, 1) = + std::pow(_tmp71, Scalar(2)) + std::pow(_tmp72, Scalar(2)) + std::pow(_tmp73, Scalar(2)); + _hessian(2, 1) = _tmp71 * _tmp80 + _tmp72 * _tmp81 + _tmp73 * _tmp82; + _hessian(3, 1) = _tmp71 * _tmp84 + _tmp72 * _tmp85 + _tmp73 * _tmp86; + _hessian(4, 1) = _tmp71 * _tmp89 + _tmp72 * _tmp90 + _tmp73 * _tmp91; + _hessian(5, 1) = _tmp71 * _tmp93 + _tmp72 * _tmp94 + _tmp73 * _tmp95; + _hessian(9, 1) = _tmp71 * _tmp96 + _tmp72 * _tmp97 + _tmp73 * _tmp98; + _hessian(10, 1) = _tmp100 * _tmp72 + _tmp101 * _tmp73 + _tmp71 * _tmp99; + _hessian(11, 1) = _tmp102 * _tmp71 + _tmp103 * _tmp72 + _tmp104 * _tmp73; + _hessian(2, 2) = + std::pow(_tmp80, Scalar(2)) + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp82, Scalar(2)); + _hessian(3, 2) = _tmp80 * _tmp84 + _tmp81 * _tmp85 + _tmp82 * _tmp86; + _hessian(4, 2) = _tmp80 * _tmp89 + _tmp81 * _tmp90 + _tmp82 * _tmp91; + _hessian(5, 2) = _tmp80 * _tmp93 + _tmp81 * _tmp94 + _tmp82 * _tmp95; + _hessian(9, 2) = _tmp80 * _tmp96 + _tmp81 * _tmp97 + _tmp82 * _tmp98; + _hessian(10, 2) = _tmp100 * _tmp81 + _tmp101 * _tmp82 + _tmp80 * _tmp99; + _hessian(11, 2) = _tmp102 * _tmp80 + _tmp103 * _tmp81 + _tmp104 * _tmp82; + _hessian(3, 3) = + std::pow(_tmp84, Scalar(2)) + std::pow(_tmp85, Scalar(2)) + std::pow(_tmp86, Scalar(2)); + _hessian(4, 3) = _tmp84 * _tmp89 + _tmp85 * _tmp90 + _tmp86 * _tmp91; + _hessian(5, 3) = _tmp84 * _tmp93 + _tmp85 * _tmp94 + _tmp86 * _tmp95; + _hessian(9, 3) = _tmp84 * _tmp96 + _tmp85 * _tmp97 + _tmp86 * _tmp98; + _hessian(10, 3) = _tmp100 * _tmp85 + _tmp101 * _tmp86 + _tmp84 * _tmp99; + _hessian(11, 3) = _tmp102 * _tmp84 + _tmp103 * _tmp85 + _tmp104 * _tmp86; + _hessian(4, 4) = + std::pow(_tmp89, Scalar(2)) + std::pow(_tmp90, Scalar(2)) + std::pow(_tmp91, Scalar(2)); + _hessian(5, 4) = _tmp89 * _tmp93 + _tmp90 * _tmp94 + _tmp91 * _tmp95; + _hessian(9, 4) = _tmp89 * _tmp96 + _tmp90 * _tmp97 + _tmp91 * _tmp98; + _hessian(10, 4) = _tmp100 * _tmp90 + _tmp101 * _tmp91 + _tmp89 * _tmp99; + _hessian(11, 4) = _tmp102 * _tmp89 + _tmp103 * _tmp90 + _tmp104 * _tmp91; + _hessian(5, 5) = + std::pow(_tmp93, Scalar(2)) + std::pow(_tmp94, Scalar(2)) + std::pow(_tmp95, Scalar(2)); + _hessian(9, 5) = _tmp93 * _tmp96 + _tmp94 * _tmp97 + _tmp95 * _tmp98; + _hessian(10, 5) = _tmp100 * _tmp94 + _tmp101 * _tmp95 + _tmp93 * _tmp99; + _hessian(11, 5) = _tmp102 * _tmp93 + _tmp103 * _tmp94 + _tmp104 * _tmp95; + _hessian(9, 9) = + std::pow(_tmp96, Scalar(2)) + std::pow(_tmp97, Scalar(2)) + std::pow(_tmp98, Scalar(2)); + _hessian(10, 9) = _tmp100 * _tmp97 + _tmp101 * _tmp98 + _tmp96 * _tmp99; + _hessian(11, 9) = _tmp102 * _tmp96 + _tmp103 * _tmp97 + _tmp104 * _tmp98; + _hessian(10, 10) = + std::pow(_tmp100, Scalar(2)) + std::pow(_tmp101, Scalar(2)) + std::pow(_tmp99, Scalar(2)); + _hessian(11, 10) = _tmp100 * _tmp103 + _tmp101 * _tmp104 + _tmp102 * _tmp99; + _hessian(11, 11) = + std::pow(_tmp102, Scalar(2)) + std::pow(_tmp103, Scalar(2)) + std::pow(_tmp104, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp41 * _tmp59 + _tmp42 * _tmp60 + _tmp43 * _tmp61; + _rhs(1, 0) = _tmp41 * _tmp71 + _tmp42 * _tmp72 + _tmp43 * _tmp73; + _rhs(2, 0) = _tmp41 * _tmp80 + _tmp42 * _tmp81 + _tmp43 * _tmp82; + _rhs(3, 0) = _tmp41 * _tmp84 + _tmp42 * _tmp85 + _tmp43 * _tmp86; + _rhs(4, 0) = _tmp41 * _tmp89 + _tmp42 * _tmp90 + _tmp43 * _tmp91; + _rhs(5, 0) = _tmp41 * _tmp93 + _tmp42 * _tmp94 + _tmp43 * _tmp95; + _rhs(6, 0) = 0; + _rhs(7, 0) = 0; + _rhs(8, 0) = 0; + _rhs(9, 0) = _tmp41 * _tmp96 + _tmp42 * _tmp97 + _tmp43 * _tmp98; + _rhs(10, 0) = _tmp100 * _tmp42 + _tmp101 * _tmp43 + _tmp41 * _tmp99; + _rhs(11, 0) = _tmp102 * _tmp41 + _tmp103 * _tmp42 + _tmp104 * _tmp43; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_rotation.h b/gen/cpp/sym/factors/between_factor_pose3_rotation.h index 0cc300f3a..a51370ffe 100644 --- a/gen/cpp/sym/factors/between_factor_pose3_rotation.h +++ b/gen/cpp/sym/factors/between_factor_pose3_rotation.h @@ -1,408 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_R_b. - * - * In vector space terms that would be: - * (b - a) - a_R_b - * - * In lie group terms: - * local_coordinates(a_R_b, between(a, b)) - * to_tangent(compose(inverse(a_R_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x12) jacobian of res wrt args a (6), b (6) - * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) - * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) - */ -template -void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3& b, - const sym::Rot3& a_R_b, - const Eigen::Matrix& sqrt_info, 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: 742 - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - const Eigen::Matrix& _a_R_b = a_R_b.Data(); - - // Intermediate terms (209) - const Scalar _tmp0 = _a[1] * _b[1]; - const Scalar _tmp1 = _a[2] * _b[2]; - const Scalar _tmp2 = _a[0] * _b[0]; - const Scalar _tmp3 = _a[3] * _b[3]; - const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; - const Scalar _tmp5 = _a[2] * _b[1]; - const Scalar _tmp6 = _a[0] * _b[3]; - const Scalar _tmp7 = _a[1] * _b[2]; - const Scalar _tmp8 = _a[3] * _b[0]; - const Scalar _tmp9 = _tmp5 - _tmp6 - _tmp7 + _tmp8; - const Scalar _tmp10 = _a[3] * _b[1]; - const Scalar _tmp11 = _a[1] * _b[3]; - const Scalar _tmp12 = _a[0] * _b[2]; - const Scalar _tmp13 = _a[2] * _b[0]; - const Scalar _tmp14 = _tmp10 - _tmp11 + _tmp12 - _tmp13; - const Scalar _tmp15 = _a[0] * _b[1]; - const Scalar _tmp16 = _a[2] * _b[3]; - const Scalar _tmp17 = _a[3] * _b[2]; - const Scalar _tmp18 = _a[1] * _b[0]; - const Scalar _tmp19 = -_tmp15 - _tmp16 + _tmp17 + _tmp18; - const Scalar _tmp20 = - -_a_R_b[0] * _tmp4 - _a_R_b[1] * _tmp19 + _a_R_b[2] * _tmp14 + _a_R_b[3] * _tmp9; - const Scalar _tmp21 = 1 - epsilon; - const Scalar _tmp22 = _a_R_b[0] * _tmp9; - const Scalar _tmp23 = _a_R_b[1] * _tmp14; - const Scalar _tmp24 = _a_R_b[2] * _tmp19; - const Scalar _tmp25 = -_tmp22 - _tmp23 - _tmp24; - const Scalar _tmp26 = _a_R_b[3] * _tmp4; - const Scalar _tmp27 = std::min(_tmp21, std::fabs(_tmp25 - _tmp26)); - const Scalar _tmp28 = - std::pow(Scalar(1 - std::pow(_tmp27, Scalar(2))), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp29 = std::acos(_tmp27); - const Scalar _tmp30 = - 2 * std::min(0, (((-_tmp25 + _tmp26) > 0) - ((-_tmp25 + _tmp26) < 0))) + 1; - const Scalar _tmp31 = 2 * _tmp30; - const Scalar _tmp32 = _tmp28 * _tmp29 * _tmp31; - const Scalar _tmp33 = _tmp20 * _tmp32; - const Scalar _tmp34 = - -_a_R_b[0] * _tmp14 + _a_R_b[1] * _tmp9 - _a_R_b[2] * _tmp4 + _a_R_b[3] * _tmp19; - const Scalar _tmp35 = _tmp34 * sqrt_info(0, 2); - const Scalar _tmp36 = - _a_R_b[0] * _tmp19 - _a_R_b[1] * _tmp4 - _a_R_b[2] * _tmp9 + _a_R_b[3] * _tmp14; - const Scalar _tmp37 = _tmp32 * _tmp36; - const Scalar _tmp38 = _tmp32 * _tmp35 + _tmp33 * sqrt_info(0, 0) + _tmp37 * sqrt_info(0, 1); - const Scalar _tmp39 = _tmp34 * sqrt_info(1, 2); - const Scalar _tmp40 = _tmp32 * _tmp39 + _tmp33 * sqrt_info(1, 0) + _tmp37 * sqrt_info(1, 1); - const Scalar _tmp41 = _tmp31 * sqrt_info(2, 2); - const Scalar _tmp42 = - _tmp28 * _tmp29 * _tmp34 * _tmp41 + _tmp33 * sqrt_info(2, 0) + _tmp37 * sqrt_info(2, 1); - const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp0; - const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp3; - const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp1; - const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp2; - const Scalar _tmp47 = -_tmp43 - _tmp44 - _tmp45 - _tmp46; - const Scalar _tmp48 = _a_R_b[1] * _tmp47; - const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp15; - const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp16; - const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp17; - const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp18; - const Scalar _tmp53 = -_tmp49 - _tmp50 + _tmp51 + _tmp52; - const Scalar _tmp54 = -_a_R_b[0] * _tmp53; - const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp5; - const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp6; - const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp7; - const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp8; - const Scalar _tmp59 = _tmp55 - _tmp56 - _tmp57 + _tmp58; - const Scalar _tmp60 = _a_R_b[2] * _tmp59; - const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp10; - const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp11; - const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp12; - const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp13; - const Scalar _tmp65 = -_tmp61 + _tmp62 - _tmp63 + _tmp64; - const Scalar _tmp66 = _a_R_b[3] * _tmp65; - const Scalar _tmp67 = _tmp48 + _tmp54 - _tmp60 + _tmp66; - const Scalar _tmp68 = _tmp22 + _tmp23 + _tmp24 + _tmp26; - const Scalar _tmp69 = std::fabs(_tmp68); - const Scalar _tmp70 = std::min(_tmp21, _tmp69); - const Scalar _tmp71 = 1 - std::pow(_tmp70, Scalar(2)); - const Scalar _tmp72 = std::acos(_tmp70); - const Scalar _tmp73 = _tmp72 / std::sqrt(_tmp71); - const Scalar _tmp74 = _tmp31 * _tmp73; - const Scalar _tmp75 = _tmp67 * _tmp74; - const Scalar _tmp76 = -_a_R_b[1] * _tmp59; - const Scalar _tmp77 = _a_R_b[2] * _tmp47; - const Scalar _tmp78 = _a_R_b[3] * _tmp53; - const Scalar _tmp79 = _a_R_b[0] * _tmp65; - const Scalar _tmp80 = _tmp78 + _tmp79; - const Scalar _tmp81 = _tmp76 - _tmp77 + _tmp80; - const Scalar _tmp82 = _tmp74 * _tmp81; - const Scalar _tmp83 = _a_R_b[3] * _tmp47; - const Scalar _tmp84 = _a_R_b[0] * _tmp59; - const Scalar _tmp85 = _a_R_b[2] * _tmp53; - const Scalar _tmp86 = -_a_R_b[1] * _tmp65; - const Scalar _tmp87 = _tmp85 + _tmp86; - const Scalar _tmp88 = _tmp83 - _tmp84 + _tmp87; - const Scalar _tmp89 = _tmp74 * sqrt_info(0, 0); - const Scalar _tmp90 = _tmp36 * sqrt_info(0, 1); - const Scalar _tmp91 = _a_R_b[3] * _tmp59; - const Scalar _tmp92 = _a_R_b[0] * _tmp47; - const Scalar _tmp93 = _a_R_b[1] * _tmp53; - const Scalar _tmp94 = _a_R_b[2] * _tmp65; - const Scalar _tmp95 = _tmp93 + _tmp94; - const Scalar _tmp96 = _tmp91 + _tmp92 + _tmp95; - const Scalar _tmp97 = _tmp30 * ((((_tmp21 - _tmp69) > 0) - ((_tmp21 - _tmp69) < 0)) + 1) * - (((_tmp68) > 0) - ((_tmp68) < 0)); - const Scalar _tmp98 = _tmp97 / _tmp71; - const Scalar _tmp99 = _tmp96 * _tmp98; - const Scalar _tmp100 = _tmp35 * _tmp98; - const Scalar _tmp101 = _tmp20 * _tmp98; - const Scalar _tmp102 = _tmp101 * sqrt_info(0, 0); - const Scalar _tmp103 = _tmp70 * _tmp72 * _tmp97 / (_tmp71 * std::sqrt(_tmp71)); - const Scalar _tmp104 = _tmp20 * sqrt_info(0, 0); - const Scalar _tmp105 = _tmp103 * _tmp104; - const Scalar _tmp106 = _tmp103 * _tmp96; - const Scalar _tmp107 = -_tmp100 * _tmp96 - _tmp102 * _tmp96 + _tmp105 * _tmp96 + - _tmp106 * _tmp35 + _tmp106 * _tmp90 + _tmp75 * sqrt_info(0, 2) + - _tmp82 * sqrt_info(0, 1) + _tmp88 * _tmp89 - _tmp90 * _tmp99; - const Scalar _tmp108 = _tmp74 * sqrt_info(1, 1); - const Scalar _tmp109 = _tmp74 * _tmp88; - const Scalar _tmp110 = _tmp39 * _tmp98; - const Scalar _tmp111 = _tmp36 * sqrt_info(1, 1); - const Scalar _tmp112 = _tmp101 * sqrt_info(1, 0); - const Scalar _tmp113 = _tmp20 * sqrt_info(1, 0); - const Scalar _tmp114 = _tmp106 * _tmp111 + _tmp106 * _tmp113 + _tmp106 * _tmp39 + - _tmp108 * _tmp81 + _tmp109 * sqrt_info(1, 0) - _tmp110 * _tmp96 - - _tmp111 * _tmp99 - _tmp112 * _tmp96 + _tmp75 * sqrt_info(1, 2); - const Scalar _tmp115 = _tmp41 * _tmp73; - const Scalar _tmp116 = _tmp101 * sqrt_info(2, 0); - const Scalar _tmp117 = _tmp34 * sqrt_info(2, 2); - const Scalar _tmp118 = _tmp36 * sqrt_info(2, 1); - const Scalar _tmp119 = _tmp20 * sqrt_info(2, 0); - const Scalar _tmp120 = _tmp106 * _tmp117 + _tmp106 * _tmp118 + _tmp106 * _tmp119 + - _tmp109 * sqrt_info(2, 0) + _tmp115 * _tmp67 - _tmp116 * _tmp96 - - _tmp117 * _tmp99 - _tmp118 * _tmp99 + _tmp82 * sqrt_info(2, 1); - const Scalar _tmp121 = _tmp61 - _tmp62 + _tmp63 - _tmp64; - const Scalar _tmp122 = _a_R_b[0] * _tmp121; - const Scalar _tmp123 = _tmp49 + _tmp50 - _tmp51 - _tmp52; - const Scalar _tmp124 = _a_R_b[3] * _tmp123; - const Scalar _tmp125 = _tmp124 + _tmp76; - const Scalar _tmp126 = -_tmp122 + _tmp125 + _tmp77; - const Scalar _tmp127 = _a_R_b[3] * _tmp121; - const Scalar _tmp128 = _a_R_b[0] * _tmp123; - const Scalar _tmp129 = _tmp128 + _tmp60; - const Scalar _tmp130 = _tmp127 + _tmp129 + _tmp48; - const Scalar _tmp131 = _tmp130 * _tmp98; - const Scalar _tmp132 = _tmp131 * _tmp36; - const Scalar _tmp133 = _tmp101 * _tmp130; - const Scalar _tmp134 = _a_R_b[1] * _tmp121; - const Scalar _tmp135 = -_a_R_b[2] * _tmp123; - const Scalar _tmp136 = _tmp135 + _tmp84; - const Scalar _tmp137 = -_tmp134 + _tmp136 + _tmp83; - const Scalar _tmp138 = _tmp137 * _tmp74; - const Scalar _tmp139 = -_a_R_b[2] * _tmp121; - const Scalar _tmp140 = _a_R_b[1] * _tmp123; - const Scalar _tmp141 = _tmp140 + _tmp91; - const Scalar _tmp142 = _tmp139 + _tmp141 - _tmp92; - const Scalar _tmp143 = _tmp74 * sqrt_info(0, 2); - const Scalar _tmp144 = _tmp103 * _tmp130; - const Scalar _tmp145 = _tmp144 * _tmp20; - const Scalar _tmp146 = -_tmp100 * _tmp130 + _tmp126 * _tmp89 - _tmp132 * sqrt_info(0, 1) - - _tmp133 * sqrt_info(0, 0) + _tmp138 * sqrt_info(0, 1) + _tmp142 * _tmp143 + - _tmp144 * _tmp35 + _tmp144 * _tmp90 + _tmp145 * sqrt_info(0, 0); - const Scalar _tmp147 = _tmp74 * sqrt_info(1, 0); - const Scalar _tmp148 = _tmp74 * sqrt_info(1, 2); - const Scalar _tmp149 = _tmp108 * _tmp137 - _tmp110 * _tmp130 - _tmp111 * _tmp131 + - _tmp111 * _tmp144 - _tmp112 * _tmp130 + _tmp126 * _tmp147 + - _tmp142 * _tmp148 + _tmp144 * _tmp39 + _tmp145 * sqrt_info(1, 0); - const Scalar _tmp150 = _tmp74 * sqrt_info(2, 0); - const Scalar _tmp151 = _tmp115 * _tmp142 - _tmp117 * _tmp131 + _tmp117 * _tmp144 + - _tmp118 * _tmp144 + _tmp126 * _tmp150 - _tmp132 * sqrt_info(2, 1) - - _tmp133 * sqrt_info(2, 0) + _tmp138 * sqrt_info(2, 1) + - _tmp145 * sqrt_info(2, 0); - const Scalar _tmp152 = -_tmp55 + _tmp56 + _tmp57 - _tmp58; - const Scalar _tmp153 = _a_R_b[2] * _tmp152; - const Scalar _tmp154 = _tmp127 + _tmp153; - const Scalar _tmp155 = _tmp154 - _tmp48 + _tmp54; - const Scalar _tmp156 = _a_R_b[3] * _tmp152; - const Scalar _tmp157 = _tmp139 + _tmp156 + _tmp92 - _tmp93; - const Scalar _tmp158 = _tmp157 * _tmp74; - const Scalar _tmp159 = _a_R_b[1] * _tmp152; - const Scalar _tmp160 = _tmp122 + _tmp159; - const Scalar _tmp161 = _tmp160 + _tmp77 + _tmp78; - const Scalar _tmp162 = _tmp103 * _tmp161; - const Scalar _tmp163 = -_a_R_b[0] * _tmp152; - const Scalar _tmp164 = _tmp134 + _tmp163; - const Scalar _tmp165 = _tmp164 + _tmp83 - _tmp85; - const Scalar _tmp166 = _tmp161 * _tmp98; - const Scalar _tmp167 = -_tmp100 * _tmp161 - _tmp102 * _tmp161 + _tmp104 * _tmp162 + - _tmp143 * _tmp165 + _tmp155 * _tmp89 + _tmp158 * sqrt_info(0, 1) + - _tmp162 * _tmp35 + _tmp162 * _tmp90 - _tmp166 * _tmp90; - const Scalar _tmp168 = _tmp108 * _tmp157 - _tmp110 * _tmp161 + _tmp111 * _tmp162 - - _tmp111 * _tmp166 - _tmp112 * _tmp161 + _tmp113 * _tmp162 + - _tmp147 * _tmp155 + _tmp148 * _tmp165 + _tmp162 * _tmp39; - const Scalar _tmp169 = _tmp115 * _tmp165 - _tmp116 * _tmp161 + _tmp117 * _tmp162 - - _tmp117 * _tmp166 + _tmp118 * _tmp162 - _tmp118 * _tmp166 + - _tmp119 * _tmp162 + _tmp150 * _tmp155 + _tmp158 * sqrt_info(2, 1); - const Scalar _tmp170 = _tmp43 + _tmp44 + _tmp45 + _tmp46; - const Scalar _tmp171 = _a_R_b[0] * _tmp170; - const Scalar _tmp172 = _tmp156 + _tmp171; - const Scalar _tmp173 = _tmp172 + _tmp95; - const Scalar _tmp174 = _tmp173 * _tmp98; - const Scalar _tmp175 = _tmp174 * _tmp36; - const Scalar _tmp176 = _tmp101 * _tmp173; - const Scalar _tmp177 = _a_R_b[1] * _tmp170; - const Scalar _tmp178 = _tmp177 + _tmp66; - const Scalar _tmp179 = -_tmp153 + _tmp178 + _tmp54; - const Scalar _tmp180 = _a_R_b[3] * _tmp170; - const Scalar _tmp181 = _tmp163 + _tmp180 + _tmp87; - const Scalar _tmp182 = _a_R_b[2] * _tmp170; - const Scalar _tmp183 = -_tmp159 - _tmp182 + _tmp80; - const Scalar _tmp184 = _tmp183 * _tmp74; - const Scalar _tmp185 = _tmp103 * _tmp173; - const Scalar _tmp186 = -_tmp100 * _tmp173 + _tmp105 * _tmp173 + _tmp143 * _tmp179 - - _tmp175 * sqrt_info(0, 1) - _tmp176 * sqrt_info(0, 0) + _tmp181 * _tmp89 + - _tmp184 * sqrt_info(0, 1) + _tmp185 * _tmp35 + _tmp185 * _tmp90; - const Scalar _tmp187 = _tmp108 * _tmp183 - _tmp110 * _tmp173 - _tmp111 * _tmp174 + - _tmp111 * _tmp185 - _tmp112 * _tmp173 + _tmp113 * _tmp185 + - _tmp147 * _tmp181 + _tmp148 * _tmp179 + _tmp185 * _tmp39; - const Scalar _tmp188 = _tmp115 * _tmp179 - _tmp117 * _tmp174 + _tmp117 * _tmp185 + - _tmp118 * _tmp185 + _tmp119 * _tmp185 + _tmp150 * _tmp181 - - _tmp175 * sqrt_info(2, 1) - _tmp176 * sqrt_info(2, 0) + - _tmp184 * sqrt_info(2, 1); - const Scalar _tmp189 = _tmp125 + _tmp182 - _tmp79; - const Scalar _tmp190 = _tmp129 + _tmp178; - const Scalar _tmp191 = _tmp190 * _tmp98; - const Scalar _tmp192 = _tmp141 - _tmp171 - _tmp94; - const Scalar _tmp193 = _tmp103 * _tmp190; - const Scalar _tmp194 = _tmp136 + _tmp180 + _tmp86; - const Scalar _tmp195 = _tmp194 * _tmp74; - const Scalar _tmp196 = -_tmp100 * _tmp190 - _tmp102 * _tmp190 + _tmp104 * _tmp193 + - _tmp143 * _tmp192 + _tmp189 * _tmp89 - _tmp191 * _tmp90 + - _tmp193 * _tmp35 + _tmp193 * _tmp90 + _tmp195 * sqrt_info(0, 1); - const Scalar _tmp197 = _tmp108 * _tmp194 - _tmp110 * _tmp190 - _tmp111 * _tmp191 + - _tmp111 * _tmp193 - _tmp112 * _tmp190 + _tmp113 * _tmp193 + - _tmp147 * _tmp189 + _tmp148 * _tmp192 + _tmp193 * _tmp39; - const Scalar _tmp198 = _tmp115 * _tmp192 - _tmp116 * _tmp190 - _tmp117 * _tmp191 + - _tmp117 * _tmp193 - _tmp118 * _tmp191 + _tmp118 * _tmp193 + - _tmp119 * _tmp193 + _tmp150 * _tmp189 + _tmp195 * sqrt_info(2, 1); - const Scalar _tmp199 = _tmp124 + _tmp160 + _tmp182; - const Scalar _tmp200 = _tmp103 * _tmp199; - const Scalar _tmp201 = _tmp199 * _tmp98; - const Scalar _tmp202 = _tmp139 - _tmp140 + _tmp172; - const Scalar _tmp203 = _tmp202 * _tmp74; - const Scalar _tmp204 = _tmp135 + _tmp164 + _tmp180; - const Scalar _tmp205 = -_tmp128 + _tmp154 - _tmp177; - const Scalar _tmp206 = -_tmp100 * _tmp199 - _tmp102 * _tmp199 + _tmp104 * _tmp200 + - _tmp143 * _tmp204 + _tmp200 * _tmp35 + _tmp200 * _tmp90 - - _tmp201 * _tmp90 + _tmp203 * sqrt_info(0, 1) + _tmp205 * _tmp89; - const Scalar _tmp207 = _tmp108 * _tmp202 - _tmp110 * _tmp199 + _tmp111 * _tmp200 - - _tmp111 * _tmp201 - _tmp112 * _tmp199 + _tmp113 * _tmp200 + - _tmp147 * _tmp205 + _tmp148 * _tmp204 + _tmp200 * _tmp39; - const Scalar _tmp208 = _tmp115 * _tmp204 - _tmp116 * _tmp199 + _tmp117 * _tmp200 - - _tmp117 * _tmp201 + _tmp118 * _tmp200 - _tmp118 * _tmp201 + - _tmp119 * _tmp200 + _tmp150 * _tmp205 + _tmp203 * sqrt_info(2, 1); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp38; - _res(1, 0) = _tmp40; - _res(2, 0) = _tmp42; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp107; - _jacobian(1, 0) = _tmp114; - _jacobian(2, 0) = _tmp120; - _jacobian(0, 1) = _tmp146; - _jacobian(1, 1) = _tmp149; - _jacobian(2, 1) = _tmp151; - _jacobian(0, 2) = _tmp167; - _jacobian(1, 2) = _tmp168; - _jacobian(2, 2) = _tmp169; - _jacobian(0, 3) = 0; - _jacobian(1, 3) = 0; - _jacobian(2, 3) = 0; - _jacobian(0, 4) = 0; - _jacobian(1, 4) = 0; - _jacobian(2, 4) = 0; - _jacobian(0, 5) = 0; - _jacobian(1, 5) = 0; - _jacobian(2, 5) = 0; - _jacobian(0, 6) = _tmp186; - _jacobian(1, 6) = _tmp187; - _jacobian(2, 6) = _tmp188; - _jacobian(0, 7) = _tmp196; - _jacobian(1, 7) = _tmp197; - _jacobian(2, 7) = _tmp198; - _jacobian(0, 8) = _tmp206; - _jacobian(1, 8) = _tmp207; - _jacobian(2, 8) = _tmp208; - _jacobian(0, 9) = 0; - _jacobian(1, 9) = 0; - _jacobian(2, 9) = 0; - _jacobian(0, 10) = 0; - _jacobian(1, 10) = 0; - _jacobian(2, 10) = 0; - _jacobian(0, 11) = 0; - _jacobian(1, 11) = 0; - _jacobian(2, 11) = 0; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian.setZero(); - - _hessian(0, 0) = - std::pow(_tmp107, Scalar(2)) + std::pow(_tmp114, Scalar(2)) + std::pow(_tmp120, Scalar(2)); - _hessian(1, 0) = _tmp107 * _tmp146 + _tmp114 * _tmp149 + _tmp120 * _tmp151; - _hessian(2, 0) = _tmp107 * _tmp167 + _tmp114 * _tmp168 + _tmp120 * _tmp169; - _hessian(6, 0) = _tmp107 * _tmp186 + _tmp114 * _tmp187 + _tmp120 * _tmp188; - _hessian(7, 0) = _tmp107 * _tmp196 + _tmp114 * _tmp197 + _tmp120 * _tmp198; - _hessian(8, 0) = _tmp107 * _tmp206 + _tmp114 * _tmp207 + _tmp120 * _tmp208; - _hessian(1, 1) = - std::pow(_tmp146, Scalar(2)) + std::pow(_tmp149, Scalar(2)) + std::pow(_tmp151, Scalar(2)); - _hessian(2, 1) = _tmp146 * _tmp167 + _tmp149 * _tmp168 + _tmp151 * _tmp169; - _hessian(6, 1) = _tmp146 * _tmp186 + _tmp149 * _tmp187 + _tmp151 * _tmp188; - _hessian(7, 1) = _tmp146 * _tmp196 + _tmp149 * _tmp197 + _tmp151 * _tmp198; - _hessian(8, 1) = _tmp146 * _tmp206 + _tmp149 * _tmp207 + _tmp151 * _tmp208; - _hessian(2, 2) = - std::pow(_tmp167, Scalar(2)) + std::pow(_tmp168, Scalar(2)) + std::pow(_tmp169, Scalar(2)); - _hessian(6, 2) = _tmp167 * _tmp186 + _tmp168 * _tmp187 + _tmp169 * _tmp188; - _hessian(7, 2) = _tmp167 * _tmp196 + _tmp168 * _tmp197 + _tmp169 * _tmp198; - _hessian(8, 2) = _tmp167 * _tmp206 + _tmp168 * _tmp207 + _tmp169 * _tmp208; - _hessian(6, 6) = - std::pow(_tmp186, Scalar(2)) + std::pow(_tmp187, Scalar(2)) + std::pow(_tmp188, Scalar(2)); - _hessian(7, 6) = _tmp186 * _tmp196 + _tmp187 * _tmp197 + _tmp188 * _tmp198; - _hessian(8, 6) = _tmp186 * _tmp206 + _tmp187 * _tmp207 + _tmp188 * _tmp208; - _hessian(7, 7) = - std::pow(_tmp196, Scalar(2)) + std::pow(_tmp197, Scalar(2)) + std::pow(_tmp198, Scalar(2)); - _hessian(8, 7) = _tmp196 * _tmp206 + _tmp197 * _tmp207 + _tmp198 * _tmp208; - _hessian(8, 8) = - std::pow(_tmp206, Scalar(2)) + std::pow(_tmp207, Scalar(2)) + std::pow(_tmp208, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp107 * _tmp38 + _tmp114 * _tmp40 + _tmp120 * _tmp42; - _rhs(1, 0) = _tmp146 * _tmp38 + _tmp149 * _tmp40 + _tmp151 * _tmp42; - _rhs(2, 0) = _tmp167 * _tmp38 + _tmp168 * _tmp40 + _tmp169 * _tmp42; - _rhs(3, 0) = 0; - _rhs(4, 0) = 0; - _rhs(5, 0) = 0; - _rhs(6, 0) = _tmp186 * _tmp38 + _tmp187 * _tmp40 + _tmp188 * _tmp42; - _rhs(7, 0) = _tmp196 * _tmp38 + _tmp197 * _tmp40 + _tmp198 * _tmp42; - _rhs(8, 0) = _tmp206 * _tmp38 + _tmp207 * _tmp40 + _tmp208 * _tmp42; - _rhs(9, 0) = 0; - _rhs(10, 0) = 0; - _rhs(11, 0) = 0; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_pose3_rotation/between_factor_pose3_rotation_diagonal.h" +#include "./between_factor_pose3_rotation/between_factor_pose3_rotation_isotropic.h" +#include "./between_factor_pose3_rotation/between_factor_pose3_rotation_square.h" diff --git a/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_diagonal.h b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_diagonal.h new file mode 100644 index 000000000..27aea2da6 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_diagonal.h @@ -0,0 +1,327 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_R_b. + * + * In vector space terms that would be: + * (b - a) - a_R_b + * + * In lie group terms: + * local_coordinates(a_R_b, between(a, b)) + * to_tangent(compose(inverse(a_R_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3& b, + const sym::Rot3& a_R_b, + const Eigen::Matrix& sqrt_info, 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: 481 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_R_b = a_R_b.Data(); + + // Intermediate terms (156) + const Scalar _tmp0 = _a[1] * _b[1]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[3] * _b[3]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a[2] * _b[1]; + const Scalar _tmp6 = _a[0] * _b[3]; + const Scalar _tmp7 = _a[1] * _b[2]; + const Scalar _tmp8 = _a[3] * _b[0]; + const Scalar _tmp9 = _tmp5 - _tmp6 - _tmp7 + _tmp8; + const Scalar _tmp10 = _a[3] * _b[1]; + const Scalar _tmp11 = _a[1] * _b[3]; + const Scalar _tmp12 = _a[0] * _b[2]; + const Scalar _tmp13 = _a[2] * _b[0]; + const Scalar _tmp14 = _tmp10 - _tmp11 + _tmp12 - _tmp13; + const Scalar _tmp15 = _a[0] * _b[1]; + const Scalar _tmp16 = _a[2] * _b[3]; + const Scalar _tmp17 = _a[3] * _b[2]; + const Scalar _tmp18 = _a[1] * _b[0]; + const Scalar _tmp19 = -_tmp15 - _tmp16 + _tmp17 + _tmp18; + const Scalar _tmp20 = + -_a_R_b[0] * _tmp4 - _a_R_b[1] * _tmp19 + _a_R_b[2] * _tmp14 + _a_R_b[3] * _tmp9; + const Scalar _tmp21 = 1 - epsilon; + const Scalar _tmp22 = _a_R_b[0] * _tmp9; + const Scalar _tmp23 = _a_R_b[1] * _tmp14; + const Scalar _tmp24 = _a_R_b[2] * _tmp19; + const Scalar _tmp25 = -_tmp22 - _tmp23 - _tmp24; + const Scalar _tmp26 = _a_R_b[3] * _tmp4; + const Scalar _tmp27 = std::min(_tmp21, std::fabs(_tmp25 - _tmp26)); + const Scalar _tmp28 = std::acos(_tmp27) / std::sqrt(Scalar(1 - std::pow(_tmp27, Scalar(2)))); + const Scalar _tmp29 = + 2 * std::min(0, (((-_tmp25 + _tmp26) > 0) - ((-_tmp25 + _tmp26) < 0))) + 1; + const Scalar _tmp30 = 2 * _tmp29; + const Scalar _tmp31 = _tmp30 * sqrt_info(0, 0); + const Scalar _tmp32 = _tmp20 * _tmp28 * _tmp31; + const Scalar _tmp33 = + _a_R_b[0] * _tmp19 - _a_R_b[1] * _tmp4 - _a_R_b[2] * _tmp9 + _a_R_b[3] * _tmp14; + const Scalar _tmp34 = _tmp30 * sqrt_info(1, 0); + const Scalar _tmp35 = _tmp28 * _tmp33 * _tmp34; + const Scalar _tmp36 = + -_a_R_b[0] * _tmp14 + _a_R_b[1] * _tmp9 - _a_R_b[2] * _tmp4 + _a_R_b[3] * _tmp19; + const Scalar _tmp37 = _tmp30 * sqrt_info(2, 0); + const Scalar _tmp38 = _tmp28 * _tmp36 * _tmp37; + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp43 = -_tmp39 - _tmp40 - _tmp41 - _tmp42; + const Scalar _tmp44 = _a_R_b[0] * _tmp43; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp49 = _tmp45 - _tmp46 - _tmp47 + _tmp48; + const Scalar _tmp50 = _a_R_b[3] * _tmp49; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp55 = -_tmp51 - _tmp52 + _tmp53 + _tmp54; + const Scalar _tmp56 = _a_R_b[1] * _tmp55; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp61 = -_tmp57 + _tmp58 - _tmp59 + _tmp60; + const Scalar _tmp62 = _a_R_b[2] * _tmp61; + const Scalar _tmp63 = _tmp56 + _tmp62; + const Scalar _tmp64 = _tmp44 + _tmp50 + _tmp63; + const Scalar _tmp65 = _tmp20 * sqrt_info(0, 0); + const Scalar _tmp66 = _tmp22 + _tmp23 + _tmp24 + _tmp26; + const Scalar _tmp67 = std::fabs(_tmp66); + const Scalar _tmp68 = std::min(_tmp21, _tmp67); + const Scalar _tmp69 = std::acos(_tmp68); + const Scalar _tmp70 = 1 - std::pow(_tmp68, Scalar(2)); + const Scalar _tmp71 = _tmp29 * ((((_tmp21 - _tmp67) > 0) - ((_tmp21 - _tmp67) < 0)) + 1) * + (((_tmp66) > 0) - ((_tmp66) < 0)); + const Scalar _tmp72 = _tmp68 * _tmp69 * _tmp71 / (_tmp70 * std::sqrt(_tmp70)); + const Scalar _tmp73 = _tmp65 * _tmp72; + const Scalar _tmp74 = _tmp71 / _tmp70; + const Scalar _tmp75 = _tmp64 * _tmp74; + const Scalar _tmp76 = _a_R_b[3] * _tmp43; + const Scalar _tmp77 = _a_R_b[0] * _tmp49; + const Scalar _tmp78 = _a_R_b[2] * _tmp55; + const Scalar _tmp79 = -_a_R_b[1] * _tmp61; + const Scalar _tmp80 = _tmp78 + _tmp79; + const Scalar _tmp81 = _tmp69 / std::sqrt(_tmp70); + const Scalar _tmp82 = _tmp31 * _tmp81; + const Scalar _tmp83 = _tmp64 * _tmp73 - _tmp65 * _tmp75 + _tmp82 * (_tmp76 - _tmp77 + _tmp80); + const Scalar _tmp84 = _tmp33 * sqrt_info(1, 0); + const Scalar _tmp85 = _tmp72 * _tmp84; + const Scalar _tmp86 = _a_R_b[2] * _tmp43; + const Scalar _tmp87 = -_a_R_b[1] * _tmp49; + const Scalar _tmp88 = _a_R_b[3] * _tmp55; + const Scalar _tmp89 = _a_R_b[0] * _tmp61; + const Scalar _tmp90 = _tmp88 + _tmp89; + const Scalar _tmp91 = _tmp34 * _tmp81; + const Scalar _tmp92 = _tmp64 * _tmp85 - _tmp75 * _tmp84 + _tmp91 * (-_tmp86 + _tmp87 + _tmp90); + const Scalar _tmp93 = _tmp36 * sqrt_info(2, 0); + const Scalar _tmp94 = _tmp72 * _tmp93; + const Scalar _tmp95 = _a_R_b[1] * _tmp43; + const Scalar _tmp96 = _a_R_b[2] * _tmp49; + const Scalar _tmp97 = -_a_R_b[0] * _tmp55; + const Scalar _tmp98 = _a_R_b[3] * _tmp61; + const Scalar _tmp99 = _tmp97 + _tmp98; + const Scalar _tmp100 = _tmp37 * _tmp81; + const Scalar _tmp101 = _tmp100 * (_tmp95 - _tmp96 + _tmp99) + _tmp64 * _tmp94 - _tmp75 * _tmp93; + const Scalar _tmp102 = _tmp57 - _tmp58 + _tmp59 - _tmp60; + const Scalar _tmp103 = _a_R_b[3] * _tmp102; + const Scalar _tmp104 = _tmp51 + _tmp52 - _tmp53 - _tmp54; + const Scalar _tmp105 = _a_R_b[0] * _tmp104; + const Scalar _tmp106 = _tmp105 + _tmp96; + const Scalar _tmp107 = _tmp103 + _tmp106 + _tmp95; + const Scalar _tmp108 = _tmp107 * _tmp74; + const Scalar _tmp109 = _a_R_b[3] * _tmp104; + const Scalar _tmp110 = _a_R_b[0] * _tmp102; + const Scalar _tmp111 = + _tmp107 * _tmp73 - _tmp108 * _tmp65 + _tmp82 * (_tmp109 - _tmp110 + _tmp86 + _tmp87); + const Scalar _tmp112 = _a_R_b[1] * _tmp102; + const Scalar _tmp113 = -_a_R_b[2] * _tmp104; + const Scalar _tmp114 = _tmp113 + _tmp77; + const Scalar _tmp115 = + _tmp107 * _tmp85 - _tmp108 * _tmp84 + _tmp91 * (-_tmp112 + _tmp114 + _tmp76); + const Scalar _tmp116 = -_a_R_b[2] * _tmp102; + const Scalar _tmp117 = _a_R_b[1] * _tmp104; + const Scalar _tmp118 = _tmp117 + _tmp50; + const Scalar _tmp119 = + _tmp100 * (_tmp116 + _tmp118 - _tmp44) + _tmp107 * _tmp94 - _tmp108 * _tmp93; + const Scalar _tmp120 = -_tmp45 + _tmp46 + _tmp47 - _tmp48; + const Scalar _tmp121 = _a_R_b[1] * _tmp120; + const Scalar _tmp122 = _tmp110 + _tmp121; + const Scalar _tmp123 = _tmp122 + _tmp86 + _tmp88; + const Scalar _tmp124 = _a_R_b[2] * _tmp120; + const Scalar _tmp125 = _tmp103 + _tmp124; + const Scalar _tmp126 = _tmp123 * _tmp74; + const Scalar _tmp127 = _tmp123 * _tmp73 - _tmp126 * _tmp65 + _tmp82 * (_tmp125 - _tmp95 + _tmp97); + const Scalar _tmp128 = _a_R_b[3] * _tmp120; + const Scalar _tmp129 = _tmp116 + _tmp128; + const Scalar _tmp130 = _tmp123 * _tmp85 - _tmp126 * _tmp84 + _tmp91 * (_tmp129 + _tmp44 - _tmp56); + const Scalar _tmp131 = -_a_R_b[0] * _tmp120; + const Scalar _tmp132 = + _tmp100 * (_tmp112 + _tmp131 + _tmp76 - _tmp78) + _tmp123 * _tmp94 - _tmp126 * _tmp93; + const Scalar _tmp133 = _tmp39 + _tmp40 + _tmp41 + _tmp42; + const Scalar _tmp134 = _a_R_b[3] * _tmp133; + const Scalar _tmp135 = _tmp131 + _tmp134; + const Scalar _tmp136 = _a_R_b[0] * _tmp133; + const Scalar _tmp137 = _tmp128 + _tmp136 + _tmp63; + const Scalar _tmp138 = _tmp137 * _tmp74; + const Scalar _tmp139 = _tmp137 * _tmp72; + const Scalar _tmp140 = -_tmp138 * _tmp65 + _tmp139 * _tmp65 + _tmp82 * (_tmp135 + _tmp80); + const Scalar _tmp141 = _a_R_b[2] * _tmp133; + const Scalar _tmp142 = + -_tmp138 * _tmp84 + _tmp139 * _tmp84 + _tmp91 * (-_tmp121 - _tmp141 + _tmp90); + const Scalar _tmp143 = _a_R_b[1] * _tmp133; + const Scalar _tmp144 = + _tmp100 * (-_tmp124 + _tmp143 + _tmp99) - _tmp138 * _tmp93 + _tmp139 * _tmp93; + const Scalar _tmp145 = _tmp106 + _tmp143 + _tmp98; + const Scalar _tmp146 = _tmp145 * _tmp74; + const Scalar _tmp147 = _tmp109 + _tmp141; + const Scalar _tmp148 = _tmp145 * _tmp73 - _tmp146 * _tmp65 + _tmp82 * (_tmp147 + _tmp87 - _tmp89); + const Scalar _tmp149 = + _tmp145 * _tmp85 - _tmp146 * _tmp84 + _tmp91 * (_tmp114 + _tmp134 + _tmp79); + const Scalar _tmp150 = + _tmp100 * (_tmp118 - _tmp136 - _tmp62) + _tmp145 * _tmp94 - _tmp146 * _tmp93; + const Scalar _tmp151 = _tmp122 + _tmp147; + const Scalar _tmp152 = _tmp151 * _tmp74; + const Scalar _tmp153 = + _tmp151 * _tmp73 - _tmp152 * _tmp65 + _tmp82 * (-_tmp105 + _tmp125 - _tmp143); + const Scalar _tmp154 = + _tmp151 * _tmp85 - _tmp152 * _tmp84 + _tmp91 * (-_tmp117 + _tmp129 + _tmp136); + const Scalar _tmp155 = + _tmp100 * (_tmp112 + _tmp113 + _tmp135) + _tmp151 * _tmp94 - _tmp152 * _tmp93; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp32; + _res(1, 0) = _tmp35; + _res(2, 0) = _tmp38; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp83; + _jacobian(1, 0) = _tmp92; + _jacobian(2, 0) = _tmp101; + _jacobian(0, 1) = _tmp111; + _jacobian(1, 1) = _tmp115; + _jacobian(2, 1) = _tmp119; + _jacobian(0, 2) = _tmp127; + _jacobian(1, 2) = _tmp130; + _jacobian(2, 2) = _tmp132; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(0, 6) = _tmp140; + _jacobian(1, 6) = _tmp142; + _jacobian(2, 6) = _tmp144; + _jacobian(0, 7) = _tmp148; + _jacobian(1, 7) = _tmp149; + _jacobian(2, 7) = _tmp150; + _jacobian(0, 8) = _tmp153; + _jacobian(1, 8) = _tmp154; + _jacobian(2, 8) = _tmp155; + _jacobian(0, 9) = 0; + _jacobian(1, 9) = 0; + _jacobian(2, 9) = 0; + _jacobian(0, 10) = 0; + _jacobian(1, 10) = 0; + _jacobian(2, 10) = 0; + _jacobian(0, 11) = 0; + _jacobian(1, 11) = 0; + _jacobian(2, 11) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp101, Scalar(2)) + std::pow(_tmp83, Scalar(2)) + std::pow(_tmp92, Scalar(2)); + _hessian(1, 0) = _tmp101 * _tmp119 + _tmp111 * _tmp83 + _tmp115 * _tmp92; + _hessian(2, 0) = _tmp101 * _tmp132 + _tmp127 * _tmp83 + _tmp130 * _tmp92; + _hessian(6, 0) = _tmp101 * _tmp144 + _tmp140 * _tmp83 + _tmp142 * _tmp92; + _hessian(7, 0) = _tmp101 * _tmp150 + _tmp148 * _tmp83 + _tmp149 * _tmp92; + _hessian(8, 0) = _tmp101 * _tmp155 + _tmp153 * _tmp83 + _tmp154 * _tmp92; + _hessian(1, 1) = + std::pow(_tmp111, Scalar(2)) + std::pow(_tmp115, Scalar(2)) + std::pow(_tmp119, Scalar(2)); + _hessian(2, 1) = _tmp111 * _tmp127 + _tmp115 * _tmp130 + _tmp119 * _tmp132; + _hessian(6, 1) = _tmp111 * _tmp140 + _tmp115 * _tmp142 + _tmp119 * _tmp144; + _hessian(7, 1) = _tmp111 * _tmp148 + _tmp115 * _tmp149 + _tmp119 * _tmp150; + _hessian(8, 1) = _tmp111 * _tmp153 + _tmp115 * _tmp154 + _tmp119 * _tmp155; + _hessian(2, 2) = + std::pow(_tmp127, Scalar(2)) + std::pow(_tmp130, Scalar(2)) + std::pow(_tmp132, Scalar(2)); + _hessian(6, 2) = _tmp127 * _tmp140 + _tmp130 * _tmp142 + _tmp132 * _tmp144; + _hessian(7, 2) = _tmp127 * _tmp148 + _tmp130 * _tmp149 + _tmp132 * _tmp150; + _hessian(8, 2) = _tmp127 * _tmp153 + _tmp130 * _tmp154 + _tmp132 * _tmp155; + _hessian(6, 6) = + std::pow(_tmp140, Scalar(2)) + std::pow(_tmp142, Scalar(2)) + std::pow(_tmp144, Scalar(2)); + _hessian(7, 6) = _tmp140 * _tmp148 + _tmp142 * _tmp149 + _tmp144 * _tmp150; + _hessian(8, 6) = _tmp140 * _tmp153 + _tmp142 * _tmp154 + _tmp144 * _tmp155; + _hessian(7, 7) = + std::pow(_tmp148, Scalar(2)) + std::pow(_tmp149, Scalar(2)) + std::pow(_tmp150, Scalar(2)); + _hessian(8, 7) = _tmp148 * _tmp153 + _tmp149 * _tmp154 + _tmp150 * _tmp155; + _hessian(8, 8) = + std::pow(_tmp153, Scalar(2)) + std::pow(_tmp154, Scalar(2)) + std::pow(_tmp155, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp101 * _tmp38 + _tmp32 * _tmp83 + _tmp35 * _tmp92; + _rhs(1, 0) = _tmp111 * _tmp32 + _tmp115 * _tmp35 + _tmp119 * _tmp38; + _rhs(2, 0) = _tmp127 * _tmp32 + _tmp130 * _tmp35 + _tmp132 * _tmp38; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + _rhs(6, 0) = _tmp140 * _tmp32 + _tmp142 * _tmp35 + _tmp144 * _tmp38; + _rhs(7, 0) = _tmp148 * _tmp32 + _tmp149 * _tmp35 + _tmp150 * _tmp38; + _rhs(8, 0) = _tmp153 * _tmp32 + _tmp154 * _tmp35 + _tmp155 * _tmp38; + _rhs(9, 0) = 0; + _rhs(10, 0) = 0; + _rhs(11, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_isotropic.h b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_isotropic.h new file mode 100644 index 000000000..407361e73 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_isotropic.h @@ -0,0 +1,325 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_R_b. + * + * In vector space terms that would be: + * (b - a) - a_R_b + * + * In lie group terms: + * local_coordinates(a_R_b, between(a, b)) + * to_tangent(compose(inverse(a_R_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3& b, + const sym::Rot3& a_R_b, const Scalar sqrt_info, + 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: 474 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_R_b = a_R_b.Data(); + + // Intermediate terms (149) + const Scalar _tmp0 = _a[1] * _b[1]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[3] * _b[3]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a[2] * _b[1]; + const Scalar _tmp6 = _a[0] * _b[3]; + const Scalar _tmp7 = _a[1] * _b[2]; + const Scalar _tmp8 = _a[3] * _b[0]; + const Scalar _tmp9 = _tmp5 - _tmp6 - _tmp7 + _tmp8; + const Scalar _tmp10 = _a[3] * _b[1]; + const Scalar _tmp11 = _a[1] * _b[3]; + const Scalar _tmp12 = _a[0] * _b[2]; + const Scalar _tmp13 = _a[2] * _b[0]; + const Scalar _tmp14 = _tmp10 - _tmp11 + _tmp12 - _tmp13; + const Scalar _tmp15 = _a[0] * _b[1]; + const Scalar _tmp16 = _a[2] * _b[3]; + const Scalar _tmp17 = _a[3] * _b[2]; + const Scalar _tmp18 = _a[1] * _b[0]; + const Scalar _tmp19 = -_tmp15 - _tmp16 + _tmp17 + _tmp18; + const Scalar _tmp20 = + -_a_R_b[0] * _tmp4 - _a_R_b[1] * _tmp19 + _a_R_b[2] * _tmp14 + _a_R_b[3] * _tmp9; + const Scalar _tmp21 = 1 - epsilon; + const Scalar _tmp22 = _a_R_b[0] * _tmp9; + const Scalar _tmp23 = _a_R_b[1] * _tmp14; + const Scalar _tmp24 = _a_R_b[2] * _tmp19; + const Scalar _tmp25 = -_tmp22 - _tmp23 - _tmp24; + const Scalar _tmp26 = _a_R_b[3] * _tmp4; + const Scalar _tmp27 = std::min(_tmp21, std::fabs(_tmp25 - _tmp26)); + const Scalar _tmp28 = + sqrt_info * + (2 * std::min(0, (((-_tmp25 + _tmp26) > 0) - ((-_tmp25 + _tmp26) < 0))) + 1); + const Scalar _tmp29 = 2 * _tmp28; + const Scalar _tmp30 = + _tmp29 * std::acos(_tmp27) / std::sqrt(Scalar(1 - std::pow(_tmp27, Scalar(2)))); + const Scalar _tmp31 = _tmp20 * _tmp30; + const Scalar _tmp32 = + _a_R_b[0] * _tmp19 - _a_R_b[1] * _tmp4 - _a_R_b[2] * _tmp9 + _a_R_b[3] * _tmp14; + const Scalar _tmp33 = _tmp30 * _tmp32; + const Scalar _tmp34 = + -_a_R_b[0] * _tmp14 + _a_R_b[1] * _tmp9 - _a_R_b[2] * _tmp4 + _a_R_b[3] * _tmp19; + const Scalar _tmp35 = _tmp30 * _tmp34; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp40 = -_tmp36 - _tmp37 - _tmp38 - _tmp39; + const Scalar _tmp41 = _a_R_b[3] * _tmp40; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp46 = _tmp42 - _tmp43 - _tmp44 + _tmp45; + const Scalar _tmp47 = _a_R_b[0] * _tmp46; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp52 = -_tmp48 - _tmp49 + _tmp50 + _tmp51; + const Scalar _tmp53 = _a_R_b[2] * _tmp52; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp58 = -_tmp54 + _tmp55 - _tmp56 + _tmp57; + const Scalar _tmp59 = -_a_R_b[1] * _tmp58; + const Scalar _tmp60 = _tmp53 + _tmp59; + const Scalar _tmp61 = _tmp22 + _tmp23 + _tmp24 + _tmp26; + const Scalar _tmp62 = std::fabs(_tmp61); + const Scalar _tmp63 = std::min(_tmp21, _tmp62); + const Scalar _tmp64 = 1 - std::pow(_tmp63, Scalar(2)); + const Scalar _tmp65 = std::acos(_tmp63); + const Scalar _tmp66 = _tmp29 * _tmp65 / std::sqrt(_tmp64); + const Scalar _tmp67 = _a_R_b[0] * _tmp40; + const Scalar _tmp68 = _a_R_b[3] * _tmp46; + const Scalar _tmp69 = _a_R_b[1] * _tmp52; + const Scalar _tmp70 = _a_R_b[2] * _tmp58; + const Scalar _tmp71 = _tmp69 + _tmp70; + const Scalar _tmp72 = _tmp67 + _tmp68 + _tmp71; + const Scalar _tmp73 = _tmp28 * ((((_tmp21 - _tmp62) > 0) - ((_tmp21 - _tmp62) < 0)) + 1) * + (((_tmp61) > 0) - ((_tmp61) < 0)); + const Scalar _tmp74 = _tmp63 * _tmp65 * _tmp73 / (_tmp64 * std::sqrt(_tmp64)); + const Scalar _tmp75 = _tmp72 * _tmp74; + const Scalar _tmp76 = _tmp73 / _tmp64; + const Scalar _tmp77 = _tmp20 * _tmp76; + const Scalar _tmp78 = _tmp20 * _tmp75 + _tmp66 * (_tmp41 - _tmp47 + _tmp60) - _tmp72 * _tmp77; + const Scalar _tmp79 = _a_R_b[2] * _tmp40; + const Scalar _tmp80 = -_a_R_b[1] * _tmp46; + const Scalar _tmp81 = _a_R_b[3] * _tmp52; + const Scalar _tmp82 = _a_R_b[0] * _tmp58; + const Scalar _tmp83 = _tmp81 + _tmp82; + const Scalar _tmp84 = _tmp32 * _tmp76; + const Scalar _tmp85 = _tmp32 * _tmp75 + _tmp66 * (-_tmp79 + _tmp80 + _tmp83) - _tmp72 * _tmp84; + const Scalar _tmp86 = _a_R_b[1] * _tmp40; + const Scalar _tmp87 = _a_R_b[2] * _tmp46; + const Scalar _tmp88 = -_a_R_b[0] * _tmp52; + const Scalar _tmp89 = _a_R_b[3] * _tmp58; + const Scalar _tmp90 = _tmp88 + _tmp89; + const Scalar _tmp91 = _tmp34 * _tmp76; + const Scalar _tmp92 = _tmp34 * _tmp75 + _tmp66 * (_tmp86 - _tmp87 + _tmp90) - _tmp72 * _tmp91; + const Scalar _tmp93 = _tmp54 - _tmp55 + _tmp56 - _tmp57; + const Scalar _tmp94 = _a_R_b[3] * _tmp93; + const Scalar _tmp95 = _tmp48 + _tmp49 - _tmp50 - _tmp51; + const Scalar _tmp96 = _a_R_b[0] * _tmp95; + const Scalar _tmp97 = _tmp87 + _tmp96; + const Scalar _tmp98 = _tmp86 + _tmp94 + _tmp97; + const Scalar _tmp99 = _tmp74 * _tmp98; + const Scalar _tmp100 = _a_R_b[3] * _tmp95; + const Scalar _tmp101 = _a_R_b[0] * _tmp93; + const Scalar _tmp102 = _tmp76 * _tmp98; + const Scalar _tmp103 = + -_tmp102 * _tmp20 + _tmp20 * _tmp99 + _tmp66 * (_tmp100 - _tmp101 + _tmp79 + _tmp80); + const Scalar _tmp104 = _a_R_b[1] * _tmp93; + const Scalar _tmp105 = -_a_R_b[2] * _tmp95; + const Scalar _tmp106 = _tmp105 + _tmp47; + const Scalar _tmp107 = + -_tmp102 * _tmp32 + _tmp32 * _tmp99 + _tmp66 * (-_tmp104 + _tmp106 + _tmp41); + const Scalar _tmp108 = -_a_R_b[2] * _tmp93; + const Scalar _tmp109 = _a_R_b[1] * _tmp95; + const Scalar _tmp110 = _tmp109 + _tmp68; + const Scalar _tmp111 = + -_tmp102 * _tmp34 + _tmp34 * _tmp99 + _tmp66 * (_tmp108 + _tmp110 - _tmp67); + const Scalar _tmp112 = -_tmp42 + _tmp43 + _tmp44 - _tmp45; + const Scalar _tmp113 = _a_R_b[1] * _tmp112; + const Scalar _tmp114 = _tmp101 + _tmp113; + const Scalar _tmp115 = _tmp114 + _tmp79 + _tmp81; + const Scalar _tmp116 = _tmp115 * _tmp76; + const Scalar _tmp117 = _tmp115 * _tmp74; + const Scalar _tmp118 = _a_R_b[2] * _tmp112; + const Scalar _tmp119 = _tmp118 + _tmp94; + const Scalar _tmp120 = + -_tmp116 * _tmp20 + _tmp117 * _tmp20 + _tmp66 * (_tmp119 - _tmp86 + _tmp88); + const Scalar _tmp121 = _a_R_b[3] * _tmp112; + const Scalar _tmp122 = _tmp108 + _tmp121; + const Scalar _tmp123 = + -_tmp116 * _tmp32 + _tmp117 * _tmp32 + _tmp66 * (_tmp122 + _tmp67 - _tmp69); + const Scalar _tmp124 = -_a_R_b[0] * _tmp112; + const Scalar _tmp125 = + -_tmp116 * _tmp34 + _tmp117 * _tmp34 + _tmp66 * (_tmp104 + _tmp124 + _tmp41 - _tmp53); + const Scalar _tmp126 = _tmp36 + _tmp37 + _tmp38 + _tmp39; + const Scalar _tmp127 = _a_R_b[3] * _tmp126; + const Scalar _tmp128 = _tmp124 + _tmp127; + const Scalar _tmp129 = _a_R_b[0] * _tmp126; + const Scalar _tmp130 = _tmp121 + _tmp129 + _tmp71; + const Scalar _tmp131 = _tmp130 * _tmp74; + const Scalar _tmp132 = -_tmp130 * _tmp77 + _tmp131 * _tmp20 + _tmp66 * (_tmp128 + _tmp60); + const Scalar _tmp133 = _a_R_b[2] * _tmp126; + const Scalar _tmp134 = + -_tmp130 * _tmp84 + _tmp131 * _tmp32 + _tmp66 * (-_tmp113 - _tmp133 + _tmp83); + const Scalar _tmp135 = _a_R_b[1] * _tmp126; + const Scalar _tmp136 = + -_tmp130 * _tmp91 + _tmp131 * _tmp34 + _tmp66 * (-_tmp118 + _tmp135 + _tmp90); + const Scalar _tmp137 = _tmp135 + _tmp89 + _tmp97; + const Scalar _tmp138 = _tmp137 * _tmp76; + const Scalar _tmp139 = _tmp100 + _tmp133; + const Scalar _tmp140 = _tmp137 * _tmp74; + const Scalar _tmp141 = + -_tmp138 * _tmp20 + _tmp140 * _tmp20 + _tmp66 * (_tmp139 + _tmp80 - _tmp82); + const Scalar _tmp142 = + -_tmp138 * _tmp32 + _tmp140 * _tmp32 + _tmp66 * (_tmp106 + _tmp127 + _tmp59); + const Scalar _tmp143 = + -_tmp138 * _tmp34 + _tmp140 * _tmp34 + _tmp66 * (_tmp110 - _tmp129 - _tmp70); + const Scalar _tmp144 = _tmp114 + _tmp139; + const Scalar _tmp145 = _tmp144 * _tmp74; + const Scalar _tmp146 = + -_tmp144 * _tmp77 + _tmp145 * _tmp20 + _tmp66 * (_tmp119 - _tmp135 - _tmp96); + const Scalar _tmp147 = + -_tmp144 * _tmp84 + _tmp145 * _tmp32 + _tmp66 * (-_tmp109 + _tmp122 + _tmp129); + const Scalar _tmp148 = + -_tmp144 * _tmp91 + _tmp145 * _tmp34 + _tmp66 * (_tmp104 + _tmp105 + _tmp128); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp31; + _res(1, 0) = _tmp33; + _res(2, 0) = _tmp35; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp78; + _jacobian(1, 0) = _tmp85; + _jacobian(2, 0) = _tmp92; + _jacobian(0, 1) = _tmp103; + _jacobian(1, 1) = _tmp107; + _jacobian(2, 1) = _tmp111; + _jacobian(0, 2) = _tmp120; + _jacobian(1, 2) = _tmp123; + _jacobian(2, 2) = _tmp125; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(0, 6) = _tmp132; + _jacobian(1, 6) = _tmp134; + _jacobian(2, 6) = _tmp136; + _jacobian(0, 7) = _tmp141; + _jacobian(1, 7) = _tmp142; + _jacobian(2, 7) = _tmp143; + _jacobian(0, 8) = _tmp146; + _jacobian(1, 8) = _tmp147; + _jacobian(2, 8) = _tmp148; + _jacobian(0, 9) = 0; + _jacobian(1, 9) = 0; + _jacobian(2, 9) = 0; + _jacobian(0, 10) = 0; + _jacobian(1, 10) = 0; + _jacobian(2, 10) = 0; + _jacobian(0, 11) = 0; + _jacobian(1, 11) = 0; + _jacobian(2, 11) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp78, Scalar(2)) + std::pow(_tmp85, Scalar(2)) + std::pow(_tmp92, Scalar(2)); + _hessian(1, 0) = _tmp103 * _tmp78 + _tmp107 * _tmp85 + _tmp111 * _tmp92; + _hessian(2, 0) = _tmp120 * _tmp78 + _tmp123 * _tmp85 + _tmp125 * _tmp92; + _hessian(6, 0) = _tmp132 * _tmp78 + _tmp134 * _tmp85 + _tmp136 * _tmp92; + _hessian(7, 0) = _tmp141 * _tmp78 + _tmp142 * _tmp85 + _tmp143 * _tmp92; + _hessian(8, 0) = _tmp146 * _tmp78 + _tmp147 * _tmp85 + _tmp148 * _tmp92; + _hessian(1, 1) = + std::pow(_tmp103, Scalar(2)) + std::pow(_tmp107, Scalar(2)) + std::pow(_tmp111, Scalar(2)); + _hessian(2, 1) = _tmp103 * _tmp120 + _tmp107 * _tmp123 + _tmp111 * _tmp125; + _hessian(6, 1) = _tmp103 * _tmp132 + _tmp107 * _tmp134 + _tmp111 * _tmp136; + _hessian(7, 1) = _tmp103 * _tmp141 + _tmp107 * _tmp142 + _tmp111 * _tmp143; + _hessian(8, 1) = _tmp103 * _tmp146 + _tmp107 * _tmp147 + _tmp111 * _tmp148; + _hessian(2, 2) = + std::pow(_tmp120, Scalar(2)) + std::pow(_tmp123, Scalar(2)) + std::pow(_tmp125, Scalar(2)); + _hessian(6, 2) = _tmp120 * _tmp132 + _tmp123 * _tmp134 + _tmp125 * _tmp136; + _hessian(7, 2) = _tmp120 * _tmp141 + _tmp123 * _tmp142 + _tmp125 * _tmp143; + _hessian(8, 2) = _tmp120 * _tmp146 + _tmp123 * _tmp147 + _tmp125 * _tmp148; + _hessian(6, 6) = + std::pow(_tmp132, Scalar(2)) + std::pow(_tmp134, Scalar(2)) + std::pow(_tmp136, Scalar(2)); + _hessian(7, 6) = _tmp132 * _tmp141 + _tmp134 * _tmp142 + _tmp136 * _tmp143; + _hessian(8, 6) = _tmp132 * _tmp146 + _tmp134 * _tmp147 + _tmp136 * _tmp148; + _hessian(7, 7) = + std::pow(_tmp141, Scalar(2)) + std::pow(_tmp142, Scalar(2)) + std::pow(_tmp143, Scalar(2)); + _hessian(8, 7) = _tmp141 * _tmp146 + _tmp142 * _tmp147 + _tmp143 * _tmp148; + _hessian(8, 8) = + std::pow(_tmp146, Scalar(2)) + std::pow(_tmp147, Scalar(2)) + std::pow(_tmp148, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp31 * _tmp78 + _tmp33 * _tmp85 + _tmp35 * _tmp92; + _rhs(1, 0) = _tmp103 * _tmp31 + _tmp107 * _tmp33 + _tmp111 * _tmp35; + _rhs(2, 0) = _tmp120 * _tmp31 + _tmp123 * _tmp33 + _tmp125 * _tmp35; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + _rhs(6, 0) = _tmp132 * _tmp31 + _tmp134 * _tmp33 + _tmp136 * _tmp35; + _rhs(7, 0) = _tmp141 * _tmp31 + _tmp142 * _tmp33 + _tmp143 * _tmp35; + _rhs(8, 0) = _tmp146 * _tmp31 + _tmp147 * _tmp33 + _tmp148 * _tmp35; + _rhs(9, 0) = 0; + _rhs(10, 0) = 0; + _rhs(11, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_square.h b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_square.h new file mode 100644 index 000000000..0700c7a65 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_pose3_rotation/between_factor_pose3_rotation_square.h @@ -0,0 +1,409 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_R_b. + * + * In vector space terms that would be: + * (b - a) - a_R_b + * + * In lie group terms: + * local_coordinates(a_R_b, between(a, b)) + * to_tangent(compose(inverse(a_R_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x12) jacobian of res wrt args a (6), b (6) + * hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) + * rhs: (12x1) Gauss-Newton rhs for args a (6), b (6) + */ +template +void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3& b, + const sym::Rot3& a_R_b, + const Eigen::Matrix& sqrt_info, 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: 742 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_R_b = a_R_b.Data(); + + // Intermediate terms (209) + const Scalar _tmp0 = _a[1] * _b[1]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[3] * _b[3]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a[2] * _b[1]; + const Scalar _tmp6 = _a[0] * _b[3]; + const Scalar _tmp7 = _a[1] * _b[2]; + const Scalar _tmp8 = _a[3] * _b[0]; + const Scalar _tmp9 = _tmp5 - _tmp6 - _tmp7 + _tmp8; + const Scalar _tmp10 = _a[3] * _b[1]; + const Scalar _tmp11 = _a[1] * _b[3]; + const Scalar _tmp12 = _a[0] * _b[2]; + const Scalar _tmp13 = _a[2] * _b[0]; + const Scalar _tmp14 = _tmp10 - _tmp11 + _tmp12 - _tmp13; + const Scalar _tmp15 = _a[0] * _b[1]; + const Scalar _tmp16 = _a[2] * _b[3]; + const Scalar _tmp17 = _a[3] * _b[2]; + const Scalar _tmp18 = _a[1] * _b[0]; + const Scalar _tmp19 = -_tmp15 - _tmp16 + _tmp17 + _tmp18; + const Scalar _tmp20 = + -_a_R_b[0] * _tmp4 - _a_R_b[1] * _tmp19 + _a_R_b[2] * _tmp14 + _a_R_b[3] * _tmp9; + const Scalar _tmp21 = 1 - epsilon; + const Scalar _tmp22 = _a_R_b[0] * _tmp9; + const Scalar _tmp23 = _a_R_b[1] * _tmp14; + const Scalar _tmp24 = _a_R_b[2] * _tmp19; + const Scalar _tmp25 = -_tmp22 - _tmp23 - _tmp24; + const Scalar _tmp26 = _a_R_b[3] * _tmp4; + const Scalar _tmp27 = std::min(_tmp21, std::fabs(_tmp25 - _tmp26)); + const Scalar _tmp28 = + std::pow(Scalar(1 - std::pow(_tmp27, Scalar(2))), Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp29 = std::acos(_tmp27); + const Scalar _tmp30 = + 2 * std::min(0, (((-_tmp25 + _tmp26) > 0) - ((-_tmp25 + _tmp26) < 0))) + 1; + const Scalar _tmp31 = 2 * _tmp30; + const Scalar _tmp32 = _tmp28 * _tmp29 * _tmp31; + const Scalar _tmp33 = _tmp20 * _tmp32; + const Scalar _tmp34 = + -_a_R_b[0] * _tmp14 + _a_R_b[1] * _tmp9 - _a_R_b[2] * _tmp4 + _a_R_b[3] * _tmp19; + const Scalar _tmp35 = _tmp34 * sqrt_info(0, 2); + const Scalar _tmp36 = + _a_R_b[0] * _tmp19 - _a_R_b[1] * _tmp4 - _a_R_b[2] * _tmp9 + _a_R_b[3] * _tmp14; + const Scalar _tmp37 = _tmp32 * _tmp36; + const Scalar _tmp38 = _tmp32 * _tmp35 + _tmp33 * sqrt_info(0, 0) + _tmp37 * sqrt_info(0, 1); + const Scalar _tmp39 = _tmp34 * sqrt_info(1, 2); + const Scalar _tmp40 = _tmp32 * _tmp39 + _tmp33 * sqrt_info(1, 0) + _tmp37 * sqrt_info(1, 1); + const Scalar _tmp41 = _tmp31 * sqrt_info(2, 2); + const Scalar _tmp42 = + _tmp28 * _tmp29 * _tmp34 * _tmp41 + _tmp33 * sqrt_info(2, 0) + _tmp37 * sqrt_info(2, 1); + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp47 = -_tmp43 - _tmp44 - _tmp45 - _tmp46; + const Scalar _tmp48 = _a_R_b[1] * _tmp47; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp53 = -_tmp49 - _tmp50 + _tmp51 + _tmp52; + const Scalar _tmp54 = -_a_R_b[0] * _tmp53; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp59 = _tmp55 - _tmp56 - _tmp57 + _tmp58; + const Scalar _tmp60 = _a_R_b[2] * _tmp59; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp65 = -_tmp61 + _tmp62 - _tmp63 + _tmp64; + const Scalar _tmp66 = _a_R_b[3] * _tmp65; + const Scalar _tmp67 = _tmp48 + _tmp54 - _tmp60 + _tmp66; + const Scalar _tmp68 = _tmp22 + _tmp23 + _tmp24 + _tmp26; + const Scalar _tmp69 = std::fabs(_tmp68); + const Scalar _tmp70 = std::min(_tmp21, _tmp69); + const Scalar _tmp71 = 1 - std::pow(_tmp70, Scalar(2)); + const Scalar _tmp72 = std::acos(_tmp70); + const Scalar _tmp73 = _tmp72 / std::sqrt(_tmp71); + const Scalar _tmp74 = _tmp31 * _tmp73; + const Scalar _tmp75 = _tmp67 * _tmp74; + const Scalar _tmp76 = -_a_R_b[1] * _tmp59; + const Scalar _tmp77 = _a_R_b[2] * _tmp47; + const Scalar _tmp78 = _a_R_b[3] * _tmp53; + const Scalar _tmp79 = _a_R_b[0] * _tmp65; + const Scalar _tmp80 = _tmp78 + _tmp79; + const Scalar _tmp81 = _tmp76 - _tmp77 + _tmp80; + const Scalar _tmp82 = _tmp74 * _tmp81; + const Scalar _tmp83 = _a_R_b[3] * _tmp47; + const Scalar _tmp84 = _a_R_b[0] * _tmp59; + const Scalar _tmp85 = _a_R_b[2] * _tmp53; + const Scalar _tmp86 = -_a_R_b[1] * _tmp65; + const Scalar _tmp87 = _tmp85 + _tmp86; + const Scalar _tmp88 = _tmp83 - _tmp84 + _tmp87; + const Scalar _tmp89 = _tmp74 * sqrt_info(0, 0); + const Scalar _tmp90 = _tmp36 * sqrt_info(0, 1); + const Scalar _tmp91 = _a_R_b[3] * _tmp59; + const Scalar _tmp92 = _a_R_b[0] * _tmp47; + const Scalar _tmp93 = _a_R_b[1] * _tmp53; + const Scalar _tmp94 = _a_R_b[2] * _tmp65; + const Scalar _tmp95 = _tmp93 + _tmp94; + const Scalar _tmp96 = _tmp91 + _tmp92 + _tmp95; + const Scalar _tmp97 = _tmp30 * ((((_tmp21 - _tmp69) > 0) - ((_tmp21 - _tmp69) < 0)) + 1) * + (((_tmp68) > 0) - ((_tmp68) < 0)); + const Scalar _tmp98 = _tmp97 / _tmp71; + const Scalar _tmp99 = _tmp96 * _tmp98; + const Scalar _tmp100 = _tmp35 * _tmp98; + const Scalar _tmp101 = _tmp20 * _tmp98; + const Scalar _tmp102 = _tmp101 * sqrt_info(0, 0); + const Scalar _tmp103 = _tmp70 * _tmp72 * _tmp97 / (_tmp71 * std::sqrt(_tmp71)); + const Scalar _tmp104 = _tmp20 * sqrt_info(0, 0); + const Scalar _tmp105 = _tmp103 * _tmp104; + const Scalar _tmp106 = _tmp103 * _tmp96; + const Scalar _tmp107 = -_tmp100 * _tmp96 - _tmp102 * _tmp96 + _tmp105 * _tmp96 + + _tmp106 * _tmp35 + _tmp106 * _tmp90 + _tmp75 * sqrt_info(0, 2) + + _tmp82 * sqrt_info(0, 1) + _tmp88 * _tmp89 - _tmp90 * _tmp99; + const Scalar _tmp108 = _tmp74 * sqrt_info(1, 1); + const Scalar _tmp109 = _tmp74 * _tmp88; + const Scalar _tmp110 = _tmp39 * _tmp98; + const Scalar _tmp111 = _tmp36 * sqrt_info(1, 1); + const Scalar _tmp112 = _tmp101 * sqrt_info(1, 0); + const Scalar _tmp113 = _tmp20 * sqrt_info(1, 0); + const Scalar _tmp114 = _tmp106 * _tmp111 + _tmp106 * _tmp113 + _tmp106 * _tmp39 + + _tmp108 * _tmp81 + _tmp109 * sqrt_info(1, 0) - _tmp110 * _tmp96 - + _tmp111 * _tmp99 - _tmp112 * _tmp96 + _tmp75 * sqrt_info(1, 2); + const Scalar _tmp115 = _tmp41 * _tmp73; + const Scalar _tmp116 = _tmp101 * sqrt_info(2, 0); + const Scalar _tmp117 = _tmp34 * sqrt_info(2, 2); + const Scalar _tmp118 = _tmp36 * sqrt_info(2, 1); + const Scalar _tmp119 = _tmp20 * sqrt_info(2, 0); + const Scalar _tmp120 = _tmp106 * _tmp117 + _tmp106 * _tmp118 + _tmp106 * _tmp119 + + _tmp109 * sqrt_info(2, 0) + _tmp115 * _tmp67 - _tmp116 * _tmp96 - + _tmp117 * _tmp99 - _tmp118 * _tmp99 + _tmp82 * sqrt_info(2, 1); + const Scalar _tmp121 = _tmp61 - _tmp62 + _tmp63 - _tmp64; + const Scalar _tmp122 = _a_R_b[0] * _tmp121; + const Scalar _tmp123 = _tmp49 + _tmp50 - _tmp51 - _tmp52; + const Scalar _tmp124 = _a_R_b[3] * _tmp123; + const Scalar _tmp125 = _tmp124 + _tmp76; + const Scalar _tmp126 = -_tmp122 + _tmp125 + _tmp77; + const Scalar _tmp127 = _a_R_b[3] * _tmp121; + const Scalar _tmp128 = _a_R_b[0] * _tmp123; + const Scalar _tmp129 = _tmp128 + _tmp60; + const Scalar _tmp130 = _tmp127 + _tmp129 + _tmp48; + const Scalar _tmp131 = _tmp130 * _tmp98; + const Scalar _tmp132 = _tmp131 * _tmp36; + const Scalar _tmp133 = _tmp101 * _tmp130; + const Scalar _tmp134 = _a_R_b[1] * _tmp121; + const Scalar _tmp135 = -_a_R_b[2] * _tmp123; + const Scalar _tmp136 = _tmp135 + _tmp84; + const Scalar _tmp137 = -_tmp134 + _tmp136 + _tmp83; + const Scalar _tmp138 = _tmp137 * _tmp74; + const Scalar _tmp139 = -_a_R_b[2] * _tmp121; + const Scalar _tmp140 = _a_R_b[1] * _tmp123; + const Scalar _tmp141 = _tmp140 + _tmp91; + const Scalar _tmp142 = _tmp139 + _tmp141 - _tmp92; + const Scalar _tmp143 = _tmp74 * sqrt_info(0, 2); + const Scalar _tmp144 = _tmp103 * _tmp130; + const Scalar _tmp145 = _tmp144 * _tmp20; + const Scalar _tmp146 = -_tmp100 * _tmp130 + _tmp126 * _tmp89 - _tmp132 * sqrt_info(0, 1) - + _tmp133 * sqrt_info(0, 0) + _tmp138 * sqrt_info(0, 1) + _tmp142 * _tmp143 + + _tmp144 * _tmp35 + _tmp144 * _tmp90 + _tmp145 * sqrt_info(0, 0); + const Scalar _tmp147 = _tmp74 * sqrt_info(1, 0); + const Scalar _tmp148 = _tmp74 * sqrt_info(1, 2); + const Scalar _tmp149 = _tmp108 * _tmp137 - _tmp110 * _tmp130 - _tmp111 * _tmp131 + + _tmp111 * _tmp144 - _tmp112 * _tmp130 + _tmp126 * _tmp147 + + _tmp142 * _tmp148 + _tmp144 * _tmp39 + _tmp145 * sqrt_info(1, 0); + const Scalar _tmp150 = _tmp74 * sqrt_info(2, 0); + const Scalar _tmp151 = _tmp115 * _tmp142 - _tmp117 * _tmp131 + _tmp117 * _tmp144 + + _tmp118 * _tmp144 + _tmp126 * _tmp150 - _tmp132 * sqrt_info(2, 1) - + _tmp133 * sqrt_info(2, 0) + _tmp138 * sqrt_info(2, 1) + + _tmp145 * sqrt_info(2, 0); + const Scalar _tmp152 = -_tmp55 + _tmp56 + _tmp57 - _tmp58; + const Scalar _tmp153 = _a_R_b[2] * _tmp152; + const Scalar _tmp154 = _tmp127 + _tmp153; + const Scalar _tmp155 = _tmp154 - _tmp48 + _tmp54; + const Scalar _tmp156 = _a_R_b[3] * _tmp152; + const Scalar _tmp157 = _tmp139 + _tmp156 + _tmp92 - _tmp93; + const Scalar _tmp158 = _tmp157 * _tmp74; + const Scalar _tmp159 = _a_R_b[1] * _tmp152; + const Scalar _tmp160 = _tmp122 + _tmp159; + const Scalar _tmp161 = _tmp160 + _tmp77 + _tmp78; + const Scalar _tmp162 = _tmp103 * _tmp161; + const Scalar _tmp163 = -_a_R_b[0] * _tmp152; + const Scalar _tmp164 = _tmp134 + _tmp163; + const Scalar _tmp165 = _tmp164 + _tmp83 - _tmp85; + const Scalar _tmp166 = _tmp161 * _tmp98; + const Scalar _tmp167 = -_tmp100 * _tmp161 - _tmp102 * _tmp161 + _tmp104 * _tmp162 + + _tmp143 * _tmp165 + _tmp155 * _tmp89 + _tmp158 * sqrt_info(0, 1) + + _tmp162 * _tmp35 + _tmp162 * _tmp90 - _tmp166 * _tmp90; + const Scalar _tmp168 = _tmp108 * _tmp157 - _tmp110 * _tmp161 + _tmp111 * _tmp162 - + _tmp111 * _tmp166 - _tmp112 * _tmp161 + _tmp113 * _tmp162 + + _tmp147 * _tmp155 + _tmp148 * _tmp165 + _tmp162 * _tmp39; + const Scalar _tmp169 = _tmp115 * _tmp165 - _tmp116 * _tmp161 + _tmp117 * _tmp162 - + _tmp117 * _tmp166 + _tmp118 * _tmp162 - _tmp118 * _tmp166 + + _tmp119 * _tmp162 + _tmp150 * _tmp155 + _tmp158 * sqrt_info(2, 1); + const Scalar _tmp170 = _tmp43 + _tmp44 + _tmp45 + _tmp46; + const Scalar _tmp171 = _a_R_b[0] * _tmp170; + const Scalar _tmp172 = _tmp156 + _tmp171; + const Scalar _tmp173 = _tmp172 + _tmp95; + const Scalar _tmp174 = _tmp173 * _tmp98; + const Scalar _tmp175 = _tmp174 * _tmp36; + const Scalar _tmp176 = _tmp101 * _tmp173; + const Scalar _tmp177 = _a_R_b[1] * _tmp170; + const Scalar _tmp178 = _tmp177 + _tmp66; + const Scalar _tmp179 = -_tmp153 + _tmp178 + _tmp54; + const Scalar _tmp180 = _a_R_b[3] * _tmp170; + const Scalar _tmp181 = _tmp163 + _tmp180 + _tmp87; + const Scalar _tmp182 = _a_R_b[2] * _tmp170; + const Scalar _tmp183 = -_tmp159 - _tmp182 + _tmp80; + const Scalar _tmp184 = _tmp183 * _tmp74; + const Scalar _tmp185 = _tmp103 * _tmp173; + const Scalar _tmp186 = -_tmp100 * _tmp173 + _tmp105 * _tmp173 + _tmp143 * _tmp179 - + _tmp175 * sqrt_info(0, 1) - _tmp176 * sqrt_info(0, 0) + _tmp181 * _tmp89 + + _tmp184 * sqrt_info(0, 1) + _tmp185 * _tmp35 + _tmp185 * _tmp90; + const Scalar _tmp187 = _tmp108 * _tmp183 - _tmp110 * _tmp173 - _tmp111 * _tmp174 + + _tmp111 * _tmp185 - _tmp112 * _tmp173 + _tmp113 * _tmp185 + + _tmp147 * _tmp181 + _tmp148 * _tmp179 + _tmp185 * _tmp39; + const Scalar _tmp188 = _tmp115 * _tmp179 - _tmp117 * _tmp174 + _tmp117 * _tmp185 + + _tmp118 * _tmp185 + _tmp119 * _tmp185 + _tmp150 * _tmp181 - + _tmp175 * sqrt_info(2, 1) - _tmp176 * sqrt_info(2, 0) + + _tmp184 * sqrt_info(2, 1); + const Scalar _tmp189 = _tmp125 + _tmp182 - _tmp79; + const Scalar _tmp190 = _tmp129 + _tmp178; + const Scalar _tmp191 = _tmp190 * _tmp98; + const Scalar _tmp192 = _tmp141 - _tmp171 - _tmp94; + const Scalar _tmp193 = _tmp103 * _tmp190; + const Scalar _tmp194 = _tmp136 + _tmp180 + _tmp86; + const Scalar _tmp195 = _tmp194 * _tmp74; + const Scalar _tmp196 = -_tmp100 * _tmp190 - _tmp102 * _tmp190 + _tmp104 * _tmp193 + + _tmp143 * _tmp192 + _tmp189 * _tmp89 - _tmp191 * _tmp90 + + _tmp193 * _tmp35 + _tmp193 * _tmp90 + _tmp195 * sqrt_info(0, 1); + const Scalar _tmp197 = _tmp108 * _tmp194 - _tmp110 * _tmp190 - _tmp111 * _tmp191 + + _tmp111 * _tmp193 - _tmp112 * _tmp190 + _tmp113 * _tmp193 + + _tmp147 * _tmp189 + _tmp148 * _tmp192 + _tmp193 * _tmp39; + const Scalar _tmp198 = _tmp115 * _tmp192 - _tmp116 * _tmp190 - _tmp117 * _tmp191 + + _tmp117 * _tmp193 - _tmp118 * _tmp191 + _tmp118 * _tmp193 + + _tmp119 * _tmp193 + _tmp150 * _tmp189 + _tmp195 * sqrt_info(2, 1); + const Scalar _tmp199 = _tmp124 + _tmp160 + _tmp182; + const Scalar _tmp200 = _tmp103 * _tmp199; + const Scalar _tmp201 = _tmp199 * _tmp98; + const Scalar _tmp202 = _tmp139 - _tmp140 + _tmp172; + const Scalar _tmp203 = _tmp202 * _tmp74; + const Scalar _tmp204 = _tmp135 + _tmp164 + _tmp180; + const Scalar _tmp205 = -_tmp128 + _tmp154 - _tmp177; + const Scalar _tmp206 = -_tmp100 * _tmp199 - _tmp102 * _tmp199 + _tmp104 * _tmp200 + + _tmp143 * _tmp204 + _tmp200 * _tmp35 + _tmp200 * _tmp90 - + _tmp201 * _tmp90 + _tmp203 * sqrt_info(0, 1) + _tmp205 * _tmp89; + const Scalar _tmp207 = _tmp108 * _tmp202 - _tmp110 * _tmp199 + _tmp111 * _tmp200 - + _tmp111 * _tmp201 - _tmp112 * _tmp199 + _tmp113 * _tmp200 + + _tmp147 * _tmp205 + _tmp148 * _tmp204 + _tmp200 * _tmp39; + const Scalar _tmp208 = _tmp115 * _tmp204 - _tmp116 * _tmp199 + _tmp117 * _tmp200 - + _tmp117 * _tmp201 + _tmp118 * _tmp200 - _tmp118 * _tmp201 + + _tmp119 * _tmp200 + _tmp150 * _tmp205 + _tmp203 * sqrt_info(2, 1); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp38; + _res(1, 0) = _tmp40; + _res(2, 0) = _tmp42; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp107; + _jacobian(1, 0) = _tmp114; + _jacobian(2, 0) = _tmp120; + _jacobian(0, 1) = _tmp146; + _jacobian(1, 1) = _tmp149; + _jacobian(2, 1) = _tmp151; + _jacobian(0, 2) = _tmp167; + _jacobian(1, 2) = _tmp168; + _jacobian(2, 2) = _tmp169; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + _jacobian(0, 6) = _tmp186; + _jacobian(1, 6) = _tmp187; + _jacobian(2, 6) = _tmp188; + _jacobian(0, 7) = _tmp196; + _jacobian(1, 7) = _tmp197; + _jacobian(2, 7) = _tmp198; + _jacobian(0, 8) = _tmp206; + _jacobian(1, 8) = _tmp207; + _jacobian(2, 8) = _tmp208; + _jacobian(0, 9) = 0; + _jacobian(1, 9) = 0; + _jacobian(2, 9) = 0; + _jacobian(0, 10) = 0; + _jacobian(1, 10) = 0; + _jacobian(2, 10) = 0; + _jacobian(0, 11) = 0; + _jacobian(1, 11) = 0; + _jacobian(2, 11) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp107, Scalar(2)) + std::pow(_tmp114, Scalar(2)) + std::pow(_tmp120, Scalar(2)); + _hessian(1, 0) = _tmp107 * _tmp146 + _tmp114 * _tmp149 + _tmp120 * _tmp151; + _hessian(2, 0) = _tmp107 * _tmp167 + _tmp114 * _tmp168 + _tmp120 * _tmp169; + _hessian(6, 0) = _tmp107 * _tmp186 + _tmp114 * _tmp187 + _tmp120 * _tmp188; + _hessian(7, 0) = _tmp107 * _tmp196 + _tmp114 * _tmp197 + _tmp120 * _tmp198; + _hessian(8, 0) = _tmp107 * _tmp206 + _tmp114 * _tmp207 + _tmp120 * _tmp208; + _hessian(1, 1) = + std::pow(_tmp146, Scalar(2)) + std::pow(_tmp149, Scalar(2)) + std::pow(_tmp151, Scalar(2)); + _hessian(2, 1) = _tmp146 * _tmp167 + _tmp149 * _tmp168 + _tmp151 * _tmp169; + _hessian(6, 1) = _tmp146 * _tmp186 + _tmp149 * _tmp187 + _tmp151 * _tmp188; + _hessian(7, 1) = _tmp146 * _tmp196 + _tmp149 * _tmp197 + _tmp151 * _tmp198; + _hessian(8, 1) = _tmp146 * _tmp206 + _tmp149 * _tmp207 + _tmp151 * _tmp208; + _hessian(2, 2) = + std::pow(_tmp167, Scalar(2)) + std::pow(_tmp168, Scalar(2)) + std::pow(_tmp169, Scalar(2)); + _hessian(6, 2) = _tmp167 * _tmp186 + _tmp168 * _tmp187 + _tmp169 * _tmp188; + _hessian(7, 2) = _tmp167 * _tmp196 + _tmp168 * _tmp197 + _tmp169 * _tmp198; + _hessian(8, 2) = _tmp167 * _tmp206 + _tmp168 * _tmp207 + _tmp169 * _tmp208; + _hessian(6, 6) = + std::pow(_tmp186, Scalar(2)) + std::pow(_tmp187, Scalar(2)) + std::pow(_tmp188, Scalar(2)); + _hessian(7, 6) = _tmp186 * _tmp196 + _tmp187 * _tmp197 + _tmp188 * _tmp198; + _hessian(8, 6) = _tmp186 * _tmp206 + _tmp187 * _tmp207 + _tmp188 * _tmp208; + _hessian(7, 7) = + std::pow(_tmp196, Scalar(2)) + std::pow(_tmp197, Scalar(2)) + std::pow(_tmp198, Scalar(2)); + _hessian(8, 7) = _tmp196 * _tmp206 + _tmp197 * _tmp207 + _tmp198 * _tmp208; + _hessian(8, 8) = + std::pow(_tmp206, Scalar(2)) + std::pow(_tmp207, Scalar(2)) + std::pow(_tmp208, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp107 * _tmp38 + _tmp114 * _tmp40 + _tmp120 * _tmp42; + _rhs(1, 0) = _tmp146 * _tmp38 + _tmp149 * _tmp40 + _tmp151 * _tmp42; + _rhs(2, 0) = _tmp167 * _tmp38 + _tmp168 * _tmp40 + _tmp169 * _tmp42; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + _rhs(6, 0) = _tmp186 * _tmp38 + _tmp187 * _tmp40 + _tmp188 * _tmp42; + _rhs(7, 0) = _tmp196 * _tmp38 + _tmp197 * _tmp40 + _tmp198 * _tmp42; + _rhs(8, 0) = _tmp206 * _tmp38 + _tmp207 * _tmp40 + _tmp208 * _tmp42; + _rhs(9, 0) = 0; + _rhs(10, 0) = 0; + _rhs(11, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot2.h b/gen/cpp/sym/factors/between_factor_rot2.h index a2691787e..226c0a312 100644 --- a/gen/cpp/sym/factors/between_factor_rot2.h +++ b/gen/cpp/sym/factors/between_factor_rot2.h @@ -1,110 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_T_b. - * - * In vector space terms that would be: - * (b - a) - a_T_b - * - * In lie group terms: - * local_coordinates(a_T_b, between(a, b)) - * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (1x2) jacobian of res wrt args a (1), b (1) - * hessian: (2x2) Gauss-Newton hessian for args a (1), b (1) - * rhs: (2x1) Gauss-Newton rhs for args a (1), b (1) - */ -template -void BetweenFactorRot2(const sym::Rot2& a, const sym::Rot2& b, - const sym::Rot2& a_T_b, const Eigen::Matrix& sqrt_info, - 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: 59 - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - const Eigen::Matrix& _a_T_b = a_T_b.Data(); - - // Intermediate terms (27) - const Scalar _tmp0 = _a[0] * _b[0]; - const Scalar _tmp1 = _a[1] * _b[1]; - const Scalar _tmp2 = _tmp0 + _tmp1; - const Scalar _tmp3 = _a_T_b[1] * _tmp2; - const Scalar _tmp4 = _a[0] * _b[1]; - const Scalar _tmp5 = _a[1] * _b[0]; - const Scalar _tmp6 = _tmp4 - _tmp5; - const Scalar _tmp7 = _a_T_b[0] * _tmp6; - const Scalar _tmp8 = -_tmp3 + _tmp7; - const Scalar _tmp9 = _a_T_b[0] * _tmp2; - const Scalar _tmp10 = _a_T_b[1] * _tmp6; - const Scalar _tmp11 = _tmp10 + _tmp9; - const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); - const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); - const Scalar _tmp14 = -_tmp0 - _tmp1; - const Scalar _tmp15 = Scalar(1.0) / (_tmp12); - const Scalar _tmp16 = std::pow(_tmp12, Scalar(2)); - const Scalar _tmp17 = _tmp8 / _tmp16; - const Scalar _tmp18 = - _tmp15 * (_a_T_b[0] * _tmp14 - _tmp10) - _tmp17 * (_a_T_b[1] * _tmp14 + _tmp7); - const Scalar _tmp19 = _tmp16 + std::pow(_tmp8, Scalar(2)); - const Scalar _tmp20 = _tmp16 / _tmp19; - const Scalar _tmp21 = _tmp20 * sqrt_info(0, 0); - const Scalar _tmp22 = -_tmp4 + _tmp5; - const Scalar _tmp23 = - _tmp15 * (-_a_T_b[1] * _tmp22 + _tmp9) - _tmp17 * (_a_T_b[0] * _tmp22 + _tmp3); - const Scalar _tmp24 = std::pow(sqrt_info(0, 0), Scalar(2)); - const Scalar _tmp25 = std::pow(_tmp12, Scalar(4)) * _tmp24 / std::pow(_tmp19, Scalar(2)); - const Scalar _tmp26 = _tmp13 * _tmp20 * _tmp24; - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp13 * sqrt_info(0, 0); - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp18 * _tmp21; - _jacobian(0, 1) = _tmp21 * _tmp23; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = std::pow(_tmp18, Scalar(2)) * _tmp25; - _hessian(1, 0) = _tmp18 * _tmp23 * _tmp25; - _hessian(0, 1) = 0; - _hessian(1, 1) = std::pow(_tmp23, Scalar(2)) * _tmp25; - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp18 * _tmp26; - _rhs(1, 0) = _tmp23 * _tmp26; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_rot2/between_factor_rot2_diagonal.h" +#include "./between_factor_rot2/between_factor_rot2_isotropic.h" +#include "./between_factor_rot2/between_factor_rot2_square.h" diff --git a/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_diagonal.h b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_diagonal.h new file mode 100644 index 000000000..571f0d999 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_diagonal.h @@ -0,0 +1,111 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x2) jacobian of res wrt args a (1), b (1) + * hessian: (2x2) Gauss-Newton hessian for args a (1), b (1) + * rhs: (2x1) Gauss-Newton rhs for args a (1), b (1) + */ +template +void BetweenFactorRot2(const sym::Rot2& a, const sym::Rot2& b, + const sym::Rot2& a_T_b, const Eigen::Matrix& sqrt_info, + 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: 59 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (27) + const Scalar _tmp0 = _a[0] * _b[0]; + const Scalar _tmp1 = _a[1] * _b[1]; + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = _a_T_b[1] * _tmp2; + const Scalar _tmp4 = _a[0] * _b[1]; + const Scalar _tmp5 = _a[1] * _b[0]; + const Scalar _tmp6 = _tmp4 - _tmp5; + const Scalar _tmp7 = _a_T_b[0] * _tmp6; + const Scalar _tmp8 = -_tmp3 + _tmp7; + const Scalar _tmp9 = _a_T_b[0] * _tmp2; + const Scalar _tmp10 = _a_T_b[1] * _tmp6; + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); + const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); + const Scalar _tmp14 = -_tmp0 - _tmp1; + const Scalar _tmp15 = Scalar(1.0) / (_tmp12); + const Scalar _tmp16 = std::pow(_tmp12, Scalar(2)); + const Scalar _tmp17 = _tmp8 / _tmp16; + const Scalar _tmp18 = + _tmp15 * (_a_T_b[0] * _tmp14 - _tmp10) - _tmp17 * (_a_T_b[1] * _tmp14 + _tmp7); + const Scalar _tmp19 = _tmp16 + std::pow(_tmp8, Scalar(2)); + const Scalar _tmp20 = _tmp16 / _tmp19; + const Scalar _tmp21 = _tmp20 * sqrt_info(0, 0); + const Scalar _tmp22 = -_tmp4 + _tmp5; + const Scalar _tmp23 = + _tmp15 * (-_a_T_b[1] * _tmp22 + _tmp9) - _tmp17 * (_a_T_b[0] * _tmp22 + _tmp3); + const Scalar _tmp24 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp25 = std::pow(_tmp12, Scalar(4)) * _tmp24 / std::pow(_tmp19, Scalar(2)); + const Scalar _tmp26 = _tmp13 * _tmp20 * _tmp24; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp13 * sqrt_info(0, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp18 * _tmp21; + _jacobian(0, 1) = _tmp21 * _tmp23; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(_tmp18, Scalar(2)) * _tmp25; + _hessian(1, 0) = _tmp18 * _tmp23 * _tmp25; + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(_tmp23, Scalar(2)) * _tmp25; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp18 * _tmp26; + _rhs(1, 0) = _tmp23 * _tmp26; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_isotropic.h b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_isotropic.h new file mode 100644 index 000000000..ff0c8dd50 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_isotropic.h @@ -0,0 +1,111 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x2) jacobian of res wrt args a (1), b (1) + * hessian: (2x2) Gauss-Newton hessian for args a (1), b (1) + * rhs: (2x1) Gauss-Newton rhs for args a (1), b (1) + */ +template +void BetweenFactorRot2(const sym::Rot2& a, const sym::Rot2& b, + const sym::Rot2& a_T_b, const Scalar sqrt_info, 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: 59 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (27) + const Scalar _tmp0 = _a[0] * _b[0]; + const Scalar _tmp1 = _a[1] * _b[1]; + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = _a_T_b[1] * _tmp2; + const Scalar _tmp4 = _a[0] * _b[1]; + const Scalar _tmp5 = _a[1] * _b[0]; + const Scalar _tmp6 = _tmp4 - _tmp5; + const Scalar _tmp7 = _a_T_b[0] * _tmp6; + const Scalar _tmp8 = -_tmp3 + _tmp7; + const Scalar _tmp9 = _a_T_b[0] * _tmp2; + const Scalar _tmp10 = _a_T_b[1] * _tmp6; + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); + const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); + const Scalar _tmp14 = -_tmp0 - _tmp1; + const Scalar _tmp15 = Scalar(1.0) / (_tmp12); + const Scalar _tmp16 = std::pow(_tmp12, Scalar(2)); + const Scalar _tmp17 = _tmp8 / _tmp16; + const Scalar _tmp18 = + _tmp15 * (_a_T_b[0] * _tmp14 - _tmp10) - _tmp17 * (_a_T_b[1] * _tmp14 + _tmp7); + const Scalar _tmp19 = _tmp16 + std::pow(_tmp8, Scalar(2)); + const Scalar _tmp20 = _tmp16 / _tmp19; + const Scalar _tmp21 = _tmp20 * sqrt_info; + const Scalar _tmp22 = -_tmp4 + _tmp5; + const Scalar _tmp23 = + _tmp15 * (-_a_T_b[1] * _tmp22 + _tmp9) - _tmp17 * (_a_T_b[0] * _tmp22 + _tmp3); + const Scalar _tmp24 = std::pow(sqrt_info, Scalar(2)); + const Scalar _tmp25 = std::pow(_tmp12, Scalar(4)) * _tmp24 / std::pow(_tmp19, Scalar(2)); + const Scalar _tmp26 = _tmp13 * _tmp20 * _tmp24; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp13 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp18 * _tmp21; + _jacobian(0, 1) = _tmp21 * _tmp23; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(_tmp18, Scalar(2)) * _tmp25; + _hessian(1, 0) = _tmp18 * _tmp23 * _tmp25; + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(_tmp23, Scalar(2)) * _tmp25; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp18 * _tmp26; + _rhs(1, 0) = _tmp23 * _tmp26; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_square.h b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_square.h new file mode 100644 index 000000000..571f0d999 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot2/between_factor_rot2_square.h @@ -0,0 +1,111 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x2) jacobian of res wrt args a (1), b (1) + * hessian: (2x2) Gauss-Newton hessian for args a (1), b (1) + * rhs: (2x1) Gauss-Newton rhs for args a (1), b (1) + */ +template +void BetweenFactorRot2(const sym::Rot2& a, const sym::Rot2& b, + const sym::Rot2& a_T_b, const Eigen::Matrix& sqrt_info, + 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: 59 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (27) + const Scalar _tmp0 = _a[0] * _b[0]; + const Scalar _tmp1 = _a[1] * _b[1]; + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = _a_T_b[1] * _tmp2; + const Scalar _tmp4 = _a[0] * _b[1]; + const Scalar _tmp5 = _a[1] * _b[0]; + const Scalar _tmp6 = _tmp4 - _tmp5; + const Scalar _tmp7 = _a_T_b[0] * _tmp6; + const Scalar _tmp8 = -_tmp3 + _tmp7; + const Scalar _tmp9 = _a_T_b[0] * _tmp2; + const Scalar _tmp10 = _a_T_b[1] * _tmp6; + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = _tmp11 + epsilon * ((((_tmp11) > 0) - ((_tmp11) < 0)) + Scalar(0.5)); + const Scalar _tmp13 = std::atan2(_tmp8, _tmp12); + const Scalar _tmp14 = -_tmp0 - _tmp1; + const Scalar _tmp15 = Scalar(1.0) / (_tmp12); + const Scalar _tmp16 = std::pow(_tmp12, Scalar(2)); + const Scalar _tmp17 = _tmp8 / _tmp16; + const Scalar _tmp18 = + _tmp15 * (_a_T_b[0] * _tmp14 - _tmp10) - _tmp17 * (_a_T_b[1] * _tmp14 + _tmp7); + const Scalar _tmp19 = _tmp16 + std::pow(_tmp8, Scalar(2)); + const Scalar _tmp20 = _tmp16 / _tmp19; + const Scalar _tmp21 = _tmp20 * sqrt_info(0, 0); + const Scalar _tmp22 = -_tmp4 + _tmp5; + const Scalar _tmp23 = + _tmp15 * (-_a_T_b[1] * _tmp22 + _tmp9) - _tmp17 * (_a_T_b[0] * _tmp22 + _tmp3); + const Scalar _tmp24 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp25 = std::pow(_tmp12, Scalar(4)) * _tmp24 / std::pow(_tmp19, Scalar(2)); + const Scalar _tmp26 = _tmp13 * _tmp20 * _tmp24; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp13 * sqrt_info(0, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp18 * _tmp21; + _jacobian(0, 1) = _tmp21 * _tmp23; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(_tmp18, Scalar(2)) * _tmp25; + _hessian(1, 0) = _tmp18 * _tmp23 * _tmp25; + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(_tmp23, Scalar(2)) * _tmp25; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp18 * _tmp26; + _rhs(1, 0) = _tmp23 * _tmp26; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot3.h b/gen/cpp/sym/factors/between_factor_rot3.h index 926aabb16..dc50f69f7 100644 --- a/gen/cpp/sym/factors/between_factor_rot3.h +++ b/gen/cpp/sym/factors/between_factor_rot3.h @@ -1,404 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between between(a, b) and a_T_b. - * - * In vector space terms that would be: - * (b - a) - a_T_b - * - * In lie group terms: - * local_coordinates(a_T_b, between(a, b)) - * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x6) jacobian of res wrt args a (3), b (3) - * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) - * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) - */ -template -void BetweenFactorRot3(const sym::Rot3& a, const sym::Rot3& b, - const sym::Rot3& a_T_b, const Eigen::Matrix& sqrt_info, - 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: 754 - - // Input arrays - const Eigen::Matrix& _a = a.Data(); - const Eigen::Matrix& _b = b.Data(); - const Eigen::Matrix& _a_T_b = a_T_b.Data(); - - // Intermediate terms (216) - const Scalar _tmp0 = _a[3] * _b[3]; - const Scalar _tmp1 = _a[2] * _b[2]; - const Scalar _tmp2 = _a[0] * _b[0]; - const Scalar _tmp3 = _a[1] * _b[1]; - const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; - const Scalar _tmp5 = _a_T_b[3] * _tmp4; - const Scalar _tmp6 = _a[3] * _b[1]; - const Scalar _tmp7 = _a[2] * _b[0]; - const Scalar _tmp8 = _a[0] * _b[2]; - const Scalar _tmp9 = _a[1] * _b[3]; - const Scalar _tmp10 = _tmp6 - _tmp7 + _tmp8 - _tmp9; - const Scalar _tmp11 = _a_T_b[1] * _tmp10; - const Scalar _tmp12 = _a[3] * _b[0]; - const Scalar _tmp13 = _a[2] * _b[1]; - const Scalar _tmp14 = _a[0] * _b[3]; - const Scalar _tmp15 = _a[1] * _b[2]; - const Scalar _tmp16 = _tmp12 + _tmp13 - _tmp14 - _tmp15; - const Scalar _tmp17 = _a_T_b[0] * _tmp16; - const Scalar _tmp18 = _a[3] * _b[2]; - const Scalar _tmp19 = _a[2] * _b[3]; - const Scalar _tmp20 = _a[0] * _b[1]; - const Scalar _tmp21 = _a[1] * _b[0]; - const Scalar _tmp22 = _tmp18 - _tmp19 - _tmp20 + _tmp21; - const Scalar _tmp23 = _a_T_b[2] * _tmp22; - const Scalar _tmp24 = _tmp11 + _tmp17 + _tmp23 + _tmp5; - const Scalar _tmp25 = 2 * std::min(0, (((_tmp24) > 0) - ((_tmp24) < 0))) + 1; - const Scalar _tmp26 = 2 * _tmp25; - const Scalar _tmp27 = _tmp26 * sqrt_info(0, 2); - const Scalar _tmp28 = - -_a_T_b[0] * _tmp10 + _a_T_b[1] * _tmp16 - _a_T_b[2] * _tmp4 + _a_T_b[3] * _tmp22; - const Scalar _tmp29 = 1 - epsilon; - const Scalar _tmp30 = std::min(_tmp29, std::fabs(_tmp24)); - const Scalar _tmp31 = std::acos(_tmp30) / std::sqrt(Scalar(1 - std::pow(_tmp30, Scalar(2)))); - const Scalar _tmp32 = _tmp28 * _tmp31; - const Scalar _tmp33 = - _a_T_b[0] * _tmp22 - _a_T_b[1] * _tmp4 - _a_T_b[2] * _tmp16 + _a_T_b[3] * _tmp10; - const Scalar _tmp34 = _tmp26 * _tmp31; - const Scalar _tmp35 = _tmp33 * _tmp34; - const Scalar _tmp36 = - -_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp22 + _a_T_b[2] * _tmp10 + _a_T_b[3] * _tmp16; - const Scalar _tmp37 = _tmp34 * _tmp36; - const Scalar _tmp38 = _tmp27 * _tmp32 + _tmp35 * sqrt_info(0, 1) + _tmp37 * sqrt_info(0, 0); - const Scalar _tmp39 = _tmp26 * sqrt_info(1, 2); - const Scalar _tmp40 = _tmp33 * sqrt_info(1, 1); - const Scalar _tmp41 = _tmp32 * _tmp39 + _tmp34 * _tmp40 + _tmp37 * sqrt_info(1, 0); - const Scalar _tmp42 = _tmp26 * sqrt_info(2, 2); - const Scalar _tmp43 = _tmp32 * _tmp42 + _tmp35 * sqrt_info(2, 1) + _tmp37 * sqrt_info(2, 0); - const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp0; - const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp1; - const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp2; - const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp3; - const Scalar _tmp48 = -_tmp44 - _tmp45 - _tmp46 - _tmp47; - const Scalar _tmp49 = _a_T_b[3] * _tmp48; - const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp6; - const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp7; - const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp8; - const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp9; - const Scalar _tmp54 = -_tmp50 + _tmp51 - _tmp52 + _tmp53; - const Scalar _tmp55 = -_a_T_b[1] * _tmp54; - const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp18; - const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp19; - const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp20; - const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp21; - const Scalar _tmp60 = _tmp56 - _tmp57 - _tmp58 + _tmp59; - const Scalar _tmp61 = _a_T_b[2] * _tmp60; - const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp12; - const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp13; - const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp14; - const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp15; - const Scalar _tmp66 = _tmp62 + _tmp63 - _tmp64 - _tmp65; - const Scalar _tmp67 = _a_T_b[0] * _tmp66; - const Scalar _tmp68 = _tmp11 + _tmp17 + _tmp23 + _tmp5; - const Scalar _tmp69 = std::fabs(_tmp68); - const Scalar _tmp70 = std::min(_tmp29, _tmp69); - const Scalar _tmp71 = 1 - std::pow(_tmp70, Scalar(2)); - const Scalar _tmp72 = std::acos(_tmp70); - const Scalar _tmp73 = _tmp72 / std::sqrt(_tmp71); - const Scalar _tmp74 = _tmp26 * _tmp73; - const Scalar _tmp75 = _tmp74 * (_tmp49 + _tmp55 + _tmp61 - _tmp67); - const Scalar _tmp76 = _tmp36 * sqrt_info(0, 0); - const Scalar _tmp77 = _a_T_b[3] * _tmp66; - const Scalar _tmp78 = _a_T_b[0] * _tmp48; - const Scalar _tmp79 = _a_T_b[2] * _tmp54; - const Scalar _tmp80 = _a_T_b[1] * _tmp60; - const Scalar _tmp81 = _tmp79 + _tmp80; - const Scalar _tmp82 = _tmp77 + _tmp78 + _tmp81; - const Scalar _tmp83 = _tmp25 * ((((_tmp29 - _tmp69) > 0) - ((_tmp29 - _tmp69) < 0)) + 1) * - (((_tmp68) > 0) - ((_tmp68) < 0)); - const Scalar _tmp84 = _tmp83 / _tmp71; - const Scalar _tmp85 = _tmp82 * _tmp84; - const Scalar _tmp86 = _tmp33 * _tmp84; - const Scalar _tmp87 = _tmp86 * sqrt_info(0, 1); - const Scalar _tmp88 = _tmp70 * _tmp72 * _tmp83 / (_tmp71 * std::sqrt(_tmp71)); - const Scalar _tmp89 = _tmp28 * _tmp88; - const Scalar _tmp90 = _tmp82 * _tmp89; - const Scalar _tmp91 = _tmp82 * _tmp88; - const Scalar _tmp92 = -_a_T_b[1] * _tmp66; - const Scalar _tmp93 = _a_T_b[2] * _tmp48; - const Scalar _tmp94 = _a_T_b[0] * _tmp54; - const Scalar _tmp95 = _a_T_b[3] * _tmp60; - const Scalar _tmp96 = _tmp94 + _tmp95; - const Scalar _tmp97 = _tmp92 - _tmp93 + _tmp96; - const Scalar _tmp98 = _tmp74 * sqrt_info(0, 1); - const Scalar _tmp99 = _a_T_b[1] * _tmp48; - const Scalar _tmp100 = _a_T_b[3] * _tmp54; - const Scalar _tmp101 = -_a_T_b[0] * _tmp60; - const Scalar _tmp102 = _a_T_b[2] * _tmp66; - const Scalar _tmp103 = _tmp100 + _tmp101 - _tmp102 + _tmp99; - const Scalar _tmp104 = _tmp103 * _tmp73; - const Scalar _tmp105 = _tmp33 * _tmp91; - const Scalar _tmp106 = _tmp28 * _tmp84; - const Scalar _tmp107 = _tmp106 * _tmp82; - const Scalar _tmp108 = _tmp104 * _tmp27 + _tmp105 * sqrt_info(0, 1) - _tmp107 * sqrt_info(0, 2) + - _tmp75 * sqrt_info(0, 0) - _tmp76 * _tmp85 + _tmp76 * _tmp91 - - _tmp82 * _tmp87 + _tmp90 * sqrt_info(0, 2) + _tmp97 * _tmp98; - const Scalar _tmp109 = _tmp36 * sqrt_info(1, 0); - const Scalar _tmp110 = _tmp40 * _tmp84; - const Scalar _tmp111 = _tmp89 * sqrt_info(1, 2); - const Scalar _tmp112 = _tmp74 * sqrt_info(1, 1); - const Scalar _tmp113 = _tmp39 * _tmp73; - const Scalar _tmp114 = _tmp103 * _tmp113 - _tmp107 * sqrt_info(1, 2) - _tmp109 * _tmp85 + - _tmp109 * _tmp91 - _tmp110 * _tmp82 + _tmp111 * _tmp82 + _tmp112 * _tmp97 + - _tmp40 * _tmp91 + _tmp75 * sqrt_info(1, 0); - const Scalar _tmp115 = _tmp36 * sqrt_info(2, 0); - const Scalar _tmp116 = _tmp86 * sqrt_info(2, 1); - const Scalar _tmp117 = _tmp74 * sqrt_info(2, 1); - const Scalar _tmp118 = _tmp104 * _tmp42 + _tmp105 * sqrt_info(2, 1) - _tmp107 * sqrt_info(2, 2) - - _tmp115 * _tmp85 + _tmp115 * _tmp91 - _tmp116 * _tmp82 + _tmp117 * _tmp97 + - _tmp75 * sqrt_info(2, 0) + _tmp90 * sqrt_info(2, 2); - const Scalar _tmp119 = _tmp50 - _tmp51 + _tmp52 - _tmp53; - const Scalar _tmp120 = _a_T_b[3] * _tmp119; - const Scalar _tmp121 = -_tmp56 + _tmp57 + _tmp58 - _tmp59; - const Scalar _tmp122 = _a_T_b[0] * _tmp121; - const Scalar _tmp123 = _tmp102 + _tmp122; - const Scalar _tmp124 = _tmp120 + _tmp123 + _tmp99; - const Scalar _tmp125 = _tmp106 * _tmp124; - const Scalar _tmp126 = _tmp124 * _tmp84; - const Scalar _tmp127 = _a_T_b[1] * _tmp119; - const Scalar _tmp128 = -_a_T_b[2] * _tmp121; - const Scalar _tmp129 = _tmp128 + _tmp67; - const Scalar _tmp130 = -_tmp127 + _tmp129 + _tmp49; - const Scalar _tmp131 = _tmp130 * _tmp74; - const Scalar _tmp132 = _tmp124 * _tmp88; - const Scalar _tmp133 = _tmp132 * _tmp33; - const Scalar _tmp134 = _tmp132 * _tmp28; - const Scalar _tmp135 = -_a_T_b[2] * _tmp119; - const Scalar _tmp136 = _a_T_b[1] * _tmp121; - const Scalar _tmp137 = _tmp136 + _tmp77; - const Scalar _tmp138 = _tmp135 + _tmp137 - _tmp78; - const Scalar _tmp139 = _tmp138 * _tmp73; - const Scalar _tmp140 = _a_T_b[0] * _tmp119; - const Scalar _tmp141 = _a_T_b[3] * _tmp121; - const Scalar _tmp142 = _tmp74 * (-_tmp140 + _tmp141 + _tmp92 + _tmp93); - const Scalar _tmp143 = -_tmp124 * _tmp87 - _tmp125 * sqrt_info(0, 2) - _tmp126 * _tmp76 + - _tmp131 * sqrt_info(0, 1) + _tmp132 * _tmp76 + _tmp133 * sqrt_info(0, 1) + - _tmp134 * sqrt_info(0, 2) + _tmp139 * _tmp27 + _tmp142 * sqrt_info(0, 0); - const Scalar _tmp144 = -_tmp109 * _tmp126 + _tmp109 * _tmp132 - _tmp110 * _tmp124 + - _tmp113 * _tmp138 - _tmp125 * sqrt_info(1, 2) + _tmp131 * sqrt_info(1, 1) + - _tmp132 * _tmp40 + _tmp134 * sqrt_info(1, 2) + _tmp142 * sqrt_info(1, 0); - const Scalar _tmp145 = -_tmp115 * _tmp126 + _tmp115 * _tmp132 - _tmp116 * _tmp124 + - _tmp117 * _tmp130 - _tmp125 * sqrt_info(2, 2) + _tmp133 * sqrt_info(2, 1) + - _tmp134 * sqrt_info(2, 2) + _tmp139 * _tmp42 + _tmp142 * sqrt_info(2, 0); - const Scalar _tmp146 = -_tmp62 - _tmp63 + _tmp64 + _tmp65; - const Scalar _tmp147 = _a_T_b[1] * _tmp146; - const Scalar _tmp148 = _tmp140 + _tmp147; - const Scalar _tmp149 = _tmp148 + _tmp93 + _tmp95; - const Scalar _tmp150 = _tmp149 * _tmp89; - const Scalar _tmp151 = _tmp149 * _tmp84; - const Scalar _tmp152 = _tmp149 * _tmp88; - const Scalar _tmp153 = _tmp152 * _tmp33; - const Scalar _tmp154 = _a_T_b[2] * _tmp146; - const Scalar _tmp155 = _tmp120 + _tmp154; - const Scalar _tmp156 = _tmp74 * (_tmp101 + _tmp155 - _tmp99); - const Scalar _tmp157 = _a_T_b[3] * _tmp146; - const Scalar _tmp158 = _tmp135 + _tmp157 + _tmp78 - _tmp80; - const Scalar _tmp159 = _tmp106 * _tmp149; - const Scalar _tmp160 = -_a_T_b[0] * _tmp146; - const Scalar _tmp161 = _tmp127 + _tmp160; - const Scalar _tmp162 = _tmp161 + _tmp49 - _tmp61; - const Scalar _tmp163 = _tmp162 * _tmp73; - const Scalar _tmp164 = -_tmp149 * _tmp87 + _tmp150 * sqrt_info(0, 2) - _tmp151 * _tmp76 + - _tmp152 * _tmp76 + _tmp153 * sqrt_info(0, 1) + _tmp156 * sqrt_info(0, 0) + - _tmp158 * _tmp98 - _tmp159 * sqrt_info(0, 2) + _tmp163 * _tmp27; - const Scalar _tmp165 = -_tmp109 * _tmp151 + _tmp109 * _tmp152 - _tmp110 * _tmp149 + - _tmp112 * _tmp158 + _tmp113 * _tmp162 + _tmp150 * sqrt_info(1, 2) + - _tmp152 * _tmp40 + _tmp156 * sqrt_info(1, 0) - _tmp159 * sqrt_info(1, 2); - const Scalar _tmp166 = -_tmp115 * _tmp151 + _tmp115 * _tmp152 - _tmp116 * _tmp149 + - _tmp117 * _tmp158 + _tmp150 * sqrt_info(2, 2) + _tmp153 * sqrt_info(2, 1) + - _tmp156 * sqrt_info(2, 0) - _tmp159 * sqrt_info(2, 2) + _tmp163 * _tmp42; - const Scalar _tmp167 = _tmp44 + _tmp45 + _tmp46 + _tmp47; - const Scalar _tmp168 = _a_T_b[0] * _tmp167; - const Scalar _tmp169 = _tmp157 + _tmp168; - const Scalar _tmp170 = _tmp169 + _tmp81; - const Scalar _tmp171 = _tmp170 * _tmp84; - const Scalar _tmp172 = _tmp170 * _tmp88; - const Scalar _tmp173 = _tmp172 * _tmp33; - const Scalar _tmp174 = _tmp170 * _tmp89; - const Scalar _tmp175 = _a_T_b[3] * _tmp167; - const Scalar _tmp176 = _tmp175 + _tmp55; - const Scalar _tmp177 = _tmp74 * (_tmp160 + _tmp176 + _tmp61); - const Scalar _tmp178 = _a_T_b[2] * _tmp167; - const Scalar _tmp179 = _tmp74 * (-_tmp147 - _tmp178 + _tmp96); - const Scalar _tmp180 = _a_T_b[1] * _tmp167; - const Scalar _tmp181 = _tmp100 + _tmp180; - const Scalar _tmp182 = _tmp101 - _tmp154 + _tmp181; - const Scalar _tmp183 = _tmp182 * _tmp73; - const Scalar _tmp184 = _tmp106 * _tmp170; - const Scalar _tmp185 = -_tmp170 * _tmp87 - _tmp171 * _tmp76 + _tmp172 * _tmp76 + - _tmp173 * sqrt_info(0, 1) + _tmp174 * sqrt_info(0, 2) + - _tmp177 * sqrt_info(0, 0) + _tmp179 * sqrt_info(0, 1) + _tmp183 * _tmp27 - - _tmp184 * sqrt_info(0, 2); - const Scalar _tmp186 = -_tmp109 * _tmp171 + _tmp109 * _tmp172 - _tmp110 * _tmp170 + - _tmp111 * _tmp170 + _tmp113 * _tmp182 + _tmp172 * _tmp40 + - _tmp177 * sqrt_info(1, 0) + _tmp179 * sqrt_info(1, 1) - - _tmp184 * sqrt_info(1, 2); - const Scalar _tmp187 = -_tmp115 * _tmp171 + _tmp115 * _tmp172 - _tmp116 * _tmp170 + - _tmp173 * sqrt_info(2, 1) + _tmp174 * sqrt_info(2, 2) + - _tmp177 * sqrt_info(2, 0) + _tmp179 * sqrt_info(2, 1) + _tmp183 * _tmp42 - - _tmp184 * sqrt_info(2, 2); - const Scalar _tmp188 = _tmp123 + _tmp181; - const Scalar _tmp189 = _tmp188 * _tmp84; - const Scalar _tmp190 = _tmp189 * _tmp33; - const Scalar _tmp191 = _tmp188 * _tmp89; - const Scalar _tmp192 = _tmp188 * _tmp88; - const Scalar _tmp193 = _tmp192 * _tmp33; - const Scalar _tmp194 = _tmp129 + _tmp176; - const Scalar _tmp195 = _tmp194 * _tmp74; - const Scalar _tmp196 = _tmp141 + _tmp178; - const Scalar _tmp197 = _tmp74 * (_tmp196 + _tmp92 - _tmp94); - const Scalar _tmp198 = _tmp106 * _tmp188; - const Scalar _tmp199 = _tmp137 - _tmp168 - _tmp79; - const Scalar _tmp200 = _tmp199 * _tmp73; - const Scalar _tmp201 = -_tmp189 * _tmp76 - _tmp190 * sqrt_info(0, 1) + _tmp191 * sqrt_info(0, 2) + - _tmp192 * _tmp76 + _tmp193 * sqrt_info(0, 1) + _tmp195 * sqrt_info(0, 1) + - _tmp197 * sqrt_info(0, 0) - _tmp198 * sqrt_info(0, 2) + _tmp200 * _tmp27; - const Scalar _tmp202 = -_tmp109 * _tmp189 + _tmp109 * _tmp192 + _tmp113 * _tmp199 - - _tmp189 * _tmp40 + _tmp191 * sqrt_info(1, 2) + _tmp192 * _tmp40 + - _tmp195 * sqrt_info(1, 1) + _tmp197 * sqrt_info(1, 0) - - _tmp198 * sqrt_info(1, 2); - const Scalar _tmp203 = -_tmp115 * _tmp189 + _tmp115 * _tmp192 + _tmp117 * _tmp194 - - _tmp190 * sqrt_info(2, 1) + _tmp191 * sqrt_info(2, 2) + - _tmp193 * sqrt_info(2, 1) + _tmp197 * sqrt_info(2, 0) - - _tmp198 * sqrt_info(2, 2) + _tmp200 * _tmp42; - const Scalar _tmp204 = _tmp148 + _tmp196; - const Scalar _tmp205 = _tmp204 * _tmp88; - const Scalar _tmp206 = _tmp73 * (_tmp128 + _tmp161 + _tmp175); - const Scalar _tmp207 = _tmp205 * _tmp33; - const Scalar _tmp208 = _tmp106 * _tmp204; - const Scalar _tmp209 = _tmp204 * _tmp89; - const Scalar _tmp210 = _tmp204 * _tmp84; - const Scalar _tmp211 = _tmp135 - _tmp136 + _tmp169; - const Scalar _tmp212 = _tmp74 * (-_tmp122 + _tmp155 - _tmp180); - const Scalar _tmp213 = -_tmp204 * _tmp87 + _tmp205 * _tmp76 + _tmp206 * _tmp27 + - _tmp207 * sqrt_info(0, 1) - _tmp208 * sqrt_info(0, 2) + - _tmp209 * sqrt_info(0, 2) - _tmp210 * _tmp76 + _tmp211 * _tmp98 + - _tmp212 * sqrt_info(0, 0); - const Scalar _tmp214 = _tmp109 * _tmp205 - _tmp109 * _tmp210 - _tmp110 * _tmp204 + - _tmp111 * _tmp204 + _tmp112 * _tmp211 + _tmp205 * _tmp40 + - _tmp206 * _tmp39 - _tmp208 * sqrt_info(1, 2) + _tmp212 * sqrt_info(1, 0); - const Scalar _tmp215 = _tmp115 * _tmp205 - _tmp115 * _tmp210 - _tmp116 * _tmp204 + - _tmp117 * _tmp211 + _tmp206 * _tmp42 + _tmp207 * sqrt_info(2, 1) - - _tmp208 * sqrt_info(2, 2) + _tmp209 * sqrt_info(2, 2) + - _tmp212 * sqrt_info(2, 0); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp38; - _res(1, 0) = _tmp41; - _res(2, 0) = _tmp43; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp108; - _jacobian(1, 0) = _tmp114; - _jacobian(2, 0) = _tmp118; - _jacobian(0, 1) = _tmp143; - _jacobian(1, 1) = _tmp144; - _jacobian(2, 1) = _tmp145; - _jacobian(0, 2) = _tmp164; - _jacobian(1, 2) = _tmp165; - _jacobian(2, 2) = _tmp166; - _jacobian(0, 3) = _tmp185; - _jacobian(1, 3) = _tmp186; - _jacobian(2, 3) = _tmp187; - _jacobian(0, 4) = _tmp201; - _jacobian(1, 4) = _tmp202; - _jacobian(2, 4) = _tmp203; - _jacobian(0, 5) = _tmp213; - _jacobian(1, 5) = _tmp214; - _jacobian(2, 5) = _tmp215; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = - std::pow(_tmp108, Scalar(2)) + std::pow(_tmp114, Scalar(2)) + std::pow(_tmp118, Scalar(2)); - _hessian(1, 0) = _tmp108 * _tmp143 + _tmp114 * _tmp144 + _tmp118 * _tmp145; - _hessian(2, 0) = _tmp108 * _tmp164 + _tmp114 * _tmp165 + _tmp118 * _tmp166; - _hessian(3, 0) = _tmp108 * _tmp185 + _tmp114 * _tmp186 + _tmp118 * _tmp187; - _hessian(4, 0) = _tmp108 * _tmp201 + _tmp114 * _tmp202 + _tmp118 * _tmp203; - _hessian(5, 0) = _tmp108 * _tmp213 + _tmp114 * _tmp214 + _tmp118 * _tmp215; - _hessian(0, 1) = 0; - _hessian(1, 1) = - std::pow(_tmp143, Scalar(2)) + std::pow(_tmp144, Scalar(2)) + std::pow(_tmp145, Scalar(2)); - _hessian(2, 1) = _tmp143 * _tmp164 + _tmp144 * _tmp165 + _tmp145 * _tmp166; - _hessian(3, 1) = _tmp143 * _tmp185 + _tmp144 * _tmp186 + _tmp145 * _tmp187; - _hessian(4, 1) = _tmp143 * _tmp201 + _tmp144 * _tmp202 + _tmp145 * _tmp203; - _hessian(5, 1) = _tmp143 * _tmp213 + _tmp144 * _tmp214 + _tmp145 * _tmp215; - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = - std::pow(_tmp164, Scalar(2)) + std::pow(_tmp165, Scalar(2)) + std::pow(_tmp166, Scalar(2)); - _hessian(3, 2) = _tmp164 * _tmp185 + _tmp165 * _tmp186 + _tmp166 * _tmp187; - _hessian(4, 2) = _tmp164 * _tmp201 + _tmp165 * _tmp202 + _tmp166 * _tmp203; - _hessian(5, 2) = _tmp164 * _tmp213 + _tmp165 * _tmp214 + _tmp166 * _tmp215; - _hessian(0, 3) = 0; - _hessian(1, 3) = 0; - _hessian(2, 3) = 0; - _hessian(3, 3) = - std::pow(_tmp185, Scalar(2)) + std::pow(_tmp186, Scalar(2)) + std::pow(_tmp187, Scalar(2)); - _hessian(4, 3) = _tmp185 * _tmp201 + _tmp186 * _tmp202 + _tmp187 * _tmp203; - _hessian(5, 3) = _tmp185 * _tmp213 + _tmp186 * _tmp214 + _tmp187 * _tmp215; - _hessian(0, 4) = 0; - _hessian(1, 4) = 0; - _hessian(2, 4) = 0; - _hessian(3, 4) = 0; - _hessian(4, 4) = - std::pow(_tmp201, Scalar(2)) + std::pow(_tmp202, Scalar(2)) + std::pow(_tmp203, Scalar(2)); - _hessian(5, 4) = _tmp201 * _tmp213 + _tmp202 * _tmp214 + _tmp203 * _tmp215; - _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(_tmp213, Scalar(2)) + std::pow(_tmp214, Scalar(2)) + std::pow(_tmp215, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp108 * _tmp38 + _tmp114 * _tmp41 + _tmp118 * _tmp43; - _rhs(1, 0) = _tmp143 * _tmp38 + _tmp144 * _tmp41 + _tmp145 * _tmp43; - _rhs(2, 0) = _tmp164 * _tmp38 + _tmp165 * _tmp41 + _tmp166 * _tmp43; - _rhs(3, 0) = _tmp185 * _tmp38 + _tmp186 * _tmp41 + _tmp187 * _tmp43; - _rhs(4, 0) = _tmp201 * _tmp38 + _tmp202 * _tmp41 + _tmp203 * _tmp43; - _rhs(5, 0) = _tmp213 * _tmp38 + _tmp214 * _tmp41 + _tmp215 * _tmp43; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./between_factor_rot3/between_factor_rot3_diagonal.h" +#include "./between_factor_rot3/between_factor_rot3_isotropic.h" +#include "./between_factor_rot3/between_factor_rot3_square.h" diff --git a/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_diagonal.h b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_diagonal.h new file mode 100644 index 000000000..815e2aa5f --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_diagonal.h @@ -0,0 +1,312 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorRot3(const sym::Rot3& a, const sym::Rot3& b, + const sym::Rot3& a_T_b, const Eigen::Matrix& sqrt_info, + 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: 475 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (152) + const Scalar _tmp0 = _a[3] * _b[3]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[1] * _b[1]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a_T_b[3] * _tmp4; + const Scalar _tmp6 = _a[3] * _b[1]; + const Scalar _tmp7 = _a[2] * _b[0]; + const Scalar _tmp8 = _a[0] * _b[2]; + const Scalar _tmp9 = _a[1] * _b[3]; + const Scalar _tmp10 = _tmp6 - _tmp7 + _tmp8 - _tmp9; + const Scalar _tmp11 = _a_T_b[1] * _tmp10; + const Scalar _tmp12 = _a[3] * _b[0]; + const Scalar _tmp13 = _a[2] * _b[1]; + const Scalar _tmp14 = _a[0] * _b[3]; + const Scalar _tmp15 = _a[1] * _b[2]; + const Scalar _tmp16 = _tmp12 + _tmp13 - _tmp14 - _tmp15; + const Scalar _tmp17 = _a_T_b[0] * _tmp16; + const Scalar _tmp18 = _a[3] * _b[2]; + const Scalar _tmp19 = _a[2] * _b[3]; + const Scalar _tmp20 = _a[0] * _b[1]; + const Scalar _tmp21 = _a[1] * _b[0]; + const Scalar _tmp22 = _tmp18 - _tmp19 - _tmp20 + _tmp21; + const Scalar _tmp23 = _a_T_b[2] * _tmp22; + const Scalar _tmp24 = _tmp11 + _tmp17 + _tmp23 + _tmp5; + const Scalar _tmp25 = 2 * std::min(0, (((_tmp24) > 0) - ((_tmp24) < 0))) + 1; + const Scalar _tmp26 = 2 * _tmp25; + const Scalar _tmp27 = 1 - epsilon; + const Scalar _tmp28 = std::min(_tmp27, std::fabs(_tmp24)); + const Scalar _tmp29 = std::acos(_tmp28) / std::sqrt(Scalar(1 - std::pow(_tmp28, Scalar(2)))); + const Scalar _tmp30 = _tmp26 * _tmp29; + const Scalar _tmp31 = sqrt_info(0, 0) * (-_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp22 + + _a_T_b[2] * _tmp10 + _a_T_b[3] * _tmp16); + const Scalar _tmp32 = _tmp30 * _tmp31; + const Scalar _tmp33 = sqrt_info(1, 0) * (_a_T_b[0] * _tmp22 - _a_T_b[1] * _tmp4 - + _a_T_b[2] * _tmp16 + _a_T_b[3] * _tmp10); + const Scalar _tmp34 = _tmp30 * _tmp33; + const Scalar _tmp35 = + -_a_T_b[0] * _tmp10 + _a_T_b[1] * _tmp16 - _a_T_b[2] * _tmp4 + _a_T_b[3] * _tmp22; + const Scalar _tmp36 = _tmp26 * sqrt_info(2, 0); + const Scalar _tmp37 = _tmp29 * _tmp35 * _tmp36; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp42 = -_tmp38 - _tmp39 - _tmp40 - _tmp41; + const Scalar _tmp43 = _a_T_b[3] * _tmp42; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp48 = -_tmp44 + _tmp45 - _tmp46 + _tmp47; + const Scalar _tmp49 = -_a_T_b[1] * _tmp48; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp54 = _tmp50 - _tmp51 - _tmp52 + _tmp53; + const Scalar _tmp55 = _a_T_b[2] * _tmp54; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp60 = _tmp56 + _tmp57 - _tmp58 - _tmp59; + const Scalar _tmp61 = _a_T_b[0] * _tmp60; + const Scalar _tmp62 = _tmp11 + _tmp17 + _tmp23 + _tmp5; + const Scalar _tmp63 = std::fabs(_tmp62); + const Scalar _tmp64 = std::min(_tmp27, _tmp63); + const Scalar _tmp65 = 1 - std::pow(_tmp64, Scalar(2)); + const Scalar _tmp66 = std::acos(_tmp64); + const Scalar _tmp67 = _tmp66 / std::sqrt(_tmp65); + const Scalar _tmp68 = _tmp26 * _tmp67; + const Scalar _tmp69 = _tmp68 * sqrt_info(0, 0); + const Scalar _tmp70 = _a_T_b[3] * _tmp60; + const Scalar _tmp71 = _a_T_b[0] * _tmp42; + const Scalar _tmp72 = _a_T_b[2] * _tmp48; + const Scalar _tmp73 = _a_T_b[1] * _tmp54; + const Scalar _tmp74 = _tmp72 + _tmp73; + const Scalar _tmp75 = _tmp70 + _tmp71 + _tmp74; + const Scalar _tmp76 = _tmp25 * ((((_tmp27 - _tmp63) > 0) - ((_tmp27 - _tmp63) < 0)) + 1) * + (((_tmp62) > 0) - ((_tmp62) < 0)); + const Scalar _tmp77 = _tmp64 * _tmp66 * _tmp76 / (_tmp65 * std::sqrt(_tmp65)); + const Scalar _tmp78 = _tmp31 * _tmp77; + const Scalar _tmp79 = _tmp76 / _tmp65; + const Scalar _tmp80 = _tmp31 * _tmp79; + const Scalar _tmp81 = + _tmp69 * (_tmp43 + _tmp49 + _tmp55 - _tmp61) + _tmp75 * _tmp78 - _tmp75 * _tmp80; + const Scalar _tmp82 = _tmp33 * _tmp79; + const Scalar _tmp83 = _tmp33 * _tmp77; + const Scalar _tmp84 = -_a_T_b[1] * _tmp60; + const Scalar _tmp85 = _a_T_b[2] * _tmp42; + const Scalar _tmp86 = _a_T_b[0] * _tmp48; + const Scalar _tmp87 = _a_T_b[3] * _tmp54; + const Scalar _tmp88 = _tmp86 + _tmp87; + const Scalar _tmp89 = _tmp68 * sqrt_info(1, 0); + const Scalar _tmp90 = -_tmp75 * _tmp82 + _tmp75 * _tmp83 + _tmp89 * (_tmp84 - _tmp85 + _tmp88); + const Scalar _tmp91 = _tmp35 * sqrt_info(2, 0); + const Scalar _tmp92 = _tmp79 * _tmp91; + const Scalar _tmp93 = _tmp77 * _tmp91; + const Scalar _tmp94 = _a_T_b[1] * _tmp42; + const Scalar _tmp95 = _a_T_b[3] * _tmp48; + const Scalar _tmp96 = -_a_T_b[0] * _tmp54; + const Scalar _tmp97 = _a_T_b[2] * _tmp60; + const Scalar _tmp98 = _tmp36 * _tmp67; + const Scalar _tmp99 = + -_tmp75 * _tmp92 + _tmp75 * _tmp93 + _tmp98 * (_tmp94 + _tmp95 + _tmp96 - _tmp97); + const Scalar _tmp100 = _tmp44 - _tmp45 + _tmp46 - _tmp47; + const Scalar _tmp101 = _a_T_b[0] * _tmp100; + const Scalar _tmp102 = -_tmp50 + _tmp51 + _tmp52 - _tmp53; + const Scalar _tmp103 = _a_T_b[3] * _tmp102; + const Scalar _tmp104 = _a_T_b[3] * _tmp100; + const Scalar _tmp105 = _a_T_b[0] * _tmp102; + const Scalar _tmp106 = _tmp105 + _tmp97; + const Scalar _tmp107 = _tmp104 + _tmp106 + _tmp94; + const Scalar _tmp108 = + _tmp107 * _tmp78 - _tmp107 * _tmp80 + _tmp69 * (-_tmp101 + _tmp103 + _tmp84 + _tmp85); + const Scalar _tmp109 = _a_T_b[1] * _tmp100; + const Scalar _tmp110 = -_a_T_b[2] * _tmp102; + const Scalar _tmp111 = _tmp110 + _tmp61; + const Scalar _tmp112 = + -_tmp107 * _tmp82 + _tmp107 * _tmp83 + _tmp89 * (-_tmp109 + _tmp111 + _tmp43); + const Scalar _tmp113 = -_a_T_b[2] * _tmp100; + const Scalar _tmp114 = _a_T_b[1] * _tmp102; + const Scalar _tmp115 = _tmp114 + _tmp70; + const Scalar _tmp116 = + -_tmp107 * _tmp92 + _tmp107 * _tmp93 + _tmp98 * (_tmp113 + _tmp115 - _tmp71); + const Scalar _tmp117 = -_tmp56 - _tmp57 + _tmp58 + _tmp59; + const Scalar _tmp118 = _a_T_b[2] * _tmp117; + const Scalar _tmp119 = _tmp104 + _tmp118; + const Scalar _tmp120 = _a_T_b[1] * _tmp117; + const Scalar _tmp121 = _tmp101 + _tmp120; + const Scalar _tmp122 = _tmp121 + _tmp85 + _tmp87; + const Scalar _tmp123 = _tmp122 * _tmp78 - _tmp122 * _tmp80 + _tmp69 * (_tmp119 - _tmp94 + _tmp96); + const Scalar _tmp124 = _a_T_b[3] * _tmp117; + const Scalar _tmp125 = + -_tmp122 * _tmp82 + _tmp122 * _tmp83 + _tmp89 * (_tmp113 + _tmp124 + _tmp71 - _tmp73); + const Scalar _tmp126 = -_a_T_b[0] * _tmp117; + const Scalar _tmp127 = _tmp109 + _tmp126; + const Scalar _tmp128 = + -_tmp122 * _tmp92 + _tmp122 * _tmp93 + _tmp98 * (_tmp127 + _tmp43 - _tmp55); + const Scalar _tmp129 = _tmp38 + _tmp39 + _tmp40 + _tmp41; + const Scalar _tmp130 = _a_T_b[0] * _tmp129; + const Scalar _tmp131 = _tmp124 + _tmp130; + const Scalar _tmp132 = _tmp131 + _tmp74; + const Scalar _tmp133 = _tmp132 * _tmp79; + const Scalar _tmp134 = _a_T_b[3] * _tmp129; + const Scalar _tmp135 = _tmp134 + _tmp49; + const Scalar _tmp136 = + _tmp132 * _tmp78 - _tmp133 * _tmp31 + _tmp69 * (_tmp126 + _tmp135 + _tmp55); + const Scalar _tmp137 = _a_T_b[2] * _tmp129; + const Scalar _tmp138 = + _tmp132 * _tmp83 - _tmp133 * _tmp33 + _tmp89 * (-_tmp120 - _tmp137 + _tmp88); + const Scalar _tmp139 = _a_T_b[1] * _tmp129; + const Scalar _tmp140 = _tmp139 + _tmp95; + const Scalar _tmp141 = + _tmp132 * _tmp93 - _tmp133 * _tmp91 + _tmp98 * (-_tmp118 + _tmp140 + _tmp96); + const Scalar _tmp142 = _tmp106 + _tmp140; + const Scalar _tmp143 = _tmp103 + _tmp137; + const Scalar _tmp144 = _tmp142 * _tmp78 - _tmp142 * _tmp80 + _tmp69 * (_tmp143 + _tmp84 - _tmp86); + const Scalar _tmp145 = -_tmp142 * _tmp82 + _tmp142 * _tmp83 + _tmp89 * (_tmp111 + _tmp135); + const Scalar _tmp146 = + -_tmp142 * _tmp92 + _tmp142 * _tmp93 + _tmp98 * (_tmp115 - _tmp130 - _tmp72); + const Scalar _tmp147 = _tmp121 + _tmp143; + const Scalar _tmp148 = _tmp147 * _tmp79; + const Scalar _tmp149 = + _tmp147 * _tmp78 - _tmp148 * _tmp31 + _tmp69 * (-_tmp105 + _tmp119 - _tmp139); + const Scalar _tmp150 = + _tmp147 * _tmp83 - _tmp148 * _tmp33 + _tmp89 * (_tmp113 - _tmp114 + _tmp131); + const Scalar _tmp151 = + _tmp147 * _tmp93 - _tmp148 * _tmp91 + _tmp98 * (_tmp110 + _tmp127 + _tmp134); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp32; + _res(1, 0) = _tmp34; + _res(2, 0) = _tmp37; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp81; + _jacobian(1, 0) = _tmp90; + _jacobian(2, 0) = _tmp99; + _jacobian(0, 1) = _tmp108; + _jacobian(1, 1) = _tmp112; + _jacobian(2, 1) = _tmp116; + _jacobian(0, 2) = _tmp123; + _jacobian(1, 2) = _tmp125; + _jacobian(2, 2) = _tmp128; + _jacobian(0, 3) = _tmp136; + _jacobian(1, 3) = _tmp138; + _jacobian(2, 3) = _tmp141; + _jacobian(0, 4) = _tmp144; + _jacobian(1, 4) = _tmp145; + _jacobian(2, 4) = _tmp146; + _jacobian(0, 5) = _tmp149; + _jacobian(1, 5) = _tmp150; + _jacobian(2, 5) = _tmp151; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp90, Scalar(2)) + std::pow(_tmp99, Scalar(2)); + _hessian(1, 0) = _tmp108 * _tmp81 + _tmp112 * _tmp90 + _tmp116 * _tmp99; + _hessian(2, 0) = _tmp123 * _tmp81 + _tmp125 * _tmp90 + _tmp128 * _tmp99; + _hessian(3, 0) = _tmp136 * _tmp81 + _tmp138 * _tmp90 + _tmp141 * _tmp99; + _hessian(4, 0) = _tmp144 * _tmp81 + _tmp145 * _tmp90 + _tmp146 * _tmp99; + _hessian(5, 0) = _tmp149 * _tmp81 + _tmp150 * _tmp90 + _tmp151 * _tmp99; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp108, Scalar(2)) + std::pow(_tmp112, Scalar(2)) + std::pow(_tmp116, Scalar(2)); + _hessian(2, 1) = _tmp108 * _tmp123 + _tmp112 * _tmp125 + _tmp116 * _tmp128; + _hessian(3, 1) = _tmp108 * _tmp136 + _tmp112 * _tmp138 + _tmp116 * _tmp141; + _hessian(4, 1) = _tmp108 * _tmp144 + _tmp112 * _tmp145 + _tmp116 * _tmp146; + _hessian(5, 1) = _tmp108 * _tmp149 + _tmp112 * _tmp150 + _tmp116 * _tmp151; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp123, Scalar(2)) + std::pow(_tmp125, Scalar(2)) + std::pow(_tmp128, Scalar(2)); + _hessian(3, 2) = _tmp123 * _tmp136 + _tmp125 * _tmp138 + _tmp128 * _tmp141; + _hessian(4, 2) = _tmp123 * _tmp144 + _tmp125 * _tmp145 + _tmp128 * _tmp146; + _hessian(5, 2) = _tmp123 * _tmp149 + _tmp125 * _tmp150 + _tmp128 * _tmp151; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = + std::pow(_tmp136, Scalar(2)) + std::pow(_tmp138, Scalar(2)) + std::pow(_tmp141, Scalar(2)); + _hessian(4, 3) = _tmp136 * _tmp144 + _tmp138 * _tmp145 + _tmp141 * _tmp146; + _hessian(5, 3) = _tmp136 * _tmp149 + _tmp138 * _tmp150 + _tmp141 * _tmp151; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = + std::pow(_tmp144, Scalar(2)) + std::pow(_tmp145, Scalar(2)) + std::pow(_tmp146, Scalar(2)); + _hessian(5, 4) = _tmp144 * _tmp149 + _tmp145 * _tmp150 + _tmp146 * _tmp151; + _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(_tmp149, Scalar(2)) + std::pow(_tmp150, Scalar(2)) + std::pow(_tmp151, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp32 * _tmp81 + _tmp34 * _tmp90 + _tmp37 * _tmp99; + _rhs(1, 0) = _tmp108 * _tmp32 + _tmp112 * _tmp34 + _tmp116 * _tmp37; + _rhs(2, 0) = _tmp123 * _tmp32 + _tmp125 * _tmp34 + _tmp128 * _tmp37; + _rhs(3, 0) = _tmp136 * _tmp32 + _tmp138 * _tmp34 + _tmp141 * _tmp37; + _rhs(4, 0) = _tmp144 * _tmp32 + _tmp145 * _tmp34 + _tmp146 * _tmp37; + _rhs(5, 0) = _tmp149 * _tmp32 + _tmp150 * _tmp34 + _tmp151 * _tmp37; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_isotropic.h b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_isotropic.h new file mode 100644 index 000000000..7939d5de8 --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_isotropic.h @@ -0,0 +1,306 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorRot3(const sym::Rot3& a, const sym::Rot3& b, + const sym::Rot3& a_T_b, const Scalar sqrt_info, 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: 468 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (145) + const Scalar _tmp0 = _a[3] * _b[3]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[1] * _b[1]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a[3] * _b[1]; + const Scalar _tmp6 = _a[2] * _b[0]; + const Scalar _tmp7 = _a[0] * _b[2]; + const Scalar _tmp8 = _a[1] * _b[3]; + const Scalar _tmp9 = _tmp5 - _tmp6 + _tmp7 - _tmp8; + const Scalar _tmp10 = _a[3] * _b[0]; + const Scalar _tmp11 = _a[2] * _b[1]; + const Scalar _tmp12 = _a[0] * _b[3]; + const Scalar _tmp13 = _a[1] * _b[2]; + const Scalar _tmp14 = _tmp10 + _tmp11 - _tmp12 - _tmp13; + const Scalar _tmp15 = _a[3] * _b[2]; + const Scalar _tmp16 = _a[2] * _b[3]; + const Scalar _tmp17 = _a[0] * _b[1]; + const Scalar _tmp18 = _a[1] * _b[0]; + const Scalar _tmp19 = _tmp15 - _tmp16 - _tmp17 + _tmp18; + const Scalar _tmp20 = + -_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp19 + _a_T_b[2] * _tmp9 + _a_T_b[3] * _tmp14; + const Scalar _tmp21 = _a_T_b[3] * _tmp4; + const Scalar _tmp22 = _a_T_b[1] * _tmp9; + const Scalar _tmp23 = _a_T_b[0] * _tmp14; + const Scalar _tmp24 = _a_T_b[2] * _tmp19; + const Scalar _tmp25 = _tmp21 + _tmp22 + _tmp23 + _tmp24; + const Scalar _tmp26 = 1 - epsilon; + const Scalar _tmp27 = std::min(_tmp26, std::fabs(_tmp25)); + const Scalar _tmp28 = + sqrt_info * (2 * std::min(0, (((_tmp25) > 0) - ((_tmp25) < 0))) + 1); + const Scalar _tmp29 = 2 * _tmp28; + const Scalar _tmp30 = + _tmp29 * std::acos(_tmp27) / std::sqrt(Scalar(1 - std::pow(_tmp27, Scalar(2)))); + const Scalar _tmp31 = _tmp20 * _tmp30; + const Scalar _tmp32 = + _a_T_b[0] * _tmp19 - _a_T_b[1] * _tmp4 - _a_T_b[2] * _tmp14 + _a_T_b[3] * _tmp9; + const Scalar _tmp33 = _tmp30 * _tmp32; + const Scalar _tmp34 = + -_a_T_b[0] * _tmp9 + _a_T_b[1] * _tmp14 - _a_T_b[2] * _tmp4 + _a_T_b[3] * _tmp19; + const Scalar _tmp35 = _tmp30 * _tmp34; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp40 = _tmp36 + _tmp37 - _tmp38 - _tmp39; + const Scalar _tmp41 = _a_T_b[3] * _tmp40; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp46 = -_tmp42 - _tmp43 - _tmp44 - _tmp45; + const Scalar _tmp47 = _a_T_b[0] * _tmp46; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp52 = -_tmp48 + _tmp49 - _tmp50 + _tmp51; + const Scalar _tmp53 = _a_T_b[2] * _tmp52; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp58 = _tmp54 - _tmp55 - _tmp56 + _tmp57; + const Scalar _tmp59 = _a_T_b[1] * _tmp58; + const Scalar _tmp60 = _tmp53 + _tmp59; + const Scalar _tmp61 = _tmp41 + _tmp47 + _tmp60; + const Scalar _tmp62 = _tmp21 + _tmp22 + _tmp23 + _tmp24; + const Scalar _tmp63 = std::fabs(_tmp62); + const Scalar _tmp64 = std::min(_tmp26, _tmp63); + const Scalar _tmp65 = std::acos(_tmp64); + const Scalar _tmp66 = 1 - std::pow(_tmp64, Scalar(2)); + const Scalar _tmp67 = _tmp28 * ((((_tmp26 - _tmp63) > 0) - ((_tmp26 - _tmp63) < 0)) + 1) * + (((_tmp62) > 0) - ((_tmp62) < 0)); + const Scalar _tmp68 = _tmp64 * _tmp65 * _tmp67 / (_tmp66 * std::sqrt(_tmp66)); + const Scalar _tmp69 = _tmp20 * _tmp68; + const Scalar _tmp70 = _tmp67 / _tmp66; + const Scalar _tmp71 = _tmp20 * _tmp70; + const Scalar _tmp72 = _a_T_b[3] * _tmp46; + const Scalar _tmp73 = -_a_T_b[1] * _tmp52; + const Scalar _tmp74 = _a_T_b[2] * _tmp58; + const Scalar _tmp75 = _a_T_b[0] * _tmp40; + const Scalar _tmp76 = _tmp29 * _tmp65 / std::sqrt(_tmp66); + const Scalar _tmp77 = + _tmp61 * _tmp69 - _tmp61 * _tmp71 + _tmp76 * (_tmp72 + _tmp73 + _tmp74 - _tmp75); + const Scalar _tmp78 = -_a_T_b[1] * _tmp40; + const Scalar _tmp79 = _a_T_b[2] * _tmp46; + const Scalar _tmp80 = _a_T_b[0] * _tmp52; + const Scalar _tmp81 = _a_T_b[3] * _tmp58; + const Scalar _tmp82 = _tmp80 + _tmp81; + const Scalar _tmp83 = _tmp32 * _tmp70; + const Scalar _tmp84 = _tmp32 * _tmp68; + const Scalar _tmp85 = -_tmp61 * _tmp83 + _tmp61 * _tmp84 + _tmp76 * (_tmp78 - _tmp79 + _tmp82); + const Scalar _tmp86 = _tmp34 * _tmp68; + const Scalar _tmp87 = _a_T_b[1] * _tmp46; + const Scalar _tmp88 = _a_T_b[3] * _tmp52; + const Scalar _tmp89 = -_a_T_b[0] * _tmp58; + const Scalar _tmp90 = _a_T_b[2] * _tmp40; + const Scalar _tmp91 = _tmp34 * _tmp70; + const Scalar _tmp92 = + _tmp61 * _tmp86 - _tmp61 * _tmp91 + _tmp76 * (_tmp87 + _tmp88 + _tmp89 - _tmp90); + const Scalar _tmp93 = _tmp48 - _tmp49 + _tmp50 - _tmp51; + const Scalar _tmp94 = _a_T_b[3] * _tmp93; + const Scalar _tmp95 = -_tmp54 + _tmp55 + _tmp56 - _tmp57; + const Scalar _tmp96 = _a_T_b[0] * _tmp95; + const Scalar _tmp97 = _tmp90 + _tmp96; + const Scalar _tmp98 = _tmp87 + _tmp94 + _tmp97; + const Scalar _tmp99 = _a_T_b[0] * _tmp93; + const Scalar _tmp100 = _a_T_b[3] * _tmp95; + const Scalar _tmp101 = + _tmp69 * _tmp98 - _tmp71 * _tmp98 + _tmp76 * (_tmp100 + _tmp78 + _tmp79 - _tmp99); + const Scalar _tmp102 = _a_T_b[1] * _tmp93; + const Scalar _tmp103 = -_a_T_b[2] * _tmp95; + const Scalar _tmp104 = _tmp103 + _tmp75; + const Scalar _tmp105 = _tmp76 * (-_tmp102 + _tmp104 + _tmp72) - _tmp83 * _tmp98 + _tmp84 * _tmp98; + const Scalar _tmp106 = -_a_T_b[2] * _tmp93; + const Scalar _tmp107 = _a_T_b[1] * _tmp95; + const Scalar _tmp108 = _tmp107 + _tmp41; + const Scalar _tmp109 = _tmp76 * (_tmp106 + _tmp108 - _tmp47) + _tmp86 * _tmp98 - _tmp91 * _tmp98; + const Scalar _tmp110 = -_tmp36 - _tmp37 + _tmp38 + _tmp39; + const Scalar _tmp111 = _a_T_b[2] * _tmp110; + const Scalar _tmp112 = _tmp111 + _tmp94; + const Scalar _tmp113 = _a_T_b[1] * _tmp110; + const Scalar _tmp114 = _tmp113 + _tmp99; + const Scalar _tmp115 = _tmp114 + _tmp79 + _tmp81; + const Scalar _tmp116 = _tmp115 * _tmp68; + const Scalar _tmp117 = + -_tmp115 * _tmp71 + _tmp116 * _tmp20 + _tmp76 * (_tmp112 - _tmp87 + _tmp89); + const Scalar _tmp118 = _a_T_b[3] * _tmp110; + const Scalar _tmp119 = + -_tmp115 * _tmp83 + _tmp116 * _tmp32 + _tmp76 * (_tmp106 + _tmp118 + _tmp47 - _tmp59); + const Scalar _tmp120 = -_a_T_b[0] * _tmp110; + const Scalar _tmp121 = _tmp102 + _tmp120; + const Scalar _tmp122 = + -_tmp115 * _tmp91 + _tmp116 * _tmp34 + _tmp76 * (_tmp121 + _tmp72 - _tmp74); + const Scalar _tmp123 = _tmp42 + _tmp43 + _tmp44 + _tmp45; + const Scalar _tmp124 = _a_T_b[0] * _tmp123; + const Scalar _tmp125 = _tmp118 + _tmp124; + const Scalar _tmp126 = _tmp125 + _tmp60; + const Scalar _tmp127 = _tmp126 * _tmp70; + const Scalar _tmp128 = _a_T_b[3] * _tmp123; + const Scalar _tmp129 = _tmp128 + _tmp73; + const Scalar _tmp130 = + _tmp126 * _tmp69 - _tmp127 * _tmp20 + _tmp76 * (_tmp120 + _tmp129 + _tmp74); + const Scalar _tmp131 = _a_T_b[2] * _tmp123; + const Scalar _tmp132 = + _tmp126 * _tmp84 - _tmp127 * _tmp32 + _tmp76 * (-_tmp113 - _tmp131 + _tmp82); + const Scalar _tmp133 = _a_T_b[1] * _tmp123; + const Scalar _tmp134 = _tmp133 + _tmp88; + const Scalar _tmp135 = + _tmp126 * _tmp86 - _tmp126 * _tmp91 + _tmp76 * (-_tmp111 + _tmp134 + _tmp89); + const Scalar _tmp136 = _tmp134 + _tmp97; + const Scalar _tmp137 = _tmp100 + _tmp131; + const Scalar _tmp138 = _tmp136 * _tmp69 - _tmp136 * _tmp71 + _tmp76 * (_tmp137 + _tmp78 - _tmp80); + const Scalar _tmp139 = -_tmp136 * _tmp83 + _tmp136 * _tmp84 + _tmp76 * (_tmp104 + _tmp129); + const Scalar _tmp140 = + _tmp136 * _tmp86 - _tmp136 * _tmp91 + _tmp76 * (_tmp108 - _tmp124 - _tmp53); + const Scalar _tmp141 = _tmp114 + _tmp137; + const Scalar _tmp142 = + _tmp141 * _tmp69 - _tmp141 * _tmp71 + _tmp76 * (_tmp112 - _tmp133 - _tmp96); + const Scalar _tmp143 = + -_tmp141 * _tmp83 + _tmp141 * _tmp84 + _tmp76 * (_tmp106 - _tmp107 + _tmp125); + const Scalar _tmp144 = + _tmp141 * _tmp86 - _tmp141 * _tmp91 + _tmp76 * (_tmp103 + _tmp121 + _tmp128); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp31; + _res(1, 0) = _tmp33; + _res(2, 0) = _tmp35; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp77; + _jacobian(1, 0) = _tmp85; + _jacobian(2, 0) = _tmp92; + _jacobian(0, 1) = _tmp101; + _jacobian(1, 1) = _tmp105; + _jacobian(2, 1) = _tmp109; + _jacobian(0, 2) = _tmp117; + _jacobian(1, 2) = _tmp119; + _jacobian(2, 2) = _tmp122; + _jacobian(0, 3) = _tmp130; + _jacobian(1, 3) = _tmp132; + _jacobian(2, 3) = _tmp135; + _jacobian(0, 4) = _tmp138; + _jacobian(1, 4) = _tmp139; + _jacobian(2, 4) = _tmp140; + _jacobian(0, 5) = _tmp142; + _jacobian(1, 5) = _tmp143; + _jacobian(2, 5) = _tmp144; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp77, Scalar(2)) + std::pow(_tmp85, Scalar(2)) + std::pow(_tmp92, Scalar(2)); + _hessian(1, 0) = _tmp101 * _tmp77 + _tmp105 * _tmp85 + _tmp109 * _tmp92; + _hessian(2, 0) = _tmp117 * _tmp77 + _tmp119 * _tmp85 + _tmp122 * _tmp92; + _hessian(3, 0) = _tmp130 * _tmp77 + _tmp132 * _tmp85 + _tmp135 * _tmp92; + _hessian(4, 0) = _tmp138 * _tmp77 + _tmp139 * _tmp85 + _tmp140 * _tmp92; + _hessian(5, 0) = _tmp142 * _tmp77 + _tmp143 * _tmp85 + _tmp144 * _tmp92; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp101, Scalar(2)) + std::pow(_tmp105, Scalar(2)) + std::pow(_tmp109, Scalar(2)); + _hessian(2, 1) = _tmp101 * _tmp117 + _tmp105 * _tmp119 + _tmp109 * _tmp122; + _hessian(3, 1) = _tmp101 * _tmp130 + _tmp105 * _tmp132 + _tmp109 * _tmp135; + _hessian(4, 1) = _tmp101 * _tmp138 + _tmp105 * _tmp139 + _tmp109 * _tmp140; + _hessian(5, 1) = _tmp101 * _tmp142 + _tmp105 * _tmp143 + _tmp109 * _tmp144; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp117, Scalar(2)) + std::pow(_tmp119, Scalar(2)) + std::pow(_tmp122, Scalar(2)); + _hessian(3, 2) = _tmp117 * _tmp130 + _tmp119 * _tmp132 + _tmp122 * _tmp135; + _hessian(4, 2) = _tmp117 * _tmp138 + _tmp119 * _tmp139 + _tmp122 * _tmp140; + _hessian(5, 2) = _tmp117 * _tmp142 + _tmp119 * _tmp143 + _tmp122 * _tmp144; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = + std::pow(_tmp130, Scalar(2)) + std::pow(_tmp132, Scalar(2)) + std::pow(_tmp135, Scalar(2)); + _hessian(4, 3) = _tmp130 * _tmp138 + _tmp132 * _tmp139 + _tmp135 * _tmp140; + _hessian(5, 3) = _tmp130 * _tmp142 + _tmp132 * _tmp143 + _tmp135 * _tmp144; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = + std::pow(_tmp138, Scalar(2)) + std::pow(_tmp139, Scalar(2)) + std::pow(_tmp140, Scalar(2)); + _hessian(5, 4) = _tmp138 * _tmp142 + _tmp139 * _tmp143 + _tmp140 * _tmp144; + _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(_tmp142, Scalar(2)) + std::pow(_tmp143, Scalar(2)) + std::pow(_tmp144, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp31 * _tmp77 + _tmp33 * _tmp85 + _tmp35 * _tmp92; + _rhs(1, 0) = _tmp101 * _tmp31 + _tmp105 * _tmp33 + _tmp109 * _tmp35; + _rhs(2, 0) = _tmp117 * _tmp31 + _tmp119 * _tmp33 + _tmp122 * _tmp35; + _rhs(3, 0) = _tmp130 * _tmp31 + _tmp132 * _tmp33 + _tmp135 * _tmp35; + _rhs(4, 0) = _tmp138 * _tmp31 + _tmp139 * _tmp33 + _tmp140 * _tmp35; + _rhs(5, 0) = _tmp142 * _tmp31 + _tmp143 * _tmp33 + _tmp144 * _tmp35; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_square.h b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_square.h new file mode 100644 index 000000000..cb9cebfdb --- /dev/null +++ b/gen/cpp/sym/factors/between_factor_rot3/between_factor_rot3_square.h @@ -0,0 +1,405 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between between(a, b) and a_T_b. + * + * In vector space terms that would be: + * (b - a) - a_T_b + * + * In lie group terms: + * local_coordinates(a_T_b, between(a, b)) + * to_tangent(compose(inverse(a_T_b), compose(inverse(a), b))) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt args a (3), b (3) + * hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) + * rhs: (6x1) Gauss-Newton rhs for args a (3), b (3) + */ +template +void BetweenFactorRot3(const sym::Rot3& a, const sym::Rot3& b, + const sym::Rot3& a_T_b, const Eigen::Matrix& sqrt_info, + 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: 754 + + // Input arrays + const Eigen::Matrix& _a = a.Data(); + const Eigen::Matrix& _b = b.Data(); + const Eigen::Matrix& _a_T_b = a_T_b.Data(); + + // Intermediate terms (216) + const Scalar _tmp0 = _a[3] * _b[3]; + const Scalar _tmp1 = _a[2] * _b[2]; + const Scalar _tmp2 = _a[0] * _b[0]; + const Scalar _tmp3 = _a[1] * _b[1]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = _a_T_b[3] * _tmp4; + const Scalar _tmp6 = _a[3] * _b[1]; + const Scalar _tmp7 = _a[2] * _b[0]; + const Scalar _tmp8 = _a[0] * _b[2]; + const Scalar _tmp9 = _a[1] * _b[3]; + const Scalar _tmp10 = _tmp6 - _tmp7 + _tmp8 - _tmp9; + const Scalar _tmp11 = _a_T_b[1] * _tmp10; + const Scalar _tmp12 = _a[3] * _b[0]; + const Scalar _tmp13 = _a[2] * _b[1]; + const Scalar _tmp14 = _a[0] * _b[3]; + const Scalar _tmp15 = _a[1] * _b[2]; + const Scalar _tmp16 = _tmp12 + _tmp13 - _tmp14 - _tmp15; + const Scalar _tmp17 = _a_T_b[0] * _tmp16; + const Scalar _tmp18 = _a[3] * _b[2]; + const Scalar _tmp19 = _a[2] * _b[3]; + const Scalar _tmp20 = _a[0] * _b[1]; + const Scalar _tmp21 = _a[1] * _b[0]; + const Scalar _tmp22 = _tmp18 - _tmp19 - _tmp20 + _tmp21; + const Scalar _tmp23 = _a_T_b[2] * _tmp22; + const Scalar _tmp24 = _tmp11 + _tmp17 + _tmp23 + _tmp5; + const Scalar _tmp25 = 2 * std::min(0, (((_tmp24) > 0) - ((_tmp24) < 0))) + 1; + const Scalar _tmp26 = 2 * _tmp25; + const Scalar _tmp27 = _tmp26 * sqrt_info(0, 2); + const Scalar _tmp28 = + -_a_T_b[0] * _tmp10 + _a_T_b[1] * _tmp16 - _a_T_b[2] * _tmp4 + _a_T_b[3] * _tmp22; + const Scalar _tmp29 = 1 - epsilon; + const Scalar _tmp30 = std::min(_tmp29, std::fabs(_tmp24)); + const Scalar _tmp31 = std::acos(_tmp30) / std::sqrt(Scalar(1 - std::pow(_tmp30, Scalar(2)))); + const Scalar _tmp32 = _tmp28 * _tmp31; + const Scalar _tmp33 = + _a_T_b[0] * _tmp22 - _a_T_b[1] * _tmp4 - _a_T_b[2] * _tmp16 + _a_T_b[3] * _tmp10; + const Scalar _tmp34 = _tmp26 * _tmp31; + const Scalar _tmp35 = _tmp33 * _tmp34; + const Scalar _tmp36 = + -_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp22 + _a_T_b[2] * _tmp10 + _a_T_b[3] * _tmp16; + const Scalar _tmp37 = _tmp34 * _tmp36; + const Scalar _tmp38 = _tmp27 * _tmp32 + _tmp35 * sqrt_info(0, 1) + _tmp37 * sqrt_info(0, 0); + const Scalar _tmp39 = _tmp26 * sqrt_info(1, 2); + const Scalar _tmp40 = _tmp33 * sqrt_info(1, 1); + const Scalar _tmp41 = _tmp32 * _tmp39 + _tmp34 * _tmp40 + _tmp37 * sqrt_info(1, 0); + const Scalar _tmp42 = _tmp26 * sqrt_info(2, 2); + const Scalar _tmp43 = _tmp32 * _tmp42 + _tmp35 * sqrt_info(2, 1) + _tmp37 * sqrt_info(2, 0); + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp48 = -_tmp44 - _tmp45 - _tmp46 - _tmp47; + const Scalar _tmp49 = _a_T_b[3] * _tmp48; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp54 = -_tmp50 + _tmp51 - _tmp52 + _tmp53; + const Scalar _tmp55 = -_a_T_b[1] * _tmp54; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp60 = _tmp56 - _tmp57 - _tmp58 + _tmp59; + const Scalar _tmp61 = _a_T_b[2] * _tmp60; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp66 = _tmp62 + _tmp63 - _tmp64 - _tmp65; + const Scalar _tmp67 = _a_T_b[0] * _tmp66; + const Scalar _tmp68 = _tmp11 + _tmp17 + _tmp23 + _tmp5; + const Scalar _tmp69 = std::fabs(_tmp68); + const Scalar _tmp70 = std::min(_tmp29, _tmp69); + const Scalar _tmp71 = 1 - std::pow(_tmp70, Scalar(2)); + const Scalar _tmp72 = std::acos(_tmp70); + const Scalar _tmp73 = _tmp72 / std::sqrt(_tmp71); + const Scalar _tmp74 = _tmp26 * _tmp73; + const Scalar _tmp75 = _tmp74 * (_tmp49 + _tmp55 + _tmp61 - _tmp67); + const Scalar _tmp76 = _tmp36 * sqrt_info(0, 0); + const Scalar _tmp77 = _a_T_b[3] * _tmp66; + const Scalar _tmp78 = _a_T_b[0] * _tmp48; + const Scalar _tmp79 = _a_T_b[2] * _tmp54; + const Scalar _tmp80 = _a_T_b[1] * _tmp60; + const Scalar _tmp81 = _tmp79 + _tmp80; + const Scalar _tmp82 = _tmp77 + _tmp78 + _tmp81; + const Scalar _tmp83 = _tmp25 * ((((_tmp29 - _tmp69) > 0) - ((_tmp29 - _tmp69) < 0)) + 1) * + (((_tmp68) > 0) - ((_tmp68) < 0)); + const Scalar _tmp84 = _tmp83 / _tmp71; + const Scalar _tmp85 = _tmp82 * _tmp84; + const Scalar _tmp86 = _tmp33 * _tmp84; + const Scalar _tmp87 = _tmp86 * sqrt_info(0, 1); + const Scalar _tmp88 = _tmp70 * _tmp72 * _tmp83 / (_tmp71 * std::sqrt(_tmp71)); + const Scalar _tmp89 = _tmp28 * _tmp88; + const Scalar _tmp90 = _tmp82 * _tmp89; + const Scalar _tmp91 = _tmp82 * _tmp88; + const Scalar _tmp92 = -_a_T_b[1] * _tmp66; + const Scalar _tmp93 = _a_T_b[2] * _tmp48; + const Scalar _tmp94 = _a_T_b[0] * _tmp54; + const Scalar _tmp95 = _a_T_b[3] * _tmp60; + const Scalar _tmp96 = _tmp94 + _tmp95; + const Scalar _tmp97 = _tmp92 - _tmp93 + _tmp96; + const Scalar _tmp98 = _tmp74 * sqrt_info(0, 1); + const Scalar _tmp99 = _a_T_b[1] * _tmp48; + const Scalar _tmp100 = _a_T_b[3] * _tmp54; + const Scalar _tmp101 = -_a_T_b[0] * _tmp60; + const Scalar _tmp102 = _a_T_b[2] * _tmp66; + const Scalar _tmp103 = _tmp100 + _tmp101 - _tmp102 + _tmp99; + const Scalar _tmp104 = _tmp103 * _tmp73; + const Scalar _tmp105 = _tmp33 * _tmp91; + const Scalar _tmp106 = _tmp28 * _tmp84; + const Scalar _tmp107 = _tmp106 * _tmp82; + const Scalar _tmp108 = _tmp104 * _tmp27 + _tmp105 * sqrt_info(0, 1) - _tmp107 * sqrt_info(0, 2) + + _tmp75 * sqrt_info(0, 0) - _tmp76 * _tmp85 + _tmp76 * _tmp91 - + _tmp82 * _tmp87 + _tmp90 * sqrt_info(0, 2) + _tmp97 * _tmp98; + const Scalar _tmp109 = _tmp36 * sqrt_info(1, 0); + const Scalar _tmp110 = _tmp40 * _tmp84; + const Scalar _tmp111 = _tmp89 * sqrt_info(1, 2); + const Scalar _tmp112 = _tmp74 * sqrt_info(1, 1); + const Scalar _tmp113 = _tmp39 * _tmp73; + const Scalar _tmp114 = _tmp103 * _tmp113 - _tmp107 * sqrt_info(1, 2) - _tmp109 * _tmp85 + + _tmp109 * _tmp91 - _tmp110 * _tmp82 + _tmp111 * _tmp82 + _tmp112 * _tmp97 + + _tmp40 * _tmp91 + _tmp75 * sqrt_info(1, 0); + const Scalar _tmp115 = _tmp36 * sqrt_info(2, 0); + const Scalar _tmp116 = _tmp86 * sqrt_info(2, 1); + const Scalar _tmp117 = _tmp74 * sqrt_info(2, 1); + const Scalar _tmp118 = _tmp104 * _tmp42 + _tmp105 * sqrt_info(2, 1) - _tmp107 * sqrt_info(2, 2) - + _tmp115 * _tmp85 + _tmp115 * _tmp91 - _tmp116 * _tmp82 + _tmp117 * _tmp97 + + _tmp75 * sqrt_info(2, 0) + _tmp90 * sqrt_info(2, 2); + const Scalar _tmp119 = _tmp50 - _tmp51 + _tmp52 - _tmp53; + const Scalar _tmp120 = _a_T_b[3] * _tmp119; + const Scalar _tmp121 = -_tmp56 + _tmp57 + _tmp58 - _tmp59; + const Scalar _tmp122 = _a_T_b[0] * _tmp121; + const Scalar _tmp123 = _tmp102 + _tmp122; + const Scalar _tmp124 = _tmp120 + _tmp123 + _tmp99; + const Scalar _tmp125 = _tmp106 * _tmp124; + const Scalar _tmp126 = _tmp124 * _tmp84; + const Scalar _tmp127 = _a_T_b[1] * _tmp119; + const Scalar _tmp128 = -_a_T_b[2] * _tmp121; + const Scalar _tmp129 = _tmp128 + _tmp67; + const Scalar _tmp130 = -_tmp127 + _tmp129 + _tmp49; + const Scalar _tmp131 = _tmp130 * _tmp74; + const Scalar _tmp132 = _tmp124 * _tmp88; + const Scalar _tmp133 = _tmp132 * _tmp33; + const Scalar _tmp134 = _tmp132 * _tmp28; + const Scalar _tmp135 = -_a_T_b[2] * _tmp119; + const Scalar _tmp136 = _a_T_b[1] * _tmp121; + const Scalar _tmp137 = _tmp136 + _tmp77; + const Scalar _tmp138 = _tmp135 + _tmp137 - _tmp78; + const Scalar _tmp139 = _tmp138 * _tmp73; + const Scalar _tmp140 = _a_T_b[0] * _tmp119; + const Scalar _tmp141 = _a_T_b[3] * _tmp121; + const Scalar _tmp142 = _tmp74 * (-_tmp140 + _tmp141 + _tmp92 + _tmp93); + const Scalar _tmp143 = -_tmp124 * _tmp87 - _tmp125 * sqrt_info(0, 2) - _tmp126 * _tmp76 + + _tmp131 * sqrt_info(0, 1) + _tmp132 * _tmp76 + _tmp133 * sqrt_info(0, 1) + + _tmp134 * sqrt_info(0, 2) + _tmp139 * _tmp27 + _tmp142 * sqrt_info(0, 0); + const Scalar _tmp144 = -_tmp109 * _tmp126 + _tmp109 * _tmp132 - _tmp110 * _tmp124 + + _tmp113 * _tmp138 - _tmp125 * sqrt_info(1, 2) + _tmp131 * sqrt_info(1, 1) + + _tmp132 * _tmp40 + _tmp134 * sqrt_info(1, 2) + _tmp142 * sqrt_info(1, 0); + const Scalar _tmp145 = -_tmp115 * _tmp126 + _tmp115 * _tmp132 - _tmp116 * _tmp124 + + _tmp117 * _tmp130 - _tmp125 * sqrt_info(2, 2) + _tmp133 * sqrt_info(2, 1) + + _tmp134 * sqrt_info(2, 2) + _tmp139 * _tmp42 + _tmp142 * sqrt_info(2, 0); + const Scalar _tmp146 = -_tmp62 - _tmp63 + _tmp64 + _tmp65; + const Scalar _tmp147 = _a_T_b[1] * _tmp146; + const Scalar _tmp148 = _tmp140 + _tmp147; + const Scalar _tmp149 = _tmp148 + _tmp93 + _tmp95; + const Scalar _tmp150 = _tmp149 * _tmp89; + const Scalar _tmp151 = _tmp149 * _tmp84; + const Scalar _tmp152 = _tmp149 * _tmp88; + const Scalar _tmp153 = _tmp152 * _tmp33; + const Scalar _tmp154 = _a_T_b[2] * _tmp146; + const Scalar _tmp155 = _tmp120 + _tmp154; + const Scalar _tmp156 = _tmp74 * (_tmp101 + _tmp155 - _tmp99); + const Scalar _tmp157 = _a_T_b[3] * _tmp146; + const Scalar _tmp158 = _tmp135 + _tmp157 + _tmp78 - _tmp80; + const Scalar _tmp159 = _tmp106 * _tmp149; + const Scalar _tmp160 = -_a_T_b[0] * _tmp146; + const Scalar _tmp161 = _tmp127 + _tmp160; + const Scalar _tmp162 = _tmp161 + _tmp49 - _tmp61; + const Scalar _tmp163 = _tmp162 * _tmp73; + const Scalar _tmp164 = -_tmp149 * _tmp87 + _tmp150 * sqrt_info(0, 2) - _tmp151 * _tmp76 + + _tmp152 * _tmp76 + _tmp153 * sqrt_info(0, 1) + _tmp156 * sqrt_info(0, 0) + + _tmp158 * _tmp98 - _tmp159 * sqrt_info(0, 2) + _tmp163 * _tmp27; + const Scalar _tmp165 = -_tmp109 * _tmp151 + _tmp109 * _tmp152 - _tmp110 * _tmp149 + + _tmp112 * _tmp158 + _tmp113 * _tmp162 + _tmp150 * sqrt_info(1, 2) + + _tmp152 * _tmp40 + _tmp156 * sqrt_info(1, 0) - _tmp159 * sqrt_info(1, 2); + const Scalar _tmp166 = -_tmp115 * _tmp151 + _tmp115 * _tmp152 - _tmp116 * _tmp149 + + _tmp117 * _tmp158 + _tmp150 * sqrt_info(2, 2) + _tmp153 * sqrt_info(2, 1) + + _tmp156 * sqrt_info(2, 0) - _tmp159 * sqrt_info(2, 2) + _tmp163 * _tmp42; + const Scalar _tmp167 = _tmp44 + _tmp45 + _tmp46 + _tmp47; + const Scalar _tmp168 = _a_T_b[0] * _tmp167; + const Scalar _tmp169 = _tmp157 + _tmp168; + const Scalar _tmp170 = _tmp169 + _tmp81; + const Scalar _tmp171 = _tmp170 * _tmp84; + const Scalar _tmp172 = _tmp170 * _tmp88; + const Scalar _tmp173 = _tmp172 * _tmp33; + const Scalar _tmp174 = _tmp170 * _tmp89; + const Scalar _tmp175 = _a_T_b[3] * _tmp167; + const Scalar _tmp176 = _tmp175 + _tmp55; + const Scalar _tmp177 = _tmp74 * (_tmp160 + _tmp176 + _tmp61); + const Scalar _tmp178 = _a_T_b[2] * _tmp167; + const Scalar _tmp179 = _tmp74 * (-_tmp147 - _tmp178 + _tmp96); + const Scalar _tmp180 = _a_T_b[1] * _tmp167; + const Scalar _tmp181 = _tmp100 + _tmp180; + const Scalar _tmp182 = _tmp101 - _tmp154 + _tmp181; + const Scalar _tmp183 = _tmp182 * _tmp73; + const Scalar _tmp184 = _tmp106 * _tmp170; + const Scalar _tmp185 = -_tmp170 * _tmp87 - _tmp171 * _tmp76 + _tmp172 * _tmp76 + + _tmp173 * sqrt_info(0, 1) + _tmp174 * sqrt_info(0, 2) + + _tmp177 * sqrt_info(0, 0) + _tmp179 * sqrt_info(0, 1) + _tmp183 * _tmp27 - + _tmp184 * sqrt_info(0, 2); + const Scalar _tmp186 = -_tmp109 * _tmp171 + _tmp109 * _tmp172 - _tmp110 * _tmp170 + + _tmp111 * _tmp170 + _tmp113 * _tmp182 + _tmp172 * _tmp40 + + _tmp177 * sqrt_info(1, 0) + _tmp179 * sqrt_info(1, 1) - + _tmp184 * sqrt_info(1, 2); + const Scalar _tmp187 = -_tmp115 * _tmp171 + _tmp115 * _tmp172 - _tmp116 * _tmp170 + + _tmp173 * sqrt_info(2, 1) + _tmp174 * sqrt_info(2, 2) + + _tmp177 * sqrt_info(2, 0) + _tmp179 * sqrt_info(2, 1) + _tmp183 * _tmp42 - + _tmp184 * sqrt_info(2, 2); + const Scalar _tmp188 = _tmp123 + _tmp181; + const Scalar _tmp189 = _tmp188 * _tmp84; + const Scalar _tmp190 = _tmp189 * _tmp33; + const Scalar _tmp191 = _tmp188 * _tmp89; + const Scalar _tmp192 = _tmp188 * _tmp88; + const Scalar _tmp193 = _tmp192 * _tmp33; + const Scalar _tmp194 = _tmp129 + _tmp176; + const Scalar _tmp195 = _tmp194 * _tmp74; + const Scalar _tmp196 = _tmp141 + _tmp178; + const Scalar _tmp197 = _tmp74 * (_tmp196 + _tmp92 - _tmp94); + const Scalar _tmp198 = _tmp106 * _tmp188; + const Scalar _tmp199 = _tmp137 - _tmp168 - _tmp79; + const Scalar _tmp200 = _tmp199 * _tmp73; + const Scalar _tmp201 = -_tmp189 * _tmp76 - _tmp190 * sqrt_info(0, 1) + _tmp191 * sqrt_info(0, 2) + + _tmp192 * _tmp76 + _tmp193 * sqrt_info(0, 1) + _tmp195 * sqrt_info(0, 1) + + _tmp197 * sqrt_info(0, 0) - _tmp198 * sqrt_info(0, 2) + _tmp200 * _tmp27; + const Scalar _tmp202 = -_tmp109 * _tmp189 + _tmp109 * _tmp192 + _tmp113 * _tmp199 - + _tmp189 * _tmp40 + _tmp191 * sqrt_info(1, 2) + _tmp192 * _tmp40 + + _tmp195 * sqrt_info(1, 1) + _tmp197 * sqrt_info(1, 0) - + _tmp198 * sqrt_info(1, 2); + const Scalar _tmp203 = -_tmp115 * _tmp189 + _tmp115 * _tmp192 + _tmp117 * _tmp194 - + _tmp190 * sqrt_info(2, 1) + _tmp191 * sqrt_info(2, 2) + + _tmp193 * sqrt_info(2, 1) + _tmp197 * sqrt_info(2, 0) - + _tmp198 * sqrt_info(2, 2) + _tmp200 * _tmp42; + const Scalar _tmp204 = _tmp148 + _tmp196; + const Scalar _tmp205 = _tmp204 * _tmp88; + const Scalar _tmp206 = _tmp73 * (_tmp128 + _tmp161 + _tmp175); + const Scalar _tmp207 = _tmp205 * _tmp33; + const Scalar _tmp208 = _tmp106 * _tmp204; + const Scalar _tmp209 = _tmp204 * _tmp89; + const Scalar _tmp210 = _tmp204 * _tmp84; + const Scalar _tmp211 = _tmp135 - _tmp136 + _tmp169; + const Scalar _tmp212 = _tmp74 * (-_tmp122 + _tmp155 - _tmp180); + const Scalar _tmp213 = -_tmp204 * _tmp87 + _tmp205 * _tmp76 + _tmp206 * _tmp27 + + _tmp207 * sqrt_info(0, 1) - _tmp208 * sqrt_info(0, 2) + + _tmp209 * sqrt_info(0, 2) - _tmp210 * _tmp76 + _tmp211 * _tmp98 + + _tmp212 * sqrt_info(0, 0); + const Scalar _tmp214 = _tmp109 * _tmp205 - _tmp109 * _tmp210 - _tmp110 * _tmp204 + + _tmp111 * _tmp204 + _tmp112 * _tmp211 + _tmp205 * _tmp40 + + _tmp206 * _tmp39 - _tmp208 * sqrt_info(1, 2) + _tmp212 * sqrt_info(1, 0); + const Scalar _tmp215 = _tmp115 * _tmp205 - _tmp115 * _tmp210 - _tmp116 * _tmp204 + + _tmp117 * _tmp211 + _tmp206 * _tmp42 + _tmp207 * sqrt_info(2, 1) - + _tmp208 * sqrt_info(2, 2) + _tmp209 * sqrt_info(2, 2) + + _tmp212 * sqrt_info(2, 0); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp38; + _res(1, 0) = _tmp41; + _res(2, 0) = _tmp43; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp108; + _jacobian(1, 0) = _tmp114; + _jacobian(2, 0) = _tmp118; + _jacobian(0, 1) = _tmp143; + _jacobian(1, 1) = _tmp144; + _jacobian(2, 1) = _tmp145; + _jacobian(0, 2) = _tmp164; + _jacobian(1, 2) = _tmp165; + _jacobian(2, 2) = _tmp166; + _jacobian(0, 3) = _tmp185; + _jacobian(1, 3) = _tmp186; + _jacobian(2, 3) = _tmp187; + _jacobian(0, 4) = _tmp201; + _jacobian(1, 4) = _tmp202; + _jacobian(2, 4) = _tmp203; + _jacobian(0, 5) = _tmp213; + _jacobian(1, 5) = _tmp214; + _jacobian(2, 5) = _tmp215; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp108, Scalar(2)) + std::pow(_tmp114, Scalar(2)) + std::pow(_tmp118, Scalar(2)); + _hessian(1, 0) = _tmp108 * _tmp143 + _tmp114 * _tmp144 + _tmp118 * _tmp145; + _hessian(2, 0) = _tmp108 * _tmp164 + _tmp114 * _tmp165 + _tmp118 * _tmp166; + _hessian(3, 0) = _tmp108 * _tmp185 + _tmp114 * _tmp186 + _tmp118 * _tmp187; + _hessian(4, 0) = _tmp108 * _tmp201 + _tmp114 * _tmp202 + _tmp118 * _tmp203; + _hessian(5, 0) = _tmp108 * _tmp213 + _tmp114 * _tmp214 + _tmp118 * _tmp215; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp143, Scalar(2)) + std::pow(_tmp144, Scalar(2)) + std::pow(_tmp145, Scalar(2)); + _hessian(2, 1) = _tmp143 * _tmp164 + _tmp144 * _tmp165 + _tmp145 * _tmp166; + _hessian(3, 1) = _tmp143 * _tmp185 + _tmp144 * _tmp186 + _tmp145 * _tmp187; + _hessian(4, 1) = _tmp143 * _tmp201 + _tmp144 * _tmp202 + _tmp145 * _tmp203; + _hessian(5, 1) = _tmp143 * _tmp213 + _tmp144 * _tmp214 + _tmp145 * _tmp215; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp164, Scalar(2)) + std::pow(_tmp165, Scalar(2)) + std::pow(_tmp166, Scalar(2)); + _hessian(3, 2) = _tmp164 * _tmp185 + _tmp165 * _tmp186 + _tmp166 * _tmp187; + _hessian(4, 2) = _tmp164 * _tmp201 + _tmp165 * _tmp202 + _tmp166 * _tmp203; + _hessian(5, 2) = _tmp164 * _tmp213 + _tmp165 * _tmp214 + _tmp166 * _tmp215; + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = + std::pow(_tmp185, Scalar(2)) + std::pow(_tmp186, Scalar(2)) + std::pow(_tmp187, Scalar(2)); + _hessian(4, 3) = _tmp185 * _tmp201 + _tmp186 * _tmp202 + _tmp187 * _tmp203; + _hessian(5, 3) = _tmp185 * _tmp213 + _tmp186 * _tmp214 + _tmp187 * _tmp215; + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = + std::pow(_tmp201, Scalar(2)) + std::pow(_tmp202, Scalar(2)) + std::pow(_tmp203, Scalar(2)); + _hessian(5, 4) = _tmp201 * _tmp213 + _tmp202 * _tmp214 + _tmp203 * _tmp215; + _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(_tmp213, Scalar(2)) + std::pow(_tmp214, Scalar(2)) + std::pow(_tmp215, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp108 * _tmp38 + _tmp114 * _tmp41 + _tmp118 * _tmp43; + _rhs(1, 0) = _tmp143 * _tmp38 + _tmp144 * _tmp41 + _tmp145 * _tmp43; + _rhs(2, 0) = _tmp164 * _tmp38 + _tmp165 * _tmp41 + _tmp166 * _tmp43; + _rhs(3, 0) = _tmp185 * _tmp38 + _tmp186 * _tmp41 + _tmp187 * _tmp43; + _rhs(4, 0) = _tmp201 * _tmp38 + _tmp202 * _tmp41 + _tmp203 * _tmp43; + _rhs(5, 0) = _tmp213 * _tmp38 + _tmp214 * _tmp41 + _tmp215 * _tmp43; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_matrix31.h b/gen/cpp/sym/factors/prior_factor_matrix31.h index b4ef0ac67..c985c04d7 100644 --- a/gen/cpp/sym/factors/prior_factor_matrix31.h +++ b/gen/cpp/sym/factors/prior_factor_matrix31.h @@ -1,106 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x3) jacobian of res wrt arg value (3) - * hessian: (3x3) Gauss-Newton hessian for arg value (3) - * rhs: (3x1) Gauss-Newton rhs for arg value (3) - */ -template -void PriorFactorMatrix31(const Eigen::Matrix& value, - const Eigen::Matrix& prior, - const Eigen::Matrix& sqrt_info, 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: 63 - - // Unused inputs - (void)epsilon; - - // Input arrays - - // Intermediate terms (6) - const Scalar _tmp0 = -prior(1, 0) + value(1, 0); - const Scalar _tmp1 = -prior(2, 0) + value(2, 0); - const Scalar _tmp2 = -prior(0, 0) + value(0, 0); - const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp2 * sqrt_info(0, 0); - const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp2 * sqrt_info(1, 0); - const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp2 * sqrt_info(2, 0); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp3; - _res(1, 0) = _tmp4; - _res(2, 0) = _tmp5; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = sqrt_info(0, 0); - _jacobian(1, 0) = sqrt_info(1, 0); - _jacobian(2, 0) = sqrt_info(2, 0); - _jacobian(0, 1) = sqrt_info(0, 1); - _jacobian(1, 1) = sqrt_info(1, 1); - _jacobian(2, 1) = sqrt_info(2, 1); - _jacobian(0, 2) = sqrt_info(0, 2); - _jacobian(1, 2) = sqrt_info(1, 2); - _jacobian(2, 2) = sqrt_info(2, 2); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = std::pow(sqrt_info(0, 0), Scalar(2)) + std::pow(sqrt_info(1, 0), Scalar(2)) + - std::pow(sqrt_info(2, 0), Scalar(2)); - _hessian(1, 0) = sqrt_info(0, 0) * sqrt_info(0, 1) + sqrt_info(1, 0) * sqrt_info(1, 1) + - sqrt_info(2, 0) * sqrt_info(2, 1); - _hessian(2, 0) = sqrt_info(0, 0) * sqrt_info(0, 2) + sqrt_info(1, 0) * sqrt_info(1, 2) + - sqrt_info(2, 0) * sqrt_info(2, 2); - _hessian(0, 1) = 0; - _hessian(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + - std::pow(sqrt_info(2, 1), Scalar(2)); - _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + - sqrt_info(2, 1) * sqrt_info(2, 2); - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + - std::pow(sqrt_info(2, 2), Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp3 * sqrt_info(0, 0) + _tmp4 * sqrt_info(1, 0) + _tmp5 * sqrt_info(2, 0); - _rhs(1, 0) = _tmp3 * sqrt_info(0, 1) + _tmp4 * sqrt_info(1, 1) + _tmp5 * sqrt_info(2, 1); - _rhs(2, 0) = _tmp3 * sqrt_info(0, 2) + _tmp4 * sqrt_info(1, 2) + _tmp5 * sqrt_info(2, 2); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_matrix31/prior_factor_matrix31_diagonal.h" +#include "./prior_factor_matrix31/prior_factor_matrix31_isotropic.h" +#include "./prior_factor_matrix31/prior_factor_matrix31_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_diagonal.h b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_diagonal.h new file mode 100644 index 000000000..97fe1349e --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_diagonal.h @@ -0,0 +1,93 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorMatrix31(const Eigen::Matrix& value, + const Eigen::Matrix& prior, + const Eigen::Matrix& sqrt_info, 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: 12 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (6) + const Scalar _tmp0 = -prior(0, 0) + value(0, 0); + const Scalar _tmp1 = -prior(1, 0) + value(1, 0); + const Scalar _tmp2 = -prior(2, 0) + value(2, 0); + const Scalar _tmp3 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp4 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp5 = std::pow(sqrt_info(2, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info(0, 0); + _res(1, 0) = _tmp1 * sqrt_info(1, 0); + _res(2, 0) = _tmp2 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = sqrt_info(0, 0); + _jacobian(1, 1) = sqrt_info(1, 0); + _jacobian(2, 2) = sqrt_info(2, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp3; + _hessian(1, 1) = _tmp4; + _hessian(2, 2) = _tmp5; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp0 * _tmp3; + _rhs(1, 0) = _tmp1 * _tmp4; + _rhs(2, 0) = _tmp2 * _tmp5; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_isotropic.h b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_isotropic.h new file mode 100644 index 000000000..bc2d64cb4 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_isotropic.h @@ -0,0 +1,90 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorMatrix31(const Eigen::Matrix& value, + const Eigen::Matrix& prior, const Scalar sqrt_info, + 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: 10 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (4) + const Scalar _tmp0 = -prior(0, 0) + value(0, 0); + const Scalar _tmp1 = -prior(1, 0) + value(1, 0); + const Scalar _tmp2 = -prior(2, 0) + value(2, 0); + const Scalar _tmp3 = std::pow(sqrt_info, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info; + _res(1, 0) = _tmp1 * sqrt_info; + _res(2, 0) = _tmp2 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = sqrt_info; + _jacobian(1, 1) = sqrt_info; + _jacobian(2, 2) = sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp3; + _hessian(1, 1) = _tmp3; + _hessian(2, 2) = _tmp3; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp0 * _tmp3; + _rhs(1, 0) = _tmp1 * _tmp3; + _rhs(2, 0) = _tmp2 * _tmp3; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_square.h b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_square.h new file mode 100644 index 000000000..be1fc49b0 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_matrix31/prior_factor_matrix31_square.h @@ -0,0 +1,107 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorMatrix31(const Eigen::Matrix& value, + const Eigen::Matrix& prior, + const Eigen::Matrix& sqrt_info, 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: 63 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (6) + const Scalar _tmp0 = -prior(1, 0) + value(1, 0); + const Scalar _tmp1 = -prior(2, 0) + value(2, 0); + const Scalar _tmp2 = -prior(0, 0) + value(0, 0); + const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp2 * sqrt_info(0, 0); + const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp2 * sqrt_info(1, 0); + const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp2 * sqrt_info(2, 0); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp3; + _res(1, 0) = _tmp4; + _res(2, 0) = _tmp5; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = sqrt_info(0, 0); + _jacobian(1, 0) = sqrt_info(1, 0); + _jacobian(2, 0) = sqrt_info(2, 0); + _jacobian(0, 1) = sqrt_info(0, 1); + _jacobian(1, 1) = sqrt_info(1, 1); + _jacobian(2, 1) = sqrt_info(2, 1); + _jacobian(0, 2) = sqrt_info(0, 2); + _jacobian(1, 2) = sqrt_info(1, 2); + _jacobian(2, 2) = sqrt_info(2, 2); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(sqrt_info(0, 0), Scalar(2)) + std::pow(sqrt_info(1, 0), Scalar(2)) + + std::pow(sqrt_info(2, 0), Scalar(2)); + _hessian(1, 0) = sqrt_info(0, 0) * sqrt_info(0, 1) + sqrt_info(1, 0) * sqrt_info(1, 1) + + sqrt_info(2, 0) * sqrt_info(2, 1); + _hessian(2, 0) = sqrt_info(0, 0) * sqrt_info(0, 2) + sqrt_info(1, 0) * sqrt_info(1, 2) + + sqrt_info(2, 0) * sqrt_info(2, 2); + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + + std::pow(sqrt_info(2, 1), Scalar(2)); + _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + + sqrt_info(2, 1) * sqrt_info(2, 2); + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + + std::pow(sqrt_info(2, 2), Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp3 * sqrt_info(0, 0) + _tmp4 * sqrt_info(1, 0) + _tmp5 * sqrt_info(2, 0); + _rhs(1, 0) = _tmp3 * sqrt_info(0, 1) + _tmp4 * sqrt_info(1, 1) + _tmp5 * sqrt_info(2, 1); + _rhs(2, 0) = _tmp3 * sqrt_info(0, 2) + _tmp4 * sqrt_info(1, 2) + _tmp5 * sqrt_info(2, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose2.h b/gen/cpp/sym/factors/prior_factor_pose2.h index ecf877fc3..d9684375d 100644 --- a/gen/cpp/sym/factors/prior_factor_pose2.h +++ b/gen/cpp/sym/factors/prior_factor_pose2.h @@ -1,119 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x3) jacobian of res wrt arg value (3) - * hessian: (3x3) Gauss-Newton hessian for arg value (3) - * rhs: (3x1) Gauss-Newton rhs for arg value (3) - */ -template -void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& prior, - const Eigen::Matrix& sqrt_info, 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: 94 - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - const Eigen::Matrix& _prior = prior.Data(); - - // Intermediate terms (19) - const Scalar _tmp0 = -_prior[2] + _value[2]; - const Scalar _tmp1 = -_prior[3] + _value[3]; - const Scalar _tmp2 = _prior[0] * _value[1]; - const Scalar _tmp3 = _prior[1] * _value[0]; - const Scalar _tmp4 = _tmp2 - _tmp3; - const Scalar _tmp5 = _prior[0] * _value[0] + _prior[1] * _value[1]; - const Scalar _tmp6 = _tmp5 + epsilon * ((((_tmp5) > 0) - ((_tmp5) < 0)) + Scalar(0.5)); - const Scalar _tmp7 = std::atan2(_tmp4, _tmp6); - const Scalar _tmp8 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp7 * sqrt_info(0, 0); - const Scalar _tmp9 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp7 * sqrt_info(1, 0); - const Scalar _tmp10 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp7 * sqrt_info(2, 0); - const Scalar _tmp11 = std::pow(_tmp6, Scalar(2)); - const Scalar _tmp12 = _tmp5 / _tmp6 - _tmp4 * (-_tmp2 + _tmp3) / _tmp11; - const Scalar _tmp13 = _tmp11 + std::pow(_tmp4, Scalar(2)); - const Scalar _tmp14 = _tmp11 * _tmp12 / _tmp13; - const Scalar _tmp15 = _tmp14 * sqrt_info(0, 0); - const Scalar _tmp16 = _tmp14 * sqrt_info(1, 0); - const Scalar _tmp17 = _tmp14 * sqrt_info(2, 0); - const Scalar _tmp18 = - std::pow(_tmp12, Scalar(2)) * std::pow(_tmp6, Scalar(4)) / std::pow(_tmp13, Scalar(2)); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp8; - _res(1, 0) = _tmp9; - _res(2, 0) = _tmp10; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp15; - _jacobian(1, 0) = _tmp16; - _jacobian(2, 0) = _tmp17; - _jacobian(0, 1) = sqrt_info(0, 1); - _jacobian(1, 1) = sqrt_info(1, 1); - _jacobian(2, 1) = sqrt_info(2, 1); - _jacobian(0, 2) = sqrt_info(0, 2); - _jacobian(1, 2) = sqrt_info(1, 2); - _jacobian(2, 2) = sqrt_info(2, 2); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = _tmp18 * std::pow(sqrt_info(0, 0), Scalar(2)) + - _tmp18 * std::pow(sqrt_info(1, 0), Scalar(2)) + - _tmp18 * std::pow(sqrt_info(2, 0), Scalar(2)); - _hessian(1, 0) = _tmp15 * sqrt_info(0, 1) + _tmp16 * sqrt_info(1, 1) + _tmp17 * sqrt_info(2, 1); - _hessian(2, 0) = _tmp15 * sqrt_info(0, 2) + _tmp16 * sqrt_info(1, 2) + _tmp17 * sqrt_info(2, 2); - _hessian(0, 1) = 0; - _hessian(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + - std::pow(sqrt_info(2, 1), Scalar(2)); - _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + - sqrt_info(2, 1) * sqrt_info(2, 2); - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + - std::pow(sqrt_info(2, 2), Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp10 * _tmp17 + _tmp15 * _tmp8 + _tmp16 * _tmp9; - _rhs(1, 0) = _tmp10 * sqrt_info(2, 1) + _tmp8 * sqrt_info(0, 1) + _tmp9 * sqrt_info(1, 1); - _rhs(2, 0) = _tmp10 * sqrt_info(2, 2) + _tmp8 * sqrt_info(0, 2) + _tmp9 * sqrt_info(1, 2); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_pose2/prior_factor_pose2_diagonal.h" +#include "./prior_factor_pose2/prior_factor_pose2_isotropic.h" +#include "./prior_factor_pose2/prior_factor_pose2_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_diagonal.h b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_diagonal.h new file mode 100644 index 000000000..c0dddb103 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_diagonal.h @@ -0,0 +1,103 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& prior, + const Eigen::Matrix& sqrt_info, 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: 40 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (15) + const Scalar _tmp0 = _prior[0] * _value[1]; + const Scalar _tmp1 = _prior[1] * _value[0]; + const Scalar _tmp2 = _tmp0 - _tmp1; + const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); + const Scalar _tmp6 = -_prior[2] + _value[2]; + const Scalar _tmp7 = -_prior[3] + _value[3]; + const Scalar _tmp8 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp9 = -_tmp2 * (-_tmp0 + _tmp1) / _tmp8 + _tmp3 / _tmp4; + const Scalar _tmp10 = std::pow(_tmp2, Scalar(2)) + _tmp8; + const Scalar _tmp11 = _tmp8 * _tmp9 / _tmp10; + const Scalar _tmp12 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp13 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp14 = std::pow(sqrt_info(2, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp5 * sqrt_info(0, 0); + _res(1, 0) = _tmp6 * sqrt_info(1, 0); + _res(2, 0) = _tmp7 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = _tmp11 * sqrt_info(0, 0); + _jacobian(1, 1) = sqrt_info(1, 0); + _jacobian(2, 2) = sqrt_info(2, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp12 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp9, Scalar(2)) / + std::pow(_tmp10, Scalar(2)); + _hessian(1, 1) = _tmp13; + _hessian(2, 2) = _tmp14; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp11 * _tmp12 * _tmp5; + _rhs(1, 0) = _tmp13 * _tmp6; + _rhs(2, 0) = _tmp14 * _tmp7; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_isotropic.h b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_isotropic.h new file mode 100644 index 000000000..926cd5250 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_isotropic.h @@ -0,0 +1,101 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& prior, + const Scalar sqrt_info, 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: 38 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (13) + const Scalar _tmp0 = _prior[0] * _value[1]; + const Scalar _tmp1 = _prior[1] * _value[0]; + const Scalar _tmp2 = _tmp0 - _tmp1; + const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); + const Scalar _tmp6 = -_prior[2] + _value[2]; + const Scalar _tmp7 = -_prior[3] + _value[3]; + const Scalar _tmp8 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp9 = -_tmp2 * (-_tmp0 + _tmp1) / _tmp8 + _tmp3 / _tmp4; + const Scalar _tmp10 = std::pow(_tmp2, Scalar(2)) + _tmp8; + const Scalar _tmp11 = _tmp8 * _tmp9 / _tmp10; + const Scalar _tmp12 = std::pow(sqrt_info, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp5 * sqrt_info; + _res(1, 0) = _tmp6 * sqrt_info; + _res(2, 0) = _tmp7 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = _tmp11 * sqrt_info; + _jacobian(1, 1) = sqrt_info; + _jacobian(2, 2) = sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = _tmp12 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp9, Scalar(2)) / + std::pow(_tmp10, Scalar(2)); + _hessian(1, 1) = _tmp12; + _hessian(2, 2) = _tmp12; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp11 * _tmp12 * _tmp5; + _rhs(1, 0) = _tmp12 * _tmp6; + _rhs(2, 0) = _tmp12 * _tmp7; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_square.h b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_square.h new file mode 100644 index 000000000..dda9add69 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose2/prior_factor_pose2_square.h @@ -0,0 +1,120 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& prior, + const Eigen::Matrix& sqrt_info, 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: 94 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (19) + const Scalar _tmp0 = -_prior[2] + _value[2]; + const Scalar _tmp1 = -_prior[3] + _value[3]; + const Scalar _tmp2 = _prior[0] * _value[1]; + const Scalar _tmp3 = _prior[1] * _value[0]; + const Scalar _tmp4 = _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp6 = _tmp5 + epsilon * ((((_tmp5) > 0) - ((_tmp5) < 0)) + Scalar(0.5)); + const Scalar _tmp7 = std::atan2(_tmp4, _tmp6); + const Scalar _tmp8 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + _tmp7 * sqrt_info(0, 0); + const Scalar _tmp9 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2) + _tmp7 * sqrt_info(1, 0); + const Scalar _tmp10 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 2) + _tmp7 * sqrt_info(2, 0); + const Scalar _tmp11 = std::pow(_tmp6, Scalar(2)); + const Scalar _tmp12 = _tmp5 / _tmp6 - _tmp4 * (-_tmp2 + _tmp3) / _tmp11; + const Scalar _tmp13 = _tmp11 + std::pow(_tmp4, Scalar(2)); + const Scalar _tmp14 = _tmp11 * _tmp12 / _tmp13; + const Scalar _tmp15 = _tmp14 * sqrt_info(0, 0); + const Scalar _tmp16 = _tmp14 * sqrt_info(1, 0); + const Scalar _tmp17 = _tmp14 * sqrt_info(2, 0); + const Scalar _tmp18 = + std::pow(_tmp12, Scalar(2)) * std::pow(_tmp6, Scalar(4)) / std::pow(_tmp13, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp8; + _res(1, 0) = _tmp9; + _res(2, 0) = _tmp10; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp15; + _jacobian(1, 0) = _tmp16; + _jacobian(2, 0) = _tmp17; + _jacobian(0, 1) = sqrt_info(0, 1); + _jacobian(1, 1) = sqrt_info(1, 1); + _jacobian(2, 1) = sqrt_info(2, 1); + _jacobian(0, 2) = sqrt_info(0, 2); + _jacobian(1, 2) = sqrt_info(1, 2); + _jacobian(2, 2) = sqrt_info(2, 2); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = _tmp18 * std::pow(sqrt_info(0, 0), Scalar(2)) + + _tmp18 * std::pow(sqrt_info(1, 0), Scalar(2)) + + _tmp18 * std::pow(sqrt_info(2, 0), Scalar(2)); + _hessian(1, 0) = _tmp15 * sqrt_info(0, 1) + _tmp16 * sqrt_info(1, 1) + _tmp17 * sqrt_info(2, 1); + _hessian(2, 0) = _tmp15 * sqrt_info(0, 2) + _tmp16 * sqrt_info(1, 2) + _tmp17 * sqrt_info(2, 2); + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + + std::pow(sqrt_info(2, 1), Scalar(2)); + _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + + sqrt_info(2, 1) * sqrt_info(2, 2); + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + + std::pow(sqrt_info(2, 2), Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp10 * _tmp17 + _tmp15 * _tmp8 + _tmp16 * _tmp9; + _rhs(1, 0) = _tmp10 * sqrt_info(2, 1) + _tmp8 * sqrt_info(0, 1) + _tmp9 * sqrt_info(1, 1); + _rhs(2, 0) = _tmp10 * sqrt_info(2, 2) + _tmp8 * sqrt_info(0, 2) + _tmp9 * sqrt_info(1, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3.h b/gen/cpp/sym/factors/prior_factor_pose3.h index 6bc187c7e..c47c3b820 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3.h +++ b/gen/cpp/sym/factors/prior_factor_pose3.h @@ -1,392 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (6x6) jacobian of res wrt arg value (6) - * hessian: (6x6) Gauss-Newton hessian for arg value (6) - * rhs: (6x1) Gauss-Newton rhs for arg value (6) - */ -template -void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& prior, - const Eigen::Matrix& sqrt_info, 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: 812 - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - const Eigen::Matrix& _prior = prior.Data(); - - // Intermediate terms (127) - const Scalar _tmp0 = _prior[2] * _value[3]; - const Scalar _tmp1 = _prior[1] * _value[0]; - const Scalar _tmp2 = _prior[0] * _value[1]; - const Scalar _tmp3 = _prior[3] * _value[2]; - const Scalar _tmp4 = _prior[3] * _value[3]; - const Scalar _tmp5 = _prior[2] * _value[2]; - const Scalar _tmp6 = _prior[1] * _value[1]; - const Scalar _tmp7 = _prior[0] * _value[0]; - const Scalar _tmp8 = _tmp4 + _tmp5 + _tmp6 + _tmp7; - const Scalar _tmp9 = (((_tmp8) > 0) - ((_tmp8) < 0)); - const Scalar _tmp10 = 2 * std::min(0, _tmp9) + 1; - const Scalar _tmp11 = _tmp10 * (-_tmp0 + _tmp1 - _tmp2 + _tmp3); - const Scalar _tmp12 = std::fabs(_tmp8); - const Scalar _tmp13 = 1 - epsilon; - const Scalar _tmp14 = std::min(_tmp12, _tmp13); - const Scalar _tmp15 = std::acos(_tmp14); - const Scalar _tmp16 = 1 - std::pow(_tmp14, Scalar(2)); - const Scalar _tmp17 = 2 * _tmp15 / std::sqrt(_tmp16); - const Scalar _tmp18 = _tmp11 * _tmp17; - const Scalar _tmp19 = -_prior[6] + _value[6]; - const Scalar _tmp20 = -_prior[5] + _value[5]; - const Scalar _tmp21 = -_prior[4] + _value[4]; - const Scalar _tmp22 = _prior[2] * _value[0]; - const Scalar _tmp23 = _prior[1] * _value[3]; - const Scalar _tmp24 = _prior[0] * _value[2]; - const Scalar _tmp25 = _prior[3] * _value[1]; - const Scalar _tmp26 = -_tmp22 - _tmp23 + _tmp24 + _tmp25; - const Scalar _tmp27 = _tmp10 * _tmp17; - const Scalar _tmp28 = _tmp26 * _tmp27; - const Scalar _tmp29 = _prior[2] * _value[1]; - const Scalar _tmp30 = _prior[1] * _value[2]; - const Scalar _tmp31 = _prior[0] * _value[3]; - const Scalar _tmp32 = _prior[3] * _value[0]; - const Scalar _tmp33 = _tmp29 - _tmp30 - _tmp31 + _tmp32; - const Scalar _tmp34 = _tmp27 * _tmp33; - const Scalar _tmp35 = _tmp18 * sqrt_info(0, 2) + _tmp19 * sqrt_info(0, 5) + - _tmp20 * sqrt_info(0, 4) + _tmp21 * sqrt_info(0, 3) + - _tmp28 * sqrt_info(0, 1) + _tmp34 * sqrt_info(0, 0); - const Scalar _tmp36 = _tmp11 * sqrt_info(1, 2); - const Scalar _tmp37 = _tmp17 * _tmp36 + _tmp19 * sqrt_info(1, 5) + _tmp20 * sqrt_info(1, 4) + - _tmp21 * sqrt_info(1, 3) + _tmp28 * sqrt_info(1, 1) + - _tmp34 * sqrt_info(1, 0); - const Scalar _tmp38 = _tmp18 * sqrt_info(2, 2) + _tmp19 * sqrt_info(2, 5) + - _tmp20 * sqrt_info(2, 4) + _tmp21 * sqrt_info(2, 3) + - _tmp28 * sqrt_info(2, 1) + _tmp34 * sqrt_info(2, 0); - const Scalar _tmp39 = _tmp18 * sqrt_info(3, 2) + _tmp19 * sqrt_info(3, 5) + - _tmp20 * sqrt_info(3, 4) + _tmp21 * sqrt_info(3, 3) + - _tmp28 * sqrt_info(3, 1) + _tmp34 * sqrt_info(3, 0); - const Scalar _tmp40 = _tmp18 * sqrt_info(4, 2) + _tmp19 * sqrt_info(4, 5) + - _tmp20 * sqrt_info(4, 4) + _tmp21 * sqrt_info(4, 3) + - _tmp28 * sqrt_info(4, 1) + _tmp34 * sqrt_info(4, 0); - const Scalar _tmp41 = _tmp27 * sqrt_info(5, 1); - const Scalar _tmp42 = _tmp18 * sqrt_info(5, 2) + _tmp19 * sqrt_info(5, 5) + - _tmp20 * sqrt_info(5, 4) + _tmp21 * sqrt_info(5, 3) + _tmp26 * _tmp41 + - _tmp34 * sqrt_info(5, 0); - const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp29; - const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp30; - const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp31; - const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp32; - const Scalar _tmp47 = -_tmp43 + _tmp44 + _tmp45 - _tmp46; - const Scalar _tmp48 = _tmp9 * ((((-_tmp12 + _tmp13) > 0) - ((-_tmp12 + _tmp13) < 0)) + 1); - const Scalar _tmp49 = _tmp48 / _tmp16; - const Scalar _tmp50 = _tmp47 * _tmp49; - const Scalar _tmp51 = _tmp10 * _tmp50; - const Scalar _tmp52 = _tmp33 * _tmp51; - const Scalar _tmp53 = _tmp14 * _tmp15 * _tmp48 / (_tmp16 * std::sqrt(_tmp16)); - const Scalar _tmp54 = _tmp10 * _tmp53; - const Scalar _tmp55 = _tmp47 * _tmp54; - const Scalar _tmp56 = _tmp26 * _tmp55; - const Scalar _tmp57 = _tmp26 * _tmp51; - const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp0; - const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp1; - const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp2; - const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp3; - const Scalar _tmp62 = -_tmp58 + _tmp59 - _tmp60 + _tmp61; - const Scalar _tmp63 = _tmp27 * _tmp62; - const Scalar _tmp64 = _tmp11 * _tmp53; - const Scalar _tmp65 = _tmp47 * _tmp64; - const Scalar _tmp66 = _tmp11 * _tmp50; - const Scalar _tmp67 = _tmp33 * _tmp55; - const Scalar _tmp68 = - _tmp27 * ((Scalar(1) / Scalar(2)) * _tmp4 + (Scalar(1) / Scalar(2)) * _tmp5 + - (Scalar(1) / Scalar(2)) * _tmp6 + (Scalar(1) / Scalar(2)) * _tmp7); - const Scalar _tmp69 = (Scalar(1) / Scalar(2)) * _tmp22; - const Scalar _tmp70 = (Scalar(1) / Scalar(2)) * _tmp23; - const Scalar _tmp71 = (Scalar(1) / Scalar(2)) * _tmp24; - const Scalar _tmp72 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp73 = _tmp69 + _tmp70 - _tmp71 - _tmp72; - const Scalar _tmp74 = _tmp27 * _tmp73; - const Scalar _tmp75 = - -_tmp52 * sqrt_info(0, 0) + _tmp56 * sqrt_info(0, 1) - _tmp57 * sqrt_info(0, 1) + - _tmp63 * sqrt_info(0, 1) + _tmp65 * sqrt_info(0, 2) - _tmp66 * sqrt_info(0, 2) + - _tmp67 * sqrt_info(0, 0) + _tmp68 * sqrt_info(0, 0) + _tmp74 * sqrt_info(0, 2); - const Scalar _tmp76 = _tmp47 * _tmp53; - const Scalar _tmp77 = _tmp33 * sqrt_info(1, 0); - const Scalar _tmp78 = _tmp27 * sqrt_info(1, 2); - const Scalar _tmp79 = -_tmp36 * _tmp50 + _tmp36 * _tmp76 - _tmp52 * sqrt_info(1, 0) + - _tmp55 * _tmp77 + _tmp56 * sqrt_info(1, 1) - _tmp57 * sqrt_info(1, 1) + - _tmp63 * sqrt_info(1, 1) + _tmp68 * sqrt_info(1, 0) + _tmp73 * _tmp78; - const Scalar _tmp80 = _tmp11 * sqrt_info(2, 2); - const Scalar _tmp81 = -_tmp50 * _tmp80 - _tmp52 * sqrt_info(2, 0) + _tmp56 * sqrt_info(2, 1) - - _tmp57 * sqrt_info(2, 1) + _tmp63 * sqrt_info(2, 1) + - _tmp67 * sqrt_info(2, 0) + _tmp68 * sqrt_info(2, 0) + - _tmp74 * sqrt_info(2, 2) + _tmp76 * _tmp80; - const Scalar _tmp82 = _tmp33 * sqrt_info(3, 0); - const Scalar _tmp83 = -_tmp52 * sqrt_info(3, 0) + _tmp55 * _tmp82 + _tmp56 * sqrt_info(3, 1) - - _tmp57 * sqrt_info(3, 1) + _tmp63 * sqrt_info(3, 1) + - _tmp65 * sqrt_info(3, 2) - _tmp66 * sqrt_info(3, 2) + - _tmp68 * sqrt_info(3, 0) + _tmp74 * sqrt_info(3, 2); - const Scalar _tmp84 = _tmp26 * sqrt_info(4, 1); - const Scalar _tmp85 = _tmp64 * sqrt_info(4, 2); - const Scalar _tmp86 = _tmp47 * _tmp85 - _tmp51 * _tmp84 - _tmp52 * sqrt_info(4, 0) + - _tmp55 * _tmp84 + _tmp63 * sqrt_info(4, 1) - _tmp66 * sqrt_info(4, 2) + - _tmp67 * sqrt_info(4, 0) + _tmp68 * sqrt_info(4, 0) + - _tmp74 * sqrt_info(4, 2); - const Scalar _tmp87 = _tmp26 * sqrt_info(5, 1); - const Scalar _tmp88 = _tmp41 * _tmp62 - _tmp51 * _tmp87 - _tmp52 * sqrt_info(5, 0) + - _tmp55 * _tmp87 + _tmp65 * sqrt_info(5, 2) - _tmp66 * sqrt_info(5, 2) + - _tmp67 * sqrt_info(5, 0) + _tmp68 * sqrt_info(5, 0) + - _tmp74 * sqrt_info(5, 2); - const Scalar _tmp89 = _tmp49 * _tmp73; - const Scalar _tmp90 = _tmp11 * _tmp89; - const Scalar _tmp91 = _tmp58 - _tmp59 + _tmp60 - _tmp61; - const Scalar _tmp92 = _tmp27 * _tmp91; - const Scalar _tmp93 = _tmp10 * _tmp89; - const Scalar _tmp94 = _tmp26 * _tmp93; - const Scalar _tmp95 = _tmp54 * _tmp73; - const Scalar _tmp96 = _tmp33 * _tmp95; - const Scalar _tmp97 = _tmp64 * _tmp73; - const Scalar _tmp98 = _tmp33 * _tmp93; - const Scalar _tmp99 = _tmp43 - _tmp44 - _tmp45 + _tmp46; - const Scalar _tmp100 = _tmp27 * _tmp99; - const Scalar _tmp101 = _tmp26 * _tmp95; - const Scalar _tmp102 = - _tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 1) + _tmp68 * sqrt_info(0, 1) - - _tmp90 * sqrt_info(0, 2) + _tmp92 * sqrt_info(0, 0) - _tmp94 * sqrt_info(0, 1) + - _tmp96 * sqrt_info(0, 0) + _tmp97 * sqrt_info(0, 2) - _tmp98 * sqrt_info(0, 0); - const Scalar _tmp103 = _tmp53 * _tmp73; - const Scalar _tmp104 = _tmp101 * sqrt_info(1, 1) + _tmp103 * _tmp36 - _tmp36 * _tmp89 + - _tmp68 * sqrt_info(1, 1) + _tmp77 * _tmp95 + _tmp78 * _tmp99 + - _tmp92 * sqrt_info(1, 0) - _tmp94 * sqrt_info(1, 1) - - _tmp98 * sqrt_info(1, 0); - const Scalar _tmp105 = _tmp100 * sqrt_info(2, 2) + _tmp101 * sqrt_info(2, 1) + _tmp103 * _tmp80 + - _tmp68 * sqrt_info(2, 1) - _tmp80 * _tmp89 + _tmp92 * sqrt_info(2, 0) - - _tmp94 * sqrt_info(2, 1) + _tmp96 * sqrt_info(2, 0) - - _tmp98 * sqrt_info(2, 0); - const Scalar _tmp106 = _tmp100 * sqrt_info(3, 2) + _tmp101 * sqrt_info(3, 1) + - _tmp68 * sqrt_info(3, 1) + _tmp82 * _tmp95 - _tmp90 * sqrt_info(3, 2) + - _tmp92 * sqrt_info(3, 0) - _tmp94 * sqrt_info(3, 1) + - _tmp97 * sqrt_info(3, 2) - _tmp98 * sqrt_info(3, 0); - const Scalar _tmp107 = _tmp100 * sqrt_info(4, 2) + _tmp68 * sqrt_info(4, 1) + _tmp73 * _tmp85 - - _tmp84 * _tmp93 + _tmp84 * _tmp95 - _tmp90 * sqrt_info(4, 2) + - _tmp92 * sqrt_info(4, 0) + _tmp96 * sqrt_info(4, 0) - - _tmp98 * sqrt_info(4, 0); - const Scalar _tmp108 = _tmp100 * sqrt_info(5, 2) + _tmp68 * sqrt_info(5, 1) + _tmp87 * _tmp95 - - _tmp90 * sqrt_info(5, 2) + _tmp92 * sqrt_info(5, 0) - - _tmp94 * sqrt_info(5, 1) + _tmp96 * sqrt_info(5, 0) + - _tmp97 * sqrt_info(5, 2) - _tmp98 * sqrt_info(5, 0); - const Scalar _tmp109 = _tmp49 * _tmp91; - const Scalar _tmp110 = _tmp10 * _tmp109; - const Scalar _tmp111 = _tmp110 * _tmp26; - const Scalar _tmp112 = _tmp110 * _tmp33; - const Scalar _tmp113 = _tmp54 * _tmp91; - const Scalar _tmp114 = _tmp113 * _tmp26; - const Scalar _tmp115 = _tmp27 * (-_tmp69 - _tmp70 + _tmp71 + _tmp72); - const Scalar _tmp116 = _tmp113 * _tmp33; - const Scalar _tmp117 = _tmp64 * _tmp91; - const Scalar _tmp118 = _tmp109 * _tmp11; - const Scalar _tmp119 = _tmp27 * _tmp47; - const Scalar _tmp120 = - -_tmp111 * sqrt_info(0, 1) - _tmp112 * sqrt_info(0, 0) + _tmp114 * sqrt_info(0, 1) + - _tmp115 * sqrt_info(0, 0) + _tmp116 * sqrt_info(0, 0) + _tmp117 * sqrt_info(0, 2) - - _tmp118 * sqrt_info(0, 2) + _tmp119 * sqrt_info(0, 1) + _tmp68 * sqrt_info(0, 2); - const Scalar _tmp121 = _tmp53 * _tmp91; - const Scalar _tmp122 = -_tmp109 * _tmp36 - _tmp111 * sqrt_info(1, 1) - _tmp112 * sqrt_info(1, 0) + - _tmp113 * _tmp77 + _tmp114 * sqrt_info(1, 1) + _tmp115 * sqrt_info(1, 0) + - _tmp119 * sqrt_info(1, 1) + _tmp121 * _tmp36 + _tmp68 * sqrt_info(1, 2); - const Scalar _tmp123 = -_tmp109 * _tmp80 - _tmp111 * sqrt_info(2, 1) - _tmp112 * sqrt_info(2, 0) + - _tmp114 * sqrt_info(2, 1) + _tmp115 * sqrt_info(2, 0) + - _tmp116 * sqrt_info(2, 0) + _tmp119 * sqrt_info(2, 1) + _tmp121 * _tmp80 + - _tmp68 * sqrt_info(2, 2); - const Scalar _tmp124 = -_tmp111 * sqrt_info(3, 1) - _tmp112 * sqrt_info(3, 0) + _tmp113 * _tmp82 + - _tmp114 * sqrt_info(3, 1) + _tmp115 * sqrt_info(3, 0) + - _tmp117 * sqrt_info(3, 2) - _tmp118 * sqrt_info(3, 2) + - _tmp119 * sqrt_info(3, 1) + _tmp68 * sqrt_info(3, 2); - const Scalar _tmp125 = -_tmp110 * _tmp84 - _tmp112 * sqrt_info(4, 0) + _tmp113 * _tmp84 + - _tmp115 * sqrt_info(4, 0) + _tmp116 * sqrt_info(4, 0) - - _tmp118 * sqrt_info(4, 2) + _tmp119 * sqrt_info(4, 1) + - _tmp68 * sqrt_info(4, 2) + _tmp85 * _tmp91; - const Scalar _tmp126 = -_tmp110 * _tmp87 - _tmp112 * sqrt_info(5, 0) + _tmp113 * _tmp87 + - _tmp115 * sqrt_info(5, 0) + _tmp116 * sqrt_info(5, 0) + - _tmp117 * sqrt_info(5, 2) - _tmp118 * sqrt_info(5, 2) + - _tmp119 * sqrt_info(5, 1) + _tmp68 * sqrt_info(5, 2); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp35; - _res(1, 0) = _tmp37; - _res(2, 0) = _tmp38; - _res(3, 0) = _tmp39; - _res(4, 0) = _tmp40; - _res(5, 0) = _tmp42; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp75; - _jacobian(1, 0) = _tmp79; - _jacobian(2, 0) = _tmp81; - _jacobian(3, 0) = _tmp83; - _jacobian(4, 0) = _tmp86; - _jacobian(5, 0) = _tmp88; - _jacobian(0, 1) = _tmp102; - _jacobian(1, 1) = _tmp104; - _jacobian(2, 1) = _tmp105; - _jacobian(3, 1) = _tmp106; - _jacobian(4, 1) = _tmp107; - _jacobian(5, 1) = _tmp108; - _jacobian(0, 2) = _tmp120; - _jacobian(1, 2) = _tmp122; - _jacobian(2, 2) = _tmp123; - _jacobian(3, 2) = _tmp124; - _jacobian(4, 2) = _tmp125; - _jacobian(5, 2) = _tmp126; - _jacobian(0, 3) = sqrt_info(0, 3); - _jacobian(1, 3) = sqrt_info(1, 3); - _jacobian(2, 3) = sqrt_info(2, 3); - _jacobian(3, 3) = sqrt_info(3, 3); - _jacobian(4, 3) = sqrt_info(4, 3); - _jacobian(5, 3) = sqrt_info(5, 3); - _jacobian(0, 4) = sqrt_info(0, 4); - _jacobian(1, 4) = sqrt_info(1, 4); - _jacobian(2, 4) = sqrt_info(2, 4); - _jacobian(3, 4) = sqrt_info(3, 4); - _jacobian(4, 4) = sqrt_info(4, 4); - _jacobian(5, 4) = sqrt_info(5, 4); - _jacobian(0, 5) = sqrt_info(0, 5); - _jacobian(1, 5) = sqrt_info(1, 5); - _jacobian(2, 5) = sqrt_info(2, 5); - _jacobian(3, 5) = sqrt_info(3, 5); - _jacobian(4, 5) = sqrt_info(4, 5); - _jacobian(5, 5) = sqrt_info(5, 5); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = std::pow(_tmp75, Scalar(2)) + std::pow(_tmp79, Scalar(2)) + - std::pow(_tmp81, Scalar(2)) + std::pow(_tmp83, Scalar(2)) + - std::pow(_tmp86, Scalar(2)) + std::pow(_tmp88, Scalar(2)); - _hessian(1, 0) = _tmp102 * _tmp75 + _tmp104 * _tmp79 + _tmp105 * _tmp81 + _tmp106 * _tmp83 + - _tmp107 * _tmp86 + _tmp108 * _tmp88; - _hessian(2, 0) = _tmp120 * _tmp75 + _tmp122 * _tmp79 + _tmp123 * _tmp81 + _tmp124 * _tmp83 + - _tmp125 * _tmp86 + _tmp126 * _tmp88; - _hessian(3, 0) = _tmp75 * sqrt_info(0, 3) + _tmp79 * sqrt_info(1, 3) + - _tmp81 * sqrt_info(2, 3) + _tmp83 * sqrt_info(3, 3) + - _tmp86 * sqrt_info(4, 3) + _tmp88 * sqrt_info(5, 3); - _hessian(4, 0) = _tmp75 * sqrt_info(0, 4) + _tmp79 * sqrt_info(1, 4) + - _tmp81 * sqrt_info(2, 4) + _tmp83 * sqrt_info(3, 4) + - _tmp86 * sqrt_info(4, 4) + _tmp88 * sqrt_info(5, 4); - _hessian(5, 0) = _tmp75 * sqrt_info(0, 5) + _tmp79 * sqrt_info(1, 5) + - _tmp81 * sqrt_info(2, 5) + _tmp83 * sqrt_info(3, 5) + - _tmp86 * sqrt_info(4, 5) + _tmp88 * sqrt_info(5, 5); - _hessian(0, 1) = 0; - _hessian(1, 1) = std::pow(_tmp102, Scalar(2)) + std::pow(_tmp104, Scalar(2)) + - std::pow(_tmp105, Scalar(2)) + std::pow(_tmp106, Scalar(2)) + - std::pow(_tmp107, Scalar(2)) + std::pow(_tmp108, Scalar(2)); - _hessian(2, 1) = _tmp102 * _tmp120 + _tmp104 * _tmp122 + _tmp105 * _tmp123 + _tmp106 * _tmp124 + - _tmp107 * _tmp125 + _tmp108 * _tmp126; - _hessian(3, 1) = _tmp102 * sqrt_info(0, 3) + _tmp104 * sqrt_info(1, 3) + - _tmp105 * sqrt_info(2, 3) + _tmp106 * sqrt_info(3, 3) + - _tmp107 * sqrt_info(4, 3) + _tmp108 * sqrt_info(5, 3); - _hessian(4, 1) = _tmp102 * sqrt_info(0, 4) + _tmp104 * sqrt_info(1, 4) + - _tmp105 * sqrt_info(2, 4) + _tmp106 * sqrt_info(3, 4) + - _tmp107 * sqrt_info(4, 4) + _tmp108 * sqrt_info(5, 4); - _hessian(5, 1) = _tmp102 * sqrt_info(0, 5) + _tmp104 * sqrt_info(1, 5) + - _tmp105 * sqrt_info(2, 5) + _tmp106 * sqrt_info(3, 5) + - _tmp107 * sqrt_info(4, 5) + _tmp108 * sqrt_info(5, 5); - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = std::pow(_tmp120, Scalar(2)) + std::pow(_tmp122, Scalar(2)) + - std::pow(_tmp123, Scalar(2)) + std::pow(_tmp124, Scalar(2)) + - std::pow(_tmp125, Scalar(2)) + std::pow(_tmp126, Scalar(2)); - _hessian(3, 2) = _tmp120 * sqrt_info(0, 3) + _tmp122 * sqrt_info(1, 3) + - _tmp123 * sqrt_info(2, 3) + _tmp124 * sqrt_info(3, 3) + - _tmp125 * sqrt_info(4, 3) + _tmp126 * sqrt_info(5, 3); - _hessian(4, 2) = _tmp120 * sqrt_info(0, 4) + _tmp122 * sqrt_info(1, 4) + - _tmp123 * sqrt_info(2, 4) + _tmp124 * sqrt_info(3, 4) + - _tmp125 * sqrt_info(4, 4) + _tmp126 * sqrt_info(5, 4); - _hessian(5, 2) = _tmp120 * sqrt_info(0, 5) + _tmp122 * sqrt_info(1, 5) + - _tmp123 * sqrt_info(2, 5) + _tmp124 * sqrt_info(3, 5) + - _tmp125 * sqrt_info(4, 5) + _tmp126 * sqrt_info(5, 5); - _hessian(0, 3) = 0; - _hessian(1, 3) = 0; - _hessian(2, 3) = 0; - _hessian(3, 3) = std::pow(sqrt_info(0, 3), Scalar(2)) + std::pow(sqrt_info(1, 3), Scalar(2)) + - std::pow(sqrt_info(2, 3), Scalar(2)) + std::pow(sqrt_info(3, 3), Scalar(2)) + - std::pow(sqrt_info(4, 3), Scalar(2)) + std::pow(sqrt_info(5, 3), Scalar(2)); - _hessian(4, 3) = sqrt_info(0, 3) * sqrt_info(0, 4) + sqrt_info(1, 3) * sqrt_info(1, 4) + - sqrt_info(2, 3) * sqrt_info(2, 4) + sqrt_info(3, 3) * sqrt_info(3, 4) + - sqrt_info(4, 3) * sqrt_info(4, 4) + sqrt_info(5, 3) * sqrt_info(5, 4); - _hessian(5, 3) = sqrt_info(0, 3) * sqrt_info(0, 5) + sqrt_info(1, 3) * sqrt_info(1, 5) + - sqrt_info(2, 3) * sqrt_info(2, 5) + sqrt_info(3, 3) * sqrt_info(3, 5) + - sqrt_info(4, 3) * sqrt_info(4, 5) + sqrt_info(5, 3) * sqrt_info(5, 5); - _hessian(0, 4) = 0; - _hessian(1, 4) = 0; - _hessian(2, 4) = 0; - _hessian(3, 4) = 0; - _hessian(4, 4) = std::pow(sqrt_info(0, 4), Scalar(2)) + std::pow(sqrt_info(1, 4), Scalar(2)) + - std::pow(sqrt_info(2, 4), Scalar(2)) + std::pow(sqrt_info(3, 4), Scalar(2)) + - std::pow(sqrt_info(4, 4), Scalar(2)) + std::pow(sqrt_info(5, 4), Scalar(2)); - _hessian(5, 4) = sqrt_info(0, 4) * sqrt_info(0, 5) + sqrt_info(1, 4) * sqrt_info(1, 5) + - sqrt_info(2, 4) * sqrt_info(2, 5) + sqrt_info(3, 4) * sqrt_info(3, 5) + - sqrt_info(4, 4) * sqrt_info(4, 5) + sqrt_info(5, 4) * sqrt_info(5, 5); - _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(sqrt_info(0, 5), Scalar(2)) + std::pow(sqrt_info(1, 5), Scalar(2)) + - std::pow(sqrt_info(2, 5), Scalar(2)) + std::pow(sqrt_info(3, 5), Scalar(2)) + - std::pow(sqrt_info(4, 5), Scalar(2)) + std::pow(sqrt_info(5, 5), Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp35 * _tmp75 + _tmp37 * _tmp79 + _tmp38 * _tmp81 + _tmp39 * _tmp83 + - _tmp40 * _tmp86 + _tmp42 * _tmp88; - _rhs(1, 0) = _tmp102 * _tmp35 + _tmp104 * _tmp37 + _tmp105 * _tmp38 + _tmp106 * _tmp39 + - _tmp107 * _tmp40 + _tmp108 * _tmp42; - _rhs(2, 0) = _tmp120 * _tmp35 + _tmp122 * _tmp37 + _tmp123 * _tmp38 + _tmp124 * _tmp39 + - _tmp125 * _tmp40 + _tmp126 * _tmp42; - _rhs(3, 0) = _tmp35 * sqrt_info(0, 3) + _tmp37 * sqrt_info(1, 3) + _tmp38 * sqrt_info(2, 3) + - _tmp39 * sqrt_info(3, 3) + _tmp40 * sqrt_info(4, 3) + _tmp42 * sqrt_info(5, 3); - _rhs(4, 0) = _tmp35 * sqrt_info(0, 4) + _tmp37 * sqrt_info(1, 4) + _tmp38 * sqrt_info(2, 4) + - _tmp39 * sqrt_info(3, 4) + _tmp40 * sqrt_info(4, 4) + _tmp42 * sqrt_info(5, 4); - _rhs(5, 0) = _tmp35 * sqrt_info(0, 5) + _tmp37 * sqrt_info(1, 5) + _tmp38 * sqrt_info(2, 5) + - _tmp39 * sqrt_info(3, 5) + _tmp40 * sqrt_info(4, 5) + _tmp42 * sqrt_info(5, 5); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_pose3/prior_factor_pose3_diagonal.h" +#include "./prior_factor_pose3/prior_factor_pose3_isotropic.h" +#include "./prior_factor_pose3/prior_factor_pose3_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_diagonal.h b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_diagonal.h new file mode 100644 index 000000000..0534da9e1 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_diagonal.h @@ -0,0 +1,200 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& prior, + const Eigen::Matrix& sqrt_info, 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: 212 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (85) + const Scalar _tmp0 = _prior[2] * _value[1]; + const Scalar _tmp1 = _prior[1] * _value[2]; + const Scalar _tmp2 = _prior[0] * _value[3]; + const Scalar _tmp3 = _prior[3] * _value[0]; + const Scalar _tmp4 = _tmp0 - _tmp1 - _tmp2 + _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[2] * _value[2]; + const Scalar _tmp7 = _prior[1] * _value[1]; + const Scalar _tmp8 = _prior[0] * _value[0]; + const Scalar _tmp9 = _tmp5 + _tmp6 + _tmp7 + _tmp8; + const Scalar _tmp10 = std::fabs(_tmp9); + const Scalar _tmp11 = 1 - epsilon; + const Scalar _tmp12 = std::min(_tmp10, _tmp11); + const Scalar _tmp13 = std::acos(_tmp12); + const Scalar _tmp14 = (((_tmp9) > 0) - ((_tmp9) < 0)); + const Scalar _tmp15 = 2 * std::min(0, _tmp14) + 1; + const Scalar _tmp16 = _tmp15 * sqrt_info(0, 0); + const Scalar _tmp17 = _tmp13 * _tmp16; + const Scalar _tmp18 = 1 - std::pow(_tmp12, Scalar(2)); + const Scalar _tmp19 = 2 / std::sqrt(_tmp18); + const Scalar _tmp20 = _tmp17 * _tmp19; + const Scalar _tmp21 = _tmp20 * _tmp4; + const Scalar _tmp22 = _tmp13 * _tmp15; + const Scalar _tmp23 = _tmp19 * _tmp22; + const Scalar _tmp24 = _prior[2] * _value[0]; + const Scalar _tmp25 = _prior[1] * _value[3]; + const Scalar _tmp26 = _prior[0] * _value[2]; + const Scalar _tmp27 = _prior[3] * _value[1]; + const Scalar _tmp28 = sqrt_info(1, 0) * (-_tmp24 - _tmp25 + _tmp26 + _tmp27); + const Scalar _tmp29 = _tmp23 * _tmp28; + const Scalar _tmp30 = _prior[2] * _value[3]; + const Scalar _tmp31 = _prior[1] * _value[0]; + const Scalar _tmp32 = _prior[0] * _value[1]; + const Scalar _tmp33 = _prior[3] * _value[2]; + const Scalar _tmp34 = -_tmp30 + _tmp31 - _tmp32 + _tmp33; + const Scalar _tmp35 = _tmp23 * sqrt_info(2, 0); + const Scalar _tmp36 = _tmp34 * _tmp35; + const Scalar _tmp37 = -_prior[4] + _value[4]; + const Scalar _tmp38 = -_prior[5] + _value[5]; + const Scalar _tmp39 = -_prior[6] + _value[6]; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp44 = -_tmp40 + _tmp41 + _tmp42 - _tmp43; + const Scalar _tmp45 = _tmp14 * ((((-_tmp10 + _tmp11) > 0) - ((-_tmp10 + _tmp11) < 0)) + 1); + const Scalar _tmp46 = _tmp12 * _tmp45 / (_tmp18 * std::sqrt(_tmp18)); + const Scalar _tmp47 = _tmp17 * _tmp4 * _tmp46; + const Scalar _tmp48 = _tmp16 * _tmp4; + const Scalar _tmp49 = _tmp45 / _tmp18; + const Scalar _tmp50 = _tmp44 * _tmp49; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp52 = _tmp20 * _tmp51 + _tmp44 * _tmp47 - _tmp48 * _tmp50; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp30; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp31; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp32; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp57 = _tmp23 * sqrt_info(1, 0); + const Scalar _tmp58 = _tmp15 * _tmp28; + const Scalar _tmp59 = _tmp22 * _tmp46; + const Scalar _tmp60 = _tmp44 * _tmp59; + const Scalar _tmp61 = + _tmp28 * _tmp60 - _tmp50 * _tmp58 + _tmp57 * (-_tmp53 + _tmp54 - _tmp55 + _tmp56); + const Scalar _tmp62 = _tmp34 * sqrt_info(2, 0); + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp66 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp67 = _tmp63 + _tmp64 - _tmp65 - _tmp66; + const Scalar _tmp68 = _tmp15 * _tmp62; + const Scalar _tmp69 = _tmp35 * _tmp67 - _tmp50 * _tmp68 + _tmp60 * _tmp62; + const Scalar _tmp70 = _tmp49 * _tmp67; + const Scalar _tmp71 = _tmp53 - _tmp54 + _tmp55 - _tmp56; + const Scalar _tmp72 = _tmp20 * _tmp71 + _tmp47 * _tmp67 - _tmp48 * _tmp70; + const Scalar _tmp73 = _tmp59 * _tmp67; + const Scalar _tmp74 = _tmp28 * _tmp73 + _tmp51 * _tmp57 - _tmp58 * _tmp70; + const Scalar _tmp75 = + _tmp35 * (_tmp40 - _tmp41 - _tmp42 + _tmp43) + _tmp62 * _tmp73 - _tmp68 * _tmp70; + const Scalar _tmp76 = _tmp49 * _tmp71; + const Scalar _tmp77 = + _tmp20 * (-_tmp63 - _tmp64 + _tmp65 + _tmp66) + _tmp47 * _tmp71 - _tmp48 * _tmp76; + const Scalar _tmp78 = _tmp59 * _tmp71; + const Scalar _tmp79 = _tmp15 * _tmp76; + const Scalar _tmp80 = _tmp28 * _tmp78 - _tmp28 * _tmp79 + _tmp44 * _tmp57; + const Scalar _tmp81 = _tmp35 * _tmp51 + _tmp62 * _tmp78 - _tmp62 * _tmp79; + const Scalar _tmp82 = std::pow(sqrt_info(3, 0), Scalar(2)); + const Scalar _tmp83 = std::pow(sqrt_info(4, 0), Scalar(2)); + const Scalar _tmp84 = std::pow(sqrt_info(5, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp21; + _res(1, 0) = _tmp29; + _res(2, 0) = _tmp36; + _res(3, 0) = _tmp37 * sqrt_info(3, 0); + _res(4, 0) = _tmp38 * sqrt_info(4, 0); + _res(5, 0) = _tmp39 * sqrt_info(5, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = _tmp52; + _jacobian(1, 0) = _tmp61; + _jacobian(2, 0) = _tmp69; + _jacobian(0, 1) = _tmp72; + _jacobian(1, 1) = _tmp74; + _jacobian(2, 1) = _tmp75; + _jacobian(0, 2) = _tmp77; + _jacobian(1, 2) = _tmp80; + _jacobian(2, 2) = _tmp81; + _jacobian(3, 3) = sqrt_info(3, 0); + _jacobian(4, 4) = sqrt_info(4, 0); + _jacobian(5, 5) = sqrt_info(5, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp52, Scalar(2)) + std::pow(_tmp61, Scalar(2)) + std::pow(_tmp69, Scalar(2)); + _hessian(1, 0) = _tmp52 * _tmp72 + _tmp61 * _tmp74 + _tmp69 * _tmp75; + _hessian(2, 0) = _tmp52 * _tmp77 + _tmp61 * _tmp80 + _tmp69 * _tmp81; + _hessian(1, 1) = + std::pow(_tmp72, Scalar(2)) + std::pow(_tmp74, Scalar(2)) + std::pow(_tmp75, Scalar(2)); + _hessian(2, 1) = _tmp72 * _tmp77 + _tmp74 * _tmp80 + _tmp75 * _tmp81; + _hessian(2, 2) = + std::pow(_tmp77, Scalar(2)) + std::pow(_tmp80, Scalar(2)) + std::pow(_tmp81, Scalar(2)); + _hessian(3, 3) = _tmp82; + _hessian(4, 4) = _tmp83; + _hessian(5, 5) = _tmp84; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp21 * _tmp52 + _tmp29 * _tmp61 + _tmp36 * _tmp69; + _rhs(1, 0) = _tmp21 * _tmp72 + _tmp29 * _tmp74 + _tmp36 * _tmp75; + _rhs(2, 0) = _tmp21 * _tmp77 + _tmp29 * _tmp80 + _tmp36 * _tmp81; + _rhs(3, 0) = _tmp37 * _tmp82; + _rhs(4, 0) = _tmp38 * _tmp83; + _rhs(5, 0) = _tmp39 * _tmp84; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_isotropic.h b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_isotropic.h new file mode 100644 index 000000000..7df35a5f5 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_isotropic.h @@ -0,0 +1,190 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& prior, + const Scalar sqrt_info, 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: 200 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (74) + const Scalar _tmp0 = _prior[2] * _value[1]; + const Scalar _tmp1 = _prior[1] * _value[2]; + const Scalar _tmp2 = _prior[0] * _value[3]; + const Scalar _tmp3 = _prior[3] * _value[0]; + const Scalar _tmp4 = _tmp0 - _tmp1 - _tmp2 + _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[2] * _value[2]; + const Scalar _tmp7 = _prior[1] * _value[1]; + const Scalar _tmp8 = _prior[0] * _value[0]; + const Scalar _tmp9 = _tmp5 + _tmp6 + _tmp7 + _tmp8; + const Scalar _tmp10 = std::fabs(_tmp9); + const Scalar _tmp11 = 1 - epsilon; + const Scalar _tmp12 = std::min(_tmp10, _tmp11); + const Scalar _tmp13 = 1 - std::pow(_tmp12, Scalar(2)); + const Scalar _tmp14 = (((_tmp9) > 0) - ((_tmp9) < 0)); + const Scalar _tmp15 = sqrt_info * (2 * std::min(0, _tmp14) + 1); + const Scalar _tmp16 = _tmp15 * std::acos(_tmp12); + const Scalar _tmp17 = 2 * _tmp16 / std::sqrt(_tmp13); + const Scalar _tmp18 = _tmp17 * _tmp4; + const Scalar _tmp19 = _prior[2] * _value[0]; + const Scalar _tmp20 = _prior[1] * _value[3]; + const Scalar _tmp21 = _prior[0] * _value[2]; + const Scalar _tmp22 = _prior[3] * _value[1]; + const Scalar _tmp23 = -_tmp19 - _tmp20 + _tmp21 + _tmp22; + const Scalar _tmp24 = _tmp17 * _tmp23; + const Scalar _tmp25 = _prior[2] * _value[3]; + const Scalar _tmp26 = _prior[1] * _value[0]; + const Scalar _tmp27 = _prior[0] * _value[1]; + const Scalar _tmp28 = _prior[3] * _value[2]; + const Scalar _tmp29 = -_tmp25 + _tmp26 - _tmp27 + _tmp28; + const Scalar _tmp30 = _tmp17 * _tmp29; + const Scalar _tmp31 = -_prior[4] + _value[4]; + const Scalar _tmp32 = -_prior[5] + _value[5]; + const Scalar _tmp33 = -_prior[6] + _value[6]; + const Scalar _tmp34 = + _tmp17 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp39 = -_tmp35 + _tmp36 + _tmp37 - _tmp38; + const Scalar _tmp40 = _tmp14 * ((((-_tmp10 + _tmp11) > 0) - ((-_tmp10 + _tmp11) < 0)) + 1); + const Scalar _tmp41 = _tmp12 * _tmp16 / (_tmp13 * std::sqrt(_tmp13)); + const Scalar _tmp42 = _tmp40 * _tmp41; + const Scalar _tmp43 = _tmp4 * _tmp42; + const Scalar _tmp44 = _tmp15 / _tmp13; + const Scalar _tmp45 = _tmp40 * _tmp44; + const Scalar _tmp46 = _tmp4 * _tmp45; + const Scalar _tmp47 = _tmp34 + _tmp39 * _tmp43 - _tmp39 * _tmp46; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp52 = _tmp23 * _tmp40; + const Scalar _tmp53 = _tmp39 * _tmp52; + const Scalar _tmp54 = + _tmp17 * (-_tmp48 + _tmp49 - _tmp50 + _tmp51) + _tmp41 * _tmp53 - _tmp44 * _tmp53; + const Scalar _tmp55 = _tmp29 * _tmp42; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp60 = _tmp56 + _tmp57 - _tmp58 - _tmp59; + const Scalar _tmp61 = _tmp29 * _tmp45; + const Scalar _tmp62 = _tmp17 * _tmp60 + _tmp39 * _tmp55 - _tmp39 * _tmp61; + const Scalar _tmp63 = _tmp45 * _tmp60; + const Scalar _tmp64 = _tmp48 - _tmp49 + _tmp50 - _tmp51; + const Scalar _tmp65 = _tmp17 * _tmp64 - _tmp4 * _tmp63 + _tmp43 * _tmp60; + const Scalar _tmp66 = _tmp41 * _tmp52; + const Scalar _tmp67 = _tmp44 * _tmp52; + const Scalar _tmp68 = _tmp34 + _tmp60 * _tmp66 - _tmp60 * _tmp67; + const Scalar _tmp69 = + _tmp17 * (_tmp35 - _tmp36 - _tmp37 + _tmp38) - _tmp29 * _tmp63 + _tmp55 * _tmp60; + const Scalar _tmp70 = + _tmp17 * (-_tmp56 - _tmp57 + _tmp58 + _tmp59) + _tmp43 * _tmp64 - _tmp46 * _tmp64; + const Scalar _tmp71 = _tmp17 * _tmp39 + _tmp64 * _tmp66 - _tmp64 * _tmp67; + const Scalar _tmp72 = _tmp34 + _tmp55 * _tmp64 - _tmp61 * _tmp64; + const Scalar _tmp73 = std::pow(sqrt_info, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp18; + _res(1, 0) = _tmp24; + _res(2, 0) = _tmp30; + _res(3, 0) = _tmp31 * sqrt_info; + _res(4, 0) = _tmp32 * sqrt_info; + _res(5, 0) = _tmp33 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 0) = _tmp47; + _jacobian(1, 0) = _tmp54; + _jacobian(2, 0) = _tmp62; + _jacobian(0, 1) = _tmp65; + _jacobian(1, 1) = _tmp68; + _jacobian(2, 1) = _tmp69; + _jacobian(0, 2) = _tmp70; + _jacobian(1, 2) = _tmp71; + _jacobian(2, 2) = _tmp72; + _jacobian(3, 3) = sqrt_info; + _jacobian(4, 4) = sqrt_info; + _jacobian(5, 5) = sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp47, Scalar(2)) + std::pow(_tmp54, Scalar(2)) + std::pow(_tmp62, Scalar(2)); + _hessian(1, 0) = _tmp47 * _tmp65 + _tmp54 * _tmp68 + _tmp62 * _tmp69; + _hessian(2, 0) = _tmp47 * _tmp70 + _tmp54 * _tmp71 + _tmp62 * _tmp72; + _hessian(1, 1) = + std::pow(_tmp65, Scalar(2)) + std::pow(_tmp68, Scalar(2)) + std::pow(_tmp69, Scalar(2)); + _hessian(2, 1) = _tmp65 * _tmp70 + _tmp68 * _tmp71 + _tmp69 * _tmp72; + _hessian(2, 2) = + std::pow(_tmp70, Scalar(2)) + std::pow(_tmp71, Scalar(2)) + std::pow(_tmp72, Scalar(2)); + _hessian(3, 3) = _tmp73; + _hessian(4, 4) = _tmp73; + _hessian(5, 5) = _tmp73; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp18 * _tmp47 + _tmp24 * _tmp54 + _tmp30 * _tmp62; + _rhs(1, 0) = _tmp18 * _tmp65 + _tmp24 * _tmp68 + _tmp30 * _tmp69; + _rhs(2, 0) = _tmp18 * _tmp70 + _tmp24 * _tmp71 + _tmp30 * _tmp72; + _rhs(3, 0) = _tmp31 * _tmp73; + _rhs(4, 0) = _tmp32 * _tmp73; + _rhs(5, 0) = _tmp33 * _tmp73; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_square.h b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_square.h new file mode 100644 index 000000000..d6c8942a2 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3/prior_factor_pose3_square.h @@ -0,0 +1,393 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (6x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& prior, + const Eigen::Matrix& sqrt_info, 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: 812 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (127) + const Scalar _tmp0 = _prior[2] * _value[3]; + const Scalar _tmp1 = _prior[1] * _value[0]; + const Scalar _tmp2 = _prior[0] * _value[1]; + const Scalar _tmp3 = _prior[3] * _value[2]; + const Scalar _tmp4 = _prior[3] * _value[3]; + const Scalar _tmp5 = _prior[2] * _value[2]; + const Scalar _tmp6 = _prior[1] * _value[1]; + const Scalar _tmp7 = _prior[0] * _value[0]; + const Scalar _tmp8 = _tmp4 + _tmp5 + _tmp6 + _tmp7; + const Scalar _tmp9 = (((_tmp8) > 0) - ((_tmp8) < 0)); + const Scalar _tmp10 = 2 * std::min(0, _tmp9) + 1; + const Scalar _tmp11 = _tmp10 * (-_tmp0 + _tmp1 - _tmp2 + _tmp3); + const Scalar _tmp12 = std::fabs(_tmp8); + const Scalar _tmp13 = 1 - epsilon; + const Scalar _tmp14 = std::min(_tmp12, _tmp13); + const Scalar _tmp15 = std::acos(_tmp14); + const Scalar _tmp16 = 1 - std::pow(_tmp14, Scalar(2)); + const Scalar _tmp17 = 2 * _tmp15 / std::sqrt(_tmp16); + const Scalar _tmp18 = _tmp11 * _tmp17; + const Scalar _tmp19 = -_prior[6] + _value[6]; + const Scalar _tmp20 = -_prior[5] + _value[5]; + const Scalar _tmp21 = -_prior[4] + _value[4]; + const Scalar _tmp22 = _prior[2] * _value[0]; + const Scalar _tmp23 = _prior[1] * _value[3]; + const Scalar _tmp24 = _prior[0] * _value[2]; + const Scalar _tmp25 = _prior[3] * _value[1]; + const Scalar _tmp26 = -_tmp22 - _tmp23 + _tmp24 + _tmp25; + const Scalar _tmp27 = _tmp10 * _tmp17; + const Scalar _tmp28 = _tmp26 * _tmp27; + const Scalar _tmp29 = _prior[2] * _value[1]; + const Scalar _tmp30 = _prior[1] * _value[2]; + const Scalar _tmp31 = _prior[0] * _value[3]; + const Scalar _tmp32 = _prior[3] * _value[0]; + const Scalar _tmp33 = _tmp29 - _tmp30 - _tmp31 + _tmp32; + const Scalar _tmp34 = _tmp27 * _tmp33; + const Scalar _tmp35 = _tmp18 * sqrt_info(0, 2) + _tmp19 * sqrt_info(0, 5) + + _tmp20 * sqrt_info(0, 4) + _tmp21 * sqrt_info(0, 3) + + _tmp28 * sqrt_info(0, 1) + _tmp34 * sqrt_info(0, 0); + const Scalar _tmp36 = _tmp11 * sqrt_info(1, 2); + const Scalar _tmp37 = _tmp17 * _tmp36 + _tmp19 * sqrt_info(1, 5) + _tmp20 * sqrt_info(1, 4) + + _tmp21 * sqrt_info(1, 3) + _tmp28 * sqrt_info(1, 1) + + _tmp34 * sqrt_info(1, 0); + const Scalar _tmp38 = _tmp18 * sqrt_info(2, 2) + _tmp19 * sqrt_info(2, 5) + + _tmp20 * sqrt_info(2, 4) + _tmp21 * sqrt_info(2, 3) + + _tmp28 * sqrt_info(2, 1) + _tmp34 * sqrt_info(2, 0); + const Scalar _tmp39 = _tmp18 * sqrt_info(3, 2) + _tmp19 * sqrt_info(3, 5) + + _tmp20 * sqrt_info(3, 4) + _tmp21 * sqrt_info(3, 3) + + _tmp28 * sqrt_info(3, 1) + _tmp34 * sqrt_info(3, 0); + const Scalar _tmp40 = _tmp18 * sqrt_info(4, 2) + _tmp19 * sqrt_info(4, 5) + + _tmp20 * sqrt_info(4, 4) + _tmp21 * sqrt_info(4, 3) + + _tmp28 * sqrt_info(4, 1) + _tmp34 * sqrt_info(4, 0); + const Scalar _tmp41 = _tmp27 * sqrt_info(5, 1); + const Scalar _tmp42 = _tmp18 * sqrt_info(5, 2) + _tmp19 * sqrt_info(5, 5) + + _tmp20 * sqrt_info(5, 4) + _tmp21 * sqrt_info(5, 3) + _tmp26 * _tmp41 + + _tmp34 * sqrt_info(5, 0); + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp29; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp30; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp31; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp32; + const Scalar _tmp47 = -_tmp43 + _tmp44 + _tmp45 - _tmp46; + const Scalar _tmp48 = _tmp9 * ((((-_tmp12 + _tmp13) > 0) - ((-_tmp12 + _tmp13) < 0)) + 1); + const Scalar _tmp49 = _tmp48 / _tmp16; + const Scalar _tmp50 = _tmp47 * _tmp49; + const Scalar _tmp51 = _tmp10 * _tmp50; + const Scalar _tmp52 = _tmp33 * _tmp51; + const Scalar _tmp53 = _tmp14 * _tmp15 * _tmp48 / (_tmp16 * std::sqrt(_tmp16)); + const Scalar _tmp54 = _tmp10 * _tmp53; + const Scalar _tmp55 = _tmp47 * _tmp54; + const Scalar _tmp56 = _tmp26 * _tmp55; + const Scalar _tmp57 = _tmp26 * _tmp51; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp62 = -_tmp58 + _tmp59 - _tmp60 + _tmp61; + const Scalar _tmp63 = _tmp27 * _tmp62; + const Scalar _tmp64 = _tmp11 * _tmp53; + const Scalar _tmp65 = _tmp47 * _tmp64; + const Scalar _tmp66 = _tmp11 * _tmp50; + const Scalar _tmp67 = _tmp33 * _tmp55; + const Scalar _tmp68 = + _tmp27 * ((Scalar(1) / Scalar(2)) * _tmp4 + (Scalar(1) / Scalar(2)) * _tmp5 + + (Scalar(1) / Scalar(2)) * _tmp6 + (Scalar(1) / Scalar(2)) * _tmp7); + const Scalar _tmp69 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp70 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp71 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp72 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp73 = _tmp69 + _tmp70 - _tmp71 - _tmp72; + const Scalar _tmp74 = _tmp27 * _tmp73; + const Scalar _tmp75 = + -_tmp52 * sqrt_info(0, 0) + _tmp56 * sqrt_info(0, 1) - _tmp57 * sqrt_info(0, 1) + + _tmp63 * sqrt_info(0, 1) + _tmp65 * sqrt_info(0, 2) - _tmp66 * sqrt_info(0, 2) + + _tmp67 * sqrt_info(0, 0) + _tmp68 * sqrt_info(0, 0) + _tmp74 * sqrt_info(0, 2); + const Scalar _tmp76 = _tmp47 * _tmp53; + const Scalar _tmp77 = _tmp33 * sqrt_info(1, 0); + const Scalar _tmp78 = _tmp27 * sqrt_info(1, 2); + const Scalar _tmp79 = -_tmp36 * _tmp50 + _tmp36 * _tmp76 - _tmp52 * sqrt_info(1, 0) + + _tmp55 * _tmp77 + _tmp56 * sqrt_info(1, 1) - _tmp57 * sqrt_info(1, 1) + + _tmp63 * sqrt_info(1, 1) + _tmp68 * sqrt_info(1, 0) + _tmp73 * _tmp78; + const Scalar _tmp80 = _tmp11 * sqrt_info(2, 2); + const Scalar _tmp81 = -_tmp50 * _tmp80 - _tmp52 * sqrt_info(2, 0) + _tmp56 * sqrt_info(2, 1) - + _tmp57 * sqrt_info(2, 1) + _tmp63 * sqrt_info(2, 1) + + _tmp67 * sqrt_info(2, 0) + _tmp68 * sqrt_info(2, 0) + + _tmp74 * sqrt_info(2, 2) + _tmp76 * _tmp80; + const Scalar _tmp82 = _tmp33 * sqrt_info(3, 0); + const Scalar _tmp83 = -_tmp52 * sqrt_info(3, 0) + _tmp55 * _tmp82 + _tmp56 * sqrt_info(3, 1) - + _tmp57 * sqrt_info(3, 1) + _tmp63 * sqrt_info(3, 1) + + _tmp65 * sqrt_info(3, 2) - _tmp66 * sqrt_info(3, 2) + + _tmp68 * sqrt_info(3, 0) + _tmp74 * sqrt_info(3, 2); + const Scalar _tmp84 = _tmp26 * sqrt_info(4, 1); + const Scalar _tmp85 = _tmp64 * sqrt_info(4, 2); + const Scalar _tmp86 = _tmp47 * _tmp85 - _tmp51 * _tmp84 - _tmp52 * sqrt_info(4, 0) + + _tmp55 * _tmp84 + _tmp63 * sqrt_info(4, 1) - _tmp66 * sqrt_info(4, 2) + + _tmp67 * sqrt_info(4, 0) + _tmp68 * sqrt_info(4, 0) + + _tmp74 * sqrt_info(4, 2); + const Scalar _tmp87 = _tmp26 * sqrt_info(5, 1); + const Scalar _tmp88 = _tmp41 * _tmp62 - _tmp51 * _tmp87 - _tmp52 * sqrt_info(5, 0) + + _tmp55 * _tmp87 + _tmp65 * sqrt_info(5, 2) - _tmp66 * sqrt_info(5, 2) + + _tmp67 * sqrt_info(5, 0) + _tmp68 * sqrt_info(5, 0) + + _tmp74 * sqrt_info(5, 2); + const Scalar _tmp89 = _tmp49 * _tmp73; + const Scalar _tmp90 = _tmp11 * _tmp89; + const Scalar _tmp91 = _tmp58 - _tmp59 + _tmp60 - _tmp61; + const Scalar _tmp92 = _tmp27 * _tmp91; + const Scalar _tmp93 = _tmp10 * _tmp89; + const Scalar _tmp94 = _tmp26 * _tmp93; + const Scalar _tmp95 = _tmp54 * _tmp73; + const Scalar _tmp96 = _tmp33 * _tmp95; + const Scalar _tmp97 = _tmp64 * _tmp73; + const Scalar _tmp98 = _tmp33 * _tmp93; + const Scalar _tmp99 = _tmp43 - _tmp44 - _tmp45 + _tmp46; + const Scalar _tmp100 = _tmp27 * _tmp99; + const Scalar _tmp101 = _tmp26 * _tmp95; + const Scalar _tmp102 = + _tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 1) + _tmp68 * sqrt_info(0, 1) - + _tmp90 * sqrt_info(0, 2) + _tmp92 * sqrt_info(0, 0) - _tmp94 * sqrt_info(0, 1) + + _tmp96 * sqrt_info(0, 0) + _tmp97 * sqrt_info(0, 2) - _tmp98 * sqrt_info(0, 0); + const Scalar _tmp103 = _tmp53 * _tmp73; + const Scalar _tmp104 = _tmp101 * sqrt_info(1, 1) + _tmp103 * _tmp36 - _tmp36 * _tmp89 + + _tmp68 * sqrt_info(1, 1) + _tmp77 * _tmp95 + _tmp78 * _tmp99 + + _tmp92 * sqrt_info(1, 0) - _tmp94 * sqrt_info(1, 1) - + _tmp98 * sqrt_info(1, 0); + const Scalar _tmp105 = _tmp100 * sqrt_info(2, 2) + _tmp101 * sqrt_info(2, 1) + _tmp103 * _tmp80 + + _tmp68 * sqrt_info(2, 1) - _tmp80 * _tmp89 + _tmp92 * sqrt_info(2, 0) - + _tmp94 * sqrt_info(2, 1) + _tmp96 * sqrt_info(2, 0) - + _tmp98 * sqrt_info(2, 0); + const Scalar _tmp106 = _tmp100 * sqrt_info(3, 2) + _tmp101 * sqrt_info(3, 1) + + _tmp68 * sqrt_info(3, 1) + _tmp82 * _tmp95 - _tmp90 * sqrt_info(3, 2) + + _tmp92 * sqrt_info(3, 0) - _tmp94 * sqrt_info(3, 1) + + _tmp97 * sqrt_info(3, 2) - _tmp98 * sqrt_info(3, 0); + const Scalar _tmp107 = _tmp100 * sqrt_info(4, 2) + _tmp68 * sqrt_info(4, 1) + _tmp73 * _tmp85 - + _tmp84 * _tmp93 + _tmp84 * _tmp95 - _tmp90 * sqrt_info(4, 2) + + _tmp92 * sqrt_info(4, 0) + _tmp96 * sqrt_info(4, 0) - + _tmp98 * sqrt_info(4, 0); + const Scalar _tmp108 = _tmp100 * sqrt_info(5, 2) + _tmp68 * sqrt_info(5, 1) + _tmp87 * _tmp95 - + _tmp90 * sqrt_info(5, 2) + _tmp92 * sqrt_info(5, 0) - + _tmp94 * sqrt_info(5, 1) + _tmp96 * sqrt_info(5, 0) + + _tmp97 * sqrt_info(5, 2) - _tmp98 * sqrt_info(5, 0); + const Scalar _tmp109 = _tmp49 * _tmp91; + const Scalar _tmp110 = _tmp10 * _tmp109; + const Scalar _tmp111 = _tmp110 * _tmp26; + const Scalar _tmp112 = _tmp110 * _tmp33; + const Scalar _tmp113 = _tmp54 * _tmp91; + const Scalar _tmp114 = _tmp113 * _tmp26; + const Scalar _tmp115 = _tmp27 * (-_tmp69 - _tmp70 + _tmp71 + _tmp72); + const Scalar _tmp116 = _tmp113 * _tmp33; + const Scalar _tmp117 = _tmp64 * _tmp91; + const Scalar _tmp118 = _tmp109 * _tmp11; + const Scalar _tmp119 = _tmp27 * _tmp47; + const Scalar _tmp120 = + -_tmp111 * sqrt_info(0, 1) - _tmp112 * sqrt_info(0, 0) + _tmp114 * sqrt_info(0, 1) + + _tmp115 * sqrt_info(0, 0) + _tmp116 * sqrt_info(0, 0) + _tmp117 * sqrt_info(0, 2) - + _tmp118 * sqrt_info(0, 2) + _tmp119 * sqrt_info(0, 1) + _tmp68 * sqrt_info(0, 2); + const Scalar _tmp121 = _tmp53 * _tmp91; + const Scalar _tmp122 = -_tmp109 * _tmp36 - _tmp111 * sqrt_info(1, 1) - _tmp112 * sqrt_info(1, 0) + + _tmp113 * _tmp77 + _tmp114 * sqrt_info(1, 1) + _tmp115 * sqrt_info(1, 0) + + _tmp119 * sqrt_info(1, 1) + _tmp121 * _tmp36 + _tmp68 * sqrt_info(1, 2); + const Scalar _tmp123 = -_tmp109 * _tmp80 - _tmp111 * sqrt_info(2, 1) - _tmp112 * sqrt_info(2, 0) + + _tmp114 * sqrt_info(2, 1) + _tmp115 * sqrt_info(2, 0) + + _tmp116 * sqrt_info(2, 0) + _tmp119 * sqrt_info(2, 1) + _tmp121 * _tmp80 + + _tmp68 * sqrt_info(2, 2); + const Scalar _tmp124 = -_tmp111 * sqrt_info(3, 1) - _tmp112 * sqrt_info(3, 0) + _tmp113 * _tmp82 + + _tmp114 * sqrt_info(3, 1) + _tmp115 * sqrt_info(3, 0) + + _tmp117 * sqrt_info(3, 2) - _tmp118 * sqrt_info(3, 2) + + _tmp119 * sqrt_info(3, 1) + _tmp68 * sqrt_info(3, 2); + const Scalar _tmp125 = -_tmp110 * _tmp84 - _tmp112 * sqrt_info(4, 0) + _tmp113 * _tmp84 + + _tmp115 * sqrt_info(4, 0) + _tmp116 * sqrt_info(4, 0) - + _tmp118 * sqrt_info(4, 2) + _tmp119 * sqrt_info(4, 1) + + _tmp68 * sqrt_info(4, 2) + _tmp85 * _tmp91; + const Scalar _tmp126 = -_tmp110 * _tmp87 - _tmp112 * sqrt_info(5, 0) + _tmp113 * _tmp87 + + _tmp115 * sqrt_info(5, 0) + _tmp116 * sqrt_info(5, 0) + + _tmp117 * sqrt_info(5, 2) - _tmp118 * sqrt_info(5, 2) + + _tmp119 * sqrt_info(5, 1) + _tmp68 * sqrt_info(5, 2); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp35; + _res(1, 0) = _tmp37; + _res(2, 0) = _tmp38; + _res(3, 0) = _tmp39; + _res(4, 0) = _tmp40; + _res(5, 0) = _tmp42; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp75; + _jacobian(1, 0) = _tmp79; + _jacobian(2, 0) = _tmp81; + _jacobian(3, 0) = _tmp83; + _jacobian(4, 0) = _tmp86; + _jacobian(5, 0) = _tmp88; + _jacobian(0, 1) = _tmp102; + _jacobian(1, 1) = _tmp104; + _jacobian(2, 1) = _tmp105; + _jacobian(3, 1) = _tmp106; + _jacobian(4, 1) = _tmp107; + _jacobian(5, 1) = _tmp108; + _jacobian(0, 2) = _tmp120; + _jacobian(1, 2) = _tmp122; + _jacobian(2, 2) = _tmp123; + _jacobian(3, 2) = _tmp124; + _jacobian(4, 2) = _tmp125; + _jacobian(5, 2) = _tmp126; + _jacobian(0, 3) = sqrt_info(0, 3); + _jacobian(1, 3) = sqrt_info(1, 3); + _jacobian(2, 3) = sqrt_info(2, 3); + _jacobian(3, 3) = sqrt_info(3, 3); + _jacobian(4, 3) = sqrt_info(4, 3); + _jacobian(5, 3) = sqrt_info(5, 3); + _jacobian(0, 4) = sqrt_info(0, 4); + _jacobian(1, 4) = sqrt_info(1, 4); + _jacobian(2, 4) = sqrt_info(2, 4); + _jacobian(3, 4) = sqrt_info(3, 4); + _jacobian(4, 4) = sqrt_info(4, 4); + _jacobian(5, 4) = sqrt_info(5, 4); + _jacobian(0, 5) = sqrt_info(0, 5); + _jacobian(1, 5) = sqrt_info(1, 5); + _jacobian(2, 5) = sqrt_info(2, 5); + _jacobian(3, 5) = sqrt_info(3, 5); + _jacobian(4, 5) = sqrt_info(4, 5); + _jacobian(5, 5) = sqrt_info(5, 5); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = std::pow(_tmp75, Scalar(2)) + std::pow(_tmp79, Scalar(2)) + + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp83, Scalar(2)) + + std::pow(_tmp86, Scalar(2)) + std::pow(_tmp88, Scalar(2)); + _hessian(1, 0) = _tmp102 * _tmp75 + _tmp104 * _tmp79 + _tmp105 * _tmp81 + _tmp106 * _tmp83 + + _tmp107 * _tmp86 + _tmp108 * _tmp88; + _hessian(2, 0) = _tmp120 * _tmp75 + _tmp122 * _tmp79 + _tmp123 * _tmp81 + _tmp124 * _tmp83 + + _tmp125 * _tmp86 + _tmp126 * _tmp88; + _hessian(3, 0) = _tmp75 * sqrt_info(0, 3) + _tmp79 * sqrt_info(1, 3) + + _tmp81 * sqrt_info(2, 3) + _tmp83 * sqrt_info(3, 3) + + _tmp86 * sqrt_info(4, 3) + _tmp88 * sqrt_info(5, 3); + _hessian(4, 0) = _tmp75 * sqrt_info(0, 4) + _tmp79 * sqrt_info(1, 4) + + _tmp81 * sqrt_info(2, 4) + _tmp83 * sqrt_info(3, 4) + + _tmp86 * sqrt_info(4, 4) + _tmp88 * sqrt_info(5, 4); + _hessian(5, 0) = _tmp75 * sqrt_info(0, 5) + _tmp79 * sqrt_info(1, 5) + + _tmp81 * sqrt_info(2, 5) + _tmp83 * sqrt_info(3, 5) + + _tmp86 * sqrt_info(4, 5) + _tmp88 * sqrt_info(5, 5); + _hessian(0, 1) = 0; + _hessian(1, 1) = std::pow(_tmp102, Scalar(2)) + std::pow(_tmp104, Scalar(2)) + + std::pow(_tmp105, Scalar(2)) + std::pow(_tmp106, Scalar(2)) + + std::pow(_tmp107, Scalar(2)) + std::pow(_tmp108, Scalar(2)); + _hessian(2, 1) = _tmp102 * _tmp120 + _tmp104 * _tmp122 + _tmp105 * _tmp123 + _tmp106 * _tmp124 + + _tmp107 * _tmp125 + _tmp108 * _tmp126; + _hessian(3, 1) = _tmp102 * sqrt_info(0, 3) + _tmp104 * sqrt_info(1, 3) + + _tmp105 * sqrt_info(2, 3) + _tmp106 * sqrt_info(3, 3) + + _tmp107 * sqrt_info(4, 3) + _tmp108 * sqrt_info(5, 3); + _hessian(4, 1) = _tmp102 * sqrt_info(0, 4) + _tmp104 * sqrt_info(1, 4) + + _tmp105 * sqrt_info(2, 4) + _tmp106 * sqrt_info(3, 4) + + _tmp107 * sqrt_info(4, 4) + _tmp108 * sqrt_info(5, 4); + _hessian(5, 1) = _tmp102 * sqrt_info(0, 5) + _tmp104 * sqrt_info(1, 5) + + _tmp105 * sqrt_info(2, 5) + _tmp106 * sqrt_info(3, 5) + + _tmp107 * sqrt_info(4, 5) + _tmp108 * sqrt_info(5, 5); + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = std::pow(_tmp120, Scalar(2)) + std::pow(_tmp122, Scalar(2)) + + std::pow(_tmp123, Scalar(2)) + std::pow(_tmp124, Scalar(2)) + + std::pow(_tmp125, Scalar(2)) + std::pow(_tmp126, Scalar(2)); + _hessian(3, 2) = _tmp120 * sqrt_info(0, 3) + _tmp122 * sqrt_info(1, 3) + + _tmp123 * sqrt_info(2, 3) + _tmp124 * sqrt_info(3, 3) + + _tmp125 * sqrt_info(4, 3) + _tmp126 * sqrt_info(5, 3); + _hessian(4, 2) = _tmp120 * sqrt_info(0, 4) + _tmp122 * sqrt_info(1, 4) + + _tmp123 * sqrt_info(2, 4) + _tmp124 * sqrt_info(3, 4) + + _tmp125 * sqrt_info(4, 4) + _tmp126 * sqrt_info(5, 4); + _hessian(5, 2) = _tmp120 * sqrt_info(0, 5) + _tmp122 * sqrt_info(1, 5) + + _tmp123 * sqrt_info(2, 5) + _tmp124 * sqrt_info(3, 5) + + _tmp125 * sqrt_info(4, 5) + _tmp126 * sqrt_info(5, 5); + _hessian(0, 3) = 0; + _hessian(1, 3) = 0; + _hessian(2, 3) = 0; + _hessian(3, 3) = std::pow(sqrt_info(0, 3), Scalar(2)) + std::pow(sqrt_info(1, 3), Scalar(2)) + + std::pow(sqrt_info(2, 3), Scalar(2)) + std::pow(sqrt_info(3, 3), Scalar(2)) + + std::pow(sqrt_info(4, 3), Scalar(2)) + std::pow(sqrt_info(5, 3), Scalar(2)); + _hessian(4, 3) = sqrt_info(0, 3) * sqrt_info(0, 4) + sqrt_info(1, 3) * sqrt_info(1, 4) + + sqrt_info(2, 3) * sqrt_info(2, 4) + sqrt_info(3, 3) * sqrt_info(3, 4) + + sqrt_info(4, 3) * sqrt_info(4, 4) + sqrt_info(5, 3) * sqrt_info(5, 4); + _hessian(5, 3) = sqrt_info(0, 3) * sqrt_info(0, 5) + sqrt_info(1, 3) * sqrt_info(1, 5) + + sqrt_info(2, 3) * sqrt_info(2, 5) + sqrt_info(3, 3) * sqrt_info(3, 5) + + sqrt_info(4, 3) * sqrt_info(4, 5) + sqrt_info(5, 3) * sqrt_info(5, 5); + _hessian(0, 4) = 0; + _hessian(1, 4) = 0; + _hessian(2, 4) = 0; + _hessian(3, 4) = 0; + _hessian(4, 4) = std::pow(sqrt_info(0, 4), Scalar(2)) + std::pow(sqrt_info(1, 4), Scalar(2)) + + std::pow(sqrt_info(2, 4), Scalar(2)) + std::pow(sqrt_info(3, 4), Scalar(2)) + + std::pow(sqrt_info(4, 4), Scalar(2)) + std::pow(sqrt_info(5, 4), Scalar(2)); + _hessian(5, 4) = sqrt_info(0, 4) * sqrt_info(0, 5) + sqrt_info(1, 4) * sqrt_info(1, 5) + + sqrt_info(2, 4) * sqrt_info(2, 5) + sqrt_info(3, 4) * sqrt_info(3, 5) + + sqrt_info(4, 4) * sqrt_info(4, 5) + sqrt_info(5, 4) * sqrt_info(5, 5); + _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(sqrt_info(0, 5), Scalar(2)) + std::pow(sqrt_info(1, 5), Scalar(2)) + + std::pow(sqrt_info(2, 5), Scalar(2)) + std::pow(sqrt_info(3, 5), Scalar(2)) + + std::pow(sqrt_info(4, 5), Scalar(2)) + std::pow(sqrt_info(5, 5), Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp35 * _tmp75 + _tmp37 * _tmp79 + _tmp38 * _tmp81 + _tmp39 * _tmp83 + + _tmp40 * _tmp86 + _tmp42 * _tmp88; + _rhs(1, 0) = _tmp102 * _tmp35 + _tmp104 * _tmp37 + _tmp105 * _tmp38 + _tmp106 * _tmp39 + + _tmp107 * _tmp40 + _tmp108 * _tmp42; + _rhs(2, 0) = _tmp120 * _tmp35 + _tmp122 * _tmp37 + _tmp123 * _tmp38 + _tmp124 * _tmp39 + + _tmp125 * _tmp40 + _tmp126 * _tmp42; + _rhs(3, 0) = _tmp35 * sqrt_info(0, 3) + _tmp37 * sqrt_info(1, 3) + _tmp38 * sqrt_info(2, 3) + + _tmp39 * sqrt_info(3, 3) + _tmp40 * sqrt_info(4, 3) + _tmp42 * sqrt_info(5, 3); + _rhs(4, 0) = _tmp35 * sqrt_info(0, 4) + _tmp37 * sqrt_info(1, 4) + _tmp38 * sqrt_info(2, 4) + + _tmp39 * sqrt_info(3, 4) + _tmp40 * sqrt_info(4, 4) + _tmp42 * sqrt_info(5, 4); + _rhs(5, 0) = _tmp35 * sqrt_info(0, 5) + _tmp37 * sqrt_info(1, 5) + _tmp38 * sqrt_info(2, 5) + + _tmp39 * sqrt_info(3, 5) + _tmp40 * sqrt_info(4, 5) + _tmp42 * sqrt_info(5, 5); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_position.h b/gen/cpp/sym/factors/prior_factor_pose3_position.h index 37e160c69..39e043a18 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3_position.h +++ b/gen/cpp/sym/factors/prior_factor_pose3_position.h @@ -1,120 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x6) jacobian of res wrt arg value (6) - * hessian: (6x6) Gauss-Newton hessian for arg value (6) - * rhs: (6x1) Gauss-Newton rhs for arg value (6) - */ -template -void PriorFactorPose3Position(const sym::Pose3& value, - const Eigen::Matrix& prior, - const Eigen::Matrix& sqrt_info, 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: 63 - - // Unused inputs - (void)epsilon; - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - - // Intermediate terms (6) - const Scalar _tmp0 = _value[5] - prior(1, 0); - const Scalar _tmp1 = _value[4] - prior(0, 0); - const Scalar _tmp2 = _value[6] - prior(2, 0); - const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 0) + _tmp2 * sqrt_info(0, 2); - const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 0) + _tmp2 * sqrt_info(1, 2); - const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 0) + _tmp2 * sqrt_info(2, 2); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp3; - _res(1, 0) = _tmp4; - _res(2, 0) = _tmp5; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = 0; - _jacobian(1, 0) = 0; - _jacobian(2, 0) = 0; - _jacobian(0, 1) = 0; - _jacobian(1, 1) = 0; - _jacobian(2, 1) = 0; - _jacobian(0, 2) = 0; - _jacobian(1, 2) = 0; - _jacobian(2, 2) = 0; - _jacobian(0, 3) = sqrt_info(0, 0); - _jacobian(1, 3) = sqrt_info(1, 0); - _jacobian(2, 3) = sqrt_info(2, 0); - _jacobian(0, 4) = sqrt_info(0, 1); - _jacobian(1, 4) = sqrt_info(1, 1); - _jacobian(2, 4) = sqrt_info(2, 1); - _jacobian(0, 5) = sqrt_info(0, 2); - _jacobian(1, 5) = sqrt_info(1, 2); - _jacobian(2, 5) = sqrt_info(2, 2); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian.setZero(); - - _hessian(3, 3) = std::pow(sqrt_info(0, 0), Scalar(2)) + std::pow(sqrt_info(1, 0), Scalar(2)) + - std::pow(sqrt_info(2, 0), Scalar(2)); - _hessian(4, 3) = sqrt_info(0, 0) * sqrt_info(0, 1) + sqrt_info(1, 0) * sqrt_info(1, 1) + - sqrt_info(2, 0) * sqrt_info(2, 1); - _hessian(5, 3) = sqrt_info(0, 0) * sqrt_info(0, 2) + sqrt_info(1, 0) * sqrt_info(1, 2) + - sqrt_info(2, 0) * sqrt_info(2, 2); - _hessian(4, 4) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + - std::pow(sqrt_info(2, 1), Scalar(2)); - _hessian(5, 4) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + - sqrt_info(2, 1) * sqrt_info(2, 2); - _hessian(5, 5) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + - std::pow(sqrt_info(2, 2), Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = 0; - _rhs(1, 0) = 0; - _rhs(2, 0) = 0; - _rhs(3, 0) = _tmp3 * sqrt_info(0, 0) + _tmp4 * sqrt_info(1, 0) + _tmp5 * sqrt_info(2, 0); - _rhs(4, 0) = _tmp3 * sqrt_info(0, 1) + _tmp4 * sqrt_info(1, 1) + _tmp5 * sqrt_info(2, 1); - _rhs(5, 0) = _tmp3 * sqrt_info(0, 2) + _tmp4 * sqrt_info(1, 2) + _tmp5 * sqrt_info(2, 2); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_pose3_position/prior_factor_pose3_position_diagonal.h" +#include "./prior_factor_pose3_position/prior_factor_pose3_position_isotropic.h" +#include "./prior_factor_pose3_position/prior_factor_pose3_position_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_diagonal.h b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_diagonal.h new file mode 100644 index 000000000..38360b983 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_diagonal.h @@ -0,0 +1,99 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Position(const sym::Pose3& value, + const Eigen::Matrix& prior, + const Eigen::Matrix& sqrt_info, 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: 12 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + + // Intermediate terms (6) + const Scalar _tmp0 = _value[4] - prior(0, 0); + const Scalar _tmp1 = _value[5] - prior(1, 0); + const Scalar _tmp2 = _value[6] - prior(2, 0); + const Scalar _tmp3 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp4 = std::pow(sqrt_info(1, 0), Scalar(2)); + const Scalar _tmp5 = std::pow(sqrt_info(2, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info(0, 0); + _res(1, 0) = _tmp1 * sqrt_info(1, 0); + _res(2, 0) = _tmp2 * sqrt_info(2, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 3) = sqrt_info(0, 0); + _jacobian(1, 4) = sqrt_info(1, 0); + _jacobian(2, 5) = sqrt_info(2, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(3, 3) = _tmp3; + _hessian(4, 4) = _tmp4; + _hessian(5, 5) = _tmp5; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = 0; + _rhs(1, 0) = 0; + _rhs(2, 0) = 0; + _rhs(3, 0) = _tmp0 * _tmp3; + _rhs(4, 0) = _tmp1 * _tmp4; + _rhs(5, 0) = _tmp2 * _tmp5; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_isotropic.h b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_isotropic.h new file mode 100644 index 000000000..d4bef8c9f --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_isotropic.h @@ -0,0 +1,97 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Position(const sym::Pose3& value, + const Eigen::Matrix& prior, const Scalar sqrt_info, + 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: 10 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + + // Intermediate terms (4) + const Scalar _tmp0 = _value[4] - prior(0, 0); + const Scalar _tmp1 = _value[5] - prior(1, 0); + const Scalar _tmp2 = _value[6] - prior(2, 0); + const Scalar _tmp3 = std::pow(sqrt_info, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp0 * sqrt_info; + _res(1, 0) = _tmp1 * sqrt_info; + _res(2, 0) = _tmp2 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian.setZero(); + + _jacobian(0, 3) = sqrt_info; + _jacobian(1, 4) = sqrt_info; + _jacobian(2, 5) = sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(3, 3) = _tmp3; + _hessian(4, 4) = _tmp3; + _hessian(5, 5) = _tmp3; + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = 0; + _rhs(1, 0) = 0; + _rhs(2, 0) = 0; + _rhs(3, 0) = _tmp0 * _tmp3; + _rhs(4, 0) = _tmp1 * _tmp3; + _rhs(5, 0) = _tmp2 * _tmp3; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_square.h b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_square.h new file mode 100644 index 000000000..9a46380b9 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_position/prior_factor_pose3_position_square.h @@ -0,0 +1,121 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Position(const sym::Pose3& value, + const Eigen::Matrix& prior, + const Eigen::Matrix& sqrt_info, 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: 63 + + // Unused inputs + (void)epsilon; + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + + // Intermediate terms (6) + const Scalar _tmp0 = _value[5] - prior(1, 0); + const Scalar _tmp1 = _value[4] - prior(0, 0); + const Scalar _tmp2 = _value[6] - prior(2, 0); + const Scalar _tmp3 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 0) + _tmp2 * sqrt_info(0, 2); + const Scalar _tmp4 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 0) + _tmp2 * sqrt_info(1, 2); + const Scalar _tmp5 = _tmp0 * sqrt_info(2, 1) + _tmp1 * sqrt_info(2, 0) + _tmp2 * sqrt_info(2, 2); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp3; + _res(1, 0) = _tmp4; + _res(2, 0) = _tmp5; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = 0; + _jacobian(1, 0) = 0; + _jacobian(2, 0) = 0; + _jacobian(0, 1) = 0; + _jacobian(1, 1) = 0; + _jacobian(2, 1) = 0; + _jacobian(0, 2) = 0; + _jacobian(1, 2) = 0; + _jacobian(2, 2) = 0; + _jacobian(0, 3) = sqrt_info(0, 0); + _jacobian(1, 3) = sqrt_info(1, 0); + _jacobian(2, 3) = sqrt_info(2, 0); + _jacobian(0, 4) = sqrt_info(0, 1); + _jacobian(1, 4) = sqrt_info(1, 1); + _jacobian(2, 4) = sqrt_info(2, 1); + _jacobian(0, 5) = sqrt_info(0, 2); + _jacobian(1, 5) = sqrt_info(1, 2); + _jacobian(2, 5) = sqrt_info(2, 2); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(3, 3) = std::pow(sqrt_info(0, 0), Scalar(2)) + std::pow(sqrt_info(1, 0), Scalar(2)) + + std::pow(sqrt_info(2, 0), Scalar(2)); + _hessian(4, 3) = sqrt_info(0, 0) * sqrt_info(0, 1) + sqrt_info(1, 0) * sqrt_info(1, 1) + + sqrt_info(2, 0) * sqrt_info(2, 1); + _hessian(5, 3) = sqrt_info(0, 0) * sqrt_info(0, 2) + sqrt_info(1, 0) * sqrt_info(1, 2) + + sqrt_info(2, 0) * sqrt_info(2, 2); + _hessian(4, 4) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)) + + std::pow(sqrt_info(2, 1), Scalar(2)); + _hessian(5, 4) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2) + + sqrt_info(2, 1) * sqrt_info(2, 2); + _hessian(5, 5) = std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)) + + std::pow(sqrt_info(2, 2), Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = 0; + _rhs(1, 0) = 0; + _rhs(2, 0) = 0; + _rhs(3, 0) = _tmp3 * sqrt_info(0, 0) + _tmp4 * sqrt_info(1, 0) + _tmp5 * sqrt_info(2, 0); + _rhs(4, 0) = _tmp3 * sqrt_info(0, 1) + _tmp4 * sqrt_info(1, 1) + _tmp5 * sqrt_info(2, 1); + _rhs(5, 0) = _tmp3 * sqrt_info(0, 2) + _tmp4 * sqrt_info(1, 2) + _tmp5 * sqrt_info(2, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_rotation.h b/gen/cpp/sym/factors/prior_factor_pose3_rotation.h index 99daced14..1c4dc4e75 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3_rotation.h +++ b/gen/cpp/sym/factors/prior_factor_pose3_rotation.h @@ -1,242 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x6) jacobian of res wrt arg value (6) - * hessian: (6x6) Gauss-Newton hessian for arg value (6) - * rhs: (6x1) Gauss-Newton rhs for arg value (6) - */ -template -void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3& prior, - const Eigen::Matrix& sqrt_info, 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: 362 - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - const Eigen::Matrix& _prior = prior.Data(); - - // Intermediate terms (106) - const Scalar _tmp0 = _prior[3] * _value[1]; - const Scalar _tmp1 = _prior[0] * _value[2]; - const Scalar _tmp2 = _prior[2] * _value[0]; - const Scalar _tmp3 = _prior[1] * _value[3]; - const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; - const Scalar _tmp5 = _prior[3] * _value[3]; - const Scalar _tmp6 = _prior[1] * _value[1]; - const Scalar _tmp7 = _prior[2] * _value[2]; - const Scalar _tmp8 = _prior[0] * _value[0]; - const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; - const Scalar _tmp10 = _tmp5 - _tmp9; - const Scalar _tmp11 = 2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1; - const Scalar _tmp12 = 2 * _tmp11; - const Scalar _tmp13 = 1 - epsilon; - const Scalar _tmp14 = std::min(_tmp13, std::fabs(_tmp10)); - const Scalar _tmp15 = std::acos(_tmp14) / std::sqrt(Scalar(1 - std::pow(_tmp14, Scalar(2)))); - const Scalar _tmp16 = _tmp12 * _tmp15; - const Scalar _tmp17 = _tmp16 * _tmp4; - const Scalar _tmp18 = _prior[0] * _value[1]; - const Scalar _tmp19 = _prior[3] * _value[2]; - const Scalar _tmp20 = _prior[1] * _value[0]; - const Scalar _tmp21 = _prior[2] * _value[3]; - const Scalar _tmp22 = -_tmp18 + _tmp19 + _tmp20 - _tmp21; - const Scalar _tmp23 = _tmp15 * _tmp22; - const Scalar _tmp24 = _tmp12 * _tmp23; - const Scalar _tmp25 = _prior[2] * _value[1]; - const Scalar _tmp26 = _prior[1] * _value[2]; - const Scalar _tmp27 = _prior[3] * _value[0]; - const Scalar _tmp28 = _prior[0] * _value[3]; - const Scalar _tmp29 = _tmp25 - _tmp26 + _tmp27 - _tmp28; - const Scalar _tmp30 = _tmp15 * _tmp29; - const Scalar _tmp31 = _tmp12 * _tmp30; - const Scalar _tmp32 = - _tmp17 * sqrt_info(0, 1) + _tmp24 * sqrt_info(0, 2) + _tmp31 * sqrt_info(0, 0); - const Scalar _tmp33 = _tmp12 * sqrt_info(1, 2); - const Scalar _tmp34 = _tmp17 * sqrt_info(1, 1) + _tmp23 * _tmp33 + _tmp31 * sqrt_info(1, 0); - const Scalar _tmp35 = _tmp4 * sqrt_info(2, 1); - const Scalar _tmp36 = _tmp12 * sqrt_info(2, 0); - const Scalar _tmp37 = _tmp16 * _tmp35 + _tmp24 * sqrt_info(2, 2) + _tmp30 * _tmp36; - const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp26; - const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp27; - const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp28; - const Scalar _tmp42 = _tmp38 - _tmp39 + _tmp40 - _tmp41; - const Scalar _tmp43 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); - const Scalar _tmp44 = std::min(_tmp13, _tmp43); - const Scalar _tmp45 = 1 - std::pow(_tmp44, Scalar(2)); - const Scalar _tmp46 = _tmp11 * ((((_tmp13 - _tmp43) > 0) - ((_tmp13 - _tmp43) < 0)) + 1) * - (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); - const Scalar _tmp47 = _tmp46 / _tmp45; - const Scalar _tmp48 = _tmp4 * _tmp47; - const Scalar _tmp49 = _tmp48 * sqrt_info(0, 1); - const Scalar _tmp50 = _tmp29 * _tmp47; - const Scalar _tmp51 = _tmp42 * _tmp50; - const Scalar _tmp52 = _tmp22 * _tmp47; - const Scalar _tmp53 = _tmp42 * _tmp52; - const Scalar _tmp54 = std::acos(_tmp44); - const Scalar _tmp55 = _tmp44 * _tmp46 * _tmp54 / (_tmp45 * std::sqrt(_tmp45)); - const Scalar _tmp56 = _tmp42 * _tmp55; - const Scalar _tmp57 = _tmp29 * _tmp56; - const Scalar _tmp58 = _tmp4 * sqrt_info(0, 1); - const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp18; - const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp19; - const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp20; - const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp21; - const Scalar _tmp63 = -_tmp59 + _tmp60 + _tmp61 - _tmp62; - const Scalar _tmp64 = _tmp54 / std::sqrt(_tmp45); - const Scalar _tmp65 = _tmp12 * _tmp64; - const Scalar _tmp66 = _tmp63 * _tmp65; - const Scalar _tmp67 = _tmp22 * sqrt_info(0, 2); - const Scalar _tmp68 = (Scalar(1) / Scalar(2)) * _tmp0; - const Scalar _tmp69 = (Scalar(1) / Scalar(2)) * _tmp1; - const Scalar _tmp70 = (Scalar(1) / Scalar(2)) * _tmp2; - const Scalar _tmp71 = (Scalar(1) / Scalar(2)) * _tmp3; - const Scalar _tmp72 = _tmp64 * (-_tmp68 - _tmp69 + _tmp70 + _tmp71); - const Scalar _tmp73 = _tmp12 * sqrt_info(0, 2); - const Scalar _tmp74 = - _tmp64 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + - (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); - const Scalar _tmp75 = _tmp12 * _tmp74; - const Scalar _tmp76 = -_tmp42 * _tmp49 - _tmp51 * sqrt_info(0, 0) - _tmp53 * sqrt_info(0, 2) + - _tmp56 * _tmp58 + _tmp56 * _tmp67 + _tmp57 * sqrt_info(0, 0) + - _tmp66 * sqrt_info(0, 1) + _tmp72 * _tmp73 + _tmp75 * sqrt_info(0, 0); - const Scalar _tmp77 = _tmp48 * sqrt_info(1, 1); - const Scalar _tmp78 = _tmp29 * sqrt_info(1, 0); - const Scalar _tmp79 = _tmp4 * sqrt_info(1, 1); - const Scalar _tmp80 = _tmp22 * sqrt_info(1, 2); - const Scalar _tmp81 = _tmp33 * _tmp72 - _tmp42 * _tmp77 - _tmp51 * sqrt_info(1, 0) - - _tmp53 * sqrt_info(1, 2) + _tmp56 * _tmp78 + _tmp56 * _tmp79 + - _tmp56 * _tmp80 + _tmp66 * sqrt_info(1, 1) + _tmp75 * sqrt_info(1, 0); - const Scalar _tmp82 = _tmp35 * _tmp47; - const Scalar _tmp83 = _tmp50 * sqrt_info(2, 0); - const Scalar _tmp84 = _tmp22 * sqrt_info(2, 2); - const Scalar _tmp85 = _tmp12 * _tmp72 * sqrt_info(2, 2) + _tmp35 * _tmp56 + _tmp36 * _tmp74 - - _tmp42 * _tmp82 - _tmp42 * _tmp83 - _tmp53 * sqrt_info(2, 2) + - _tmp56 * _tmp84 + _tmp57 * sqrt_info(2, 0) + _tmp66 * sqrt_info(2, 1); - const Scalar _tmp86 = _tmp68 + _tmp69 - _tmp70 - _tmp71; - const Scalar _tmp87 = _tmp50 * _tmp86; - const Scalar _tmp88 = _tmp52 * _tmp86; - const Scalar _tmp89 = _tmp55 * _tmp86; - const Scalar _tmp90 = _tmp29 * _tmp89; - const Scalar _tmp91 = _tmp42 * _tmp65; - const Scalar _tmp92 = _tmp64 * (_tmp59 - _tmp60 - _tmp61 + _tmp62); - const Scalar _tmp93 = _tmp12 * _tmp92; - const Scalar _tmp94 = -_tmp49 * _tmp86 + _tmp58 * _tmp89 + _tmp67 * _tmp89 + - _tmp75 * sqrt_info(0, 1) - _tmp87 * sqrt_info(0, 0) - - _tmp88 * sqrt_info(0, 2) + _tmp90 * sqrt_info(0, 0) + - _tmp91 * sqrt_info(0, 2) + _tmp93 * sqrt_info(0, 0); - const Scalar _tmp95 = _tmp33 * _tmp42 * _tmp64 + _tmp75 * sqrt_info(1, 1) - _tmp77 * _tmp86 + - _tmp78 * _tmp89 + _tmp79 * _tmp89 + _tmp80 * _tmp89 - - _tmp87 * sqrt_info(1, 0) - _tmp88 * sqrt_info(1, 2) + - _tmp93 * sqrt_info(1, 0); - const Scalar _tmp96 = _tmp35 * _tmp89 + _tmp36 * _tmp92 + _tmp75 * sqrt_info(2, 1) - - _tmp82 * _tmp86 - _tmp83 * _tmp86 + _tmp84 * _tmp89 - - _tmp88 * sqrt_info(2, 2) + _tmp90 * sqrt_info(2, 0) + - _tmp91 * sqrt_info(2, 2); - const Scalar _tmp97 = _tmp50 * _tmp63; - const Scalar _tmp98 = _tmp55 * _tmp63; - const Scalar _tmp99 = _tmp29 * _tmp98; - const Scalar _tmp100 = _tmp52 * _tmp63; - const Scalar _tmp101 = _tmp65 * (-_tmp38 + _tmp39 - _tmp40 + _tmp41); - const Scalar _tmp102 = _tmp65 * _tmp86; - const Scalar _tmp103 = -_tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 1) + - _tmp102 * sqrt_info(0, 0) - _tmp49 * _tmp63 + _tmp58 * _tmp98 + - _tmp67 * _tmp98 + _tmp73 * _tmp74 - _tmp97 * sqrt_info(0, 0) + - _tmp99 * sqrt_info(0, 0); - const Scalar _tmp104 = -_tmp100 * sqrt_info(1, 2) + _tmp101 * sqrt_info(1, 1) + - _tmp102 * sqrt_info(1, 0) + _tmp33 * _tmp74 - _tmp63 * _tmp77 + - _tmp78 * _tmp98 + _tmp79 * _tmp98 + _tmp80 * _tmp98 - - _tmp97 * sqrt_info(1, 0); - const Scalar _tmp105 = -_tmp100 * sqrt_info(2, 2) + _tmp101 * sqrt_info(2, 1) + _tmp35 * _tmp98 + - _tmp36 * _tmp64 * _tmp86 - _tmp63 * _tmp82 - _tmp63 * _tmp83 + - _tmp75 * sqrt_info(2, 2) + _tmp84 * _tmp98 + _tmp99 * sqrt_info(2, 0); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp32; - _res(1, 0) = _tmp34; - _res(2, 0) = _tmp37; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp76; - _jacobian(1, 0) = _tmp81; - _jacobian(2, 0) = _tmp85; - _jacobian(0, 1) = _tmp94; - _jacobian(1, 1) = _tmp95; - _jacobian(2, 1) = _tmp96; - _jacobian(0, 2) = _tmp103; - _jacobian(1, 2) = _tmp104; - _jacobian(2, 2) = _tmp105; - _jacobian(0, 3) = 0; - _jacobian(1, 3) = 0; - _jacobian(2, 3) = 0; - _jacobian(0, 4) = 0; - _jacobian(1, 4) = 0; - _jacobian(2, 4) = 0; - _jacobian(0, 5) = 0; - _jacobian(1, 5) = 0; - _jacobian(2, 5) = 0; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian.setZero(); - - _hessian(0, 0) = - std::pow(_tmp76, Scalar(2)) + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp85, Scalar(2)); - _hessian(1, 0) = _tmp76 * _tmp94 + _tmp81 * _tmp95 + _tmp85 * _tmp96; - _hessian(2, 0) = _tmp103 * _tmp76 + _tmp104 * _tmp81 + _tmp105 * _tmp85; - _hessian(1, 1) = - std::pow(_tmp94, Scalar(2)) + std::pow(_tmp95, Scalar(2)) + std::pow(_tmp96, Scalar(2)); - _hessian(2, 1) = _tmp103 * _tmp94 + _tmp104 * _tmp95 + _tmp105 * _tmp96; - _hessian(2, 2) = - std::pow(_tmp103, Scalar(2)) + std::pow(_tmp104, Scalar(2)) + std::pow(_tmp105, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp32 * _tmp76 + _tmp34 * _tmp81 + _tmp37 * _tmp85; - _rhs(1, 0) = _tmp32 * _tmp94 + _tmp34 * _tmp95 + _tmp37 * _tmp96; - _rhs(2, 0) = _tmp103 * _tmp32 + _tmp104 * _tmp34 + _tmp105 * _tmp37; - _rhs(3, 0) = 0; - _rhs(4, 0) = 0; - _rhs(5, 0) = 0; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_pose3_rotation/prior_factor_pose3_rotation_diagonal.h" +#include "./prior_factor_pose3_rotation/prior_factor_pose3_rotation_isotropic.h" +#include "./prior_factor_pose3_rotation/prior_factor_pose3_rotation_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_diagonal.h b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_diagonal.h new file mode 100644 index 000000000..cb62a3363 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_diagonal.h @@ -0,0 +1,190 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3& prior, + const Eigen::Matrix& sqrt_info, 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: 210 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (75) + const Scalar _tmp0 = _prior[3] * _value[3]; + const Scalar _tmp1 = _prior[1] * _value[1]; + const Scalar _tmp2 = _prior[2] * _value[2]; + const Scalar _tmp3 = _prior[0] * _value[0]; + const Scalar _tmp4 = -_tmp1 - _tmp2 - _tmp3; + const Scalar _tmp5 = _tmp0 - _tmp4; + const Scalar _tmp6 = 2 * std::min(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1; + const Scalar _tmp7 = 2 * _tmp6; + const Scalar _tmp8 = 1 - epsilon; + const Scalar _tmp9 = std::min(_tmp8, std::fabs(_tmp5)); + const Scalar _tmp10 = std::acos(_tmp9) / std::sqrt(Scalar(1 - std::pow(_tmp9, Scalar(2)))); + const Scalar _tmp11 = _tmp10 * _tmp7; + const Scalar _tmp12 = _prior[2] * _value[1]; + const Scalar _tmp13 = _prior[1] * _value[2]; + const Scalar _tmp14 = _prior[3] * _value[0]; + const Scalar _tmp15 = _prior[0] * _value[3]; + const Scalar _tmp16 = sqrt_info(0, 0) * (_tmp12 - _tmp13 + _tmp14 - _tmp15); + const Scalar _tmp17 = _tmp11 * _tmp16; + const Scalar _tmp18 = _prior[3] * _value[1]; + const Scalar _tmp19 = _prior[0] * _value[2]; + const Scalar _tmp20 = _prior[2] * _value[0]; + const Scalar _tmp21 = _prior[1] * _value[3]; + const Scalar _tmp22 = _tmp18 + _tmp19 - _tmp20 - _tmp21; + const Scalar _tmp23 = _tmp7 * sqrt_info(1, 0); + const Scalar _tmp24 = _tmp10 * _tmp22 * _tmp23; + const Scalar _tmp25 = _prior[0] * _value[1]; + const Scalar _tmp26 = _prior[3] * _value[2]; + const Scalar _tmp27 = _prior[1] * _value[0]; + const Scalar _tmp28 = _prior[2] * _value[3]; + const Scalar _tmp29 = sqrt_info(2, 0) * (-_tmp25 + _tmp26 + _tmp27 - _tmp28); + const Scalar _tmp30 = _tmp11 * _tmp29; + const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp35 = _tmp31 - _tmp32 + _tmp33 - _tmp34; + const Scalar _tmp36 = std::fabs(_tmp0 + _tmp1 + _tmp2 + _tmp3); + const Scalar _tmp37 = std::min(_tmp36, _tmp8); + const Scalar _tmp38 = std::acos(_tmp37); + const Scalar _tmp39 = 1 - std::pow(_tmp37, Scalar(2)); + const Scalar _tmp40 = _tmp6 * ((((-_tmp36 + _tmp8) > 0) - ((-_tmp36 + _tmp8) < 0)) + 1) * + (((-_tmp0 + _tmp4) > 0) - ((-_tmp0 + _tmp4) < 0)); + const Scalar _tmp41 = _tmp37 * _tmp38 * _tmp40 / (_tmp39 * std::sqrt(_tmp39)); + const Scalar _tmp42 = _tmp16 * _tmp41; + const Scalar _tmp43 = _tmp40 / _tmp39; + const Scalar _tmp44 = _tmp35 * _tmp43; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp0 + (Scalar(1) / Scalar(2)) * _tmp1 + + (Scalar(1) / Scalar(2)) * _tmp2 + (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp46 = _tmp38 / std::sqrt(_tmp39); + const Scalar _tmp47 = _tmp46 * _tmp7; + const Scalar _tmp48 = _tmp47 * sqrt_info(0, 0); + const Scalar _tmp49 = -_tmp16 * _tmp44 + _tmp35 * _tmp42 + _tmp45 * _tmp48; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp54 = -_tmp50 + _tmp51 + _tmp52 - _tmp53; + const Scalar _tmp55 = _tmp23 * _tmp46; + const Scalar _tmp56 = _tmp22 * sqrt_info(1, 0); + const Scalar _tmp57 = _tmp41 * _tmp56; + const Scalar _tmp58 = _tmp35 * _tmp57 - _tmp44 * _tmp56 + _tmp54 * _tmp55; + const Scalar _tmp59 = _tmp29 * _tmp41; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp64 = _tmp47 * sqrt_info(2, 0); + const Scalar _tmp65 = + -_tmp29 * _tmp44 + _tmp35 * _tmp59 + _tmp64 * (-_tmp60 - _tmp61 + _tmp62 + _tmp63); + const Scalar _tmp66 = _tmp60 + _tmp61 - _tmp62 - _tmp63; + const Scalar _tmp67 = _tmp43 * _tmp66; + const Scalar _tmp68 = + -_tmp16 * _tmp67 + _tmp42 * _tmp66 + _tmp48 * (_tmp50 - _tmp51 - _tmp52 + _tmp53); + const Scalar _tmp69 = _tmp45 * _tmp55 - _tmp56 * _tmp67 + _tmp57 * _tmp66; + const Scalar _tmp70 = -_tmp29 * _tmp67 + _tmp35 * _tmp64 + _tmp59 * _tmp66; + const Scalar _tmp71 = _tmp43 * _tmp54; + const Scalar _tmp72 = -_tmp16 * _tmp71 + _tmp42 * _tmp54 + _tmp48 * _tmp66; + const Scalar _tmp73 = + _tmp54 * _tmp57 + _tmp55 * (-_tmp31 + _tmp32 - _tmp33 + _tmp34) - _tmp56 * _tmp71; + const Scalar _tmp74 = -_tmp29 * _tmp71 + _tmp45 * _tmp64 + _tmp54 * _tmp59; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp17; + _res(1, 0) = _tmp24; + _res(2, 0) = _tmp30; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp49; + _jacobian(1, 0) = _tmp58; + _jacobian(2, 0) = _tmp65; + _jacobian(0, 1) = _tmp68; + _jacobian(1, 1) = _tmp69; + _jacobian(2, 1) = _tmp70; + _jacobian(0, 2) = _tmp72; + _jacobian(1, 2) = _tmp73; + _jacobian(2, 2) = _tmp74; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp49, Scalar(2)) + std::pow(_tmp58, Scalar(2)) + std::pow(_tmp65, Scalar(2)); + _hessian(1, 0) = _tmp49 * _tmp68 + _tmp58 * _tmp69 + _tmp65 * _tmp70; + _hessian(2, 0) = _tmp49 * _tmp72 + _tmp58 * _tmp73 + _tmp65 * _tmp74; + _hessian(1, 1) = + std::pow(_tmp68, Scalar(2)) + std::pow(_tmp69, Scalar(2)) + std::pow(_tmp70, Scalar(2)); + _hessian(2, 1) = _tmp68 * _tmp72 + _tmp69 * _tmp73 + _tmp70 * _tmp74; + _hessian(2, 2) = + std::pow(_tmp72, Scalar(2)) + std::pow(_tmp73, Scalar(2)) + std::pow(_tmp74, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp17 * _tmp49 + _tmp24 * _tmp58 + _tmp30 * _tmp65; + _rhs(1, 0) = _tmp17 * _tmp68 + _tmp24 * _tmp69 + _tmp30 * _tmp70; + _rhs(2, 0) = _tmp17 * _tmp72 + _tmp24 * _tmp73 + _tmp30 * _tmp74; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_isotropic.h b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_isotropic.h new file mode 100644 index 000000000..1247f4e6a --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_isotropic.h @@ -0,0 +1,186 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3& prior, + const Scalar sqrt_info, 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: 201 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (68) + const Scalar _tmp0 = _prior[2] * _value[1]; + const Scalar _tmp1 = _prior[1] * _value[2]; + const Scalar _tmp2 = _prior[3] * _value[0]; + const Scalar _tmp3 = _prior[0] * _value[3]; + const Scalar _tmp4 = _tmp0 - _tmp1 + _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[1] * _value[1]; + const Scalar _tmp7 = _prior[2] * _value[2]; + const Scalar _tmp8 = _prior[0] * _value[0]; + const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; + const Scalar _tmp10 = _tmp5 - _tmp9; + const Scalar _tmp11 = 1 - epsilon; + const Scalar _tmp12 = std::min(_tmp11, std::fabs(_tmp10)); + const Scalar _tmp13 = + sqrt_info * (2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1); + const Scalar _tmp14 = 2 * _tmp13; + const Scalar _tmp15 = + _tmp14 * std::acos(_tmp12) / std::sqrt(Scalar(1 - std::pow(_tmp12, Scalar(2)))); + const Scalar _tmp16 = _tmp15 * _tmp4; + const Scalar _tmp17 = _prior[3] * _value[1]; + const Scalar _tmp18 = _prior[0] * _value[2]; + const Scalar _tmp19 = _prior[2] * _value[0]; + const Scalar _tmp20 = _prior[1] * _value[3]; + const Scalar _tmp21 = _tmp17 + _tmp18 - _tmp19 - _tmp20; + const Scalar _tmp22 = _tmp15 * _tmp21; + const Scalar _tmp23 = _prior[0] * _value[1]; + const Scalar _tmp24 = _prior[3] * _value[2]; + const Scalar _tmp25 = _prior[1] * _value[0]; + const Scalar _tmp26 = _prior[2] * _value[3]; + const Scalar _tmp27 = -_tmp23 + _tmp24 + _tmp25 - _tmp26; + const Scalar _tmp28 = _tmp15 * _tmp27; + const Scalar _tmp29 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp33 = _tmp29 - _tmp30 + _tmp31 - _tmp32; + const Scalar _tmp34 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); + const Scalar _tmp35 = std::min(_tmp11, _tmp34); + const Scalar _tmp36 = std::acos(_tmp35); + const Scalar _tmp37 = 1 - std::pow(_tmp35, Scalar(2)); + const Scalar _tmp38 = _tmp13 * ((((_tmp11 - _tmp34) > 0) - ((_tmp11 - _tmp34) < 0)) + 1) * + (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); + const Scalar _tmp39 = _tmp35 * _tmp36 * _tmp38 / (_tmp37 * std::sqrt(_tmp37)); + const Scalar _tmp40 = _tmp33 * _tmp39; + const Scalar _tmp41 = _tmp38 / _tmp37; + const Scalar _tmp42 = _tmp4 * _tmp41; + const Scalar _tmp43 = _tmp14 * _tmp36 / std::sqrt(_tmp37); + const Scalar _tmp44 = + _tmp43 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); + const Scalar _tmp45 = -_tmp33 * _tmp42 + _tmp4 * _tmp40 + _tmp44; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp50 = -_tmp46 + _tmp47 + _tmp48 - _tmp49; + const Scalar _tmp51 = _tmp21 * _tmp41; + const Scalar _tmp52 = _tmp21 * _tmp40 - _tmp33 * _tmp51 + _tmp43 * _tmp50; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp57 = _tmp27 * _tmp41; + const Scalar _tmp58 = + _tmp27 * _tmp40 - _tmp33 * _tmp57 + _tmp43 * (-_tmp53 - _tmp54 + _tmp55 + _tmp56); + const Scalar _tmp59 = _tmp53 + _tmp54 - _tmp55 - _tmp56; + const Scalar _tmp60 = _tmp39 * _tmp59; + const Scalar _tmp61 = + _tmp4 * _tmp60 - _tmp42 * _tmp59 + _tmp43 * (_tmp46 - _tmp47 - _tmp48 + _tmp49); + const Scalar _tmp62 = _tmp21 * _tmp60 + _tmp44 - _tmp51 * _tmp59; + const Scalar _tmp63 = _tmp27 * _tmp60 + _tmp33 * _tmp43 - _tmp57 * _tmp59; + const Scalar _tmp64 = _tmp39 * _tmp50; + const Scalar _tmp65 = _tmp4 * _tmp64 - _tmp42 * _tmp50 + _tmp43 * _tmp59; + const Scalar _tmp66 = + _tmp21 * _tmp64 + _tmp43 * (-_tmp29 + _tmp30 - _tmp31 + _tmp32) - _tmp50 * _tmp51; + const Scalar _tmp67 = _tmp27 * _tmp64 + _tmp44 - _tmp50 * _tmp57; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp16; + _res(1, 0) = _tmp22; + _res(2, 0) = _tmp28; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp45; + _jacobian(1, 0) = _tmp52; + _jacobian(2, 0) = _tmp58; + _jacobian(0, 1) = _tmp61; + _jacobian(1, 1) = _tmp62; + _jacobian(2, 1) = _tmp63; + _jacobian(0, 2) = _tmp65; + _jacobian(1, 2) = _tmp66; + _jacobian(2, 2) = _tmp67; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp45, Scalar(2)) + std::pow(_tmp52, Scalar(2)) + std::pow(_tmp58, Scalar(2)); + _hessian(1, 0) = _tmp45 * _tmp61 + _tmp52 * _tmp62 + _tmp58 * _tmp63; + _hessian(2, 0) = _tmp45 * _tmp65 + _tmp52 * _tmp66 + _tmp58 * _tmp67; + _hessian(1, 1) = + std::pow(_tmp61, Scalar(2)) + std::pow(_tmp62, Scalar(2)) + std::pow(_tmp63, Scalar(2)); + _hessian(2, 1) = _tmp61 * _tmp65 + _tmp62 * _tmp66 + _tmp63 * _tmp67; + _hessian(2, 2) = + std::pow(_tmp65, Scalar(2)) + std::pow(_tmp66, Scalar(2)) + std::pow(_tmp67, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp16 * _tmp45 + _tmp22 * _tmp52 + _tmp28 * _tmp58; + _rhs(1, 0) = _tmp16 * _tmp61 + _tmp22 * _tmp62 + _tmp28 * _tmp63; + _rhs(2, 0) = _tmp16 * _tmp65 + _tmp22 * _tmp66 + _tmp28 * _tmp67; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_square.h b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_square.h new file mode 100644 index 000000000..91aed9e0c --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_pose3_rotation/prior_factor_pose3_rotation_square.h @@ -0,0 +1,243 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x6) jacobian of res wrt arg value (6) + * hessian: (6x6) Gauss-Newton hessian for arg value (6) + * rhs: (6x1) Gauss-Newton rhs for arg value (6) + */ +template +void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3& prior, + const Eigen::Matrix& sqrt_info, 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: 362 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (106) + const Scalar _tmp0 = _prior[3] * _value[1]; + const Scalar _tmp1 = _prior[0] * _value[2]; + const Scalar _tmp2 = _prior[2] * _value[0]; + const Scalar _tmp3 = _prior[1] * _value[3]; + const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[1] * _value[1]; + const Scalar _tmp7 = _prior[2] * _value[2]; + const Scalar _tmp8 = _prior[0] * _value[0]; + const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; + const Scalar _tmp10 = _tmp5 - _tmp9; + const Scalar _tmp11 = 2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1; + const Scalar _tmp12 = 2 * _tmp11; + const Scalar _tmp13 = 1 - epsilon; + const Scalar _tmp14 = std::min(_tmp13, std::fabs(_tmp10)); + const Scalar _tmp15 = std::acos(_tmp14) / std::sqrt(Scalar(1 - std::pow(_tmp14, Scalar(2)))); + const Scalar _tmp16 = _tmp12 * _tmp15; + const Scalar _tmp17 = _tmp16 * _tmp4; + const Scalar _tmp18 = _prior[0] * _value[1]; + const Scalar _tmp19 = _prior[3] * _value[2]; + const Scalar _tmp20 = _prior[1] * _value[0]; + const Scalar _tmp21 = _prior[2] * _value[3]; + const Scalar _tmp22 = -_tmp18 + _tmp19 + _tmp20 - _tmp21; + const Scalar _tmp23 = _tmp15 * _tmp22; + const Scalar _tmp24 = _tmp12 * _tmp23; + const Scalar _tmp25 = _prior[2] * _value[1]; + const Scalar _tmp26 = _prior[1] * _value[2]; + const Scalar _tmp27 = _prior[3] * _value[0]; + const Scalar _tmp28 = _prior[0] * _value[3]; + const Scalar _tmp29 = _tmp25 - _tmp26 + _tmp27 - _tmp28; + const Scalar _tmp30 = _tmp15 * _tmp29; + const Scalar _tmp31 = _tmp12 * _tmp30; + const Scalar _tmp32 = + _tmp17 * sqrt_info(0, 1) + _tmp24 * sqrt_info(0, 2) + _tmp31 * sqrt_info(0, 0); + const Scalar _tmp33 = _tmp12 * sqrt_info(1, 2); + const Scalar _tmp34 = _tmp17 * sqrt_info(1, 1) + _tmp23 * _tmp33 + _tmp31 * sqrt_info(1, 0); + const Scalar _tmp35 = _tmp4 * sqrt_info(2, 1); + const Scalar _tmp36 = _tmp12 * sqrt_info(2, 0); + const Scalar _tmp37 = _tmp16 * _tmp35 + _tmp24 * sqrt_info(2, 2) + _tmp30 * _tmp36; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp42 = _tmp38 - _tmp39 + _tmp40 - _tmp41; + const Scalar _tmp43 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); + const Scalar _tmp44 = std::min(_tmp13, _tmp43); + const Scalar _tmp45 = 1 - std::pow(_tmp44, Scalar(2)); + const Scalar _tmp46 = _tmp11 * ((((_tmp13 - _tmp43) > 0) - ((_tmp13 - _tmp43) < 0)) + 1) * + (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); + const Scalar _tmp47 = _tmp46 / _tmp45; + const Scalar _tmp48 = _tmp4 * _tmp47; + const Scalar _tmp49 = _tmp48 * sqrt_info(0, 1); + const Scalar _tmp50 = _tmp29 * _tmp47; + const Scalar _tmp51 = _tmp42 * _tmp50; + const Scalar _tmp52 = _tmp22 * _tmp47; + const Scalar _tmp53 = _tmp42 * _tmp52; + const Scalar _tmp54 = std::acos(_tmp44); + const Scalar _tmp55 = _tmp44 * _tmp46 * _tmp54 / (_tmp45 * std::sqrt(_tmp45)); + const Scalar _tmp56 = _tmp42 * _tmp55; + const Scalar _tmp57 = _tmp29 * _tmp56; + const Scalar _tmp58 = _tmp4 * sqrt_info(0, 1); + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp63 = -_tmp59 + _tmp60 + _tmp61 - _tmp62; + const Scalar _tmp64 = _tmp54 / std::sqrt(_tmp45); + const Scalar _tmp65 = _tmp12 * _tmp64; + const Scalar _tmp66 = _tmp63 * _tmp65; + const Scalar _tmp67 = _tmp22 * sqrt_info(0, 2); + const Scalar _tmp68 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp69 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp70 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp71 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp72 = _tmp64 * (-_tmp68 - _tmp69 + _tmp70 + _tmp71); + const Scalar _tmp73 = _tmp12 * sqrt_info(0, 2); + const Scalar _tmp74 = + _tmp64 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); + const Scalar _tmp75 = _tmp12 * _tmp74; + const Scalar _tmp76 = -_tmp42 * _tmp49 - _tmp51 * sqrt_info(0, 0) - _tmp53 * sqrt_info(0, 2) + + _tmp56 * _tmp58 + _tmp56 * _tmp67 + _tmp57 * sqrt_info(0, 0) + + _tmp66 * sqrt_info(0, 1) + _tmp72 * _tmp73 + _tmp75 * sqrt_info(0, 0); + const Scalar _tmp77 = _tmp48 * sqrt_info(1, 1); + const Scalar _tmp78 = _tmp29 * sqrt_info(1, 0); + const Scalar _tmp79 = _tmp4 * sqrt_info(1, 1); + const Scalar _tmp80 = _tmp22 * sqrt_info(1, 2); + const Scalar _tmp81 = _tmp33 * _tmp72 - _tmp42 * _tmp77 - _tmp51 * sqrt_info(1, 0) - + _tmp53 * sqrt_info(1, 2) + _tmp56 * _tmp78 + _tmp56 * _tmp79 + + _tmp56 * _tmp80 + _tmp66 * sqrt_info(1, 1) + _tmp75 * sqrt_info(1, 0); + const Scalar _tmp82 = _tmp35 * _tmp47; + const Scalar _tmp83 = _tmp50 * sqrt_info(2, 0); + const Scalar _tmp84 = _tmp22 * sqrt_info(2, 2); + const Scalar _tmp85 = _tmp12 * _tmp72 * sqrt_info(2, 2) + _tmp35 * _tmp56 + _tmp36 * _tmp74 - + _tmp42 * _tmp82 - _tmp42 * _tmp83 - _tmp53 * sqrt_info(2, 2) + + _tmp56 * _tmp84 + _tmp57 * sqrt_info(2, 0) + _tmp66 * sqrt_info(2, 1); + const Scalar _tmp86 = _tmp68 + _tmp69 - _tmp70 - _tmp71; + const Scalar _tmp87 = _tmp50 * _tmp86; + const Scalar _tmp88 = _tmp52 * _tmp86; + const Scalar _tmp89 = _tmp55 * _tmp86; + const Scalar _tmp90 = _tmp29 * _tmp89; + const Scalar _tmp91 = _tmp42 * _tmp65; + const Scalar _tmp92 = _tmp64 * (_tmp59 - _tmp60 - _tmp61 + _tmp62); + const Scalar _tmp93 = _tmp12 * _tmp92; + const Scalar _tmp94 = -_tmp49 * _tmp86 + _tmp58 * _tmp89 + _tmp67 * _tmp89 + + _tmp75 * sqrt_info(0, 1) - _tmp87 * sqrt_info(0, 0) - + _tmp88 * sqrt_info(0, 2) + _tmp90 * sqrt_info(0, 0) + + _tmp91 * sqrt_info(0, 2) + _tmp93 * sqrt_info(0, 0); + const Scalar _tmp95 = _tmp33 * _tmp42 * _tmp64 + _tmp75 * sqrt_info(1, 1) - _tmp77 * _tmp86 + + _tmp78 * _tmp89 + _tmp79 * _tmp89 + _tmp80 * _tmp89 - + _tmp87 * sqrt_info(1, 0) - _tmp88 * sqrt_info(1, 2) + + _tmp93 * sqrt_info(1, 0); + const Scalar _tmp96 = _tmp35 * _tmp89 + _tmp36 * _tmp92 + _tmp75 * sqrt_info(2, 1) - + _tmp82 * _tmp86 - _tmp83 * _tmp86 + _tmp84 * _tmp89 - + _tmp88 * sqrt_info(2, 2) + _tmp90 * sqrt_info(2, 0) + + _tmp91 * sqrt_info(2, 2); + const Scalar _tmp97 = _tmp50 * _tmp63; + const Scalar _tmp98 = _tmp55 * _tmp63; + const Scalar _tmp99 = _tmp29 * _tmp98; + const Scalar _tmp100 = _tmp52 * _tmp63; + const Scalar _tmp101 = _tmp65 * (-_tmp38 + _tmp39 - _tmp40 + _tmp41); + const Scalar _tmp102 = _tmp65 * _tmp86; + const Scalar _tmp103 = -_tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 1) + + _tmp102 * sqrt_info(0, 0) - _tmp49 * _tmp63 + _tmp58 * _tmp98 + + _tmp67 * _tmp98 + _tmp73 * _tmp74 - _tmp97 * sqrt_info(0, 0) + + _tmp99 * sqrt_info(0, 0); + const Scalar _tmp104 = -_tmp100 * sqrt_info(1, 2) + _tmp101 * sqrt_info(1, 1) + + _tmp102 * sqrt_info(1, 0) + _tmp33 * _tmp74 - _tmp63 * _tmp77 + + _tmp78 * _tmp98 + _tmp79 * _tmp98 + _tmp80 * _tmp98 - + _tmp97 * sqrt_info(1, 0); + const Scalar _tmp105 = -_tmp100 * sqrt_info(2, 2) + _tmp101 * sqrt_info(2, 1) + _tmp35 * _tmp98 + + _tmp36 * _tmp64 * _tmp86 - _tmp63 * _tmp82 - _tmp63 * _tmp83 + + _tmp75 * sqrt_info(2, 2) + _tmp84 * _tmp98 + _tmp99 * sqrt_info(2, 0); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp32; + _res(1, 0) = _tmp34; + _res(2, 0) = _tmp37; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp76; + _jacobian(1, 0) = _tmp81; + _jacobian(2, 0) = _tmp85; + _jacobian(0, 1) = _tmp94; + _jacobian(1, 1) = _tmp95; + _jacobian(2, 1) = _tmp96; + _jacobian(0, 2) = _tmp103; + _jacobian(1, 2) = _tmp104; + _jacobian(2, 2) = _tmp105; + _jacobian(0, 3) = 0; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = 0; + _jacobian(1, 4) = 0; + _jacobian(2, 4) = 0; + _jacobian(0, 5) = 0; + _jacobian(1, 5) = 0; + _jacobian(2, 5) = 0; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian.setZero(); + + _hessian(0, 0) = + std::pow(_tmp76, Scalar(2)) + std::pow(_tmp81, Scalar(2)) + std::pow(_tmp85, Scalar(2)); + _hessian(1, 0) = _tmp76 * _tmp94 + _tmp81 * _tmp95 + _tmp85 * _tmp96; + _hessian(2, 0) = _tmp103 * _tmp76 + _tmp104 * _tmp81 + _tmp105 * _tmp85; + _hessian(1, 1) = + std::pow(_tmp94, Scalar(2)) + std::pow(_tmp95, Scalar(2)) + std::pow(_tmp96, Scalar(2)); + _hessian(2, 1) = _tmp103 * _tmp94 + _tmp104 * _tmp95 + _tmp105 * _tmp96; + _hessian(2, 2) = + std::pow(_tmp103, Scalar(2)) + std::pow(_tmp104, Scalar(2)) + std::pow(_tmp105, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp32 * _tmp76 + _tmp34 * _tmp81 + _tmp37 * _tmp85; + _rhs(1, 0) = _tmp32 * _tmp94 + _tmp34 * _tmp95 + _tmp37 * _tmp96; + _rhs(2, 0) = _tmp103 * _tmp32 + _tmp104 * _tmp34 + _tmp105 * _tmp37; + _rhs(3, 0) = 0; + _rhs(4, 0) = 0; + _rhs(5, 0) = 0; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot2.h b/gen/cpp/sym/factors/prior_factor_rot2.h index 77470a2ae..ad238bca5 100644 --- a/gen/cpp/sym/factors/prior_factor_rot2.h +++ b/gen/cpp/sym/factors/prior_factor_rot2.h @@ -1,86 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (1x1) jacobian of res wrt arg value (1) - * hessian: (1x1) Gauss-Newton hessian for arg value (1) - * rhs: (1x1) Gauss-Newton rhs for arg value (1) - */ -template -void PriorFactorRot2(const sym::Rot2& value, const sym::Rot2& prior, - const Eigen::Matrix& sqrt_info, 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: 32 - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - const Eigen::Matrix& _prior = prior.Data(); - - // Intermediate terms (11) - const Scalar _tmp0 = _prior[1] * _value[0]; - const Scalar _tmp1 = _prior[0] * _value[1]; - const Scalar _tmp2 = -_tmp0 + _tmp1; - const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; - const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); - const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); - const Scalar _tmp6 = std::pow(_tmp4, Scalar(2)); - const Scalar _tmp7 = std::pow(_tmp2, Scalar(2)) + _tmp6; - const Scalar _tmp8 = -_tmp2 * (_tmp0 - _tmp1) / _tmp6 + _tmp3 / _tmp4; - const Scalar _tmp9 = _tmp6 * _tmp8 / _tmp7; - const Scalar _tmp10 = std::pow(sqrt_info(0, 0), Scalar(2)); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp5 * sqrt_info(0, 0); - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp9 * sqrt_info(0, 0); - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = _tmp10 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp8, Scalar(2)) / - std::pow(_tmp7, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp10 * _tmp5 * _tmp9; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_rot2/prior_factor_rot2_diagonal.h" +#include "./prior_factor_rot2/prior_factor_rot2_isotropic.h" +#include "./prior_factor_rot2/prior_factor_rot2_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_diagonal.h b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_diagonal.h new file mode 100644 index 000000000..77a3f73d5 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_diagonal.h @@ -0,0 +1,87 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x1) jacobian of res wrt arg value (1) + * hessian: (1x1) Gauss-Newton hessian for arg value (1) + * rhs: (1x1) Gauss-Newton rhs for arg value (1) + */ +template +void PriorFactorRot2(const sym::Rot2& value, const sym::Rot2& prior, + const Eigen::Matrix& sqrt_info, 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: 32 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (11) + const Scalar _tmp0 = _prior[1] * _value[0]; + const Scalar _tmp1 = _prior[0] * _value[1]; + const Scalar _tmp2 = -_tmp0 + _tmp1; + const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); + const Scalar _tmp6 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp7 = std::pow(_tmp2, Scalar(2)) + _tmp6; + const Scalar _tmp8 = -_tmp2 * (_tmp0 - _tmp1) / _tmp6 + _tmp3 / _tmp4; + const Scalar _tmp9 = _tmp6 * _tmp8 / _tmp7; + const Scalar _tmp10 = std::pow(sqrt_info(0, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp5 * sqrt_info(0, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp9 * sqrt_info(0, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = _tmp10 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp8, Scalar(2)) / + std::pow(_tmp7, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp10 * _tmp5 * _tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_isotropic.h b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_isotropic.h new file mode 100644 index 000000000..eccfa01fd --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_isotropic.h @@ -0,0 +1,87 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x1) jacobian of res wrt arg value (1) + * hessian: (1x1) Gauss-Newton hessian for arg value (1) + * rhs: (1x1) Gauss-Newton rhs for arg value (1) + */ +template +void PriorFactorRot2(const sym::Rot2& value, const sym::Rot2& prior, + const Scalar sqrt_info, 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: 32 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (11) + const Scalar _tmp0 = _prior[1] * _value[0]; + const Scalar _tmp1 = _prior[0] * _value[1]; + const Scalar _tmp2 = -_tmp0 + _tmp1; + const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); + const Scalar _tmp6 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp7 = std::pow(_tmp2, Scalar(2)) + _tmp6; + const Scalar _tmp8 = -_tmp2 * (_tmp0 - _tmp1) / _tmp6 + _tmp3 / _tmp4; + const Scalar _tmp9 = _tmp6 * _tmp8 / _tmp7; + const Scalar _tmp10 = std::pow(sqrt_info, Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp5 * sqrt_info; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp9 * sqrt_info; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = _tmp10 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp8, Scalar(2)) / + std::pow(_tmp7, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp10 * _tmp5 * _tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_square.h b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_square.h new file mode 100644 index 000000000..77a3f73d5 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot2/prior_factor_rot2_square.h @@ -0,0 +1,87 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (1x1) jacobian of res wrt arg value (1) + * hessian: (1x1) Gauss-Newton hessian for arg value (1) + * rhs: (1x1) Gauss-Newton rhs for arg value (1) + */ +template +void PriorFactorRot2(const sym::Rot2& value, const sym::Rot2& prior, + const Eigen::Matrix& sqrt_info, 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: 32 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (11) + const Scalar _tmp0 = _prior[1] * _value[0]; + const Scalar _tmp1 = _prior[0] * _value[1]; + const Scalar _tmp2 = -_tmp0 + _tmp1; + const Scalar _tmp3 = _prior[0] * _value[0] + _prior[1] * _value[1]; + const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5)); + const Scalar _tmp5 = std::atan2(_tmp2, _tmp4); + const Scalar _tmp6 = std::pow(_tmp4, Scalar(2)); + const Scalar _tmp7 = std::pow(_tmp2, Scalar(2)) + _tmp6; + const Scalar _tmp8 = -_tmp2 * (_tmp0 - _tmp1) / _tmp6 + _tmp3 / _tmp4; + const Scalar _tmp9 = _tmp6 * _tmp8 / _tmp7; + const Scalar _tmp10 = std::pow(sqrt_info(0, 0), Scalar(2)); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp5 * sqrt_info(0, 0); + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp9 * sqrt_info(0, 0); + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = _tmp10 * std::pow(_tmp4, Scalar(4)) * std::pow(_tmp8, Scalar(2)) / + std::pow(_tmp7, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp10 * _tmp5 * _tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot3.h b/gen/cpp/sym/factors/prior_factor_rot3.h index 5a96ea560..2bf6f8f9a 100644 --- a/gen/cpp/sym/factors/prior_factor_rot3.h +++ b/gen/cpp/sym/factors/prior_factor_rot3.h @@ -1,237 +1,3 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -#include - -namespace sym { - -/** - * Residual that penalizes the difference between a value and prior (desired / measured value). - * - * In vector space terms that would be: - * prior - value - * - * In lie group terms: - * to_tangent(compose(inverse(value), prior)) - * - * Args: - * sqrt_info: Square root information matrix to whiten residual. This can be computed from - * a covariance matrix as the cholesky decomposition of the inverse. In the case - * of a diagonal it will contain 1/sigma values. Must match the tangent dim. - * jacobian: (3x3) jacobian of res wrt arg value (3) - * hessian: (3x3) Gauss-Newton hessian for arg value (3) - * rhs: (3x1) Gauss-Newton rhs for arg value (3) - */ -template -void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& prior, - const Eigen::Matrix& sqrt_info, 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: 361 - - // Input arrays - const Eigen::Matrix& _value = value.Data(); - const Eigen::Matrix& _prior = prior.Data(); - - // Intermediate terms (109) - const Scalar _tmp0 = _prior[3] * _value[1]; - const Scalar _tmp1 = _prior[0] * _value[2]; - const Scalar _tmp2 = _prior[2] * _value[0]; - const Scalar _tmp3 = _prior[1] * _value[3]; - const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; - const Scalar _tmp5 = _prior[3] * _value[3]; - const Scalar _tmp6 = _prior[0] * _value[0]; - const Scalar _tmp7 = _prior[2] * _value[2]; - const Scalar _tmp8 = _prior[1] * _value[1]; - const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; - const Scalar _tmp10 = _tmp5 - _tmp9; - const Scalar _tmp11 = 2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1; - const Scalar _tmp12 = 2 * _tmp11; - const Scalar _tmp13 = 1 - epsilon; - const Scalar _tmp14 = std::min(_tmp13, std::fabs(_tmp10)); - const Scalar _tmp15 = std::acos(_tmp14) / std::sqrt(Scalar(1 - std::pow(_tmp14, Scalar(2)))); - const Scalar _tmp16 = _tmp12 * _tmp15; - const Scalar _tmp17 = _tmp16 * _tmp4; - const Scalar _tmp18 = _prior[3] * _value[0]; - const Scalar _tmp19 = _prior[0] * _value[3]; - const Scalar _tmp20 = _prior[2] * _value[1]; - const Scalar _tmp21 = _prior[1] * _value[2]; - const Scalar _tmp22 = _tmp18 - _tmp19 + _tmp20 - _tmp21; - const Scalar _tmp23 = _tmp16 * _tmp22; - const Scalar _tmp24 = _prior[3] * _value[2]; - const Scalar _tmp25 = _prior[0] * _value[1]; - const Scalar _tmp26 = _prior[2] * _value[3]; - const Scalar _tmp27 = _prior[1] * _value[0]; - const Scalar _tmp28 = _tmp24 - _tmp25 - _tmp26 + _tmp27; - const Scalar _tmp29 = _tmp16 * _tmp28; - const Scalar _tmp30 = - _tmp17 * sqrt_info(0, 1) + _tmp23 * sqrt_info(0, 0) + _tmp29 * sqrt_info(0, 2); - const Scalar _tmp31 = _tmp12 * sqrt_info(1, 1); - const Scalar _tmp32 = - _tmp15 * _tmp31 * _tmp4 + _tmp23 * sqrt_info(1, 0) + _tmp29 * sqrt_info(1, 2); - const Scalar _tmp33 = _tmp22 * sqrt_info(2, 0); - const Scalar _tmp34 = _tmp16 * _tmp33 + _tmp17 * sqrt_info(2, 1) + _tmp29 * sqrt_info(2, 2); - const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp18; - const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp19; - const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp20; - const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp21; - const Scalar _tmp39 = _tmp35 - _tmp36 + _tmp37 - _tmp38; - const Scalar _tmp40 = _tmp39 * sqrt_info(0, 2); - const Scalar _tmp41 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); - const Scalar _tmp42 = std::min(_tmp13, _tmp41); - const Scalar _tmp43 = std::acos(_tmp42); - const Scalar _tmp44 = 1 - std::pow(_tmp42, Scalar(2)); - const Scalar _tmp45 = _tmp11 * ((((_tmp13 - _tmp41) > 0) - ((_tmp13 - _tmp41) < 0)) + 1) * - (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); - const Scalar _tmp46 = _tmp42 * _tmp45 / (_tmp44 * std::sqrt(_tmp44)); - const Scalar _tmp47 = _tmp43 * _tmp46; - const Scalar _tmp48 = _tmp28 * _tmp47; - const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp24; - const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp26; - const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp27; - const Scalar _tmp53 = _tmp49 - _tmp50 - _tmp51 + _tmp52; - const Scalar _tmp54 = _tmp43 * _tmp53; - const Scalar _tmp55 = std::pow(_tmp44, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp56 = _tmp12 * _tmp55; - const Scalar _tmp57 = _tmp54 * _tmp56; - const Scalar _tmp58 = _tmp22 * sqrt_info(0, 0); - const Scalar _tmp59 = _tmp39 * _tmp47; - const Scalar _tmp60 = _tmp45 / _tmp44; - const Scalar _tmp61 = _tmp22 * _tmp60; - const Scalar _tmp62 = _tmp61 * sqrt_info(0, 0); - const Scalar _tmp63 = _tmp39 * _tmp60; - const Scalar _tmp64 = _tmp4 * _tmp63; - const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp0; - const Scalar _tmp66 = (Scalar(1) / Scalar(2)) * _tmp1; - const Scalar _tmp67 = (Scalar(1) / Scalar(2)) * _tmp2; - const Scalar _tmp68 = (Scalar(1) / Scalar(2)) * _tmp3; - const Scalar _tmp69 = _tmp43 * _tmp56; - const Scalar _tmp70 = _tmp69 * (-_tmp65 - _tmp66 + _tmp67 + _tmp68); - const Scalar _tmp71 = - _tmp43 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + - (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); - const Scalar _tmp72 = _tmp56 * _tmp71; - const Scalar _tmp73 = _tmp4 * sqrt_info(0, 1); - const Scalar _tmp74 = _tmp28 * _tmp60; - const Scalar _tmp75 = -_tmp39 * _tmp62 + _tmp40 * _tmp48 - _tmp40 * _tmp74 + - _tmp57 * sqrt_info(0, 1) + _tmp58 * _tmp59 + _tmp59 * _tmp73 - - _tmp64 * sqrt_info(0, 1) + _tmp70 * sqrt_info(0, 2) + - _tmp72 * sqrt_info(0, 0); - const Scalar _tmp76 = _tmp39 * _tmp48; - const Scalar _tmp77 = _tmp31 * _tmp55; - const Scalar _tmp78 = _tmp22 * sqrt_info(1, 0); - const Scalar _tmp79 = _tmp61 * sqrt_info(1, 0); - const Scalar _tmp80 = _tmp4 * sqrt_info(1, 1); - const Scalar _tmp81 = _tmp39 * _tmp74; - const Scalar _tmp82 = -_tmp39 * _tmp79 + _tmp54 * _tmp77 + _tmp59 * _tmp78 + _tmp59 * _tmp80 - - _tmp64 * sqrt_info(1, 1) + _tmp70 * sqrt_info(1, 2) + - _tmp72 * sqrt_info(1, 0) + _tmp76 * sqrt_info(1, 2) - - _tmp81 * sqrt_info(1, 2); - const Scalar _tmp83 = _tmp4 * sqrt_info(2, 1); - const Scalar _tmp84 = _tmp33 * _tmp59 - _tmp33 * _tmp63 + _tmp57 * sqrt_info(2, 1) + - _tmp59 * _tmp83 - _tmp63 * _tmp83 + _tmp70 * sqrt_info(2, 2) + - _tmp72 * sqrt_info(2, 0) + _tmp76 * sqrt_info(2, 2) - - _tmp81 * sqrt_info(2, 2); - const Scalar _tmp85 = _tmp65 + _tmp66 - _tmp67 - _tmp68; - const Scalar _tmp86 = _tmp47 * _tmp85; - const Scalar _tmp87 = _tmp4 * _tmp86; - const Scalar _tmp88 = _tmp60 * _tmp85; - const Scalar _tmp89 = _tmp4 * _tmp88; - const Scalar _tmp90 = _tmp48 * _tmp85; - const Scalar _tmp91 = _tmp69 * (-_tmp49 + _tmp50 + _tmp51 - _tmp52); - const Scalar _tmp92 = _tmp74 * _tmp85; - const Scalar _tmp93 = _tmp40 * _tmp69 + _tmp58 * _tmp86 - _tmp62 * _tmp85 + - _tmp72 * sqrt_info(0, 1) + _tmp87 * sqrt_info(0, 1) - - _tmp89 * sqrt_info(0, 1) + _tmp90 * sqrt_info(0, 2) + - _tmp91 * sqrt_info(0, 0) - _tmp92 * sqrt_info(0, 2); - const Scalar _tmp94 = _tmp39 * _tmp69; - const Scalar _tmp95 = _tmp71 * _tmp77 + _tmp78 * _tmp86 - _tmp79 * _tmp85 + - _tmp87 * sqrt_info(1, 1) - _tmp89 * sqrt_info(1, 1) + - _tmp90 * sqrt_info(1, 2) + _tmp91 * sqrt_info(1, 0) - - _tmp92 * sqrt_info(1, 2) + _tmp94 * sqrt_info(1, 2); - const Scalar _tmp96 = _tmp74 * sqrt_info(2, 2); - const Scalar _tmp97 = _tmp33 * _tmp86 - _tmp33 * _tmp88 + _tmp72 * sqrt_info(2, 1) + - _tmp83 * _tmp86 - _tmp83 * _tmp88 - _tmp85 * _tmp96 + - _tmp90 * sqrt_info(2, 2) + _tmp91 * sqrt_info(2, 0) + - _tmp94 * sqrt_info(2, 2); - const Scalar _tmp98 = _tmp46 * _tmp54; - const Scalar _tmp99 = _tmp69 * _tmp85; - const Scalar _tmp100 = _tmp53 * _tmp74; - const Scalar _tmp101 = _tmp28 * _tmp98; - const Scalar _tmp102 = -_tmp35 + _tmp36 - _tmp37 + _tmp38; - const Scalar _tmp103 = _tmp102 * _tmp69; - const Scalar _tmp104 = _tmp53 * _tmp60; - const Scalar _tmp105 = _tmp104 * _tmp4; - const Scalar _tmp106 = -_tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 2) + - _tmp103 * sqrt_info(0, 1) - _tmp105 * sqrt_info(0, 1) - _tmp53 * _tmp62 + - _tmp58 * _tmp98 + _tmp72 * sqrt_info(0, 2) + _tmp73 * _tmp98 + - _tmp99 * sqrt_info(0, 0); - const Scalar _tmp107 = -_tmp100 * sqrt_info(1, 2) + _tmp101 * sqrt_info(1, 2) + - _tmp102 * _tmp43 * _tmp77 - _tmp105 * sqrt_info(1, 1) - _tmp53 * _tmp79 + - _tmp72 * sqrt_info(1, 2) + _tmp78 * _tmp98 + _tmp80 * _tmp98 + - _tmp99 * sqrt_info(1, 0); - const Scalar _tmp108 = _tmp101 * sqrt_info(2, 2) + _tmp103 * sqrt_info(2, 1) - _tmp104 * _tmp33 - - _tmp104 * _tmp83 + _tmp33 * _tmp98 - _tmp53 * _tmp96 + - _tmp72 * sqrt_info(2, 2) + _tmp83 * _tmp98 + _tmp99 * sqrt_info(2, 0); - - // Output terms (4) - if (res != nullptr) { - Eigen::Matrix& _res = (*res); - - _res(0, 0) = _tmp30; - _res(1, 0) = _tmp32; - _res(2, 0) = _tmp34; - } - - if (jacobian != nullptr) { - Eigen::Matrix& _jacobian = (*jacobian); - - _jacobian(0, 0) = _tmp75; - _jacobian(1, 0) = _tmp82; - _jacobian(2, 0) = _tmp84; - _jacobian(0, 1) = _tmp93; - _jacobian(1, 1) = _tmp95; - _jacobian(2, 1) = _tmp97; - _jacobian(0, 2) = _tmp106; - _jacobian(1, 2) = _tmp107; - _jacobian(2, 2) = _tmp108; - } - - if (hessian != nullptr) { - Eigen::Matrix& _hessian = (*hessian); - - _hessian(0, 0) = - std::pow(_tmp75, Scalar(2)) + std::pow(_tmp82, Scalar(2)) + std::pow(_tmp84, Scalar(2)); - _hessian(1, 0) = _tmp75 * _tmp93 + _tmp82 * _tmp95 + _tmp84 * _tmp97; - _hessian(2, 0) = _tmp106 * _tmp75 + _tmp107 * _tmp82 + _tmp108 * _tmp84; - _hessian(0, 1) = 0; - _hessian(1, 1) = - std::pow(_tmp93, Scalar(2)) + std::pow(_tmp95, Scalar(2)) + std::pow(_tmp97, Scalar(2)); - _hessian(2, 1) = _tmp106 * _tmp93 + _tmp107 * _tmp95 + _tmp108 * _tmp97; - _hessian(0, 2) = 0; - _hessian(1, 2) = 0; - _hessian(2, 2) = - std::pow(_tmp106, Scalar(2)) + std::pow(_tmp107, Scalar(2)) + std::pow(_tmp108, Scalar(2)); - } - - if (rhs != nullptr) { - Eigen::Matrix& _rhs = (*rhs); - - _rhs(0, 0) = _tmp30 * _tmp75 + _tmp32 * _tmp82 + _tmp34 * _tmp84; - _rhs(1, 0) = _tmp30 * _tmp93 + _tmp32 * _tmp95 + _tmp34 * _tmp97; - _rhs(2, 0) = _tmp106 * _tmp30 + _tmp107 * _tmp32 + _tmp108 * _tmp34; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym +#include "./prior_factor_rot3/prior_factor_rot3_diagonal.h" +#include "./prior_factor_rot3/prior_factor_rot3_isotropic.h" +#include "./prior_factor_rot3/prior_factor_rot3_square.h" diff --git a/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_diagonal.h b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_diagonal.h new file mode 100644 index 000000000..e64a98349 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_diagonal.h @@ -0,0 +1,180 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& prior, + const Eigen::Matrix& sqrt_info, 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: 212 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (77) + const Scalar _tmp0 = _prior[3] * _value[0]; + const Scalar _tmp1 = _prior[0] * _value[3]; + const Scalar _tmp2 = _prior[2] * _value[1]; + const Scalar _tmp3 = _prior[1] * _value[2]; + const Scalar _tmp4 = _tmp0 - _tmp1 + _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[0] * _value[0]; + const Scalar _tmp7 = _prior[2] * _value[2]; + const Scalar _tmp8 = _prior[1] * _value[1]; + const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; + const Scalar _tmp10 = _tmp5 - _tmp9; + const Scalar _tmp11 = 2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1; + const Scalar _tmp12 = 2 * _tmp11; + const Scalar _tmp13 = _tmp12 * sqrt_info(0, 0); + const Scalar _tmp14 = 1 - epsilon; + const Scalar _tmp15 = std::min(_tmp14, std::fabs(_tmp10)); + const Scalar _tmp16 = std::acos(_tmp15) / std::sqrt(Scalar(1 - std::pow(_tmp15, Scalar(2)))); + const Scalar _tmp17 = _tmp13 * _tmp16 * _tmp4; + const Scalar _tmp18 = _prior[3] * _value[1]; + const Scalar _tmp19 = _prior[0] * _value[2]; + const Scalar _tmp20 = _prior[2] * _value[0]; + const Scalar _tmp21 = _prior[1] * _value[3]; + const Scalar _tmp22 = _tmp18 + _tmp19 - _tmp20 - _tmp21; + const Scalar _tmp23 = _tmp12 * sqrt_info(1, 0); + const Scalar _tmp24 = _tmp16 * _tmp22 * _tmp23; + const Scalar _tmp25 = _prior[3] * _value[2]; + const Scalar _tmp26 = _prior[0] * _value[1]; + const Scalar _tmp27 = _prior[2] * _value[3]; + const Scalar _tmp28 = _prior[1] * _value[0]; + const Scalar _tmp29 = _tmp25 - _tmp26 - _tmp27 + _tmp28; + const Scalar _tmp30 = _tmp12 * sqrt_info(2, 0); + const Scalar _tmp31 = _tmp16 * _tmp29 * _tmp30; + const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp36 = _tmp32 - _tmp33 + _tmp34 - _tmp35; + const Scalar _tmp37 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); + const Scalar _tmp38 = std::min(_tmp14, _tmp37); + const Scalar _tmp39 = 1 - std::pow(_tmp38, Scalar(2)); + const Scalar _tmp40 = _tmp11 * ((((_tmp14 - _tmp37) > 0) - ((_tmp14 - _tmp37) < 0)) + 1) * + (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); + const Scalar _tmp41 = _tmp40 / _tmp39; + const Scalar _tmp42 = _tmp36 * _tmp41; + const Scalar _tmp43 = _tmp4 * sqrt_info(0, 0); + const Scalar _tmp44 = std::acos(_tmp38); + const Scalar _tmp45 = _tmp38 * _tmp40 * _tmp44 / (_tmp39 * std::sqrt(_tmp39)); + const Scalar _tmp46 = _tmp36 * _tmp45; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp48 = _tmp44 / std::sqrt(_tmp39); + const Scalar _tmp49 = _tmp13 * _tmp48; + const Scalar _tmp50 = -_tmp42 * _tmp43 + _tmp43 * _tmp46 + _tmp47 * _tmp49; + const Scalar _tmp51 = _tmp22 * sqrt_info(1, 0); + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp56 = _tmp52 - _tmp53 - _tmp54 + _tmp55; + const Scalar _tmp57 = _tmp23 * _tmp48; + const Scalar _tmp58 = -_tmp42 * _tmp51 + _tmp46 * _tmp51 + _tmp56 * _tmp57; + const Scalar _tmp59 = _tmp29 * sqrt_info(2, 0); + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp64 = _tmp30 * _tmp48; + const Scalar _tmp65 = + -_tmp42 * _tmp59 + _tmp46 * _tmp59 + _tmp64 * (-_tmp60 - _tmp61 + _tmp62 + _tmp63); + const Scalar _tmp66 = _tmp60 + _tmp61 - _tmp62 - _tmp63; + const Scalar _tmp67 = _tmp41 * _tmp66; + const Scalar _tmp68 = _tmp45 * _tmp66; + const Scalar _tmp69 = + -_tmp43 * _tmp67 + _tmp43 * _tmp68 + _tmp49 * (-_tmp52 + _tmp53 + _tmp54 - _tmp55); + const Scalar _tmp70 = _tmp47 * _tmp57 - _tmp51 * _tmp67 + _tmp51 * _tmp68; + const Scalar _tmp71 = _tmp36 * _tmp64 - _tmp59 * _tmp67 + _tmp59 * _tmp68; + const Scalar _tmp72 = _tmp41 * _tmp56; + const Scalar _tmp73 = _tmp45 * _tmp56; + const Scalar _tmp74 = -_tmp43 * _tmp72 + _tmp43 * _tmp73 + _tmp49 * _tmp66; + const Scalar _tmp75 = + -_tmp51 * _tmp72 + _tmp51 * _tmp73 + _tmp57 * (-_tmp32 + _tmp33 - _tmp34 + _tmp35); + const Scalar _tmp76 = _tmp47 * _tmp64 - _tmp59 * _tmp72 + _tmp59 * _tmp73; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp17; + _res(1, 0) = _tmp24; + _res(2, 0) = _tmp31; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp50; + _jacobian(1, 0) = _tmp58; + _jacobian(2, 0) = _tmp65; + _jacobian(0, 1) = _tmp69; + _jacobian(1, 1) = _tmp70; + _jacobian(2, 1) = _tmp71; + _jacobian(0, 2) = _tmp74; + _jacobian(1, 2) = _tmp75; + _jacobian(2, 2) = _tmp76; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp50, Scalar(2)) + std::pow(_tmp58, Scalar(2)) + std::pow(_tmp65, Scalar(2)); + _hessian(1, 0) = _tmp50 * _tmp69 + _tmp58 * _tmp70 + _tmp65 * _tmp71; + _hessian(2, 0) = _tmp50 * _tmp74 + _tmp58 * _tmp75 + _tmp65 * _tmp76; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp69, Scalar(2)) + std::pow(_tmp70, Scalar(2)) + std::pow(_tmp71, Scalar(2)); + _hessian(2, 1) = _tmp69 * _tmp74 + _tmp70 * _tmp75 + _tmp71 * _tmp76; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp74, Scalar(2)) + std::pow(_tmp75, Scalar(2)) + std::pow(_tmp76, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp17 * _tmp50 + _tmp24 * _tmp58 + _tmp31 * _tmp65; + _rhs(1, 0) = _tmp17 * _tmp69 + _tmp24 * _tmp70 + _tmp31 * _tmp71; + _rhs(2, 0) = _tmp17 * _tmp74 + _tmp24 * _tmp75 + _tmp31 * _tmp76; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_isotropic.h b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_isotropic.h new file mode 100644 index 000000000..9ce5aedfd --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_isotropic.h @@ -0,0 +1,175 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& prior, + const Scalar sqrt_info, 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: 202 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (69) + const Scalar _tmp0 = _prior[3] * _value[0]; + const Scalar _tmp1 = _prior[0] * _value[3]; + const Scalar _tmp2 = _prior[2] * _value[1]; + const Scalar _tmp3 = _prior[1] * _value[2]; + const Scalar _tmp4 = _tmp0 - _tmp1 + _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[0] * _value[0]; + const Scalar _tmp7 = _prior[2] * _value[2]; + const Scalar _tmp8 = _prior[1] * _value[1]; + const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; + const Scalar _tmp10 = _tmp5 - _tmp9; + const Scalar _tmp11 = 1 - epsilon; + const Scalar _tmp12 = std::min(_tmp11, std::fabs(_tmp10)); + const Scalar _tmp13 = + sqrt_info * (2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1); + const Scalar _tmp14 = 2 * _tmp13; + const Scalar _tmp15 = + _tmp14 * std::acos(_tmp12) / std::sqrt(Scalar(1 - std::pow(_tmp12, Scalar(2)))); + const Scalar _tmp16 = _tmp15 * _tmp4; + const Scalar _tmp17 = _prior[3] * _value[1]; + const Scalar _tmp18 = _prior[0] * _value[2]; + const Scalar _tmp19 = _prior[2] * _value[0]; + const Scalar _tmp20 = _prior[1] * _value[3]; + const Scalar _tmp21 = _tmp17 + _tmp18 - _tmp19 - _tmp20; + const Scalar _tmp22 = _tmp15 * _tmp21; + const Scalar _tmp23 = _prior[3] * _value[2]; + const Scalar _tmp24 = _prior[0] * _value[1]; + const Scalar _tmp25 = _prior[2] * _value[3]; + const Scalar _tmp26 = _prior[1] * _value[0]; + const Scalar _tmp27 = _tmp23 - _tmp24 - _tmp25 + _tmp26; + const Scalar _tmp28 = _tmp15 * _tmp27; + const Scalar _tmp29 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); + const Scalar _tmp30 = std::min(_tmp11, _tmp29); + const Scalar _tmp31 = 1 - std::pow(_tmp30, Scalar(2)); + const Scalar _tmp32 = std::acos(_tmp30); + const Scalar _tmp33 = _tmp14 * _tmp32 / std::sqrt(_tmp31); + const Scalar _tmp34 = + _tmp33 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp39 = _tmp35 - _tmp36 + _tmp37 - _tmp38; + const Scalar _tmp40 = _tmp13 * ((((_tmp11 - _tmp29) > 0) - ((_tmp11 - _tmp29) < 0)) + 1) * + (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); + const Scalar _tmp41 = _tmp30 * _tmp32 * _tmp40 / (_tmp31 * std::sqrt(_tmp31)); + const Scalar _tmp42 = _tmp39 * _tmp41; + const Scalar _tmp43 = _tmp40 / _tmp31; + const Scalar _tmp44 = _tmp4 * _tmp43; + const Scalar _tmp45 = _tmp34 - _tmp39 * _tmp44 + _tmp4 * _tmp42; + const Scalar _tmp46 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp50 = _tmp46 - _tmp47 - _tmp48 + _tmp49; + const Scalar _tmp51 = _tmp21 * _tmp43; + const Scalar _tmp52 = _tmp21 * _tmp42 + _tmp33 * _tmp50 - _tmp39 * _tmp51; + const Scalar _tmp53 = _tmp27 * _tmp41; + const Scalar _tmp54 = _tmp27 * _tmp43; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp59 = + _tmp33 * (-_tmp55 - _tmp56 + _tmp57 + _tmp58) + _tmp39 * _tmp53 - _tmp39 * _tmp54; + const Scalar _tmp60 = _tmp55 + _tmp56 - _tmp57 - _tmp58; + const Scalar _tmp61 = _tmp41 * _tmp60; + const Scalar _tmp62 = + _tmp33 * (-_tmp46 + _tmp47 + _tmp48 - _tmp49) + _tmp4 * _tmp61 - _tmp44 * _tmp60; + const Scalar _tmp63 = _tmp21 * _tmp61 + _tmp34 - _tmp51 * _tmp60; + const Scalar _tmp64 = _tmp33 * _tmp39 + _tmp53 * _tmp60 - _tmp54 * _tmp60; + const Scalar _tmp65 = _tmp41 * _tmp50; + const Scalar _tmp66 = _tmp33 * _tmp60 + _tmp4 * _tmp65 - _tmp44 * _tmp50; + const Scalar _tmp67 = + _tmp21 * _tmp65 + _tmp33 * (-_tmp35 + _tmp36 - _tmp37 + _tmp38) - _tmp50 * _tmp51; + const Scalar _tmp68 = _tmp34 + _tmp50 * _tmp53 - _tmp50 * _tmp54; + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp16; + _res(1, 0) = _tmp22; + _res(2, 0) = _tmp28; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp45; + _jacobian(1, 0) = _tmp52; + _jacobian(2, 0) = _tmp59; + _jacobian(0, 1) = _tmp62; + _jacobian(1, 1) = _tmp63; + _jacobian(2, 1) = _tmp64; + _jacobian(0, 2) = _tmp66; + _jacobian(1, 2) = _tmp67; + _jacobian(2, 2) = _tmp68; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp45, Scalar(2)) + std::pow(_tmp52, Scalar(2)) + std::pow(_tmp59, Scalar(2)); + _hessian(1, 0) = _tmp45 * _tmp62 + _tmp52 * _tmp63 + _tmp59 * _tmp64; + _hessian(2, 0) = _tmp45 * _tmp66 + _tmp52 * _tmp67 + _tmp59 * _tmp68; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp62, Scalar(2)) + std::pow(_tmp63, Scalar(2)) + std::pow(_tmp64, Scalar(2)); + _hessian(2, 1) = _tmp62 * _tmp66 + _tmp63 * _tmp67 + _tmp64 * _tmp68; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp66, Scalar(2)) + std::pow(_tmp67, Scalar(2)) + std::pow(_tmp68, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp16 * _tmp45 + _tmp22 * _tmp52 + _tmp28 * _tmp59; + _rhs(1, 0) = _tmp16 * _tmp62 + _tmp22 * _tmp63 + _tmp28 * _tmp64; + _rhs(2, 0) = _tmp16 * _tmp66 + _tmp22 * _tmp67 + _tmp28 * _tmp68; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_square.h b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_square.h new file mode 100644 index 000000000..4ab84d614 --- /dev/null +++ b/gen/cpp/sym/factors/prior_factor_rot3/prior_factor_rot3_square.h @@ -0,0 +1,238 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +#include + +namespace sym { + +/** + * Residual that penalizes the difference between a value and prior (desired / measured value). + * + * In vector space terms that would be: + * prior - value + * + * In lie group terms: + * to_tangent(compose(inverse(value), prior)) + * + * Args: + * sqrt_info: Square root information matrix to whiten residual. This can be computed from + * a covariance matrix as the cholesky decomposition of the inverse. In the case + * of a diagonal it will contain 1/sigma values. Pass in a single scalar if + * isotropic, a vector if diagonal, and a square matrix otherwise. + * jacobian: (3x3) jacobian of res wrt arg value (3) + * hessian: (3x3) Gauss-Newton hessian for arg value (3) + * rhs: (3x1) Gauss-Newton rhs for arg value (3) + */ +template +void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& prior, + const Eigen::Matrix& sqrt_info, 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: 361 + + // Input arrays + const Eigen::Matrix& _value = value.Data(); + const Eigen::Matrix& _prior = prior.Data(); + + // Intermediate terms (109) + const Scalar _tmp0 = _prior[3] * _value[1]; + const Scalar _tmp1 = _prior[0] * _value[2]; + const Scalar _tmp2 = _prior[2] * _value[0]; + const Scalar _tmp3 = _prior[1] * _value[3]; + const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; + const Scalar _tmp5 = _prior[3] * _value[3]; + const Scalar _tmp6 = _prior[0] * _value[0]; + const Scalar _tmp7 = _prior[2] * _value[2]; + const Scalar _tmp8 = _prior[1] * _value[1]; + const Scalar _tmp9 = -_tmp6 - _tmp7 - _tmp8; + const Scalar _tmp10 = _tmp5 - _tmp9; + const Scalar _tmp11 = 2 * std::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1; + const Scalar _tmp12 = 2 * _tmp11; + const Scalar _tmp13 = 1 - epsilon; + const Scalar _tmp14 = std::min(_tmp13, std::fabs(_tmp10)); + const Scalar _tmp15 = std::acos(_tmp14) / std::sqrt(Scalar(1 - std::pow(_tmp14, Scalar(2)))); + const Scalar _tmp16 = _tmp12 * _tmp15; + const Scalar _tmp17 = _tmp16 * _tmp4; + const Scalar _tmp18 = _prior[3] * _value[0]; + const Scalar _tmp19 = _prior[0] * _value[3]; + const Scalar _tmp20 = _prior[2] * _value[1]; + const Scalar _tmp21 = _prior[1] * _value[2]; + const Scalar _tmp22 = _tmp18 - _tmp19 + _tmp20 - _tmp21; + const Scalar _tmp23 = _tmp16 * _tmp22; + const Scalar _tmp24 = _prior[3] * _value[2]; + const Scalar _tmp25 = _prior[0] * _value[1]; + const Scalar _tmp26 = _prior[2] * _value[3]; + const Scalar _tmp27 = _prior[1] * _value[0]; + const Scalar _tmp28 = _tmp24 - _tmp25 - _tmp26 + _tmp27; + const Scalar _tmp29 = _tmp16 * _tmp28; + const Scalar _tmp30 = + _tmp17 * sqrt_info(0, 1) + _tmp23 * sqrt_info(0, 0) + _tmp29 * sqrt_info(0, 2); + const Scalar _tmp31 = _tmp12 * sqrt_info(1, 1); + const Scalar _tmp32 = + _tmp15 * _tmp31 * _tmp4 + _tmp23 * sqrt_info(1, 0) + _tmp29 * sqrt_info(1, 2); + const Scalar _tmp33 = _tmp22 * sqrt_info(2, 0); + const Scalar _tmp34 = _tmp16 * _tmp33 + _tmp17 * sqrt_info(2, 1) + _tmp29 * sqrt_info(2, 2); + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp39 = _tmp35 - _tmp36 + _tmp37 - _tmp38; + const Scalar _tmp40 = _tmp39 * sqrt_info(0, 2); + const Scalar _tmp41 = std::fabs(_tmp5 + _tmp6 + _tmp7 + _tmp8); + const Scalar _tmp42 = std::min(_tmp13, _tmp41); + const Scalar _tmp43 = std::acos(_tmp42); + const Scalar _tmp44 = 1 - std::pow(_tmp42, Scalar(2)); + const Scalar _tmp45 = _tmp11 * ((((_tmp13 - _tmp41) > 0) - ((_tmp13 - _tmp41) < 0)) + 1) * + (((-_tmp5 + _tmp9) > 0) - ((-_tmp5 + _tmp9) < 0)); + const Scalar _tmp46 = _tmp42 * _tmp45 / (_tmp44 * std::sqrt(_tmp44)); + const Scalar _tmp47 = _tmp43 * _tmp46; + const Scalar _tmp48 = _tmp28 * _tmp47; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp52 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp53 = _tmp49 - _tmp50 - _tmp51 + _tmp52; + const Scalar _tmp54 = _tmp43 * _tmp53; + const Scalar _tmp55 = std::pow(_tmp44, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp56 = _tmp12 * _tmp55; + const Scalar _tmp57 = _tmp54 * _tmp56; + const Scalar _tmp58 = _tmp22 * sqrt_info(0, 0); + const Scalar _tmp59 = _tmp39 * _tmp47; + const Scalar _tmp60 = _tmp45 / _tmp44; + const Scalar _tmp61 = _tmp22 * _tmp60; + const Scalar _tmp62 = _tmp61 * sqrt_info(0, 0); + const Scalar _tmp63 = _tmp39 * _tmp60; + const Scalar _tmp64 = _tmp4 * _tmp63; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp66 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp67 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp68 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp69 = _tmp43 * _tmp56; + const Scalar _tmp70 = _tmp69 * (-_tmp65 - _tmp66 + _tmp67 + _tmp68); + const Scalar _tmp71 = + _tmp43 * ((Scalar(1) / Scalar(2)) * _tmp5 + (Scalar(1) / Scalar(2)) * _tmp6 + + (Scalar(1) / Scalar(2)) * _tmp7 + (Scalar(1) / Scalar(2)) * _tmp8); + const Scalar _tmp72 = _tmp56 * _tmp71; + const Scalar _tmp73 = _tmp4 * sqrt_info(0, 1); + const Scalar _tmp74 = _tmp28 * _tmp60; + const Scalar _tmp75 = -_tmp39 * _tmp62 + _tmp40 * _tmp48 - _tmp40 * _tmp74 + + _tmp57 * sqrt_info(0, 1) + _tmp58 * _tmp59 + _tmp59 * _tmp73 - + _tmp64 * sqrt_info(0, 1) + _tmp70 * sqrt_info(0, 2) + + _tmp72 * sqrt_info(0, 0); + const Scalar _tmp76 = _tmp39 * _tmp48; + const Scalar _tmp77 = _tmp31 * _tmp55; + const Scalar _tmp78 = _tmp22 * sqrt_info(1, 0); + const Scalar _tmp79 = _tmp61 * sqrt_info(1, 0); + const Scalar _tmp80 = _tmp4 * sqrt_info(1, 1); + const Scalar _tmp81 = _tmp39 * _tmp74; + const Scalar _tmp82 = -_tmp39 * _tmp79 + _tmp54 * _tmp77 + _tmp59 * _tmp78 + _tmp59 * _tmp80 - + _tmp64 * sqrt_info(1, 1) + _tmp70 * sqrt_info(1, 2) + + _tmp72 * sqrt_info(1, 0) + _tmp76 * sqrt_info(1, 2) - + _tmp81 * sqrt_info(1, 2); + const Scalar _tmp83 = _tmp4 * sqrt_info(2, 1); + const Scalar _tmp84 = _tmp33 * _tmp59 - _tmp33 * _tmp63 + _tmp57 * sqrt_info(2, 1) + + _tmp59 * _tmp83 - _tmp63 * _tmp83 + _tmp70 * sqrt_info(2, 2) + + _tmp72 * sqrt_info(2, 0) + _tmp76 * sqrt_info(2, 2) - + _tmp81 * sqrt_info(2, 2); + const Scalar _tmp85 = _tmp65 + _tmp66 - _tmp67 - _tmp68; + const Scalar _tmp86 = _tmp47 * _tmp85; + const Scalar _tmp87 = _tmp4 * _tmp86; + const Scalar _tmp88 = _tmp60 * _tmp85; + const Scalar _tmp89 = _tmp4 * _tmp88; + const Scalar _tmp90 = _tmp48 * _tmp85; + const Scalar _tmp91 = _tmp69 * (-_tmp49 + _tmp50 + _tmp51 - _tmp52); + const Scalar _tmp92 = _tmp74 * _tmp85; + const Scalar _tmp93 = _tmp40 * _tmp69 + _tmp58 * _tmp86 - _tmp62 * _tmp85 + + _tmp72 * sqrt_info(0, 1) + _tmp87 * sqrt_info(0, 1) - + _tmp89 * sqrt_info(0, 1) + _tmp90 * sqrt_info(0, 2) + + _tmp91 * sqrt_info(0, 0) - _tmp92 * sqrt_info(0, 2); + const Scalar _tmp94 = _tmp39 * _tmp69; + const Scalar _tmp95 = _tmp71 * _tmp77 + _tmp78 * _tmp86 - _tmp79 * _tmp85 + + _tmp87 * sqrt_info(1, 1) - _tmp89 * sqrt_info(1, 1) + + _tmp90 * sqrt_info(1, 2) + _tmp91 * sqrt_info(1, 0) - + _tmp92 * sqrt_info(1, 2) + _tmp94 * sqrt_info(1, 2); + const Scalar _tmp96 = _tmp74 * sqrt_info(2, 2); + const Scalar _tmp97 = _tmp33 * _tmp86 - _tmp33 * _tmp88 + _tmp72 * sqrt_info(2, 1) + + _tmp83 * _tmp86 - _tmp83 * _tmp88 - _tmp85 * _tmp96 + + _tmp90 * sqrt_info(2, 2) + _tmp91 * sqrt_info(2, 0) + + _tmp94 * sqrt_info(2, 2); + const Scalar _tmp98 = _tmp46 * _tmp54; + const Scalar _tmp99 = _tmp69 * _tmp85; + const Scalar _tmp100 = _tmp53 * _tmp74; + const Scalar _tmp101 = _tmp28 * _tmp98; + const Scalar _tmp102 = -_tmp35 + _tmp36 - _tmp37 + _tmp38; + const Scalar _tmp103 = _tmp102 * _tmp69; + const Scalar _tmp104 = _tmp53 * _tmp60; + const Scalar _tmp105 = _tmp104 * _tmp4; + const Scalar _tmp106 = -_tmp100 * sqrt_info(0, 2) + _tmp101 * sqrt_info(0, 2) + + _tmp103 * sqrt_info(0, 1) - _tmp105 * sqrt_info(0, 1) - _tmp53 * _tmp62 + + _tmp58 * _tmp98 + _tmp72 * sqrt_info(0, 2) + _tmp73 * _tmp98 + + _tmp99 * sqrt_info(0, 0); + const Scalar _tmp107 = -_tmp100 * sqrt_info(1, 2) + _tmp101 * sqrt_info(1, 2) + + _tmp102 * _tmp43 * _tmp77 - _tmp105 * sqrt_info(1, 1) - _tmp53 * _tmp79 + + _tmp72 * sqrt_info(1, 2) + _tmp78 * _tmp98 + _tmp80 * _tmp98 + + _tmp99 * sqrt_info(1, 0); + const Scalar _tmp108 = _tmp101 * sqrt_info(2, 2) + _tmp103 * sqrt_info(2, 1) - _tmp104 * _tmp33 - + _tmp104 * _tmp83 + _tmp33 * _tmp98 - _tmp53 * _tmp96 + + _tmp72 * sqrt_info(2, 2) + _tmp83 * _tmp98 + _tmp99 * sqrt_info(2, 0); + + // Output terms (4) + if (res != nullptr) { + Eigen::Matrix& _res = (*res); + + _res(0, 0) = _tmp30; + _res(1, 0) = _tmp32; + _res(2, 0) = _tmp34; + } + + if (jacobian != nullptr) { + Eigen::Matrix& _jacobian = (*jacobian); + + _jacobian(0, 0) = _tmp75; + _jacobian(1, 0) = _tmp82; + _jacobian(2, 0) = _tmp84; + _jacobian(0, 1) = _tmp93; + _jacobian(1, 1) = _tmp95; + _jacobian(2, 1) = _tmp97; + _jacobian(0, 2) = _tmp106; + _jacobian(1, 2) = _tmp107; + _jacobian(2, 2) = _tmp108; + } + + if (hessian != nullptr) { + Eigen::Matrix& _hessian = (*hessian); + + _hessian(0, 0) = + std::pow(_tmp75, Scalar(2)) + std::pow(_tmp82, Scalar(2)) + std::pow(_tmp84, Scalar(2)); + _hessian(1, 0) = _tmp75 * _tmp93 + _tmp82 * _tmp95 + _tmp84 * _tmp97; + _hessian(2, 0) = _tmp106 * _tmp75 + _tmp107 * _tmp82 + _tmp108 * _tmp84; + _hessian(0, 1) = 0; + _hessian(1, 1) = + std::pow(_tmp93, Scalar(2)) + std::pow(_tmp95, Scalar(2)) + std::pow(_tmp97, Scalar(2)); + _hessian(2, 1) = _tmp106 * _tmp93 + _tmp107 * _tmp95 + _tmp108 * _tmp97; + _hessian(0, 2) = 0; + _hessian(1, 2) = 0; + _hessian(2, 2) = + std::pow(_tmp106, Scalar(2)) + std::pow(_tmp107, Scalar(2)) + std::pow(_tmp108, Scalar(2)); + } + + if (rhs != nullptr) { + Eigen::Matrix& _rhs = (*rhs); + + _rhs(0, 0) = _tmp30 * _tmp75 + _tmp32 * _tmp82 + _tmp34 * _tmp84; + _rhs(1, 0) = _tmp30 * _tmp93 + _tmp32 * _tmp95 + _tmp34 * _tmp97; + _rhs(2, 0) = _tmp106 * _tmp30 + _tmp107 * _tmp32 + _tmp108 * _tmp34; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym