From 87267eb7e20e0997aaa87a663753dd24211e254c Mon Sep 17 00:00:00 2001 From: bradley-solliday-skydio Date: Wed, 21 Dec 2022 19:03:37 -0800 Subject: [PATCH] Demo op count reduction if assume sqrt_info is ut In many of our generated factors, we take the square root information matrix as an argument, but make no assumptions about it's sparsity pattern, assuming it's just a regular dense square matrix. However, often times we pass in special matrices, for example diagonal matrices or upper triangular matrices (which one might get if one uses cholesky decomposition on the inverse of the covariance, for example). As this commit demonstrates, passing this knowledge on to the symbolic implementation of the code can reduce op counts considerably. So, if we care about reducing op counts in these cases, a couple options come to mind: - Come up with a mechanism to specify that our sqrt_information matrices have specific shapes so that this can be used during codegen - Pull the sqrt_info matrix out of the residual and instead multiply the outputs of the residual function after evaluation. Topic: demo_sqrt_info_savings Labels: draft --- gen/cpp/sym/factors/between_factor_matrix31.h | 141 +- gen/cpp/sym/factors/between_factor_pose2.h | 211 +- gen/cpp/sym/factors/between_factor_pose3.h | 1384 ++-- .../factors/between_factor_pose3_position.h | 421 +- .../factors/between_factor_pose3_rotation.h | 541 +- gen/cpp/sym/factors/between_factor_rot3.h | 560 +- gen/cpp/sym/factors/prior_factor_matrix31.h | 49 +- gen/cpp/sym/factors/prior_factor_pose2.h | 64 +- gen/cpp/sym/factors/prior_factor_pose3.h | 461 +- .../sym/factors/prior_factor_pose3_position.h | 59 +- .../sym/factors/prior_factor_pose3_rotation.h | 271 +- gen/cpp/sym/factors/prior_factor_rot3.h | 276 +- symforce/codegen/geo_factors_codegen.py | 8 +- .../linearization.h | 7379 ++++++++--------- symforce/geo/matrix.py | 17 + 15 files changed, 5560 insertions(+), 6282 deletions(-) diff --git a/gen/cpp/sym/factors/between_factor_matrix31.h b/gen/cpp/sym/factors/between_factor_matrix31.h index dbca47b83..ad4d1ef5c 100644 --- a/gen/cpp/sym/factors/between_factor_matrix31.h +++ b/gen/cpp/sym/factors/between_factor_matrix31.h @@ -37,84 +37,69 @@ void BetweenFactorMatrix31(const Eigen::Matrix& a, Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 102 + // Total ops: 60 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (42) + // Intermediate terms (26) 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); + const Scalar _tmp2 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + + sqrt_info(0, 0) * (-a(0, 0) - a_T_b(0, 0) + b(0, 0)); + const Scalar _tmp3 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2); + const Scalar _tmp4 = std::pow(sqrt_info(0, 0), Scalar(2)); + const Scalar _tmp5 = sqrt_info(0, 0) * sqrt_info(0, 1); + const Scalar _tmp6 = sqrt_info(0, 0) * sqrt_info(0, 2); + const Scalar _tmp7 = -_tmp5; + const Scalar _tmp8 = -_tmp6; + const Scalar _tmp9 = std::pow(sqrt_info(0, 1), Scalar(2)); + const Scalar _tmp10 = std::pow(sqrt_info(1, 1), Scalar(2)); + const Scalar _tmp11 = _tmp10 + _tmp9; + const Scalar _tmp12 = sqrt_info(0, 1) * sqrt_info(0, 2); + const Scalar _tmp13 = sqrt_info(1, 1) * sqrt_info(1, 2); + const Scalar _tmp14 = _tmp12 + _tmp13; + const Scalar _tmp15 = -_tmp12 - _tmp13; + const Scalar _tmp16 = std::pow(sqrt_info(0, 2), Scalar(2)); + const Scalar _tmp17 = std::pow(sqrt_info(2, 2), Scalar(2)); + const Scalar _tmp18 = std::pow(sqrt_info(1, 2), Scalar(2)); + const Scalar _tmp19 = _tmp16 + _tmp17 + _tmp18; + const Scalar _tmp20 = _tmp2 * sqrt_info(0, 0); + const Scalar _tmp21 = _tmp2 * sqrt_info(0, 1); + const Scalar _tmp22 = _tmp3 * sqrt_info(1, 1); + const Scalar _tmp23 = _tmp2 * sqrt_info(0, 2); + const Scalar _tmp24 = _tmp1 * _tmp17; + const Scalar _tmp25 = _tmp3 * 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; + _res(0, 0) = _tmp2; + _res(1, 0) = _tmp3; + _res(2, 0) = _tmp1 * sqrt_info(2, 2); } 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(1, 0) = 0; + _jacobian(2, 0) = 0; _jacobian(0, 1) = -sqrt_info(0, 1); _jacobian(1, 1) = -sqrt_info(1, 1); - _jacobian(2, 1) = -sqrt_info(2, 1); + _jacobian(2, 1) = 0; _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(1, 3) = 0; + _jacobian(2, 3) = 0; _jacobian(0, 4) = sqrt_info(0, 1); _jacobian(1, 4) = sqrt_info(1, 1); - _jacobian(2, 4) = sqrt_info(2, 1); + _jacobian(2, 4) = 0; _jacobian(0, 5) = sqrt_info(0, 2); _jacobian(1, 5) = sqrt_info(1, 2); _jacobian(2, 5) = sqrt_info(2, 2); @@ -123,53 +108,53 @@ void BetweenFactorMatrix31(const Eigen::Matrix& a, 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, 0) = _tmp4; + _hessian(1, 0) = _tmp5; + _hessian(2, 0) = _tmp6; + _hessian(3, 0) = -_tmp4; + _hessian(4, 0) = _tmp7; + _hessian(5, 0) = _tmp8; _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(1, 1) = _tmp11; + _hessian(2, 1) = _tmp14; + _hessian(3, 1) = _tmp7; + _hessian(4, 1) = -_tmp10 - _tmp9; + _hessian(5, 1) = _tmp15; _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(2, 2) = _tmp19; + _hessian(3, 2) = _tmp8; + _hessian(4, 2) = _tmp15; + _hessian(5, 2) = -_tmp16 - _tmp17 - _tmp18; _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(3, 3) = _tmp4; + _hessian(4, 3) = _tmp5; + _hessian(5, 3) = _tmp6; _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(4, 4) = _tmp11; + _hessian(5, 4) = _tmp14; _hessian(0, 5) = 0; _hessian(1, 5) = 0; _hessian(2, 5) = 0; _hessian(3, 5) = 0; _hessian(4, 5) = 0; - _hessian(5, 5) = _tmp32; + _hessian(5, 5) = _tmp19; } 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; + _rhs(0, 0) = -_tmp20; + _rhs(1, 0) = -_tmp21 - _tmp22; + _rhs(2, 0) = -_tmp23 - _tmp24 - _tmp25; + _rhs(3, 0) = _tmp20; + _rhs(4, 0) = _tmp21 + _tmp22; + _rhs(5, 0) = _tmp23 + _tmp24 + _tmp25; } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/between_factor_pose2.h b/gen/cpp/sym/factors/between_factor_pose2.h index dc76bc5e4..e13bafa50 100644 --- a/gen/cpp/sym/factors/between_factor_pose2.h +++ b/gen/cpp/sym/factors/between_factor_pose2.h @@ -38,14 +38,14 @@ void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 265 + // Total ops: 212 // 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) + // Intermediate terms (65) 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]; @@ -67,152 +67,139 @@ void BetweenFactorPose2(const sym::Pose2& a, const sym::Pose2& b 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 _tmp21 = _tmp3 * sqrt_info(0, 2) + _tmp7 * sqrt_info(0, 1) + + sqrt_info(0, 0) * std::atan2(_tmp16, _tmp20); + const Scalar _tmp22 = _tmp3 * sqrt_info(1, 2) + _tmp7 * sqrt_info(1, 1); + const Scalar _tmp23 = -_tmp8 - _tmp9; + const Scalar _tmp24 = Scalar(1.0) / (_tmp20); + const Scalar _tmp25 = std::pow(_tmp20, Scalar(2)); + const Scalar _tmp26 = _tmp16 / _tmp25; + const Scalar _tmp27 = std::pow(_tmp16, Scalar(2)) + _tmp25; + const Scalar _tmp28 = _tmp25 * sqrt_info(0, 0) / _tmp27; + const Scalar _tmp29 = -_tmp0 + _tmp1 + _tmp2; + const Scalar _tmp30 = _tmp4 - _tmp5 - _tmp6; 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)); + _tmp28 * (_tmp24 * (_a_T_b[0] * _tmp23 - _tmp18) - _tmp26 * (_a_T_b[1] * _tmp23 + _tmp15)) + + _tmp29 * sqrt_info(0, 1) + _tmp30 * sqrt_info(0, 2); + const Scalar _tmp32 = _tmp29 * sqrt_info(1, 1) + _tmp30 * sqrt_info(1, 2); + const Scalar _tmp33 = _a[0] * sqrt_info(0, 1); + const Scalar _tmp34 = _a[1] * sqrt_info(0, 2); + const Scalar _tmp35 = -_tmp33 + _tmp34; + const Scalar _tmp36 = _a[1] * sqrt_info(1, 2); + const Scalar _tmp37 = _a[0] * sqrt_info(1, 1); + const Scalar _tmp38 = _tmp36 - _tmp37; + const Scalar _tmp39 = _a[1] * sqrt_info(2, 2); + const Scalar _tmp40 = _a[0] * sqrt_info(0, 2); + const Scalar _tmp41 = _a[1] * sqrt_info(0, 1); + const Scalar _tmp42 = -_tmp40 - _tmp41; + const Scalar _tmp43 = _a[1] * sqrt_info(1, 1); + const Scalar _tmp44 = _a[0] * sqrt_info(1, 2); + const Scalar _tmp45 = -_tmp43 - _tmp44; + const Scalar _tmp46 = _a[0] * sqrt_info(2, 2); + const Scalar _tmp47 = _tmp12 - _tmp13; + const Scalar _tmp48 = + _tmp24 * (-_a_T_b[1] * _tmp47 + _tmp17) - _tmp26 * (_a_T_b[0] * _tmp47 + _tmp11); + const Scalar _tmp49 = _tmp28 * _tmp48; + const Scalar _tmp50 = _tmp33 - _tmp34; + const Scalar _tmp51 = -_tmp36 + _tmp37; + const Scalar _tmp52 = _tmp40 + _tmp41; + const Scalar _tmp53 = _tmp43 + _tmp44; + const Scalar _tmp54 = std::pow(sqrt_info(2, 2), Scalar(2)); + const Scalar _tmp55 = _tmp30 * _tmp54; + const Scalar _tmp56 = _a[1] * _tmp55; + const Scalar _tmp57 = _a[0] * _tmp55; + const Scalar _tmp58 = std::pow(_a[1], Scalar(2)) * _tmp54; + const Scalar _tmp59 = _a[0] * _a[1] * _tmp54; + const Scalar _tmp60 = -_tmp59; + const Scalar _tmp61 = std::pow(_a[0], Scalar(2)) * _tmp54; + const Scalar _tmp62 = _tmp3 * _tmp54; + const Scalar _tmp63 = _a[1] * _tmp62; + const Scalar _tmp64 = _a[0] * _tmp62; // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp22; - _res(1, 0) = _tmp23; - _res(2, 0) = _tmp24; + _res(0, 0) = _tmp21; + _res(1, 0) = _tmp22; + _res(2, 0) = _tmp3 * sqrt_info(2, 2); } 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; + _jacobian(0, 0) = _tmp31; + _jacobian(1, 0) = _tmp32; + _jacobian(2, 0) = _tmp30 * sqrt_info(2, 2); + _jacobian(0, 1) = _tmp35; + _jacobian(1, 1) = _tmp38; + _jacobian(2, 1) = _tmp39; + _jacobian(0, 2) = _tmp42; + _jacobian(1, 2) = _tmp45; + _jacobian(2, 2) = -_tmp46; + _jacobian(0, 3) = _tmp49; + _jacobian(1, 3) = 0; + _jacobian(2, 3) = 0; + _jacobian(0, 4) = _tmp50; + _jacobian(1, 4) = _tmp51; + _jacobian(2, 4) = -_tmp39; + _jacobian(0, 5) = _tmp52; + _jacobian(1, 5) = _tmp53; + _jacobian(2, 5) = _tmp46; } 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, 0) = std::pow(_tmp30, Scalar(2)) * _tmp54 + std::pow(_tmp31, Scalar(2)) + + std::pow(_tmp32, Scalar(2)); + _hessian(1, 0) = _tmp31 * _tmp35 + _tmp32 * _tmp38 + _tmp56; + _hessian(2, 0) = _tmp31 * _tmp42 + _tmp32 * _tmp45 - _tmp57; + _hessian(3, 0) = _tmp31 * _tmp49; + _hessian(4, 0) = _tmp31 * _tmp50 + _tmp32 * _tmp51 - _tmp56; + _hessian(5, 0) = _tmp31 * _tmp52 + _tmp32 * _tmp53 + _tmp57; _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(1, 1) = std::pow(_tmp35, Scalar(2)) + std::pow(_tmp38, Scalar(2)) + _tmp58; + _hessian(2, 1) = _tmp35 * _tmp42 + _tmp38 * _tmp45 + _tmp60; + _hessian(3, 1) = _tmp35 * _tmp49; + _hessian(4, 1) = _tmp35 * _tmp50 + _tmp38 * _tmp51 - _tmp58; + _hessian(5, 1) = _tmp35 * _tmp52 + _tmp38 * _tmp53 + _tmp59; _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(2, 2) = std::pow(_tmp42, Scalar(2)) + std::pow(_tmp45, Scalar(2)) + _tmp61; + _hessian(3, 2) = _tmp42 * _tmp49; + _hessian(4, 2) = _tmp42 * _tmp50 + _tmp45 * _tmp51 + _tmp59; + _hessian(5, 2) = _tmp42 * _tmp52 + _tmp45 * _tmp53 - _tmp61; _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(3, 3) = std::pow(_tmp20, Scalar(4)) * std::pow(_tmp48, Scalar(2)) * + std::pow(sqrt_info(0, 0), Scalar(2)) / std::pow(_tmp27, Scalar(2)); + _hessian(4, 3) = _tmp49 * _tmp50; + _hessian(5, 3) = _tmp49 * _tmp52; _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(4, 4) = std::pow(_tmp50, Scalar(2)) + std::pow(_tmp51, Scalar(2)) + _tmp58; + _hessian(5, 4) = _tmp50 * _tmp52 + _tmp51 * _tmp53 + _tmp60; _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)); + _hessian(5, 5) = std::pow(_tmp52, Scalar(2)) + std::pow(_tmp53, Scalar(2)) + _tmp61; } 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; + _rhs(0, 0) = _tmp21 * _tmp31 + _tmp22 * _tmp32 + _tmp3 * _tmp55; + _rhs(1, 0) = _tmp21 * _tmp35 + _tmp22 * _tmp38 + _tmp63; + _rhs(2, 0) = _tmp21 * _tmp42 + _tmp22 * _tmp45 - _tmp64; + _rhs(3, 0) = _tmp21 * _tmp49; + _rhs(4, 0) = _tmp21 * _tmp50 + _tmp22 * _tmp51 - _tmp63; + _rhs(5, 0) = _tmp21 * _tmp52 + _tmp22 * _tmp53 + _tmp64; } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/between_factor_pose3.h b/gen/cpp/sym/factors/between_factor_pose3.h index dc6997c02..b4a592f9f 100644 --- a/gen/cpp/sym/factors/between_factor_pose3.h +++ b/gen/cpp/sym/factors/between_factor_pose3.h @@ -38,29 +38,29 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 2386 + // Total ops: 1632 // 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)); + // Intermediate terms (313) + const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); const Scalar _tmp1 = 2 * _tmp0; const Scalar _tmp2 = -_tmp1; - const Scalar _tmp3 = std::pow(_a[2], Scalar(2)); + const Scalar _tmp3 = std::pow(_a[0], 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 _tmp7 = 2 * _a[2]; + const Scalar _tmp8 = _a[1] * _tmp7; + const Scalar _tmp9 = 2 * _a[0]; + const Scalar _tmp10 = _a[3] * _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 _tmp13 = _a[1] * _tmp9; + const Scalar _tmp14 = _a[3] * _tmp7; const Scalar _tmp15 = -_tmp14; const Scalar _tmp16 = _tmp13 + _tmp15; const Scalar _tmp17 = _a[4] * _tmp16; @@ -69,9 +69,9 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b 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 _tmp23 = _tmp2 + _tmp22 + 1; + const Scalar _tmp24 = _a[2] * _tmp9; + const Scalar _tmp25 = 2 * _a[1] * _a[3]; const Scalar _tmp26 = -_tmp25; const Scalar _tmp27 = _tmp24 + _tmp26; const Scalar _tmp28 = _a[6] * _tmp27; @@ -84,763 +84,575 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b 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 _tmp38 = _a_T_b[2] * _tmp37; + const Scalar _tmp39 = _a[2] * _b[1]; + const Scalar _tmp40 = _a[0] * _b[3]; + const Scalar _tmp41 = _a[1] * _b[2]; + const Scalar _tmp42 = _a[3] * _b[0]; + const Scalar _tmp43 = _tmp39 - _tmp40 - _tmp41 + _tmp42; + const Scalar _tmp44 = _a_T_b[0] * _tmp43; + const Scalar _tmp45 = _a[3] * _b[1]; + const Scalar _tmp46 = _a[1] * _b[3]; + const Scalar _tmp47 = _a[0] * _b[2]; + const Scalar _tmp48 = _a[2] * _b[0]; + const Scalar _tmp49 = _tmp45 - _tmp46 + _tmp47 - _tmp48; + const Scalar _tmp50 = _a_T_b[1] * _tmp49; + const Scalar _tmp51 = -_tmp38 - _tmp44 - _tmp50; + const Scalar _tmp52 = _a[1] * _b[1]; + const Scalar _tmp53 = _a[2] * _b[2]; + const Scalar _tmp54 = _a[0] * _b[0]; + const Scalar _tmp55 = _a[3] * _b[3]; + const Scalar _tmp56 = _tmp52 + _tmp53 + _tmp54 + _tmp55; + const Scalar _tmp57 = _a_T_b[3] * _tmp56; + const Scalar _tmp58 = + 2 * std::min(0, (((-_tmp51 + _tmp57) > 0) - ((-_tmp51 + _tmp57) < 0))) + 1; + const Scalar _tmp59 = 2 * _tmp58; + const Scalar _tmp60 = _tmp59 * sqrt_info(0, 2); + const Scalar _tmp61 = + -_a_T_b[0] * _tmp49 + _a_T_b[1] * _tmp43 - _a_T_b[2] * _tmp56 + _a_T_b[3] * _tmp37; + const Scalar _tmp62 = 1 - epsilon; + const Scalar _tmp63 = std::min(_tmp62, std::fabs(_tmp51 - _tmp57)); + const Scalar _tmp64 = std::acos(_tmp63) / std::sqrt(Scalar(1 - std::pow(_tmp63, Scalar(2)))); + const Scalar _tmp65 = _tmp61 * _tmp64; + const Scalar _tmp66 = sqrt_info(0, 0) * (-_a_T_b[0] * _tmp56 - _a_T_b[1] * _tmp37 + + _a_T_b[2] * _tmp49 + _a_T_b[3] * _tmp43); + const Scalar _tmp67 = _tmp59 * sqrt_info(0, 1); 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); + _a_T_b[0] * _tmp37 - _a_T_b[1] * _tmp56 - _a_T_b[2] * _tmp43 + _a_T_b[3] * _tmp49; + const Scalar _tmp69 = _tmp64 * _tmp68; + const Scalar _tmp70 = _tmp22 + _tmp5; + const Scalar _tmp71 = -_tmp10; + const Scalar _tmp72 = _tmp71 + _tmp8; + const Scalar _tmp73 = _a[5] * _tmp72; + const Scalar _tmp74 = _tmp24 + _tmp25; + const Scalar _tmp75 = _a[4] * _tmp74; + const Scalar _tmp76 = _b[4] * _tmp74 + _b[5] * _tmp72; + const Scalar _tmp77 = -_a[6] * _tmp70 - _a_T_b[6] + _b[6] * _tmp70 - _tmp73 - _tmp75 + _tmp76; + const Scalar _tmp78 = _tmp19 * sqrt_info(0, 4) + _tmp32 * sqrt_info(0, 3) + + _tmp59 * _tmp64 * _tmp66 + _tmp60 * _tmp65 + _tmp67 * _tmp69 + + _tmp77 * sqrt_info(0, 5); + const Scalar _tmp79 = _tmp59 * _tmp65; + const Scalar _tmp80 = _tmp59 * sqrt_info(1, 1); + const Scalar _tmp81 = _tmp19 * sqrt_info(1, 4) + _tmp32 * sqrt_info(1, 3) + _tmp69 * _tmp80 + + _tmp77 * sqrt_info(1, 5) + _tmp79 * sqrt_info(1, 2); + const Scalar _tmp82 = _tmp19 * sqrt_info(2, 4) + _tmp32 * sqrt_info(2, 3) + + _tmp77 * sqrt_info(2, 5) + _tmp79 * sqrt_info(2, 2); + const Scalar _tmp83 = + _tmp19 * sqrt_info(3, 4) + _tmp32 * sqrt_info(3, 3) + _tmp77 * sqrt_info(3, 5); + const Scalar _tmp84 = _tmp19 * sqrt_info(4, 4) + _tmp77 * sqrt_info(4, 5); + const Scalar _tmp85 = (Scalar(1) / Scalar(2)) * _tmp39; + const Scalar _tmp86 = (Scalar(1) / Scalar(2)) * _tmp40; + const Scalar _tmp87 = (Scalar(1) / Scalar(2)) * _tmp41; + const Scalar _tmp88 = (Scalar(1) / Scalar(2)) * _tmp42; + const Scalar _tmp89 = _tmp85 - _tmp86 - _tmp87 + _tmp88; + const Scalar _tmp90 = _a_T_b[3] * _tmp89; + const Scalar _tmp91 = (Scalar(1) / Scalar(2)) * _tmp52; + const Scalar _tmp92 = (Scalar(1) / Scalar(2)) * _tmp55; + const Scalar _tmp93 = (Scalar(1) / Scalar(2)) * _tmp53; + const Scalar _tmp94 = (Scalar(1) / Scalar(2)) * _tmp54; + const Scalar _tmp95 = -_tmp91 - _tmp92 - _tmp93 - _tmp94; + const Scalar _tmp96 = _a_T_b[0] * _tmp95; + const Scalar _tmp97 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp98 = (Scalar(1) / Scalar(2)) * _tmp34; + const Scalar _tmp99 = (Scalar(1) / Scalar(2)) * _tmp35; + const Scalar _tmp100 = (Scalar(1) / Scalar(2)) * _tmp36; + const Scalar _tmp101 = _tmp100 - _tmp97 - _tmp98 + _tmp99; + const Scalar _tmp102 = _a_T_b[1] * _tmp101; + const Scalar _tmp103 = (Scalar(1) / Scalar(2)) * _tmp45; + const Scalar _tmp104 = (Scalar(1) / Scalar(2)) * _tmp46; + const Scalar _tmp105 = (Scalar(1) / Scalar(2)) * _tmp47; + const Scalar _tmp106 = (Scalar(1) / Scalar(2)) * _tmp48; + const Scalar _tmp107 = -_tmp103 + _tmp104 - _tmp105 + _tmp106; + const Scalar _tmp108 = _a_T_b[2] * _tmp107; + const Scalar _tmp109 = _tmp102 + _tmp108; + const Scalar _tmp110 = _tmp109 + _tmp90 + _tmp96; + const Scalar _tmp111 = _tmp38 + _tmp44 + _tmp50 + _tmp57; + const Scalar _tmp112 = std::fabs(_tmp111); + const Scalar _tmp113 = std::min(_tmp112, _tmp62); + const Scalar _tmp114 = 1 - std::pow(_tmp113, Scalar(2)); + const Scalar _tmp115 = _tmp58 * ((((-_tmp112 + _tmp62) > 0) - ((-_tmp112 + _tmp62) < 0)) + 1) * + (((_tmp111) > 0) - ((_tmp111) < 0)); + const Scalar _tmp116 = _tmp115 / _tmp114; + const Scalar _tmp117 = _tmp116 * _tmp68; + const Scalar _tmp118 = _tmp117 * sqrt_info(0, 1); + const Scalar _tmp119 = _a_T_b[1] * _tmp95; + const Scalar _tmp120 = _a_T_b[2] * _tmp89; + const Scalar _tmp121 = -_a_T_b[0] * _tmp101; + const Scalar _tmp122 = _a_T_b[3] * _tmp107; + const Scalar _tmp123 = _tmp121 + _tmp122; + const Scalar _tmp124 = _tmp119 - _tmp120 + _tmp123; + const Scalar _tmp125 = std::acos(_tmp113); + const Scalar _tmp126 = _tmp125 / std::sqrt(_tmp114); + const Scalar _tmp127 = _tmp126 * _tmp60; + const Scalar _tmp128 = _tmp116 * _tmp66; + const Scalar _tmp129 = _tmp113 * _tmp115 * _tmp125 / (_tmp114 * std::sqrt(_tmp114)); + const Scalar _tmp130 = _tmp129 * _tmp61; + const Scalar _tmp131 = _tmp110 * _tmp130; + const Scalar _tmp132 = _a_T_b[3] * _tmp95; + const Scalar _tmp133 = _a_T_b[0] * _tmp89; + const Scalar _tmp134 = _a_T_b[2] * _tmp101; + const Scalar _tmp135 = -_a_T_b[1] * _tmp107; + const Scalar _tmp136 = _tmp134 + _tmp135; + const Scalar _tmp137 = _tmp126 * _tmp59; + const Scalar _tmp138 = _tmp137 * sqrt_info(0, 0); + const Scalar _tmp139 = _tmp129 * _tmp68; + const Scalar _tmp140 = _tmp139 * sqrt_info(0, 1); + const Scalar _tmp141 = _tmp129 * _tmp66; + const Scalar _tmp142 = -_a_T_b[1] * _tmp89; + const Scalar _tmp143 = _a_T_b[2] * _tmp95; + const Scalar _tmp144 = _a_T_b[3] * _tmp101; + const Scalar _tmp145 = _a_T_b[0] * _tmp107; + const Scalar _tmp146 = _tmp144 + _tmp145; + const Scalar _tmp147 = _tmp142 - _tmp143 + _tmp146; + const Scalar _tmp148 = _tmp126 * _tmp67; + const Scalar _tmp149 = -_tmp20; + const Scalar _tmp150 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp151 = _tmp149 + _tmp150; + const Scalar _tmp152 = -_tmp3; + const Scalar _tmp153 = _tmp0 + _tmp152; + const Scalar _tmp154 = _tmp151 + _tmp153; + const Scalar _tmp155 = -_a[6] * _tmp154 + _b[6] * _tmp154 - _tmp73 - _tmp75 + _tmp76; + const Scalar _tmp156 = _tmp116 * _tmp61; + const Scalar _tmp157 = _tmp156 * sqrt_info(0, 2); + const Scalar _tmp158 = -_tmp150; + const Scalar _tmp159 = _tmp0 + _tmp149 + _tmp158 + _tmp3; + const Scalar _tmp160 = -_tmp8; + const Scalar _tmp161 = _tmp160 + _tmp71; + const Scalar _tmp162 = -_tmp13; + const Scalar _tmp163 = _tmp14 + _tmp162; + const Scalar _tmp164 = -_a[4] * _tmp163 - _a[5] * _tmp159 - _a[6] * _tmp161 + _b[4] * _tmp163 + + _b[5] * _tmp159 + _b[6] * _tmp161; + const Scalar _tmp165 = -_tmp110 * _tmp118 - _tmp110 * _tmp128 + _tmp110 * _tmp140 + + _tmp110 * _tmp141 - _tmp110 * _tmp157 + _tmp124 * _tmp127 + + _tmp131 * sqrt_info(0, 2) + _tmp138 * (_tmp132 - _tmp133 + _tmp136) + + _tmp147 * _tmp148 + _tmp155 * sqrt_info(0, 4) + _tmp164 * sqrt_info(0, 5); + const Scalar _tmp166 = _tmp117 * sqrt_info(1, 1); + const Scalar _tmp167 = _tmp124 * _tmp137; + const Scalar _tmp168 = _tmp61 * sqrt_info(1, 2); + const Scalar _tmp169 = _tmp129 * _tmp168; + const Scalar _tmp170 = _tmp139 * sqrt_info(1, 1); + const Scalar _tmp171 = _tmp126 * _tmp80; + const Scalar _tmp172 = _tmp156 * sqrt_info(1, 2); + const Scalar _tmp173 = -_tmp110 * _tmp166 + _tmp110 * _tmp169 + _tmp110 * _tmp170 - + _tmp110 * _tmp172 + _tmp147 * _tmp171 + _tmp155 * sqrt_info(1, 4) + + _tmp164 * sqrt_info(1, 5) + _tmp167 * sqrt_info(1, 2); + const Scalar _tmp174 = _tmp156 * sqrt_info(2, 2); + const Scalar _tmp175 = -_tmp110 * _tmp174 + _tmp131 * sqrt_info(2, 2) + + _tmp155 * sqrt_info(2, 4) + _tmp164 * sqrt_info(2, 5) + + _tmp167 * sqrt_info(2, 2); + const Scalar _tmp176 = _tmp155 * sqrt_info(3, 4) + _tmp164 * sqrt_info(3, 5); + const Scalar _tmp177 = _tmp155 * sqrt_info(4, 4) + _tmp164 * sqrt_info(4, 5); + const Scalar _tmp178 = _tmp103 - _tmp104 + _tmp105 - _tmp106; + const Scalar _tmp179 = _a_T_b[3] * _tmp178; + const Scalar _tmp180 = -_tmp100 + _tmp97 + _tmp98 - _tmp99; + const Scalar _tmp181 = _a_T_b[0] * _tmp180; + const Scalar _tmp182 = _tmp120 + _tmp181; + const Scalar _tmp183 = _tmp119 + _tmp179 + _tmp182; + const Scalar _tmp184 = -_a_T_b[2] * _tmp178; + const Scalar _tmp185 = _a_T_b[1] * _tmp180; + const Scalar _tmp186 = _tmp185 + _tmp90; + const Scalar _tmp187 = _tmp184 + _tmp186 - _tmp96; + const Scalar _tmp188 = _a_T_b[3] * _tmp180; + const Scalar _tmp189 = _a_T_b[0] * _tmp178; + const Scalar _tmp190 = _tmp129 * _tmp183; + const Scalar _tmp191 = _tmp190 * _tmp61; + const Scalar _tmp192 = _tmp190 * _tmp68; + const Scalar _tmp193 = _a_T_b[1] * _tmp178; + const Scalar _tmp194 = -_a_T_b[2] * _tmp180; + const Scalar _tmp195 = _tmp133 + _tmp194; + const Scalar _tmp196 = _tmp132 - _tmp193 + _tmp195; + const Scalar _tmp197 = -_tmp0; + const Scalar _tmp198 = _tmp197 + _tmp3; + const Scalar _tmp199 = _tmp158 + _tmp20; + const Scalar _tmp200 = _tmp198 + _tmp199; + const Scalar _tmp201 = _tmp10 + _tmp160; + const Scalar _tmp202 = -_tmp24; + const Scalar _tmp203 = _tmp202 + _tmp26; + const Scalar _tmp204 = -_a[4] * _tmp203 - _a[5] * _tmp201 - _a[6] * _tmp200 + _b[4] * _tmp203 + + _b[5] * _tmp201 + _b[6] * _tmp200; + const Scalar _tmp205 = _tmp151 + _tmp198; + const Scalar _tmp206 = -_a[4] * _tmp205 + _b[4] * _tmp205 - _tmp28 - _tmp30 + _tmp31; + const Scalar _tmp207 = -_tmp118 * _tmp183 + _tmp127 * _tmp187 - _tmp128 * _tmp183 + + _tmp138 * (_tmp142 + _tmp143 + _tmp188 - _tmp189) + _tmp148 * _tmp196 - + _tmp157 * _tmp183 + _tmp190 * _tmp66 + _tmp191 * sqrt_info(0, 2) + + _tmp192 * sqrt_info(0, 1) + _tmp204 * sqrt_info(0, 3) + + _tmp206 * sqrt_info(0, 5); + const Scalar _tmp208 = _tmp137 * _tmp187; + const Scalar _tmp209 = -_tmp166 * _tmp183 + _tmp168 * _tmp190 + _tmp171 * _tmp196 - + _tmp172 * _tmp183 + _tmp192 * sqrt_info(1, 1) + _tmp204 * sqrt_info(1, 3) + + _tmp206 * sqrt_info(1, 5) + _tmp208 * sqrt_info(1, 2); + const Scalar _tmp210 = -_tmp174 * _tmp183 + _tmp191 * sqrt_info(2, 2) + + _tmp204 * sqrt_info(2, 3) + _tmp206 * sqrt_info(2, 5) + + _tmp208 * sqrt_info(2, 2); + const Scalar _tmp211 = _tmp204 * sqrt_info(3, 3) + _tmp206 * sqrt_info(3, 5); + const Scalar _tmp212 = _tmp206 * sqrt_info(4, 5); + const Scalar _tmp213 = -_tmp85 + _tmp86 + _tmp87 - _tmp88; + const Scalar _tmp214 = _a_T_b[1] * _tmp213; + const Scalar _tmp215 = _tmp189 + _tmp214; + const Scalar _tmp216 = _tmp143 + _tmp144 + _tmp215; + const Scalar _tmp217 = _tmp130 * _tmp216; + const Scalar _tmp218 = _a_T_b[3] * _tmp213; + const Scalar _tmp219 = -_tmp102 + _tmp184 + _tmp218 + _tmp96; + const Scalar _tmp220 = _a_T_b[2] * _tmp213; + const Scalar _tmp221 = _tmp179 + _tmp220; + const Scalar _tmp222 = -_a_T_b[0] * _tmp213; + const Scalar _tmp223 = _tmp132 - _tmp134 + _tmp193 + _tmp222; + const Scalar _tmp224 = _tmp150 + _tmp152 + _tmp197 + _tmp20; + const Scalar _tmp225 = -_a[5] * _tmp224 + _b[5] * _tmp224 - _tmp12 - _tmp17 + _tmp18; + const Scalar _tmp226 = _tmp153 + _tmp199; + const Scalar _tmp227 = _tmp202 + _tmp25; + const Scalar _tmp228 = _tmp15 + _tmp162; + const Scalar _tmp229 = -_a[4] * _tmp226 - _a[5] * _tmp228 - _a[6] * _tmp227 + _b[4] * _tmp226 + + _b[5] * _tmp228 + _b[6] * _tmp227; + const Scalar _tmp230 = -_tmp118 * _tmp216 + _tmp127 * _tmp223 - _tmp128 * _tmp216 + + _tmp138 * (-_tmp119 + _tmp121 + _tmp221) + _tmp140 * _tmp216 + + _tmp141 * _tmp216 + _tmp148 * _tmp219 - _tmp157 * _tmp216 + + _tmp217 * sqrt_info(0, 2) + _tmp225 * sqrt_info(0, 3) + + _tmp229 * sqrt_info(0, 4); + const Scalar _tmp231 = _tmp137 * _tmp223; + const Scalar _tmp232 = -_tmp166 * _tmp216 + _tmp169 * _tmp216 + _tmp170 * _tmp216 + + _tmp171 * _tmp219 - _tmp172 * _tmp216 + _tmp225 * sqrt_info(1, 3) + + _tmp229 * sqrt_info(1, 4) + _tmp231 * sqrt_info(1, 2); + const Scalar _tmp233 = -_tmp174 * _tmp216 + _tmp217 * sqrt_info(2, 2) + + _tmp225 * sqrt_info(2, 3) + _tmp229 * sqrt_info(2, 4) + + _tmp231 * sqrt_info(2, 2); + const Scalar _tmp234 = _tmp225 * sqrt_info(3, 3) + _tmp229 * sqrt_info(3, 4); + const Scalar _tmp235 = _tmp229 * sqrt_info(4, 4); + const Scalar _tmp236 = _tmp1 + _tmp21 - 1; + const Scalar _tmp237 = + _tmp163 * sqrt_info(0, 4) + _tmp203 * sqrt_info(0, 5) + _tmp236 * sqrt_info(0, 3); + const Scalar _tmp238 = + _tmp163 * sqrt_info(1, 4) + _tmp203 * sqrt_info(1, 5) + _tmp236 * sqrt_info(1, 3); + const Scalar _tmp239 = + _tmp163 * sqrt_info(2, 4) + _tmp203 * sqrt_info(2, 5) + _tmp236 * sqrt_info(2, 3); + const Scalar _tmp240 = + _tmp163 * sqrt_info(3, 4) + _tmp203 * sqrt_info(3, 5) + _tmp236 * sqrt_info(3, 3); + const Scalar _tmp241 = _tmp163 * sqrt_info(4, 4) + _tmp203 * sqrt_info(4, 5); + const Scalar _tmp242 = _tmp4 - 1; + const Scalar _tmp243 = _tmp1 + _tmp242; + const Scalar _tmp244 = + _tmp201 * sqrt_info(0, 5) + _tmp228 * sqrt_info(0, 3) + _tmp243 * sqrt_info(0, 4); + const Scalar _tmp245 = + _tmp201 * sqrt_info(1, 5) + _tmp228 * sqrt_info(1, 3) + _tmp243 * sqrt_info(1, 4); + const Scalar _tmp246 = + _tmp201 * sqrt_info(2, 5) + _tmp228 * sqrt_info(2, 3) + _tmp243 * sqrt_info(2, 4); + const Scalar _tmp247 = + _tmp201 * sqrt_info(3, 5) + _tmp228 * sqrt_info(3, 3) + _tmp243 * sqrt_info(3, 4); + const Scalar _tmp248 = _tmp201 * sqrt_info(4, 5) + _tmp243 * sqrt_info(4, 4); + const Scalar _tmp249 = _tmp21 + _tmp242; + const Scalar _tmp250 = + _tmp161 * sqrt_info(0, 4) + _tmp227 * sqrt_info(0, 3) + _tmp249 * sqrt_info(0, 5); + const Scalar _tmp251 = + _tmp161 * sqrt_info(1, 4) + _tmp227 * sqrt_info(1, 3) + _tmp249 * sqrt_info(1, 5); + const Scalar _tmp252 = + _tmp161 * sqrt_info(2, 4) + _tmp227 * sqrt_info(2, 3) + _tmp249 * sqrt_info(2, 5); + const Scalar _tmp253 = + _tmp161 * sqrt_info(3, 4) + _tmp227 * sqrt_info(3, 3) + _tmp249 * sqrt_info(3, 5); + const Scalar _tmp254 = _tmp161 * sqrt_info(4, 4) + _tmp249 * sqrt_info(4, 5); + const Scalar _tmp255 = _tmp91 + _tmp92 + _tmp93 + _tmp94; + const Scalar _tmp256 = _a_T_b[0] * _tmp255; + const Scalar _tmp257 = _tmp218 + _tmp256; + const Scalar _tmp258 = _tmp109 + _tmp257; + const Scalar _tmp259 = _a_T_b[3] * _tmp255; + const Scalar _tmp260 = _tmp222 + _tmp259; + const Scalar _tmp261 = _a_T_b[1] * _tmp255; + const Scalar _tmp262 = _tmp123 - _tmp220 + _tmp261; + const Scalar _tmp263 = _tmp130 * _tmp258; + const Scalar _tmp264 = _a_T_b[2] * _tmp255; + const Scalar _tmp265 = _tmp126 * (_tmp146 - _tmp214 - _tmp264); + const Scalar _tmp266 = -_tmp118 * _tmp258 + _tmp127 * _tmp262 - _tmp128 * _tmp258 + + _tmp138 * (_tmp136 + _tmp260) + _tmp140 * _tmp258 + _tmp141 * _tmp258 - + _tmp157 * _tmp258 + _tmp263 * sqrt_info(0, 2) + _tmp265 * _tmp67; + const Scalar _tmp267 = _tmp137 * _tmp262; + const Scalar _tmp268 = -_tmp166 * _tmp258 + _tmp169 * _tmp258 + _tmp170 * _tmp258 - + _tmp172 * _tmp258 + _tmp265 * _tmp80 + _tmp267 * sqrt_info(1, 2); + const Scalar _tmp269 = -_tmp174 * _tmp258 + _tmp263 * sqrt_info(2, 2) + _tmp267 * sqrt_info(2, 2); + const Scalar _tmp270 = -_tmp108 + _tmp186 - _tmp256; + const Scalar _tmp271 = _tmp122 + _tmp182 + _tmp261; + const Scalar _tmp272 = _tmp129 * _tmp271; + const Scalar _tmp273 = _tmp272 * _tmp61; + const Scalar _tmp274 = _tmp272 * _tmp68; + const Scalar _tmp275 = _tmp188 + _tmp264; + const Scalar _tmp276 = _tmp135 + _tmp195 + _tmp259; + const Scalar _tmp277 = -_tmp118 * _tmp271 + _tmp127 * _tmp270 - _tmp128 * _tmp271 + + _tmp138 * (_tmp142 - _tmp145 + _tmp275) + _tmp148 * _tmp276 - + _tmp157 * _tmp271 + _tmp272 * _tmp66 + _tmp273 * sqrt_info(0, 2) + + _tmp274 * sqrt_info(0, 1); + const Scalar _tmp278 = _tmp137 * _tmp270; + const Scalar _tmp279 = -_tmp166 * _tmp271 + _tmp168 * _tmp272 + _tmp171 * _tmp276 - + _tmp172 * _tmp271 + _tmp274 * sqrt_info(1, 1) + _tmp278 * sqrt_info(1, 2); + const Scalar _tmp280 = -_tmp174 * _tmp271 + _tmp273 * sqrt_info(2, 2) + _tmp278 * sqrt_info(2, 2); + const Scalar _tmp281 = _tmp215 + _tmp275; + const Scalar _tmp282 = _tmp130 * _tmp281; + const Scalar _tmp283 = _tmp126 * (_tmp184 - _tmp185 + _tmp257); + const Scalar _tmp284 = _tmp193 + _tmp194 + _tmp260; + const Scalar _tmp285 = -_tmp118 * _tmp281 + _tmp127 * _tmp284 - _tmp128 * _tmp281 + + _tmp138 * (-_tmp181 + _tmp221 - _tmp261) + _tmp140 * _tmp281 + + _tmp141 * _tmp281 - _tmp157 * _tmp281 + _tmp282 * sqrt_info(0, 2) + + _tmp283 * _tmp67; + const Scalar _tmp286 = _tmp137 * _tmp284; + const Scalar _tmp287 = -_tmp166 * _tmp281 + _tmp169 * _tmp281 + _tmp170 * _tmp281 - + _tmp172 * _tmp281 + _tmp283 * _tmp80 + _tmp286 * sqrt_info(1, 2); + const Scalar _tmp288 = -_tmp174 * _tmp281 + _tmp282 * sqrt_info(2, 2) + _tmp286 * sqrt_info(2, 2); + const Scalar _tmp289 = + _tmp16 * sqrt_info(0, 4) + _tmp23 * sqrt_info(0, 3) + _tmp74 * sqrt_info(0, 5); + const Scalar _tmp290 = + _tmp16 * sqrt_info(1, 4) + _tmp23 * sqrt_info(1, 3) + _tmp74 * sqrt_info(1, 5); + const Scalar _tmp291 = + _tmp16 * sqrt_info(2, 4) + _tmp23 * sqrt_info(2, 3) + _tmp74 * sqrt_info(2, 5); + const Scalar _tmp292 = + _tmp16 * sqrt_info(3, 4) + _tmp23 * sqrt_info(3, 3) + _tmp74 * sqrt_info(3, 5); + const Scalar _tmp293 = _tmp16 * sqrt_info(4, 4) + _tmp74 * sqrt_info(4, 5); + const Scalar _tmp294 = + _tmp29 * sqrt_info(0, 3) + _tmp6 * sqrt_info(0, 4) + _tmp72 * sqrt_info(0, 5); + const Scalar _tmp295 = + _tmp29 * sqrt_info(1, 3) + _tmp6 * sqrt_info(1, 4) + _tmp72 * sqrt_info(1, 5); + const Scalar _tmp296 = + _tmp29 * sqrt_info(2, 3) + _tmp6 * sqrt_info(2, 4) + _tmp72 * sqrt_info(2, 5); + const Scalar _tmp297 = + _tmp29 * sqrt_info(3, 3) + _tmp6 * sqrt_info(3, 4) + _tmp72 * sqrt_info(3, 5); + const Scalar _tmp298 = _tmp6 * sqrt_info(4, 4) + _tmp72 * sqrt_info(4, 5); + const Scalar _tmp299 = + _tmp11 * sqrt_info(0, 4) + _tmp27 * sqrt_info(0, 3) + _tmp70 * sqrt_info(0, 5); + const Scalar _tmp300 = + _tmp11 * sqrt_info(1, 4) + _tmp27 * sqrt_info(1, 3) + _tmp70 * sqrt_info(1, 5); + const Scalar _tmp301 = + _tmp11 * sqrt_info(2, 4) + _tmp27 * sqrt_info(2, 3) + _tmp70 * sqrt_info(2, 5); + const Scalar _tmp302 = + _tmp11 * sqrt_info(3, 4) + _tmp27 * sqrt_info(3, 3) + _tmp70 * sqrt_info(3, 5); + const Scalar _tmp303 = _tmp11 * sqrt_info(4, 4) + _tmp70 * sqrt_info(4, 5); + const Scalar _tmp304 = std::pow(sqrt_info(5, 5), Scalar(2)); + const Scalar _tmp305 = _tmp164 * _tmp304; + const Scalar _tmp306 = std::pow(_tmp206, Scalar(2)); + const Scalar _tmp307 = _tmp206 * _tmp304; + const Scalar _tmp308 = _tmp201 * _tmp304; + const Scalar _tmp309 = _tmp249 * _tmp304; + const Scalar _tmp310 = _tmp304 * _tmp74; + const Scalar _tmp311 = _tmp304 * _tmp72; + const Scalar _tmp312 = _tmp304 * _tmp77; // 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; + _res(0, 0) = _tmp78; + _res(1, 0) = _tmp81; + _res(2, 0) = _tmp82; + _res(3, 0) = _tmp83; + _res(4, 0) = _tmp84; + _res(5, 0) = _tmp77 * sqrt_info(5, 5); } 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; + _jacobian(0, 0) = _tmp165; + _jacobian(1, 0) = _tmp173; + _jacobian(2, 0) = _tmp175; + _jacobian(3, 0) = _tmp176; + _jacobian(4, 0) = _tmp177; + _jacobian(5, 0) = _tmp164 * sqrt_info(5, 5); + _jacobian(0, 1) = _tmp207; + _jacobian(1, 1) = _tmp209; + _jacobian(2, 1) = _tmp210; + _jacobian(3, 1) = _tmp211; + _jacobian(4, 1) = _tmp212; + _jacobian(5, 1) = _tmp206 * sqrt_info(5, 5); + _jacobian(0, 2) = _tmp230; + _jacobian(1, 2) = _tmp232; + _jacobian(2, 2) = _tmp233; + _jacobian(3, 2) = _tmp234; + _jacobian(4, 2) = _tmp235; + _jacobian(5, 2) = 0; + _jacobian(0, 3) = _tmp237; + _jacobian(1, 3) = _tmp238; + _jacobian(2, 3) = _tmp239; + _jacobian(3, 3) = _tmp240; + _jacobian(4, 3) = _tmp241; + _jacobian(5, 3) = _tmp203 * sqrt_info(5, 5); + _jacobian(0, 4) = _tmp244; + _jacobian(1, 4) = _tmp245; + _jacobian(2, 4) = _tmp246; + _jacobian(3, 4) = _tmp247; + _jacobian(4, 4) = _tmp248; + _jacobian(5, 4) = _tmp201 * sqrt_info(5, 5); + _jacobian(0, 5) = _tmp250; + _jacobian(1, 5) = _tmp251; + _jacobian(2, 5) = _tmp252; + _jacobian(3, 5) = _tmp253; + _jacobian(4, 5) = _tmp254; + _jacobian(5, 5) = _tmp249 * sqrt_info(5, 5); + _jacobian(0, 6) = _tmp266; + _jacobian(1, 6) = _tmp268; + _jacobian(2, 6) = _tmp269; + _jacobian(3, 6) = 0; + _jacobian(4, 6) = 0; + _jacobian(5, 6) = 0; + _jacobian(0, 7) = _tmp277; + _jacobian(1, 7) = _tmp279; + _jacobian(2, 7) = _tmp280; + _jacobian(3, 7) = 0; + _jacobian(4, 7) = 0; + _jacobian(5, 7) = 0; + _jacobian(0, 8) = _tmp285; + _jacobian(1, 8) = _tmp287; + _jacobian(2, 8) = _tmp288; + _jacobian(3, 8) = 0; + _jacobian(4, 8) = 0; + _jacobian(5, 8) = 0; + _jacobian(0, 9) = _tmp289; + _jacobian(1, 9) = _tmp290; + _jacobian(2, 9) = _tmp291; + _jacobian(3, 9) = _tmp292; + _jacobian(4, 9) = _tmp293; + _jacobian(5, 9) = _tmp74 * sqrt_info(5, 5); + _jacobian(0, 10) = _tmp294; + _jacobian(1, 10) = _tmp295; + _jacobian(2, 10) = _tmp296; + _jacobian(3, 10) = _tmp297; + _jacobian(4, 10) = _tmp298; + _jacobian(5, 10) = _tmp72 * sqrt_info(5, 5); + _jacobian(0, 11) = _tmp299; + _jacobian(1, 11) = _tmp300; + _jacobian(2, 11) = _tmp301; + _jacobian(3, 11) = _tmp302; + _jacobian(4, 11) = _tmp303; + _jacobian(5, 11) = _tmp70 * sqrt_info(5, 5); } 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, 0) = std::pow(_tmp164, Scalar(2)) * _tmp304 + std::pow(_tmp165, Scalar(2)) + + std::pow(_tmp173, Scalar(2)) + std::pow(_tmp175, Scalar(2)) + + std::pow(_tmp176, Scalar(2)) + std::pow(_tmp177, Scalar(2)); + _hessian(1, 0) = _tmp165 * _tmp207 + _tmp173 * _tmp209 + _tmp175 * _tmp210 + _tmp176 * _tmp211 + + _tmp177 * _tmp212 + _tmp206 * _tmp305; + _hessian(2, 0) = _tmp165 * _tmp230 + _tmp173 * _tmp232 + _tmp175 * _tmp233 + _tmp176 * _tmp234 + + _tmp177 * _tmp235; + _hessian(3, 0) = _tmp165 * _tmp237 + _tmp173 * _tmp238 + _tmp175 * _tmp239 + _tmp176 * _tmp240 + + _tmp177 * _tmp241 + _tmp203 * _tmp305; + _hessian(4, 0) = _tmp165 * _tmp244 + _tmp173 * _tmp245 + _tmp175 * _tmp246 + _tmp176 * _tmp247 + + _tmp177 * _tmp248 + _tmp201 * _tmp305; + _hessian(5, 0) = _tmp165 * _tmp250 + _tmp173 * _tmp251 + _tmp175 * _tmp252 + _tmp176 * _tmp253 + + _tmp177 * _tmp254 + _tmp249 * _tmp305; + _hessian(6, 0) = _tmp165 * _tmp266 + _tmp173 * _tmp268 + _tmp175 * _tmp269; + _hessian(7, 0) = _tmp165 * _tmp277 + _tmp173 * _tmp279 + _tmp175 * _tmp280; + _hessian(8, 0) = _tmp165 * _tmp285 + _tmp173 * _tmp287 + _tmp175 * _tmp288; + _hessian(9, 0) = _tmp165 * _tmp289 + _tmp173 * _tmp290 + _tmp175 * _tmp291 + _tmp176 * _tmp292 + + _tmp177 * _tmp293 + _tmp305 * _tmp74; + _hessian(10, 0) = _tmp165 * _tmp294 + _tmp173 * _tmp295 + _tmp175 * _tmp296 + + _tmp176 * _tmp297 + _tmp177 * _tmp298 + _tmp305 * _tmp72; + _hessian(11, 0) = _tmp165 * _tmp299 + _tmp173 * _tmp300 + _tmp175 * _tmp301 + + _tmp176 * _tmp302 + _tmp177 * _tmp303 + _tmp305 * _tmp70; _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(1, 1) = std::pow(_tmp207, Scalar(2)) + std::pow(_tmp209, Scalar(2)) + + std::pow(_tmp210, Scalar(2)) + std::pow(_tmp211, Scalar(2)) + + _tmp304 * _tmp306 + _tmp306 * std::pow(sqrt_info(4, 5), Scalar(2)); + _hessian(2, 1) = _tmp207 * _tmp230 + _tmp209 * _tmp232 + _tmp210 * _tmp233 + _tmp211 * _tmp234 + + _tmp212 * _tmp235; + _hessian(3, 1) = _tmp203 * _tmp307 + _tmp207 * _tmp237 + _tmp209 * _tmp238 + _tmp210 * _tmp239 + + _tmp211 * _tmp240 + _tmp212 * _tmp241; + _hessian(4, 1) = _tmp201 * _tmp307 + _tmp207 * _tmp244 + _tmp209 * _tmp245 + _tmp210 * _tmp246 + + _tmp211 * _tmp247 + _tmp212 * _tmp248; + _hessian(5, 1) = _tmp207 * _tmp250 + _tmp209 * _tmp251 + _tmp210 * _tmp252 + _tmp211 * _tmp253 + + _tmp212 * _tmp254 + _tmp249 * _tmp307; + _hessian(6, 1) = _tmp207 * _tmp266 + _tmp209 * _tmp268 + _tmp210 * _tmp269; + _hessian(7, 1) = _tmp207 * _tmp277 + _tmp209 * _tmp279 + _tmp210 * _tmp280; + _hessian(8, 1) = _tmp207 * _tmp285 + _tmp209 * _tmp287 + _tmp210 * _tmp288; + _hessian(9, 1) = _tmp207 * _tmp289 + _tmp209 * _tmp290 + _tmp210 * _tmp291 + _tmp211 * _tmp292 + + _tmp212 * _tmp293 + _tmp307 * _tmp74; + _hessian(10, 1) = _tmp207 * _tmp294 + _tmp209 * _tmp295 + _tmp210 * _tmp296 + + _tmp211 * _tmp297 + _tmp212 * _tmp298 + _tmp307 * _tmp72; + _hessian(11, 1) = _tmp207 * _tmp299 + _tmp209 * _tmp300 + _tmp210 * _tmp301 + + _tmp211 * _tmp302 + _tmp212 * _tmp303 + _tmp307 * _tmp70; _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(2, 2) = std::pow(_tmp229, Scalar(2)) * std::pow(sqrt_info(4, 4), Scalar(2)) + + std::pow(_tmp230, Scalar(2)) + std::pow(_tmp232, Scalar(2)) + + std::pow(_tmp233, Scalar(2)) + std::pow(_tmp234, Scalar(2)); + _hessian(3, 2) = _tmp230 * _tmp237 + _tmp232 * _tmp238 + _tmp233 * _tmp239 + _tmp234 * _tmp240 + + _tmp235 * _tmp241; + _hessian(4, 2) = _tmp230 * _tmp244 + _tmp232 * _tmp245 + _tmp233 * _tmp246 + _tmp234 * _tmp247 + + _tmp235 * _tmp248; + _hessian(5, 2) = _tmp230 * _tmp250 + _tmp232 * _tmp251 + _tmp233 * _tmp252 + _tmp234 * _tmp253 + + _tmp235 * _tmp254; + _hessian(6, 2) = _tmp230 * _tmp266 + _tmp232 * _tmp268 + _tmp233 * _tmp269; + _hessian(7, 2) = _tmp230 * _tmp277 + _tmp232 * _tmp279 + _tmp233 * _tmp280; + _hessian(8, 2) = _tmp230 * _tmp285 + _tmp232 * _tmp287 + _tmp233 * _tmp288; + _hessian(9, 2) = _tmp230 * _tmp289 + _tmp232 * _tmp290 + _tmp233 * _tmp291 + _tmp234 * _tmp292 + + _tmp235 * _tmp293; + _hessian(10, 2) = _tmp230 * _tmp294 + _tmp232 * _tmp295 + _tmp233 * _tmp296 + + _tmp234 * _tmp297 + _tmp235 * _tmp298; + _hessian(11, 2) = _tmp230 * _tmp299 + _tmp232 * _tmp300 + _tmp233 * _tmp301 + + _tmp234 * _tmp302 + _tmp235 * _tmp303; _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(3, 3) = std::pow(_tmp203, Scalar(2)) * _tmp304 + std::pow(_tmp237, Scalar(2)) + + std::pow(_tmp238, Scalar(2)) + std::pow(_tmp239, Scalar(2)) + + std::pow(_tmp240, Scalar(2)) + std::pow(_tmp241, Scalar(2)); + _hessian(4, 3) = _tmp203 * _tmp308 + _tmp237 * _tmp244 + _tmp238 * _tmp245 + _tmp239 * _tmp246 + + _tmp240 * _tmp247 + _tmp241 * _tmp248; + _hessian(5, 3) = _tmp203 * _tmp309 + _tmp237 * _tmp250 + _tmp238 * _tmp251 + _tmp239 * _tmp252 + + _tmp240 * _tmp253 + _tmp241 * _tmp254; + _hessian(6, 3) = _tmp237 * _tmp266 + _tmp238 * _tmp268 + _tmp239 * _tmp269; + _hessian(7, 3) = _tmp237 * _tmp277 + _tmp238 * _tmp279 + _tmp239 * _tmp280; + _hessian(8, 3) = _tmp237 * _tmp285 + _tmp238 * _tmp287 + _tmp239 * _tmp288; + _hessian(9, 3) = _tmp203 * _tmp310 + _tmp237 * _tmp289 + _tmp238 * _tmp290 + _tmp239 * _tmp291 + + _tmp240 * _tmp292 + _tmp241 * _tmp293; + _hessian(10, 3) = _tmp203 * _tmp311 + _tmp237 * _tmp294 + _tmp238 * _tmp295 + + _tmp239 * _tmp296 + _tmp240 * _tmp297 + _tmp241 * _tmp298; + _hessian(11, 3) = _tmp203 * _tmp304 * _tmp70 + _tmp237 * _tmp299 + _tmp238 * _tmp300 + + _tmp239 * _tmp301 + _tmp240 * _tmp302 + _tmp241 * _tmp303; _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(4, 4) = std::pow(_tmp201, Scalar(2)) * _tmp304 + std::pow(_tmp244, Scalar(2)) + + std::pow(_tmp245, Scalar(2)) + std::pow(_tmp246, Scalar(2)) + + std::pow(_tmp247, Scalar(2)) + std::pow(_tmp248, Scalar(2)); + _hessian(5, 4) = _tmp244 * _tmp250 + _tmp245 * _tmp251 + _tmp246 * _tmp252 + _tmp247 * _tmp253 + + _tmp248 * _tmp254 + _tmp249 * _tmp308; + _hessian(6, 4) = _tmp244 * _tmp266 + _tmp245 * _tmp268 + _tmp246 * _tmp269; + _hessian(7, 4) = _tmp244 * _tmp277 + _tmp245 * _tmp279 + _tmp246 * _tmp280; + _hessian(8, 4) = _tmp244 * _tmp285 + _tmp245 * _tmp287 + _tmp246 * _tmp288; + _hessian(9, 4) = _tmp244 * _tmp289 + _tmp245 * _tmp290 + _tmp246 * _tmp291 + _tmp247 * _tmp292 + + _tmp248 * _tmp293 + _tmp308 * _tmp74; + _hessian(10, 4) = _tmp201 * _tmp311 + _tmp244 * _tmp294 + _tmp245 * _tmp295 + + _tmp246 * _tmp296 + _tmp247 * _tmp297 + _tmp248 * _tmp298; + _hessian(11, 4) = _tmp244 * _tmp299 + _tmp245 * _tmp300 + _tmp246 * _tmp301 + + _tmp247 * _tmp302 + _tmp248 * _tmp303 + _tmp308 * _tmp70; _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(5, 5) = std::pow(_tmp249, Scalar(2)) * _tmp304 + std::pow(_tmp250, Scalar(2)) + + std::pow(_tmp251, Scalar(2)) + std::pow(_tmp252, Scalar(2)) + + std::pow(_tmp253, Scalar(2)) + std::pow(_tmp254, Scalar(2)); + _hessian(6, 5) = _tmp250 * _tmp266 + _tmp251 * _tmp268 + _tmp252 * _tmp269; + _hessian(7, 5) = _tmp250 * _tmp277 + _tmp251 * _tmp279 + _tmp252 * _tmp280; + _hessian(8, 5) = _tmp250 * _tmp285 + _tmp251 * _tmp287 + _tmp252 * _tmp288; + _hessian(9, 5) = _tmp249 * _tmp310 + _tmp250 * _tmp289 + _tmp251 * _tmp290 + _tmp252 * _tmp291 + + _tmp253 * _tmp292 + _tmp254 * _tmp293; + _hessian(10, 5) = _tmp249 * _tmp311 + _tmp250 * _tmp294 + _tmp251 * _tmp295 + + _tmp252 * _tmp296 + _tmp253 * _tmp297 + _tmp254 * _tmp298; + _hessian(11, 5) = _tmp250 * _tmp299 + _tmp251 * _tmp300 + _tmp252 * _tmp301 + + _tmp253 * _tmp302 + _tmp254 * _tmp303 + _tmp309 * _tmp70; _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(6, 6) = + std::pow(_tmp266, Scalar(2)) + std::pow(_tmp268, Scalar(2)) + std::pow(_tmp269, Scalar(2)); + _hessian(7, 6) = _tmp266 * _tmp277 + _tmp268 * _tmp279 + _tmp269 * _tmp280; + _hessian(8, 6) = _tmp266 * _tmp285 + _tmp268 * _tmp287 + _tmp269 * _tmp288; + _hessian(9, 6) = _tmp266 * _tmp289 + _tmp268 * _tmp290 + _tmp269 * _tmp291; + _hessian(10, 6) = _tmp266 * _tmp294 + _tmp268 * _tmp295 + _tmp269 * _tmp296; + _hessian(11, 6) = _tmp266 * _tmp299 + _tmp268 * _tmp300 + _tmp269 * _tmp301; _hessian(0, 7) = 0; _hessian(1, 7) = 0; _hessian(2, 7) = 0; @@ -848,17 +660,12 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b _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(7, 7) = + std::pow(_tmp277, Scalar(2)) + std::pow(_tmp279, Scalar(2)) + std::pow(_tmp280, Scalar(2)); + _hessian(8, 7) = _tmp277 * _tmp285 + _tmp279 * _tmp287 + _tmp280 * _tmp288; + _hessian(9, 7) = _tmp277 * _tmp289 + _tmp279 * _tmp290 + _tmp280 * _tmp291; + _hessian(10, 7) = _tmp277 * _tmp294 + _tmp279 * _tmp295 + _tmp280 * _tmp296; + _hessian(11, 7) = _tmp277 * _tmp299 + _tmp279 * _tmp300 + _tmp280 * _tmp301; _hessian(0, 8) = 0; _hessian(1, 8) = 0; _hessian(2, 8) = 0; @@ -867,15 +674,11 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b _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(8, 8) = + std::pow(_tmp285, Scalar(2)) + std::pow(_tmp287, Scalar(2)) + std::pow(_tmp288, Scalar(2)); + _hessian(9, 8) = _tmp285 * _tmp289 + _tmp287 * _tmp290 + _tmp288 * _tmp291; + _hessian(10, 8) = _tmp285 * _tmp294 + _tmp287 * _tmp295 + _tmp288 * _tmp296; + _hessian(11, 8) = _tmp285 * _tmp299 + _tmp287 * _tmp300 + _tmp288 * _tmp301; _hessian(0, 9) = 0; _hessian(1, 9) = 0; _hessian(2, 9) = 0; @@ -885,13 +688,13 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b _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(9, 9) = std::pow(_tmp289, Scalar(2)) + std::pow(_tmp290, Scalar(2)) + + std::pow(_tmp291, Scalar(2)) + std::pow(_tmp292, Scalar(2)) + + std::pow(_tmp293, Scalar(2)) + _tmp304 * std::pow(_tmp74, Scalar(2)); + _hessian(10, 9) = _tmp289 * _tmp294 + _tmp290 * _tmp295 + _tmp291 * _tmp296 + + _tmp292 * _tmp297 + _tmp293 * _tmp298 + _tmp311 * _tmp74; + _hessian(11, 9) = _tmp289 * _tmp299 + _tmp290 * _tmp300 + _tmp291 * _tmp301 + + _tmp292 * _tmp302 + _tmp293 * _tmp303 + _tmp310 * _tmp70; _hessian(0, 10) = 0; _hessian(1, 10) = 0; _hessian(2, 10) = 0; @@ -902,11 +705,11 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b _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(10, 10) = std::pow(_tmp294, Scalar(2)) + std::pow(_tmp295, Scalar(2)) + + std::pow(_tmp296, Scalar(2)) + std::pow(_tmp297, Scalar(2)) + + std::pow(_tmp298, Scalar(2)) + _tmp304 * std::pow(_tmp72, Scalar(2)); + _hessian(11, 10) = _tmp294 * _tmp299 + _tmp295 * _tmp300 + _tmp296 * _tmp301 + + _tmp297 * _tmp302 + _tmp298 * _tmp303 + _tmp311 * _tmp70; _hessian(0, 11) = 0; _hessian(1, 11) = 0; _hessian(2, 11) = 0; @@ -918,38 +721,35 @@ void BetweenFactorPose3(const sym::Pose3& a, const sym::Pose3& b _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)); + _hessian(11, 11) = std::pow(_tmp299, Scalar(2)) + std::pow(_tmp300, Scalar(2)) + + std::pow(_tmp301, Scalar(2)) + std::pow(_tmp302, Scalar(2)) + + std::pow(_tmp303, Scalar(2)) + _tmp304 * std::pow(_tmp70, 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; + _rhs(0, 0) = _tmp164 * _tmp312 + _tmp165 * _tmp78 + _tmp173 * _tmp81 + _tmp175 * _tmp82 + + _tmp176 * _tmp83 + _tmp177 * _tmp84; + _rhs(1, 0) = _tmp206 * _tmp312 + _tmp207 * _tmp78 + _tmp209 * _tmp81 + _tmp210 * _tmp82 + + _tmp211 * _tmp83 + _tmp212 * _tmp84; + _rhs(2, 0) = _tmp230 * _tmp78 + _tmp232 * _tmp81 + _tmp233 * _tmp82 + _tmp234 * _tmp83 + + _tmp235 * _tmp84; + _rhs(3, 0) = _tmp203 * _tmp312 + _tmp237 * _tmp78 + _tmp238 * _tmp81 + _tmp239 * _tmp82 + + _tmp240 * _tmp83 + _tmp241 * _tmp84; + _rhs(4, 0) = _tmp201 * _tmp312 + _tmp244 * _tmp78 + _tmp245 * _tmp81 + _tmp246 * _tmp82 + + _tmp247 * _tmp83 + _tmp248 * _tmp84; + _rhs(5, 0) = _tmp249 * _tmp312 + _tmp250 * _tmp78 + _tmp251 * _tmp81 + _tmp252 * _tmp82 + + _tmp253 * _tmp83 + _tmp254 * _tmp84; + _rhs(6, 0) = _tmp266 * _tmp78 + _tmp268 * _tmp81 + _tmp269 * _tmp82; + _rhs(7, 0) = _tmp277 * _tmp78 + _tmp279 * _tmp81 + _tmp280 * _tmp82; + _rhs(8, 0) = _tmp285 * _tmp78 + _tmp287 * _tmp81 + _tmp288 * _tmp82; + _rhs(9, 0) = _tmp289 * _tmp78 + _tmp290 * _tmp81 + _tmp291 * _tmp82 + _tmp292 * _tmp83 + + _tmp293 * _tmp84 + _tmp312 * _tmp74; + _rhs(10, 0) = _tmp294 * _tmp78 + _tmp295 * _tmp81 + _tmp296 * _tmp82 + _tmp297 * _tmp83 + + _tmp298 * _tmp84 + _tmp312 * _tmp72; + _rhs(11, 0) = _tmp299 * _tmp78 + _tmp300 * _tmp81 + _tmp301 * _tmp82 + _tmp302 * _tmp83 + + _tmp303 * _tmp84 + _tmp312 * _tmp70; } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/between_factor_pose3_position.h b/gen/cpp/sym/factors/between_factor_pose3_position.h index 96a7afc3a..6d5d640c3 100644 --- a/gen/cpp/sym/factors/between_factor_pose3_position.h +++ b/gen/cpp/sym/factors/between_factor_pose3_position.h @@ -38,7 +38,7 @@ void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 553 + // Total ops: 500 // Unused inputs (void)epsilon; @@ -47,167 +47,152 @@ void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& _a = a.Data(); const Eigen::Matrix& _b = b.Data(); - // Intermediate terms (105) - const Scalar _tmp0 = std::pow(_a[2], Scalar(2)); + // Intermediate terms (100) + const Scalar _tmp0 = std::pow(_a[1], Scalar(2)); const Scalar _tmp1 = 2 * _tmp0; const Scalar _tmp2 = -_tmp1; - const Scalar _tmp3 = std::pow(_a[1], Scalar(2)); + const Scalar _tmp3 = std::pow(_a[2], 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 _tmp5 = 1 - _tmp4; + const Scalar _tmp6 = _tmp2 + _tmp5; + 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] * _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 _tmp18 = _b[5] * _tmp16 + _b[6] * _tmp11; + const Scalar _tmp19 = std::pow(_a[0], Scalar(2)); + const Scalar _tmp20 = 2 * _tmp19; + const Scalar _tmp21 = -_tmp20; + const Scalar _tmp22 = _tmp21 + _tmp5; + const Scalar _tmp23 = _a[2] * _tmp8; + const Scalar _tmp24 = _a[0] * _tmp14; + const Scalar _tmp25 = _tmp23 + _tmp24; + const Scalar _tmp26 = _a[6] * _tmp25; + const Scalar _tmp27 = -_tmp15; + const Scalar _tmp28 = _tmp13 + _tmp27; + const Scalar _tmp29 = _a[4] * _tmp28; + const Scalar _tmp30 = _b[4] * _tmp28 + _b[6] * _tmp25; + const Scalar _tmp31 = -_a[5] * _tmp22 + _b[5] * _tmp22 - _tmp26 - _tmp29 + _tmp30 - a_t_b(1, 0); + const Scalar _tmp32 = _tmp2 + _tmp21 + 1; + const Scalar _tmp33 = -_tmp24; + const Scalar _tmp34 = _tmp23 + _tmp33; + const Scalar _tmp35 = _a[5] * _tmp34; + const Scalar _tmp36 = _tmp7 + _tmp9; + const Scalar _tmp37 = _a[4] * _tmp36; + const Scalar _tmp38 = _b[4] * _tmp36 + _b[5] * _tmp34; + const Scalar _tmp39 = -_a[6] * _tmp32 + _b[6] * _tmp32 - _tmp35 - _tmp37 + _tmp38 - a_t_b(2, 0); + const Scalar _tmp40 = + _tmp31 * sqrt_info(0, 1) + _tmp39 * sqrt_info(0, 2) + + sqrt_info(0, 0) * (-_a[4] * _tmp6 + _b[4] * _tmp6 - _tmp12 - _tmp17 + _tmp18 - a_t_b(0, 0)); + const Scalar _tmp41 = _tmp31 * sqrt_info(1, 1) + _tmp39 * sqrt_info(1, 2); + const Scalar _tmp42 = -_tmp19; + const Scalar _tmp43 = -_tmp0; + const Scalar _tmp44 = std::pow(_a[3], Scalar(2)); + const Scalar _tmp45 = _tmp3 + _tmp42 + _tmp43 + _tmp44; + const Scalar _tmp46 = -_a[6] * _tmp45 + _b[6] * _tmp45 - _tmp35 - _tmp37 + _tmp38; + const Scalar _tmp47 = _tmp19 + _tmp43; + const Scalar _tmp48 = -_tmp44; + const Scalar _tmp49 = _tmp3 + _tmp48; + const Scalar _tmp50 = _tmp47 + _tmp49; + const Scalar _tmp51 = -_tmp23; + const Scalar _tmp52 = _tmp33 + _tmp51; + const Scalar _tmp53 = -_tmp13; + 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 = _tmp46 * sqrt_info(0, 1) + _tmp55 * sqrt_info(0, 2); + const Scalar _tmp57 = _tmp46 * sqrt_info(1, 1) + _tmp55 * sqrt_info(1, 2); + const Scalar _tmp58 = -_tmp3; + const Scalar _tmp59 = _tmp0 + _tmp19 + _tmp48 + _tmp58; + const Scalar _tmp60 = _tmp24 + _tmp51; + const Scalar _tmp61 = -_tmp7; + const Scalar _tmp62 = _tmp10 + _tmp61; + const Scalar _tmp63 = _tmp44 + _tmp58; + const Scalar _tmp64 = _tmp47 + _tmp63; + const Scalar _tmp65 = -_a[4] * _tmp64 + _b[4] * _tmp64 - _tmp12 - _tmp17 + _tmp18; + const Scalar _tmp66 = _tmp65 * sqrt_info(0, 2) + + sqrt_info(0, 0) * (-_a[4] * _tmp62 - _a[5] * _tmp60 - _a[6] * _tmp59 + + _b[4] * _tmp62 + _b[5] * _tmp60 + _b[6] * _tmp59); + const Scalar _tmp67 = _tmp65 * sqrt_info(1, 2); + const Scalar _tmp68 = _tmp0 + _tmp42; + const Scalar _tmp69 = _tmp63 + _tmp68; + const Scalar _tmp70 = _tmp49 + _tmp68; + const Scalar _tmp71 = _tmp61 + _tmp9; + const Scalar _tmp72 = _tmp27 + _tmp53; + const Scalar _tmp73 = -_a[4] * _tmp70 - _a[5] * _tmp72 - _a[6] * _tmp71 + _b[4] * _tmp70 + + _b[5] * _tmp72 + _b[6] * _tmp71; + const Scalar _tmp74 = + _tmp73 * sqrt_info(0, 1) + + sqrt_info(0, 0) * (-_a[5] * _tmp69 + _b[5] * _tmp69 - _tmp26 - _tmp29 + _tmp30); + const Scalar _tmp75 = _tmp73 * sqrt_info(1, 1); + const Scalar _tmp76 = _tmp4 - 1; + const Scalar _tmp77 = + _tmp54 * sqrt_info(0, 1) + _tmp62 * sqrt_info(0, 2) + sqrt_info(0, 0) * (_tmp1 + _tmp76); + const Scalar _tmp78 = _tmp54 * sqrt_info(1, 1) + _tmp62 * sqrt_info(1, 2); + const Scalar _tmp79 = _tmp20 + _tmp76; + const Scalar _tmp80 = + _tmp60 * sqrt_info(0, 2) + _tmp72 * sqrt_info(0, 0) + _tmp79 * sqrt_info(0, 1); + const Scalar _tmp81 = _tmp60 * sqrt_info(1, 2) + _tmp79 * sqrt_info(1, 1); + const Scalar _tmp82 = _tmp1 + _tmp20 - 1; + const Scalar _tmp83 = + _tmp52 * sqrt_info(0, 1) + _tmp71 * sqrt_info(0, 0) + _tmp82 * sqrt_info(0, 2); + const Scalar _tmp84 = _tmp52 * sqrt_info(1, 1) + _tmp82 * sqrt_info(1, 2); 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; + _tmp28 * sqrt_info(0, 1) + _tmp36 * sqrt_info(0, 2) + _tmp6 * sqrt_info(0, 0); + const Scalar _tmp86 = _tmp28 * sqrt_info(1, 1) + _tmp36 * sqrt_info(1, 2); + const Scalar _tmp87 = + _tmp16 * sqrt_info(0, 0) + _tmp22 * sqrt_info(0, 1) + _tmp34 * sqrt_info(0, 2); + const Scalar _tmp88 = _tmp22 * sqrt_info(1, 1) + _tmp34 * sqrt_info(1, 2); 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); + _tmp11 * sqrt_info(0, 0) + _tmp25 * sqrt_info(0, 1) + _tmp32 * sqrt_info(0, 2); + const Scalar _tmp90 = _tmp25 * sqrt_info(1, 1) + _tmp32 * sqrt_info(1, 2); + const Scalar _tmp91 = std::pow(sqrt_info(2, 2), Scalar(2)); + const Scalar _tmp92 = _tmp55 * _tmp91; + const Scalar _tmp93 = std::pow(_tmp65, Scalar(2)); + const Scalar _tmp94 = _tmp65 * _tmp91; + const Scalar _tmp95 = _tmp82 * _tmp91; + const Scalar _tmp96 = _tmp36 * _tmp91; + const Scalar _tmp97 = _tmp32 * _tmp91; + const Scalar _tmp98 = _tmp34 * _tmp91; + const Scalar _tmp99 = _tmp39 * _tmp91; // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp41; - _res(1, 0) = _tmp42; - _res(2, 0) = _tmp43; + _res(0, 0) = _tmp40; + _res(1, 0) = _tmp41; + _res(2, 0) = _tmp39 * sqrt_info(2, 2); } 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, 0) = _tmp56; + _jacobian(1, 0) = _tmp57; + _jacobian(2, 0) = _tmp55 * sqrt_info(2, 2); + _jacobian(0, 1) = _tmp66; + _jacobian(1, 1) = _tmp67; + _jacobian(2, 1) = _tmp65 * sqrt_info(2, 2); + _jacobian(0, 2) = _tmp74; + _jacobian(1, 2) = _tmp75; + _jacobian(2, 2) = 0; + _jacobian(0, 3) = _tmp77; + _jacobian(1, 3) = _tmp78; + _jacobian(2, 3) = _tmp62 * sqrt_info(2, 2); + _jacobian(0, 4) = _tmp80; + _jacobian(1, 4) = _tmp81; + _jacobian(2, 4) = _tmp60 * sqrt_info(2, 2); + _jacobian(0, 5) = _tmp83; + _jacobian(1, 5) = _tmp84; + _jacobian(2, 5) = _tmp82 * sqrt_info(2, 2); _jacobian(0, 6) = 0; _jacobian(1, 6) = 0; _jacobian(2, 6) = 0; @@ -217,15 +202,15 @@ void BetweenFactorPose3Position(const sym::Pose3& a, const sym::Pose3& a, const sym::Pose3& _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(0, 0) = _tmp39 * _tmp92 + _tmp40 * _tmp56 + _tmp41 * _tmp57; + _rhs(1, 0) = _tmp40 * _tmp66 + _tmp41 * _tmp67 + _tmp65 * _tmp99; + _rhs(2, 0) = _tmp40 * _tmp74 + _tmp41 * _tmp75; + _rhs(3, 0) = _tmp40 * _tmp77 + _tmp41 * _tmp78 + _tmp62 * _tmp99; + _rhs(4, 0) = _tmp40 * _tmp80 + _tmp41 * _tmp81 + _tmp60 * _tmp99; + _rhs(5, 0) = _tmp39 * _tmp95 + _tmp40 * _tmp83 + _tmp41 * _tmp84; _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; + _rhs(9, 0) = _tmp39 * _tmp96 + _tmp40 * _tmp85 + _tmp41 * _tmp86; + _rhs(10, 0) = _tmp34 * _tmp99 + _tmp40 * _tmp87 + _tmp41 * _tmp88; + _rhs(11, 0) = _tmp39 * _tmp97 + _tmp40 * _tmp89 + _tmp41 * _tmp90; } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/between_factor_pose3_rotation.h b/gen/cpp/sym/factors/between_factor_pose3_rotation.h index 0cc300f3a..a875bbb55 100644 --- a/gen/cpp/sym/factors/between_factor_pose3_rotation.h +++ b/gen/cpp/sym/factors/between_factor_pose3_rotation.h @@ -39,290 +39,239 @@ void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 742 + // Total ops: 605 // 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; + // Intermediate terms (179) + const Scalar _tmp0 = 1 - epsilon; + const Scalar _tmp1 = _a[2] * _b[1]; + const Scalar _tmp2 = _a[0] * _b[3]; + const Scalar _tmp3 = _a[1] * _b[2]; + const Scalar _tmp4 = _a[3] * _b[0]; + const Scalar _tmp5 = _tmp1 - _tmp2 - _tmp3 + _tmp4; + const Scalar _tmp6 = _a_R_b[0] * _tmp5; + const Scalar _tmp7 = _a[3] * _b[1]; + const Scalar _tmp8 = _a[1] * _b[3]; + const Scalar _tmp9 = _a[0] * _b[2]; + const Scalar _tmp10 = _a[2] * _b[0]; + const Scalar _tmp11 = -_tmp10 + _tmp7 - _tmp8 + _tmp9; + const Scalar _tmp12 = _a_R_b[1] * _tmp11; + const Scalar _tmp13 = _a[0] * _b[1]; + const Scalar _tmp14 = _a[2] * _b[3]; + const Scalar _tmp15 = _a[3] * _b[2]; + const Scalar _tmp16 = _a[1] * _b[0]; + const Scalar _tmp17 = -_tmp13 - _tmp14 + _tmp15 + _tmp16; + const Scalar _tmp18 = _a_R_b[2] * _tmp17; + const Scalar _tmp19 = -_tmp12 - _tmp18 - _tmp6; + const Scalar _tmp20 = _a[1] * _b[1]; + const Scalar _tmp21 = _a[2] * _b[2]; + const Scalar _tmp22 = _a[0] * _b[0]; + const Scalar _tmp23 = _a[3] * _b[3]; + const Scalar _tmp24 = _tmp20 + _tmp21 + _tmp22 + _tmp23; + const Scalar _tmp25 = _a_R_b[3] * _tmp24; + const Scalar _tmp26 = std::min(_tmp0, std::fabs(_tmp19 - _tmp25)); + const Scalar _tmp27 = + std::pow(Scalar(1 - std::pow(_tmp26, Scalar(2))), Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp28 = std::acos(_tmp26); + const Scalar _tmp29 = + 2 * std::min(0, (((-_tmp19 + _tmp25) > 0) - ((-_tmp19 + _tmp25) < 0))) + 1; + const Scalar _tmp30 = 2 * _tmp29; + const Scalar _tmp31 = _tmp27 * _tmp28 * _tmp30; + const Scalar _tmp32 = sqrt_info(0, 0) * (-_a_R_b[0] * _tmp24 - _a_R_b[1] * _tmp17 + + _a_R_b[2] * _tmp11 + _a_R_b[3] * _tmp5); + const Scalar _tmp33 = + -_a_R_b[0] * _tmp11 + _a_R_b[1] * _tmp5 - _a_R_b[2] * _tmp24 + _a_R_b[3] * _tmp17; + const Scalar _tmp34 = _tmp33 * sqrt_info(0, 2); + const Scalar _tmp35 = + _a_R_b[0] * _tmp17 - _a_R_b[1] * _tmp24 - _a_R_b[2] * _tmp5 + _a_R_b[3] * _tmp11; + const Scalar _tmp36 = _tmp31 * _tmp35; + const Scalar _tmp37 = _tmp31 * _tmp32 + _tmp31 * _tmp34 + _tmp36 * sqrt_info(0, 1); + const Scalar _tmp38 = _tmp30 * sqrt_info(1, 2); + const Scalar _tmp39 = _tmp27 * _tmp28 * _tmp33 * _tmp38 + _tmp36 * sqrt_info(1, 1); + const Scalar _tmp40 = _tmp33 * sqrt_info(2, 2); + const Scalar _tmp41 = _tmp31 * _tmp40; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp45 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp46 = -_tmp42 - _tmp43 - _tmp44 - _tmp45; + const Scalar _tmp47 = _a_R_b[1] * _tmp46; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp51 = (Scalar(1) / Scalar(2)) * _tmp4; + const Scalar _tmp52 = _tmp48 - _tmp49 - _tmp50 + _tmp51; + const Scalar _tmp53 = _a_R_b[2] * _tmp52; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp57 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp58 = -_tmp54 - _tmp55 + _tmp56 + _tmp57; + const Scalar _tmp59 = -_a_R_b[0] * _tmp58; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp64 = -_tmp60 + _tmp61 - _tmp62 + _tmp63; + const Scalar _tmp65 = _a_R_b[3] * _tmp64; + const Scalar _tmp66 = _tmp59 + _tmp65; + const Scalar _tmp67 = _tmp47 - _tmp53 + _tmp66; + const Scalar _tmp68 = _tmp12 + _tmp18 + _tmp25 + _tmp6; const Scalar _tmp69 = std::fabs(_tmp68); - const Scalar _tmp70 = std::min(_tmp21, _tmp69); + const Scalar _tmp70 = std::min(_tmp0, _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 _tmp74 = _tmp30 * _tmp73; + const Scalar _tmp75 = _tmp74 * sqrt_info(0, 2); + const Scalar _tmp76 = -_a_R_b[1] * _tmp52; + const Scalar _tmp77 = _a_R_b[2] * _tmp46; + const Scalar _tmp78 = _a_R_b[3] * _tmp58; + const Scalar _tmp79 = _a_R_b[0] * _tmp64; 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 _tmp82 = _tmp74 * sqrt_info(0, 1); + const Scalar _tmp83 = _a_R_b[3] * _tmp46; + const Scalar _tmp84 = _a_R_b[0] * _tmp52; + const Scalar _tmp85 = _a_R_b[2] * _tmp58; + const Scalar _tmp86 = -_a_R_b[1] * _tmp64; 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) * + const Scalar _tmp88 = _tmp74 * sqrt_info(0, 0); + const Scalar _tmp89 = _tmp35 * sqrt_info(0, 1); + const Scalar _tmp90 = _a_R_b[3] * _tmp52; + const Scalar _tmp91 = _a_R_b[0] * _tmp46; + const Scalar _tmp92 = _a_R_b[1] * _tmp58; + const Scalar _tmp93 = _a_R_b[2] * _tmp64; + const Scalar _tmp94 = _tmp92 + _tmp93; + const Scalar _tmp95 = _tmp90 + _tmp91 + _tmp94; + const Scalar _tmp96 = _tmp29 * ((((_tmp0 - _tmp69) > 0) - ((_tmp0 - _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); + const Scalar _tmp97 = _tmp96 / _tmp71; + const Scalar _tmp98 = _tmp95 * _tmp97; + const Scalar _tmp99 = _tmp70 * _tmp72 * _tmp96 / (_tmp71 * std::sqrt(_tmp71)); + const Scalar _tmp100 = _tmp32 * _tmp99; + const Scalar _tmp101 = _tmp89 * _tmp99; + const Scalar _tmp102 = _tmp95 * _tmp99; + const Scalar _tmp103 = _tmp100 * _tmp95 + _tmp101 * _tmp95 + _tmp102 * _tmp34 - _tmp32 * _tmp98 - + _tmp34 * _tmp98 + _tmp67 * _tmp75 + _tmp81 * _tmp82 + + _tmp88 * (_tmp83 - _tmp84 + _tmp87) - _tmp89 * _tmp98; + const Scalar _tmp104 = _tmp38 * _tmp73; + const Scalar _tmp105 = _tmp74 * sqrt_info(1, 1); + const Scalar _tmp106 = _tmp33 * sqrt_info(1, 2); + const Scalar _tmp107 = _tmp35 * sqrt_info(1, 1); + const Scalar _tmp108 = _tmp102 * _tmp106 + _tmp102 * _tmp107 + _tmp104 * _tmp67 + + _tmp105 * _tmp81 - _tmp106 * _tmp98 - _tmp107 * _tmp98; + const Scalar _tmp109 = _tmp74 * sqrt_info(2, 2); + const Scalar _tmp110 = _tmp102 * _tmp40 + _tmp109 * _tmp67 - _tmp40 * _tmp98; + const Scalar _tmp111 = _tmp60 - _tmp61 + _tmp62 - _tmp63; + const Scalar _tmp112 = _a_R_b[0] * _tmp111; + const Scalar _tmp113 = _tmp54 + _tmp55 - _tmp56 - _tmp57; + const Scalar _tmp114 = _a_R_b[3] * _tmp113; + const Scalar _tmp115 = _tmp114 + _tmp76; + const Scalar _tmp116 = _a_R_b[3] * _tmp111; + const Scalar _tmp117 = _a_R_b[0] * _tmp113; + const Scalar _tmp118 = _tmp117 + _tmp53; + const Scalar _tmp119 = _tmp116 + _tmp118 + _tmp47; + const Scalar _tmp120 = _tmp119 * _tmp97; + const Scalar _tmp121 = _a_R_b[1] * _tmp111; + const Scalar _tmp122 = -_a_R_b[2] * _tmp113; + const Scalar _tmp123 = _tmp122 + _tmp84; + const Scalar _tmp124 = -_tmp121 + _tmp123 + _tmp83; + const Scalar _tmp125 = -_a_R_b[2] * _tmp111; + const Scalar _tmp126 = _a_R_b[1] * _tmp113; + const Scalar _tmp127 = _tmp126 + _tmp90; + const Scalar _tmp128 = _tmp125 + _tmp127 - _tmp91; + const Scalar _tmp129 = _tmp119 * _tmp99; + const Scalar _tmp130 = -_tmp120 * _tmp32 - _tmp120 * _tmp34 - _tmp120 * _tmp89 + + _tmp124 * _tmp82 + _tmp128 * _tmp75 + _tmp129 * _tmp32 + _tmp129 * _tmp34 + + _tmp129 * _tmp89 + _tmp88 * (-_tmp112 + _tmp115 + _tmp77); + const Scalar _tmp131 = _tmp104 * _tmp128 + _tmp105 * _tmp124 - _tmp106 * _tmp120 + + _tmp106 * _tmp129 - _tmp107 * _tmp120 + _tmp107 * _tmp129; + const Scalar _tmp132 = _tmp109 * _tmp128 - _tmp120 * _tmp40 + _tmp129 * _tmp40; + const Scalar _tmp133 = -_tmp48 + _tmp49 + _tmp50 - _tmp51; + const Scalar _tmp134 = _a_R_b[2] * _tmp133; + const Scalar _tmp135 = _tmp116 + _tmp134; + const Scalar _tmp136 = _a_R_b[3] * _tmp133; + const Scalar _tmp137 = _tmp125 + _tmp136 + _tmp91 - _tmp92; + const Scalar _tmp138 = _a_R_b[1] * _tmp133; + const Scalar _tmp139 = _tmp112 + _tmp138; + const Scalar _tmp140 = _tmp139 + _tmp77 + _tmp78; + const Scalar _tmp141 = _tmp140 * _tmp99; + const Scalar _tmp142 = -_a_R_b[0] * _tmp133; + const Scalar _tmp143 = _tmp121 + _tmp142 + _tmp83 - _tmp85; + const Scalar _tmp144 = _tmp140 * _tmp97; + const Scalar _tmp145 = _tmp137 * _tmp82 + _tmp141 * _tmp32 + _tmp141 * _tmp34 + _tmp141 * _tmp89 + + _tmp143 * _tmp75 - _tmp144 * _tmp32 - _tmp144 * _tmp34 - _tmp144 * _tmp89 + + _tmp88 * (_tmp135 - _tmp47 + _tmp59); + const Scalar _tmp146 = _tmp104 * _tmp143 + _tmp105 * _tmp137 + _tmp106 * _tmp141 - + _tmp106 * _tmp144 + _tmp107 * _tmp141 - _tmp107 * _tmp144; + const Scalar _tmp147 = _tmp109 * _tmp143 + _tmp141 * _tmp40 - _tmp144 * _tmp40; + const Scalar _tmp148 = _tmp42 + _tmp43 + _tmp44 + _tmp45; + const Scalar _tmp149 = _a_R_b[0] * _tmp148; + const Scalar _tmp150 = _tmp136 + _tmp149; + const Scalar _tmp151 = _tmp150 + _tmp94; + const Scalar _tmp152 = _tmp151 * _tmp97; + const Scalar _tmp153 = _a_R_b[1] * _tmp148; + const Scalar _tmp154 = -_tmp134 + _tmp153 + _tmp66; + const Scalar _tmp155 = _a_R_b[3] * _tmp148; + const Scalar _tmp156 = _tmp142 + _tmp155; + const Scalar _tmp157 = _a_R_b[2] * _tmp148; + const Scalar _tmp158 = -_tmp138 - _tmp157 + _tmp80; + const Scalar _tmp159 = _tmp151 * _tmp99; + const Scalar _tmp160 = _tmp100 * _tmp151 - _tmp152 * _tmp32 - _tmp152 * _tmp34 - + _tmp152 * _tmp89 + _tmp154 * _tmp75 + _tmp158 * _tmp82 + _tmp159 * _tmp34 + + _tmp159 * _tmp89 + _tmp88 * (_tmp156 + _tmp87); + const Scalar _tmp161 = _tmp104 * _tmp154 + _tmp105 * _tmp158 - _tmp106 * _tmp152 + + _tmp106 * _tmp159 - _tmp107 * _tmp152 + _tmp107 * _tmp159; + const Scalar _tmp162 = _tmp109 * _tmp154 - _tmp152 * _tmp40 + _tmp159 * _tmp40; + const Scalar _tmp163 = _tmp118 + _tmp153 + _tmp65; + const Scalar _tmp164 = _tmp163 * _tmp97; + const Scalar _tmp165 = _tmp127 - _tmp149 - _tmp93; + const Scalar _tmp166 = _tmp163 * _tmp99; + const Scalar _tmp167 = _tmp123 + _tmp155 + _tmp86; + const Scalar _tmp168 = _tmp100 * _tmp163 - _tmp164 * _tmp32 - _tmp164 * _tmp34 - + _tmp164 * _tmp89 + _tmp165 * _tmp75 + _tmp166 * _tmp34 + _tmp166 * _tmp89 + + _tmp167 * _tmp82 + _tmp88 * (_tmp115 + _tmp157 - _tmp79); + const Scalar _tmp169 = _tmp104 * _tmp165 + _tmp105 * _tmp167 - _tmp106 * _tmp164 + + _tmp106 * _tmp166 - _tmp107 * _tmp164 + _tmp107 * _tmp166; + const Scalar _tmp170 = _tmp109 * _tmp165 - _tmp164 * _tmp40 + _tmp166 * _tmp40; + const Scalar _tmp171 = _tmp114 + _tmp139 + _tmp157; + const Scalar _tmp172 = _tmp171 * _tmp99; + const Scalar _tmp173 = _tmp171 * _tmp97; + const Scalar _tmp174 = _tmp125 - _tmp126 + _tmp150; + const Scalar _tmp175 = _tmp121 + _tmp122 + _tmp156; + const Scalar _tmp176 = _tmp100 * _tmp171 + _tmp101 * _tmp171 + _tmp172 * _tmp34 - + _tmp173 * _tmp32 - _tmp173 * _tmp34 - _tmp173 * _tmp89 + _tmp174 * _tmp82 + + _tmp175 * _tmp75 + _tmp88 * (-_tmp117 + _tmp135 - _tmp153); + const Scalar _tmp177 = _tmp104 * _tmp175 + _tmp105 * _tmp174 + _tmp106 * _tmp172 - + _tmp106 * _tmp173 + _tmp107 * _tmp172 - _tmp107 * _tmp173; + const Scalar _tmp178 = _tmp109 * _tmp175 + _tmp172 * _tmp40 - _tmp173 * _tmp40; // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp38; - _res(1, 0) = _tmp40; - _res(2, 0) = _tmp42; + _res(0, 0) = _tmp37; + _res(1, 0) = _tmp39; + _res(2, 0) = _tmp41; } 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, 0) = _tmp103; + _jacobian(1, 0) = _tmp108; + _jacobian(2, 0) = _tmp110; + _jacobian(0, 1) = _tmp130; + _jacobian(1, 1) = _tmp131; + _jacobian(2, 1) = _tmp132; + _jacobian(0, 2) = _tmp145; + _jacobian(1, 2) = _tmp146; + _jacobian(2, 2) = _tmp147; _jacobian(0, 3) = 0; _jacobian(1, 3) = 0; _jacobian(2, 3) = 0; @@ -332,15 +281,15 @@ void BetweenFactorPose3Rotation(const sym::Pose3& a, const sym::Pose3& a, const sym::Pose3& _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(0, 0) = _tmp103 * _tmp37 + _tmp108 * _tmp39 + _tmp110 * _tmp41; + _rhs(1, 0) = _tmp130 * _tmp37 + _tmp131 * _tmp39 + _tmp132 * _tmp41; + _rhs(2, 0) = _tmp145 * _tmp37 + _tmp146 * _tmp39 + _tmp147 * _tmp41; _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(6, 0) = _tmp160 * _tmp37 + _tmp161 * _tmp39 + _tmp162 * _tmp41; + _rhs(7, 0) = _tmp168 * _tmp37 + _tmp169 * _tmp39 + _tmp170 * _tmp41; + _rhs(8, 0) = _tmp176 * _tmp37 + _tmp177 * _tmp39 + _tmp178 * _tmp41; _rhs(9, 0) = 0; _rhs(10, 0) = 0; _rhs(11, 0) = 0; diff --git a/gen/cpp/sym/factors/between_factor_rot3.h b/gen/cpp/sym/factors/between_factor_rot3.h index 926aabb16..4bd700c6a 100644 --- a/gen/cpp/sym/factors/between_factor_rot3.h +++ b/gen/cpp/sym/factors/between_factor_rot3.h @@ -37,366 +37,312 @@ void BetweenFactorRot3(const sym::Rot3& a, const sym::Rot3& b, Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 754 + // Total ops: 610 // 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) + // Intermediate terms (184) 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 _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] * _tmp9 + _a_T_b[1] * _tmp14 - _a_T_b[2] * _tmp4 + _a_T_b[3] * _tmp19; + 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 = 2 * std::min(0, (((_tmp25) > 0) - ((_tmp25) < 0))) + 1; + const Scalar _tmp27 = 2 * _tmp26; + const Scalar _tmp28 = 1 - epsilon; + const Scalar _tmp29 = std::min(_tmp28, std::fabs(_tmp25)); + const Scalar _tmp30 = std::acos(_tmp29) / std::sqrt(Scalar(1 - std::pow(_tmp29, Scalar(2)))); + const Scalar _tmp31 = _tmp27 * _tmp30; + const Scalar _tmp32 = _tmp20 * _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); + _a_T_b[0] * _tmp19 - _a_T_b[1] * _tmp4 - _a_T_b[2] * _tmp14 + _a_T_b[3] * _tmp9; + const Scalar _tmp34 = _tmp33 * sqrt_info(0, 1); + const Scalar _tmp35 = sqrt_info(0, 0) * (-_a_T_b[0] * _tmp4 - _a_T_b[1] * _tmp19 + + _a_T_b[2] * _tmp9 + _a_T_b[3] * _tmp14); + const Scalar _tmp36 = _tmp31 * _tmp34 + _tmp31 * _tmp35 + _tmp32 * sqrt_info(0, 2); + const Scalar _tmp37 = _tmp27 * sqrt_info(1, 2); + const Scalar _tmp38 = _tmp33 * sqrt_info(1, 1); + const Scalar _tmp39 = _tmp20 * _tmp30 * _tmp37 + _tmp31 * _tmp38; + const Scalar _tmp40 = _tmp32 * sqrt_info(2, 2); + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * _tmp0; + const Scalar _tmp42 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * _tmp2; + const Scalar _tmp44 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp45 = -_tmp41 - _tmp42 - _tmp43 - _tmp44; + const Scalar _tmp46 = _a_T_b[3] * _tmp45; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp48 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp50 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp51 = _tmp47 + _tmp48 - _tmp49 - _tmp50; + const Scalar _tmp52 = _a_T_b[0] * _tmp51; + const Scalar _tmp53 = (Scalar(1) / Scalar(2)) * _tmp5; + const Scalar _tmp54 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp55 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp57 = -_tmp53 + _tmp54 - _tmp55 + _tmp56; + const Scalar _tmp58 = -_a_T_b[1] * _tmp57; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp63 = _tmp59 - _tmp60 - _tmp61 + _tmp62; + const Scalar _tmp64 = _a_T_b[2] * _tmp63; + const Scalar _tmp65 = _tmp58 + _tmp64; + const Scalar _tmp66 = _tmp21 + _tmp22 + _tmp23 + _tmp24; + const Scalar _tmp67 = std::fabs(_tmp66); + const Scalar _tmp68 = std::min(_tmp28, _tmp67); + const Scalar _tmp69 = 1 - std::pow(_tmp68, Scalar(2)); + const Scalar _tmp70 = std::acos(_tmp68); + const Scalar _tmp71 = _tmp70 / std::sqrt(_tmp69); + const Scalar _tmp72 = _tmp27 * _tmp71; + const Scalar _tmp73 = _tmp72 * sqrt_info(0, 0); + const Scalar _tmp74 = _a_T_b[3] * _tmp51; + const Scalar _tmp75 = _a_T_b[0] * _tmp45; + const Scalar _tmp76 = _a_T_b[2] * _tmp57; + const Scalar _tmp77 = _a_T_b[1] * _tmp63; + const Scalar _tmp78 = _tmp76 + _tmp77; + const Scalar _tmp79 = _tmp74 + _tmp75 + _tmp78; + const Scalar _tmp80 = _tmp26 * ((((_tmp28 - _tmp67) > 0) - ((_tmp28 - _tmp67) < 0)) + 1) * + (((_tmp66) > 0) - ((_tmp66) < 0)); + const Scalar _tmp81 = _tmp80 / _tmp69; + const Scalar _tmp82 = _tmp35 * _tmp81; + const Scalar _tmp83 = _tmp34 * _tmp81; + const Scalar _tmp84 = _tmp68 * _tmp70 * _tmp80 / (_tmp69 * std::sqrt(_tmp69)); + const Scalar _tmp85 = _tmp20 * _tmp84; + const Scalar _tmp86 = _tmp85 * sqrt_info(0, 2); + const Scalar _tmp87 = _tmp35 * _tmp84; + const Scalar _tmp88 = -_a_T_b[1] * _tmp51; + const Scalar _tmp89 = _a_T_b[2] * _tmp45; + const Scalar _tmp90 = _a_T_b[0] * _tmp57; + const Scalar _tmp91 = _a_T_b[3] * _tmp63; + const Scalar _tmp92 = _tmp90 + _tmp91; + const Scalar _tmp93 = _tmp88 - _tmp89 + _tmp92; + const Scalar _tmp94 = _tmp72 * sqrt_info(0, 1); + const Scalar _tmp95 = _a_T_b[1] * _tmp45; + const Scalar _tmp96 = _a_T_b[3] * _tmp57; + const Scalar _tmp97 = -_a_T_b[0] * _tmp63; + const Scalar _tmp98 = _a_T_b[2] * _tmp51; + const Scalar _tmp99 = _tmp95 + _tmp96 + _tmp97 - _tmp98; + const Scalar _tmp100 = _tmp72 * _tmp99; + const Scalar _tmp101 = _tmp34 * _tmp84; + const Scalar _tmp102 = _tmp20 * _tmp81; + const Scalar _tmp103 = _tmp102 * sqrt_info(0, 2); + const Scalar _tmp104 = _tmp100 * sqrt_info(0, 2) + _tmp101 * _tmp79 - _tmp103 * _tmp79 + + _tmp73 * (_tmp46 - _tmp52 + _tmp65) - _tmp79 * _tmp82 - _tmp79 * _tmp83 + + _tmp79 * _tmp86 + _tmp79 * _tmp87 + _tmp93 * _tmp94; + const Scalar _tmp105 = _tmp38 * _tmp81; + const Scalar _tmp106 = _tmp20 * sqrt_info(1, 2); + const Scalar _tmp107 = _tmp106 * _tmp84; + const Scalar _tmp108 = _tmp72 * sqrt_info(1, 1); + const Scalar _tmp109 = _tmp37 * _tmp71; + const Scalar _tmp110 = _tmp38 * _tmp84; + const Scalar _tmp111 = _tmp106 * _tmp81; + const Scalar _tmp112 = -_tmp105 * _tmp79 + _tmp107 * _tmp79 + _tmp108 * _tmp93 + + _tmp109 * _tmp99 + _tmp110 * _tmp79 - _tmp111 * _tmp79; + const Scalar _tmp113 = _tmp85 * sqrt_info(2, 2); + const Scalar _tmp114 = _tmp102 * sqrt_info(2, 2); + const Scalar _tmp115 = _tmp100 * sqrt_info(2, 2) + _tmp113 * _tmp79 - _tmp114 * _tmp79; + const Scalar _tmp116 = _tmp53 - _tmp54 + _tmp55 - _tmp56; + const Scalar _tmp117 = _a_T_b[3] * _tmp116; + const Scalar _tmp118 = -_tmp59 + _tmp60 + _tmp61 - _tmp62; + const Scalar _tmp119 = _a_T_b[0] * _tmp118; + const Scalar _tmp120 = _tmp119 + _tmp98; + const Scalar _tmp121 = _tmp117 + _tmp120 + _tmp95; + const Scalar _tmp122 = _tmp121 * _tmp81; + const Scalar _tmp123 = _tmp122 * _tmp20; + const Scalar _tmp124 = _a_T_b[1] * _tmp116; + const Scalar _tmp125 = -_a_T_b[2] * _tmp118; + const Scalar _tmp126 = _tmp72 * (-_tmp124 + _tmp125 + _tmp46 + _tmp52); + const Scalar _tmp127 = -_a_T_b[2] * _tmp116; + const Scalar _tmp128 = _a_T_b[1] * _tmp118; + const Scalar _tmp129 = _tmp128 + _tmp74; + const Scalar _tmp130 = _tmp127 + _tmp129 - _tmp75; + const Scalar _tmp131 = _tmp130 * _tmp72; + const Scalar _tmp132 = _a_T_b[0] * _tmp116; + const Scalar _tmp133 = _a_T_b[3] * _tmp118; + const Scalar _tmp134 = _tmp101 * _tmp121 + _tmp121 * _tmp86 + _tmp121 * _tmp87 - + _tmp122 * _tmp34 - _tmp122 * _tmp35 - _tmp123 * sqrt_info(0, 2) + + _tmp126 * sqrt_info(0, 1) + _tmp131 * sqrt_info(0, 2) + + _tmp73 * (-_tmp132 + _tmp133 + _tmp88 + _tmp89); + const Scalar _tmp135 = -_tmp105 * _tmp121 - _tmp106 * _tmp122 + _tmp107 * _tmp121 + + _tmp109 * _tmp130 + _tmp110 * _tmp121 + _tmp126 * sqrt_info(1, 1); + const Scalar _tmp136 = _tmp113 * _tmp121 - _tmp123 * sqrt_info(2, 2) + _tmp131 * sqrt_info(2, 2); + const Scalar _tmp137 = -_tmp47 - _tmp48 + _tmp49 + _tmp50; + const Scalar _tmp138 = _a_T_b[1] * _tmp137; + const Scalar _tmp139 = _tmp132 + _tmp138; + const Scalar _tmp140 = _tmp139 + _tmp89 + _tmp91; + const Scalar _tmp141 = _a_T_b[2] * _tmp137; + const Scalar _tmp142 = _tmp117 + _tmp141; + const Scalar _tmp143 = _a_T_b[3] * _tmp137; + const Scalar _tmp144 = _tmp127 + _tmp143 + _tmp75 - _tmp77; + const Scalar _tmp145 = _tmp102 * _tmp140; + const Scalar _tmp146 = -_a_T_b[0] * _tmp137; + const Scalar _tmp147 = _tmp124 + _tmp146; + const Scalar _tmp148 = _tmp71 * (_tmp147 + _tmp46 - _tmp64); + const Scalar _tmp149 = _tmp148 * _tmp27; + const Scalar _tmp150 = _tmp101 * _tmp140 - _tmp140 * _tmp82 - _tmp140 * _tmp83 + + _tmp140 * _tmp86 + _tmp140 * _tmp87 + _tmp144 * _tmp94 - + _tmp145 * sqrt_info(0, 2) + _tmp149 * sqrt_info(0, 2) + + _tmp73 * (_tmp142 - _tmp95 + _tmp97); + const Scalar _tmp151 = -_tmp105 * _tmp140 + _tmp107 * _tmp140 + _tmp108 * _tmp144 + + _tmp110 * _tmp140 - _tmp111 * _tmp140 + _tmp148 * _tmp37; + const Scalar _tmp152 = _tmp113 * _tmp140 - _tmp145 * sqrt_info(2, 2) + _tmp149 * sqrt_info(2, 2); + const Scalar _tmp153 = _tmp41 + _tmp42 + _tmp43 + _tmp44; + const Scalar _tmp154 = _a_T_b[0] * _tmp153; + const Scalar _tmp155 = _tmp143 + _tmp154; + const Scalar _tmp156 = _tmp155 + _tmp78; + const Scalar _tmp157 = _a_T_b[3] * _tmp153; + const Scalar _tmp158 = _a_T_b[2] * _tmp153; + const Scalar _tmp159 = -_tmp138 - _tmp158 + _tmp92; + const Scalar _tmp160 = _a_T_b[1] * _tmp153; + const Scalar _tmp161 = _tmp160 + _tmp96; + const Scalar _tmp162 = -_tmp141 + _tmp161 + _tmp97; + const Scalar _tmp163 = _tmp162 * _tmp72; + const Scalar _tmp164 = _tmp101 * _tmp156 - _tmp103 * _tmp156 - _tmp156 * _tmp82 - + _tmp156 * _tmp83 + _tmp156 * _tmp86 + _tmp156 * _tmp87 + _tmp159 * _tmp94 + + _tmp163 * sqrt_info(0, 2) + _tmp73 * (_tmp146 + _tmp157 + _tmp65); + const Scalar _tmp165 = -_tmp105 * _tmp156 + _tmp107 * _tmp156 + _tmp108 * _tmp159 + + _tmp109 * _tmp162 + _tmp110 * _tmp156 - _tmp111 * _tmp156; + const Scalar _tmp166 = _tmp113 * _tmp156 - _tmp114 * _tmp156 + _tmp163 * sqrt_info(2, 2); + const Scalar _tmp167 = _tmp120 + _tmp161; + const Scalar _tmp168 = _tmp125 + _tmp157; + const Scalar _tmp169 = _tmp168 + _tmp52 + _tmp58; + const Scalar _tmp170 = _tmp133 + _tmp158; + const Scalar _tmp171 = _tmp129 - _tmp154 - _tmp76; + const Scalar _tmp172 = _tmp171 * _tmp72; + const Scalar _tmp173 = _tmp101 * _tmp167 - _tmp103 * _tmp167 - _tmp167 * _tmp82 - + _tmp167 * _tmp83 + _tmp167 * _tmp86 + _tmp167 * _tmp87 + _tmp169 * _tmp94 + + _tmp172 * sqrt_info(0, 2) + _tmp73 * (_tmp170 + _tmp88 - _tmp90); + const Scalar _tmp174 = -_tmp105 * _tmp167 + _tmp107 * _tmp167 + _tmp108 * _tmp169 + + _tmp109 * _tmp171 + _tmp110 * _tmp167 - _tmp111 * _tmp167; + const Scalar _tmp175 = _tmp113 * _tmp167 - _tmp114 * _tmp167 + _tmp172 * sqrt_info(2, 2); + const Scalar _tmp176 = _tmp139 + _tmp170; + const Scalar _tmp177 = _tmp147 + _tmp168; + const Scalar _tmp178 = _tmp177 * _tmp72; + const Scalar _tmp179 = _tmp102 * _tmp176; + const Scalar _tmp180 = _tmp127 - _tmp128 + _tmp155; + const Scalar _tmp181 = _tmp101 * _tmp176 - _tmp176 * _tmp82 - _tmp176 * _tmp83 + + _tmp176 * _tmp86 + _tmp176 * _tmp87 + _tmp178 * sqrt_info(0, 2) - + _tmp179 * sqrt_info(0, 2) + _tmp180 * _tmp94 + + _tmp73 * (-_tmp119 + _tmp142 - _tmp160); + const Scalar _tmp182 = -_tmp105 * _tmp176 + _tmp107 * _tmp176 + _tmp108 * _tmp180 + + _tmp109 * _tmp177 + _tmp110 * _tmp176 - _tmp111 * _tmp176; + const Scalar _tmp183 = _tmp113 * _tmp176 + _tmp178 * sqrt_info(2, 2) - _tmp179 * sqrt_info(2, 2); // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp38; - _res(1, 0) = _tmp41; - _res(2, 0) = _tmp43; + _res(0, 0) = _tmp36; + _res(1, 0) = _tmp39; + _res(2, 0) = _tmp40; } 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; + _jacobian(0, 0) = _tmp104; + _jacobian(1, 0) = _tmp112; + _jacobian(2, 0) = _tmp115; + _jacobian(0, 1) = _tmp134; + _jacobian(1, 1) = _tmp135; + _jacobian(2, 1) = _tmp136; + _jacobian(0, 2) = _tmp150; + _jacobian(1, 2) = _tmp151; + _jacobian(2, 2) = _tmp152; + _jacobian(0, 3) = _tmp164; + _jacobian(1, 3) = _tmp165; + _jacobian(2, 3) = _tmp166; + _jacobian(0, 4) = _tmp173; + _jacobian(1, 4) = _tmp174; + _jacobian(2, 4) = _tmp175; + _jacobian(0, 5) = _tmp181; + _jacobian(1, 5) = _tmp182; + _jacobian(2, 5) = _tmp183; } 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; + std::pow(_tmp104, Scalar(2)) + std::pow(_tmp112, Scalar(2)) + std::pow(_tmp115, Scalar(2)); + _hessian(1, 0) = _tmp104 * _tmp134 + _tmp112 * _tmp135 + _tmp115 * _tmp136; + _hessian(2, 0) = _tmp104 * _tmp150 + _tmp112 * _tmp151 + _tmp115 * _tmp152; + _hessian(3, 0) = _tmp104 * _tmp164 + _tmp112 * _tmp165 + _tmp115 * _tmp166; + _hessian(4, 0) = _tmp104 * _tmp173 + _tmp112 * _tmp174 + _tmp115 * _tmp175; + _hessian(5, 0) = _tmp104 * _tmp181 + _tmp112 * _tmp182 + _tmp115 * _tmp183; _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; + std::pow(_tmp134, Scalar(2)) + std::pow(_tmp135, Scalar(2)) + std::pow(_tmp136, Scalar(2)); + _hessian(2, 1) = _tmp134 * _tmp150 + _tmp135 * _tmp151 + _tmp136 * _tmp152; + _hessian(3, 1) = _tmp134 * _tmp164 + _tmp135 * _tmp165 + _tmp136 * _tmp166; + _hessian(4, 1) = _tmp134 * _tmp173 + _tmp135 * _tmp174 + _tmp136 * _tmp175; + _hessian(5, 1) = _tmp134 * _tmp181 + _tmp135 * _tmp182 + _tmp136 * _tmp183; _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; + std::pow(_tmp150, Scalar(2)) + std::pow(_tmp151, Scalar(2)) + std::pow(_tmp152, Scalar(2)); + _hessian(3, 2) = _tmp150 * _tmp164 + _tmp151 * _tmp165 + _tmp152 * _tmp166; + _hessian(4, 2) = _tmp150 * _tmp173 + _tmp151 * _tmp174 + _tmp152 * _tmp175; + _hessian(5, 2) = _tmp150 * _tmp181 + _tmp151 * _tmp182 + _tmp152 * _tmp183; _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; + std::pow(_tmp164, Scalar(2)) + std::pow(_tmp165, Scalar(2)) + std::pow(_tmp166, Scalar(2)); + _hessian(4, 3) = _tmp164 * _tmp173 + _tmp165 * _tmp174 + _tmp166 * _tmp175; + _hessian(5, 3) = _tmp164 * _tmp181 + _tmp165 * _tmp182 + _tmp166 * _tmp183; _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; + std::pow(_tmp173, Scalar(2)) + std::pow(_tmp174, Scalar(2)) + std::pow(_tmp175, Scalar(2)); + _hessian(5, 4) = _tmp173 * _tmp181 + _tmp174 * _tmp182 + _tmp175 * _tmp183; _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)); + std::pow(_tmp181, Scalar(2)) + std::pow(_tmp182, Scalar(2)) + std::pow(_tmp183, 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; + _rhs(0, 0) = _tmp104 * _tmp36 + _tmp112 * _tmp39 + _tmp115 * _tmp40; + _rhs(1, 0) = _tmp134 * _tmp36 + _tmp135 * _tmp39 + _tmp136 * _tmp40; + _rhs(2, 0) = _tmp150 * _tmp36 + _tmp151 * _tmp39 + _tmp152 * _tmp40; + _rhs(3, 0) = _tmp164 * _tmp36 + _tmp165 * _tmp39 + _tmp166 * _tmp40; + _rhs(4, 0) = _tmp173 * _tmp36 + _tmp174 * _tmp39 + _tmp175 * _tmp40; + _rhs(5, 0) = _tmp181 * _tmp36 + _tmp182 * _tmp39 + _tmp183 * _tmp40; } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/prior_factor_matrix31.h b/gen/cpp/sym/factors/prior_factor_matrix31.h index b4ef0ac67..828683bb9 100644 --- a/gen/cpp/sym/factors/prior_factor_matrix31.h +++ b/gen/cpp/sym/factors/prior_factor_matrix31.h @@ -35,39 +35,39 @@ void PriorFactorMatrix31(const Eigen::Matrix& value, Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 63 + // Total ops: 35 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (6) + // Intermediate terms (5) 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); + const Scalar _tmp2 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + + sqrt_info(0, 0) * (-prior(0, 0) + value(0, 0)); + const Scalar _tmp3 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2); + const Scalar _tmp4 = std::pow(sqrt_info(2, 2), Scalar(2)); // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp3; - _res(1, 0) = _tmp4; - _res(2, 0) = _tmp5; + _res(0, 0) = _tmp2; + _res(1, 0) = _tmp3; + _res(2, 0) = _tmp1 * sqrt_info(2, 2); } 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(1, 0) = 0; + _jacobian(2, 0) = 0; _jacobian(0, 1) = sqrt_info(0, 1); _jacobian(1, 1) = sqrt_info(1, 1); - _jacobian(2, 1) = sqrt_info(2, 1); + _jacobian(2, 1) = 0; _jacobian(0, 2) = sqrt_info(0, 2); _jacobian(1, 2) = sqrt_info(1, 2); _jacobian(2, 2) = sqrt_info(2, 2); @@ -76,29 +76,24 @@ void PriorFactorMatrix31(const Eigen::Matrix& value, 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, 0) = std::pow(sqrt_info(0, 0), Scalar(2)); + _hessian(1, 0) = sqrt_info(0, 0) * sqrt_info(0, 1); + _hessian(2, 0) = sqrt_info(0, 0) * sqrt_info(0, 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(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)); + _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 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)); + _hessian(2, 2) = + _tmp4 + std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 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); + _rhs(0, 0) = _tmp2 * sqrt_info(0, 0); + _rhs(1, 0) = _tmp2 * sqrt_info(0, 1) + _tmp3 * sqrt_info(1, 1); + _rhs(2, 0) = _tmp1 * _tmp4 + _tmp2 * sqrt_info(0, 2) + _tmp3 * sqrt_info(1, 2); } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/prior_factor_pose2.h b/gen/cpp/sym/factors/prior_factor_pose2.h index ecf877fc3..e80e95fbe 100644 --- a/gen/cpp/sym/factors/prior_factor_pose2.h +++ b/gen/cpp/sym/factors/prior_factor_pose2.h @@ -36,13 +36,13 @@ void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 94 + // Total ops: 62 // Input arrays const Eigen::Matrix& _value = value.Data(); const Eigen::Matrix& _prior = prior.Data(); - // Intermediate terms (19) + // Intermediate terms (14) const Scalar _tmp0 = -_prior[2] + _value[2]; const Scalar _tmp1 = -_prior[3] + _value[3]; const Scalar _tmp2 = _prior[0] * _value[1]; @@ -50,38 +50,33 @@ void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& 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)); + const Scalar _tmp7 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + + sqrt_info(0, 0) * std::atan2(_tmp4, _tmp6); + const Scalar _tmp8 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2); + const Scalar _tmp9 = std::pow(_tmp6, Scalar(2)); + const Scalar _tmp10 = -_tmp4 * (-_tmp2 + _tmp3) / _tmp9 + _tmp5 / _tmp6; + const Scalar _tmp11 = std::pow(_tmp4, Scalar(2)) + _tmp9; + const Scalar _tmp12 = _tmp10 * _tmp9 * sqrt_info(0, 0) / _tmp11; + const Scalar _tmp13 = std::pow(sqrt_info(2, 2), Scalar(2)); // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp8; - _res(1, 0) = _tmp9; - _res(2, 0) = _tmp10; + _res(0, 0) = _tmp7; + _res(1, 0) = _tmp8; + _res(2, 0) = _tmp1 * sqrt_info(2, 2); } if (jacobian != nullptr) { Eigen::Matrix& _jacobian = (*jacobian); - _jacobian(0, 0) = _tmp15; - _jacobian(1, 0) = _tmp16; - _jacobian(2, 0) = _tmp17; + _jacobian(0, 0) = _tmp12; + _jacobian(1, 0) = 0; + _jacobian(2, 0) = 0; _jacobian(0, 1) = sqrt_info(0, 1); _jacobian(1, 1) = sqrt_info(1, 1); - _jacobian(2, 1) = sqrt_info(2, 1); + _jacobian(2, 1) = 0; _jacobian(0, 2) = sqrt_info(0, 2); _jacobian(1, 2) = sqrt_info(1, 2); _jacobian(2, 2) = sqrt_info(2, 2); @@ -90,28 +85,25 @@ void PriorFactorPose2(const sym::Pose2& value, const sym::Pose2& 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, 0) = std::pow(_tmp10, Scalar(2)) * std::pow(_tmp6, Scalar(4)) * + std::pow(sqrt_info(0, 0), Scalar(2)) / std::pow(_tmp11, Scalar(2)); + _hessian(1, 0) = _tmp12 * sqrt_info(0, 1); + _hessian(2, 0) = _tmp12 * sqrt_info(0, 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(1, 1) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)); + _hessian(2, 1) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 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)); + _hessian(2, 2) = + _tmp13 + std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 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); + _rhs(0, 0) = _tmp12 * _tmp7; + _rhs(1, 0) = _tmp7 * sqrt_info(0, 1) + _tmp8 * sqrt_info(1, 1); + _rhs(2, 0) = _tmp1 * _tmp13 + _tmp7 * sqrt_info(0, 2) + _tmp8 * sqrt_info(1, 2); } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/prior_factor_pose3.h b/gen/cpp/sym/factors/prior_factor_pose3.h index 6bc187c7e..b6720723d 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3.h +++ b/gen/cpp/sym/factors/prior_factor_pose3.h @@ -36,252 +36,175 @@ void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 812 + // Total ops: 427 // 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; + // Intermediate terms (99) + const Scalar _tmp0 = _prior[3] * _value[3]; + const Scalar _tmp1 = _prior[2] * _value[2]; + const Scalar _tmp2 = _prior[1] * _value[1]; + const Scalar _tmp3 = _prior[0] * _value[0]; + const Scalar _tmp4 = _tmp0 + _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp5 = std::fabs(_tmp4); + const Scalar _tmp6 = 1 - epsilon; + const Scalar _tmp7 = std::min(_tmp5, _tmp6); + const Scalar _tmp8 = 1 - std::pow(_tmp7, Scalar(2)); + const Scalar _tmp9 = 2 / std::sqrt(_tmp8); + const Scalar _tmp10 = (((_tmp4) > 0) - ((_tmp4) < 0)); + const Scalar _tmp11 = 2 * std::min(0, _tmp10) + 1; + const Scalar _tmp12 = std::acos(_tmp7); + const Scalar _tmp13 = _tmp11 * _tmp12; + const Scalar _tmp14 = _tmp13 * _tmp9; + const Scalar _tmp15 = _prior[2] * _value[3]; + const Scalar _tmp16 = _prior[1] * _value[0]; + const Scalar _tmp17 = _prior[0] * _value[1]; + const Scalar _tmp18 = _prior[3] * _value[2]; + const Scalar _tmp19 = -_tmp15 + _tmp16 - _tmp17 + _tmp18; + const Scalar _tmp20 = _tmp19 * sqrt_info(0, 2); + const Scalar _tmp21 = -_prior[6] + _value[6]; + const Scalar _tmp22 = -_prior[5] + _value[5]; + const Scalar _tmp23 = -_prior[4] + _value[4]; + 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 = -_tmp24 - _tmp25 + _tmp26 + _tmp27; + const Scalar _tmp29 = _tmp14 * _tmp28; + const Scalar _tmp30 = _prior[2] * _value[1]; + const Scalar _tmp31 = _prior[1] * _value[2]; + const Scalar _tmp32 = _prior[0] * _value[3]; + const Scalar _tmp33 = _prior[3] * _value[0]; + const Scalar _tmp34 = _tmp30 - _tmp31 - _tmp32 + _tmp33; + const Scalar _tmp35 = _tmp11 * sqrt_info(0, 0); + const Scalar _tmp36 = _tmp12 * _tmp35; + const Scalar _tmp37 = _tmp36 * _tmp9; + const Scalar _tmp38 = _tmp14 * _tmp20 + _tmp21 * sqrt_info(0, 5) + _tmp22 * sqrt_info(0, 4) + + _tmp23 * sqrt_info(0, 3) + _tmp29 * sqrt_info(0, 1) + _tmp34 * _tmp37; + const Scalar _tmp39 = _tmp14 * _tmp19; + const Scalar _tmp40 = _tmp21 * sqrt_info(1, 5) + _tmp22 * sqrt_info(1, 4) + + _tmp23 * sqrt_info(1, 3) + _tmp29 * sqrt_info(1, 1) + + _tmp39 * sqrt_info(1, 2); + const Scalar _tmp41 = _tmp21 * sqrt_info(2, 5) + _tmp22 * sqrt_info(2, 4) + + _tmp23 * sqrt_info(2, 3) + _tmp39 * sqrt_info(2, 2); + const Scalar _tmp42 = + _tmp21 * sqrt_info(3, 5) + _tmp22 * sqrt_info(3, 4) + _tmp23 * sqrt_info(3, 3); + const Scalar _tmp43 = _tmp21 * sqrt_info(4, 5) + _tmp22 * sqrt_info(4, 4); 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); + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp48 = -_tmp44 + _tmp45 + _tmp46 - _tmp47; + const Scalar _tmp49 = _tmp34 * _tmp48; + const Scalar _tmp50 = _tmp10 * ((((-_tmp5 + _tmp6) > 0) - ((-_tmp5 + _tmp6) < 0)) + 1); + const Scalar _tmp51 = _tmp50 / _tmp8; + const Scalar _tmp52 = _tmp35 * _tmp51; + const Scalar _tmp53 = _tmp28 * sqrt_info(0, 1); + const Scalar _tmp54 = _tmp50 * _tmp7 / (_tmp8 * std::sqrt(_tmp8)); + const Scalar _tmp55 = _tmp13 * _tmp54; + const Scalar _tmp56 = _tmp53 * _tmp55; + const Scalar _tmp57 = _tmp11 * _tmp51; + const Scalar _tmp58 = _tmp48 * _tmp57; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp16; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp17; + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp63 = _tmp14 * (-_tmp59 + _tmp60 - _tmp61 + _tmp62); + const Scalar _tmp64 = _tmp48 * _tmp55; + const Scalar _tmp65 = _tmp36 * _tmp54; + const Scalar _tmp66 = _tmp9 * ((Scalar(1) / Scalar(2)) * _tmp0 + (Scalar(1) / Scalar(2)) * _tmp1 + + (Scalar(1) / Scalar(2)) * _tmp2 + (Scalar(1) / Scalar(2)) * _tmp3); + const Scalar _tmp67 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp68 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp69 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp70 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp71 = _tmp67 + _tmp68 - _tmp69 - _tmp70; + const Scalar _tmp72 = _tmp14 * _tmp71; + const Scalar _tmp73 = -_tmp20 * _tmp58 + _tmp20 * _tmp64 + _tmp36 * _tmp66 + _tmp48 * _tmp56 - + _tmp49 * _tmp52 + _tmp49 * _tmp65 - _tmp53 * _tmp58 + + _tmp63 * sqrt_info(0, 1) + _tmp72 * sqrt_info(0, 2); + const Scalar _tmp74 = _tmp28 * sqrt_info(1, 1); + const Scalar _tmp75 = _tmp19 * sqrt_info(1, 2); + const Scalar _tmp76 = -_tmp58 * _tmp74 - _tmp58 * _tmp75 + _tmp63 * sqrt_info(1, 1) + + _tmp64 * _tmp74 + _tmp64 * _tmp75 + _tmp72 * sqrt_info(1, 2); + const Scalar _tmp77 = _tmp19 * sqrt_info(2, 2); + const Scalar _tmp78 = _tmp57 * sqrt_info(2, 2); + const Scalar _tmp79 = -_tmp19 * _tmp48 * _tmp78 + _tmp64 * _tmp77 + _tmp72 * sqrt_info(2, 2); + const Scalar _tmp80 = _tmp57 * _tmp71; + const Scalar _tmp81 = _tmp59 - _tmp60 + _tmp61 - _tmp62; + const Scalar _tmp82 = _tmp34 * _tmp65; + const Scalar _tmp83 = _tmp55 * _tmp71; + const Scalar _tmp84 = _tmp34 * _tmp52; + const Scalar _tmp85 = _tmp14 * (_tmp44 - _tmp45 - _tmp46 + _tmp47); + const Scalar _tmp86 = _tmp13 * _tmp66; + const Scalar _tmp87 = -_tmp20 * _tmp80 + _tmp20 * _tmp83 + _tmp37 * _tmp81 - _tmp53 * _tmp80 + + _tmp56 * _tmp71 + _tmp71 * _tmp82 - _tmp71 * _tmp84 + + _tmp85 * sqrt_info(0, 2) + _tmp86 * sqrt_info(0, 1); + const Scalar _tmp88 = -_tmp74 * _tmp80 + _tmp74 * _tmp83 - _tmp75 * _tmp80 + _tmp75 * _tmp83 + + _tmp85 * sqrt_info(1, 2) + _tmp86 * sqrt_info(1, 1); + const Scalar _tmp89 = -_tmp77 * _tmp80 + _tmp77 * _tmp83 + _tmp85 * sqrt_info(2, 2); + const Scalar _tmp90 = _tmp57 * _tmp81; + const Scalar _tmp91 = _tmp55 * _tmp81; + const Scalar _tmp92 = _tmp14 * _tmp48; + const Scalar _tmp93 = -_tmp20 * _tmp90 + _tmp20 * _tmp91 + + _tmp37 * (-_tmp67 - _tmp68 + _tmp69 + _tmp70) - _tmp53 * _tmp90 + + _tmp56 * _tmp81 + _tmp81 * _tmp82 - _tmp81 * _tmp84 + + _tmp86 * sqrt_info(0, 2) + _tmp92 * sqrt_info(0, 1); + const Scalar _tmp94 = _tmp19 * _tmp81; + const Scalar _tmp95 = _tmp94 * sqrt_info(1, 2); + const Scalar _tmp96 = _tmp55 * _tmp95 - _tmp57 * _tmp95 - _tmp74 * _tmp90 + _tmp74 * _tmp91 + + _tmp86 * sqrt_info(1, 2) + _tmp92 * sqrt_info(1, 1); + const Scalar _tmp97 = + _tmp55 * _tmp94 * sqrt_info(2, 2) - _tmp78 * _tmp94 + _tmp86 * sqrt_info(2, 2); + const Scalar _tmp98 = std::pow(sqrt_info(5, 5), Scalar(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; + _res(0, 0) = _tmp38; + _res(1, 0) = _tmp40; + _res(2, 0) = _tmp41; + _res(3, 0) = _tmp42; + _res(4, 0) = _tmp43; + _res(5, 0) = _tmp21 * sqrt_info(5, 5); } 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, 0) = _tmp73; + _jacobian(1, 0) = _tmp76; + _jacobian(2, 0) = _tmp79; + _jacobian(3, 0) = 0; + _jacobian(4, 0) = 0; + _jacobian(5, 0) = 0; + _jacobian(0, 1) = _tmp87; + _jacobian(1, 1) = _tmp88; + _jacobian(2, 1) = _tmp89; + _jacobian(3, 1) = 0; + _jacobian(4, 1) = 0; + _jacobian(5, 1) = 0; + _jacobian(0, 2) = _tmp93; + _jacobian(1, 2) = _tmp96; + _jacobian(2, 2) = _tmp97; + _jacobian(3, 2) = 0; + _jacobian(4, 2) = 0; + _jacobian(5, 2) = 0; _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(4, 3) = 0; + _jacobian(5, 3) = 0; _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(5, 4) = 0; _jacobian(0, 5) = sqrt_info(0, 5); _jacobian(1, 5) = sqrt_info(1, 5); _jacobian(2, 5) = sqrt_info(2, 5); @@ -293,98 +216,68 @@ void PriorFactorPose3(const sym::Pose3& value, const sym::Pose3& 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, 0) = + std::pow(_tmp73, Scalar(2)) + std::pow(_tmp76, Scalar(2)) + std::pow(_tmp79, Scalar(2)); + _hessian(1, 0) = _tmp73 * _tmp87 + _tmp76 * _tmp88 + _tmp79 * _tmp89; + _hessian(2, 0) = _tmp73 * _tmp93 + _tmp76 * _tmp96 + _tmp79 * _tmp97; + _hessian(3, 0) = _tmp73 * sqrt_info(0, 3) + _tmp76 * sqrt_info(1, 3) + _tmp79 * sqrt_info(2, 3); + _hessian(4, 0) = _tmp73 * sqrt_info(0, 4) + _tmp76 * sqrt_info(1, 4) + _tmp79 * sqrt_info(2, 4); + _hessian(5, 0) = _tmp73 * sqrt_info(0, 5) + _tmp76 * sqrt_info(1, 5) + _tmp79 * sqrt_info(2, 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(1, 1) = + std::pow(_tmp87, Scalar(2)) + std::pow(_tmp88, Scalar(2)) + std::pow(_tmp89, Scalar(2)); + _hessian(2, 1) = _tmp87 * _tmp93 + _tmp88 * _tmp96 + _tmp89 * _tmp97; + _hessian(3, 1) = _tmp87 * sqrt_info(0, 3) + _tmp88 * sqrt_info(1, 3) + _tmp89 * sqrt_info(2, 3); + _hessian(4, 1) = _tmp87 * sqrt_info(0, 4) + _tmp88 * sqrt_info(1, 4) + _tmp89 * sqrt_info(2, 4); + _hessian(5, 1) = _tmp87 * sqrt_info(0, 5) + _tmp88 * sqrt_info(1, 5) + _tmp89 * sqrt_info(2, 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(2, 2) = + std::pow(_tmp93, Scalar(2)) + std::pow(_tmp96, Scalar(2)) + std::pow(_tmp97, Scalar(2)); + _hessian(3, 2) = _tmp93 * sqrt_info(0, 3) + _tmp96 * sqrt_info(1, 3) + _tmp97 * sqrt_info(2, 3); + _hessian(4, 2) = _tmp93 * sqrt_info(0, 4) + _tmp96 * sqrt_info(1, 4) + _tmp97 * sqrt_info(2, 4); + _hessian(5, 2) = _tmp93 * sqrt_info(0, 5) + _tmp96 * sqrt_info(1, 5) + _tmp97 * sqrt_info(2, 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)); + std::pow(sqrt_info(2, 3), Scalar(2)) + std::pow(sqrt_info(3, 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); + sqrt_info(2, 3) * sqrt_info(2, 4) + sqrt_info(3, 3) * sqrt_info(3, 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); + sqrt_info(2, 3) * sqrt_info(2, 5) + sqrt_info(3, 3) * sqrt_info(3, 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)); + std::pow(sqrt_info(4, 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); + sqrt_info(4, 4) * sqrt_info(4, 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)); + _hessian(5, 5) = _tmp98 + 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)); } 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); + _rhs(0, 0) = _tmp38 * _tmp73 + _tmp40 * _tmp76 + _tmp41 * _tmp79; + _rhs(1, 0) = _tmp38 * _tmp87 + _tmp40 * _tmp88 + _tmp41 * _tmp89; + _rhs(2, 0) = _tmp38 * _tmp93 + _tmp40 * _tmp96 + _tmp41 * _tmp97; + _rhs(3, 0) = _tmp38 * sqrt_info(0, 3) + _tmp40 * sqrt_info(1, 3) + _tmp41 * sqrt_info(2, 3) + + _tmp42 * sqrt_info(3, 3); + _rhs(4, 0) = _tmp38 * sqrt_info(0, 4) + _tmp40 * sqrt_info(1, 4) + _tmp41 * sqrt_info(2, 4) + + _tmp42 * sqrt_info(3, 4) + _tmp43 * sqrt_info(4, 4); + _rhs(5, 0) = _tmp21 * _tmp98 + _tmp38 * sqrt_info(0, 5) + _tmp40 * sqrt_info(1, 5) + + _tmp41 * sqrt_info(2, 5) + _tmp42 * sqrt_info(3, 5) + _tmp43 * sqrt_info(4, 5); } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/prior_factor_pose3_position.h b/gen/cpp/sym/factors/prior_factor_pose3_position.h index 37e160c69..52cb2ba7c 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3_position.h +++ b/gen/cpp/sym/factors/prior_factor_pose3_position.h @@ -37,7 +37,7 @@ void PriorFactorPose3Position(const sym::Pose3& value, Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 63 + // Total ops: 35 // Unused inputs (void)epsilon; @@ -45,41 +45,31 @@ void PriorFactorPose3Position(const sym::Pose3& value, // Input arrays const Eigen::Matrix& _value = value.Data(); - // Intermediate terms (6) + // Intermediate terms (5) 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); + const Scalar _tmp1 = _value[6] - prior(2, 0); + const Scalar _tmp2 = _tmp0 * sqrt_info(0, 1) + _tmp1 * sqrt_info(0, 2) + + sqrt_info(0, 0) * (_value[4] - prior(0, 0)); + const Scalar _tmp3 = _tmp0 * sqrt_info(1, 1) + _tmp1 * sqrt_info(1, 2); + const Scalar _tmp4 = std::pow(sqrt_info(2, 2), Scalar(2)); // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp3; - _res(1, 0) = _tmp4; - _res(2, 0) = _tmp5; + _res(0, 0) = _tmp2; + _res(1, 0) = _tmp3; + _res(2, 0) = _tmp1 * sqrt_info(2, 2); } 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.setZero(); + _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); @@ -90,18 +80,13 @@ void PriorFactorPose3Position(const sym::Pose3& value, _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)); + _hessian(3, 3) = std::pow(sqrt_info(0, 0), Scalar(2)); + _hessian(4, 3) = sqrt_info(0, 0) * sqrt_info(0, 1); + _hessian(5, 3) = sqrt_info(0, 0) * sqrt_info(0, 2); + _hessian(4, 4) = std::pow(sqrt_info(0, 1), Scalar(2)) + std::pow(sqrt_info(1, 1), Scalar(2)); + _hessian(5, 4) = sqrt_info(0, 1) * sqrt_info(0, 2) + sqrt_info(1, 1) * sqrt_info(1, 2); + _hessian(5, 5) = + _tmp4 + std::pow(sqrt_info(0, 2), Scalar(2)) + std::pow(sqrt_info(1, 2), Scalar(2)); } if (rhs != nullptr) { @@ -110,9 +95,9 @@ void PriorFactorPose3Position(const sym::Pose3& value, _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); + _rhs(3, 0) = _tmp2 * sqrt_info(0, 0); + _rhs(4, 0) = _tmp2 * sqrt_info(0, 1) + _tmp3 * sqrt_info(1, 1); + _rhs(5, 0) = _tmp1 * _tmp4 + _tmp2 * sqrt_info(0, 2) + _tmp3 * sqrt_info(1, 2); } } // NOLINT(readability/fn_size) diff --git a/gen/cpp/sym/factors/prior_factor_pose3_rotation.h b/gen/cpp/sym/factors/prior_factor_pose3_rotation.h index 99daced14..6c2c71cc7 100644 --- a/gen/cpp/sym/factors/prior_factor_pose3_rotation.h +++ b/gen/cpp/sym/factors/prior_factor_pose3_rotation.h @@ -37,168 +37,145 @@ void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 362 + // Total ops: 288 // 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; + // Intermediate terms (98) + 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 = _tmp7 * sqrt_info(0, 1); + const Scalar _tmp9 = _prior[3] * _value[1]; + const Scalar _tmp10 = _prior[0] * _value[2]; + const Scalar _tmp11 = _prior[2] * _value[0]; + const Scalar _tmp12 = _prior[1] * _value[3]; + const Scalar _tmp13 = _tmp10 - _tmp11 - _tmp12 + _tmp9; + const Scalar _tmp14 = 1 - epsilon; + const Scalar _tmp15 = std::min(_tmp14, std::fabs(_tmp5)); + const Scalar _tmp16 = std::acos(_tmp15) / std::sqrt(Scalar(1 - std::pow(_tmp15, Scalar(2)))); + const Scalar _tmp17 = _tmp13 * _tmp16; 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 _tmp23 = _tmp16 * _tmp22; + const Scalar _tmp24 = _tmp23 * _tmp7; 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); + const Scalar _tmp29 = sqrt_info(0, 0) * (_tmp25 - _tmp26 + _tmp27 - _tmp28); + const Scalar _tmp30 = _tmp16 * _tmp29 * _tmp7 + _tmp17 * _tmp8 + _tmp24 * sqrt_info(0, 2); + const Scalar _tmp31 = _tmp7 * sqrt_info(1, 1); + const Scalar _tmp32 = _tmp17 * _tmp31 + _tmp24 * sqrt_info(1, 2); + const Scalar _tmp33 = _tmp7 * sqrt_info(2, 2); + const Scalar _tmp34 = _tmp23 * _tmp33; + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp39 = _tmp35 - _tmp36 + _tmp37 - _tmp38; + const Scalar _tmp40 = std::fabs(_tmp0 + _tmp1 + _tmp2 + _tmp3); + const Scalar _tmp41 = std::min(_tmp14, _tmp40); + const Scalar _tmp42 = 1 - std::pow(_tmp41, Scalar(2)); + const Scalar _tmp43 = _tmp6 * ((((_tmp14 - _tmp40) > 0) - ((_tmp14 - _tmp40) < 0)) + 1) * + (((-_tmp0 + _tmp4) > 0) - ((-_tmp0 + _tmp4) < 0)); + const Scalar _tmp44 = _tmp43 / _tmp42; + const Scalar _tmp45 = _tmp13 * _tmp44; + const Scalar _tmp46 = _tmp45 * sqrt_info(0, 1); + const Scalar _tmp47 = _tmp29 * _tmp44; + const Scalar _tmp48 = _tmp39 * _tmp44; + const Scalar _tmp49 = _tmp22 * sqrt_info(0, 2); + const Scalar _tmp50 = std::acos(_tmp41); + const Scalar _tmp51 = _tmp41 * _tmp43 / (_tmp42 * std::sqrt(_tmp42)); + const Scalar _tmp52 = _tmp50 * _tmp51; + const Scalar _tmp53 = _tmp39 * _tmp52; + const Scalar _tmp54 = _tmp13 * sqrt_info(0, 1); + const Scalar _tmp55 = std::pow(_tmp42, Scalar(Scalar(-1) / Scalar(2))); + 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 = _tmp50 * _tmp60; + const Scalar _tmp62 = _tmp55 * _tmp61; + const Scalar _tmp63 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp66 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp67 = -_tmp63 - _tmp64 + _tmp65 + _tmp66; + const Scalar _tmp68 = _tmp50 * _tmp55; + const Scalar _tmp69 = _tmp68 * _tmp7; + const Scalar _tmp70 = _tmp67 * _tmp69; + const Scalar _tmp71 = (Scalar(1) / Scalar(2)) * _tmp0 + (Scalar(1) / Scalar(2)) * _tmp1 + + (Scalar(1) / Scalar(2)) * _tmp2 + (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp72 = _tmp69 * sqrt_info(0, 0); + const Scalar _tmp73 = _tmp29 * _tmp53 - _tmp39 * _tmp46 - _tmp39 * _tmp47 - _tmp48 * _tmp49 + + _tmp49 * _tmp53 + _tmp53 * _tmp54 + _tmp62 * _tmp8 + + _tmp70 * sqrt_info(0, 2) + _tmp71 * _tmp72; + const Scalar _tmp74 = _tmp45 * sqrt_info(1, 1); + const Scalar _tmp75 = _tmp22 * sqrt_info(1, 2); + const Scalar _tmp76 = _tmp13 * sqrt_info(1, 1); + const Scalar _tmp77 = _tmp31 * _tmp62 - _tmp39 * _tmp74 - _tmp48 * _tmp75 + _tmp53 * _tmp75 + + _tmp53 * _tmp76 + _tmp70 * sqrt_info(1, 2); + const Scalar _tmp78 = _tmp22 * sqrt_info(2, 2); + const Scalar _tmp79 = _tmp33 * _tmp68; + const Scalar _tmp80 = -_tmp48 * _tmp78 + _tmp53 * _tmp78 + _tmp67 * _tmp79; + const Scalar _tmp81 = _tmp63 + _tmp64 - _tmp65 - _tmp66; + const Scalar _tmp82 = _tmp44 * _tmp81; + const Scalar _tmp83 = _tmp13 * _tmp82; + const Scalar _tmp84 = _tmp52 * _tmp81; + const Scalar _tmp85 = _tmp39 * _tmp69; + const Scalar _tmp86 = _tmp69 * sqrt_info(0, 1); + const Scalar _tmp87 = -_tmp29 * _tmp82 + _tmp29 * _tmp84 - _tmp49 * _tmp82 + _tmp49 * _tmp84 + + _tmp54 * _tmp84 + _tmp71 * _tmp86 + + _tmp72 * (_tmp56 - _tmp57 - _tmp58 + _tmp59) - _tmp83 * sqrt_info(0, 1) + + _tmp85 * sqrt_info(0, 2); + const Scalar _tmp88 = _tmp31 * _tmp68; + const Scalar _tmp89 = _tmp71 * _tmp88 - _tmp75 * _tmp82 + _tmp75 * _tmp84 + _tmp76 * _tmp84 - + _tmp83 * sqrt_info(1, 1) + _tmp85 * sqrt_info(1, 2); + const Scalar _tmp90 = _tmp39 * _tmp79 - _tmp78 * _tmp82 + _tmp78 * _tmp84; + const Scalar _tmp91 = _tmp51 * _tmp61; + const Scalar _tmp92 = _tmp44 * _tmp60; + const Scalar _tmp93 = -_tmp35 + _tmp36 - _tmp37 + _tmp38; + const Scalar _tmp94 = _tmp69 * _tmp71; + const Scalar _tmp95 = _tmp29 * _tmp91 - _tmp46 * _tmp60 - _tmp47 * _tmp60 + _tmp49 * _tmp91 - + _tmp49 * _tmp92 + _tmp54 * _tmp91 + _tmp72 * _tmp81 + _tmp86 * _tmp93 + + _tmp94 * sqrt_info(0, 2); + const Scalar _tmp96 = -_tmp60 * _tmp74 + _tmp75 * _tmp91 - _tmp75 * _tmp92 + _tmp76 * _tmp91 + + _tmp88 * _tmp93 + _tmp94 * sqrt_info(1, 2); + const Scalar _tmp97 = _tmp71 * _tmp79 + _tmp78 * _tmp91 - _tmp78 * _tmp92; // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); - _res(0, 0) = _tmp32; - _res(1, 0) = _tmp34; - _res(2, 0) = _tmp37; + _res(0, 0) = _tmp30; + _res(1, 0) = _tmp32; + _res(2, 0) = _tmp34; } 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, 0) = _tmp73; + _jacobian(1, 0) = _tmp77; + _jacobian(2, 0) = _tmp80; + _jacobian(0, 1) = _tmp87; + _jacobian(1, 1) = _tmp89; + _jacobian(2, 1) = _tmp90; + _jacobian(0, 2) = _tmp95; + _jacobian(1, 2) = _tmp96; + _jacobian(2, 2) = _tmp97; _jacobian(0, 3) = 0; _jacobian(1, 3) = 0; _jacobian(2, 3) = 0; @@ -216,22 +193,22 @@ void PriorFactorPose3Rotation(const sym::Pose3& value, const sym::Rot3& _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(0, 0) = _tmp30 * _tmp73 + _tmp32 * _tmp77 + _tmp34 * _tmp80; + _rhs(1, 0) = _tmp30 * _tmp87 + _tmp32 * _tmp89 + _tmp34 * _tmp90; + _rhs(2, 0) = _tmp30 * _tmp95 + _tmp32 * _tmp96 + _tmp34 * _tmp97; _rhs(3, 0) = 0; _rhs(4, 0) = 0; _rhs(5, 0) = 0; diff --git a/gen/cpp/sym/factors/prior_factor_rot3.h b/gen/cpp/sym/factors/prior_factor_rot3.h index 5a96ea560..beb4a89bf 100644 --- a/gen/cpp/sym/factors/prior_factor_rot3.h +++ b/gen/cpp/sym/factors/prior_factor_rot3.h @@ -36,153 +36,125 @@ void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& pr Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 361 + // Total ops: 287 // 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; + // Intermediate terms (94) + const Scalar _tmp0 = _prior[3] * _value[3]; + const Scalar _tmp1 = _prior[0] * _value[0]; + const Scalar _tmp2 = _prior[2] * _value[2]; + const Scalar _tmp3 = _prior[1] * _value[1]; + 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 = _tmp7 * sqrt_info(0, 1); + const Scalar _tmp9 = _prior[3] * _value[1]; + const Scalar _tmp10 = _prior[0] * _value[2]; + const Scalar _tmp11 = _prior[2] * _value[0]; + const Scalar _tmp12 = _prior[1] * _value[3]; + const Scalar _tmp13 = _tmp10 - _tmp11 - _tmp12 + _tmp9; + const Scalar _tmp14 = 1 - epsilon; + const Scalar _tmp15 = std::min(_tmp14, std::fabs(_tmp5)); + const Scalar _tmp16 = std::acos(_tmp15) / std::sqrt(Scalar(1 - std::pow(_tmp15, Scalar(2)))); + const Scalar _tmp17 = _tmp13 * _tmp16; 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 _tmp22 = sqrt_info(0, 0) * (_tmp18 - _tmp19 + _tmp20 - _tmp21); + const Scalar _tmp23 = _tmp16 * _tmp7; 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); + const Scalar _tmp29 = _tmp23 * _tmp28; + const Scalar _tmp30 = _tmp17 * _tmp8 + _tmp22 * _tmp23 + _tmp29 * sqrt_info(0, 2); + const Scalar _tmp31 = _tmp7 * sqrt_info(1, 1); + const Scalar _tmp32 = _tmp17 * _tmp31 + _tmp29 * sqrt_info(1, 2); + const Scalar _tmp33 = _tmp29 * sqrt_info(2, 2); + const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp37 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp38 = _tmp34 - _tmp35 + _tmp36 - _tmp37; + const Scalar _tmp39 = std::fabs(_tmp0 + _tmp1 + _tmp2 + _tmp3); + const Scalar _tmp40 = std::min(_tmp14, _tmp39); + const Scalar _tmp41 = std::acos(_tmp40); + const Scalar _tmp42 = 1 - std::pow(_tmp40, Scalar(2)); + const Scalar _tmp43 = _tmp6 * ((((_tmp14 - _tmp39) > 0) - ((_tmp14 - _tmp39) < 0)) + 1) * + (((-_tmp0 + _tmp4) > 0) - ((-_tmp0 + _tmp4) < 0)); + const Scalar _tmp44 = _tmp40 * _tmp41 * _tmp43 / (_tmp42 * std::sqrt(_tmp42)); + const Scalar _tmp45 = _tmp38 * _tmp44; + const Scalar _tmp46 = _tmp28 * sqrt_info(0, 2); + 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 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp51 = _tmp47 - _tmp48 - _tmp49 + _tmp50; + const Scalar _tmp52 = _tmp41 / std::sqrt(_tmp42); + const Scalar _tmp53 = _tmp51 * _tmp52; + const Scalar _tmp54 = _tmp43 / _tmp42; + const Scalar _tmp55 = _tmp22 * _tmp54; + const Scalar _tmp56 = _tmp13 * _tmp54; + const Scalar _tmp57 = _tmp38 * _tmp56; + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp60 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp62 = _tmp52 * _tmp7; + const Scalar _tmp63 = _tmp62 * (-_tmp58 - _tmp59 + _tmp60 + _tmp61); + const Scalar _tmp64 = (Scalar(1) / Scalar(2)) * _tmp0 + (Scalar(1) / Scalar(2)) * _tmp1 + + (Scalar(1) / Scalar(2)) * _tmp2 + (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp65 = _tmp62 * sqrt_info(0, 0); + const Scalar _tmp66 = _tmp13 * sqrt_info(0, 1); + const Scalar _tmp67 = _tmp28 * _tmp54; + const Scalar _tmp68 = _tmp38 * _tmp67; + const Scalar _tmp69 = _tmp22 * _tmp45 - _tmp38 * _tmp55 + _tmp45 * _tmp46 + _tmp45 * _tmp66 + + _tmp53 * _tmp8 - _tmp57 * sqrt_info(0, 1) + _tmp63 * sqrt_info(0, 2) + + _tmp64 * _tmp65 - _tmp68 * sqrt_info(0, 2); + const Scalar _tmp70 = _tmp28 * _tmp45; + const Scalar _tmp71 = _tmp13 * sqrt_info(1, 1); + const Scalar _tmp72 = _tmp31 * _tmp53 + _tmp45 * _tmp71 - _tmp57 * sqrt_info(1, 1) + + _tmp63 * sqrt_info(1, 2) - _tmp68 * sqrt_info(1, 2) + + _tmp70 * sqrt_info(1, 2); + const Scalar _tmp73 = + _tmp63 * sqrt_info(2, 2) - _tmp68 * sqrt_info(2, 2) + _tmp70 * sqrt_info(2, 2); + const Scalar _tmp74 = _tmp58 + _tmp59 - _tmp60 - _tmp61; + const Scalar _tmp75 = _tmp44 * _tmp74; + const Scalar _tmp76 = _tmp56 * _tmp74; + const Scalar _tmp77 = _tmp38 * _tmp62; + const Scalar _tmp78 = _tmp52 * _tmp64; + const Scalar _tmp79 = _tmp67 * sqrt_info(0, 2); + const Scalar _tmp80 = _tmp22 * _tmp75 + _tmp46 * _tmp75 - _tmp55 * _tmp74 + + _tmp65 * (-_tmp47 + _tmp48 + _tmp49 - _tmp50) + _tmp66 * _tmp75 - + _tmp74 * _tmp79 - _tmp76 * sqrt_info(0, 1) + _tmp77 * sqrt_info(0, 2) + + _tmp78 * _tmp8; + const Scalar _tmp81 = _tmp28 * _tmp75; + const Scalar _tmp82 = _tmp67 * _tmp74; + const Scalar _tmp83 = _tmp31 * _tmp78 + _tmp71 * _tmp75 - _tmp76 * sqrt_info(1, 1) + + _tmp77 * sqrt_info(1, 2) + _tmp81 * sqrt_info(1, 2) - + _tmp82 * sqrt_info(1, 2); + const Scalar _tmp84 = + _tmp77 * sqrt_info(2, 2) + _tmp81 * sqrt_info(2, 2) - _tmp82 * sqrt_info(2, 2); + const Scalar _tmp85 = _tmp44 * _tmp51; + const Scalar _tmp86 = _tmp62 * _tmp64; + const Scalar _tmp87 = _tmp28 * _tmp85; + const Scalar _tmp88 = _tmp52 * (-_tmp34 + _tmp35 - _tmp36 + _tmp37); + const Scalar _tmp89 = _tmp51 * _tmp56; + const Scalar _tmp90 = _tmp22 * _tmp85 - _tmp51 * _tmp55 - _tmp51 * _tmp79 + _tmp65 * _tmp74 + + _tmp66 * _tmp85 + _tmp8 * _tmp88 + _tmp86 * sqrt_info(0, 2) + + _tmp87 * sqrt_info(0, 2) - _tmp89 * sqrt_info(0, 1); + const Scalar _tmp91 = _tmp51 * _tmp67; + const Scalar _tmp92 = _tmp31 * _tmp88 + _tmp71 * _tmp85 + _tmp86 * sqrt_info(1, 2) + + _tmp87 * sqrt_info(1, 2) - _tmp89 * sqrt_info(1, 1) - + _tmp91 * sqrt_info(1, 2); + const Scalar _tmp93 = + _tmp86 * sqrt_info(2, 2) + _tmp87 * sqrt_info(2, 2) - _tmp91 * sqrt_info(2, 2); // Output terms (4) if (res != nullptr) { @@ -190,46 +162,46 @@ void PriorFactorRot3(const sym::Rot3& value, const sym::Rot3& pr _res(0, 0) = _tmp30; _res(1, 0) = _tmp32; - _res(2, 0) = _tmp34; + _res(2, 0) = _tmp33; } 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; + _jacobian(0, 0) = _tmp69; + _jacobian(1, 0) = _tmp72; + _jacobian(2, 0) = _tmp73; + _jacobian(0, 1) = _tmp80; + _jacobian(1, 1) = _tmp83; + _jacobian(2, 1) = _tmp84; + _jacobian(0, 2) = _tmp90; + _jacobian(1, 2) = _tmp92; + _jacobian(2, 2) = _tmp93; } 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; + std::pow(_tmp69, Scalar(2)) + std::pow(_tmp72, Scalar(2)) + std::pow(_tmp73, Scalar(2)); + _hessian(1, 0) = _tmp69 * _tmp80 + _tmp72 * _tmp83 + _tmp73 * _tmp84; + _hessian(2, 0) = _tmp69 * _tmp90 + _tmp72 * _tmp92 + _tmp73 * _tmp93; _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; + std::pow(_tmp80, Scalar(2)) + std::pow(_tmp83, Scalar(2)) + std::pow(_tmp84, Scalar(2)); + _hessian(2, 1) = _tmp80 * _tmp90 + _tmp83 * _tmp92 + _tmp84 * _tmp93; _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)); + std::pow(_tmp90, Scalar(2)) + std::pow(_tmp92, Scalar(2)) + std::pow(_tmp93, 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; + _rhs(0, 0) = _tmp30 * _tmp69 + _tmp32 * _tmp72 + _tmp33 * _tmp73; + _rhs(1, 0) = _tmp30 * _tmp80 + _tmp32 * _tmp83 + _tmp33 * _tmp84; + _rhs(2, 0) = _tmp30 * _tmp90 + _tmp32 * _tmp92 + _tmp33 * _tmp93; } } // NOLINT(readability/fn_size) diff --git a/symforce/codegen/geo_factors_codegen.py b/symforce/codegen/geo_factors_codegen.py index 9041306ca..d6227765b 100644 --- a/symforce/codegen/geo_factors_codegen.py +++ b/symforce/codegen/geo_factors_codegen.py @@ -64,7 +64,7 @@ def between_factor( ) # Apply noise model - residual = sqrt_info * sf.M(tangent_error) + residual = sqrt_info.upper_triangular_mul(sf.M(tangent_error)) return residual @@ -79,7 +79,7 @@ def prior_factor( tangent_error = ops.LieGroupOps.local_coordinates(prior, value, epsilon=epsilon) # Apply noise model - residual = sqrt_info * sf.M(tangent_error) + residual = sqrt_info.upper_triangular_mul(sf.M(tangent_error)) return residual @@ -158,7 +158,7 @@ def between_factor_pose3_rotation( a_R_b, ops.LieGroupOps.between(a, b).R, epsilon=epsilon ) - return sqrt_info * sf.M(tangent_error) + return sqrt_info.upper_triangular_mul(sf.M(tangent_error)) def between_factor_pose3_position( a: sf.Pose3, @@ -175,7 +175,7 @@ def between_factor_pose3_position( a_t_b, ops.LieGroupOps.between(a, b).t, epsilon=epsilon ) - return sqrt_info * sf.M(tangent_error) + return sqrt_info.upper_triangular_mul(sf.M(tangent_error)) def between_factor_pose3_translation_norm( a: sf.Pose3, diff --git a/symforce/examples/bundle_adjustment_fixed_size/gen/cpp/symforce/bundle_adjustment_fixed_size/linearization.h b/symforce/examples/bundle_adjustment_fixed_size/gen/cpp/symforce/bundle_adjustment_fixed_size/linearization.h index 74bf3c508..4ae98dfa8 100644 --- a/symforce/examples/bundle_adjustment_fixed_size/gen/cpp/symforce/bundle_adjustment_fixed_size/linearization.h +++ b/symforce/examples/bundle_adjustment_fixed_size/gen/cpp/symforce/bundle_adjustment_fixed_size/linearization.h @@ -146,7 +146,7 @@ void Linearization( Eigen::SparseMatrix* const jacobian = nullptr, Eigen::SparseMatrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { - // Total ops: 10350 + // Total ops: 9678 // Unused inputs (void)priors_0_0_target_T_src; @@ -160,2836 +160,2644 @@ void Linearization( const Eigen::Matrix& _priors_0_1_target_T_src = priors_0_1_target_T_src.Data(); const Eigen::Matrix& _priors_1_0_target_T_src = priors_1_0_target_T_src.Data(); - // Intermediate terms (2278) - const Scalar _tmp0 = 2 * _views_0_pose[3]; - const Scalar _tmp1 = _tmp0 * _views_0_pose[2]; - const Scalar _tmp2 = 2 * _views_0_pose[0]; - const Scalar _tmp3 = _tmp2 * _views_0_pose[1]; - const Scalar _tmp4 = -_tmp1 + _tmp3; - const Scalar _tmp5 = -2 * std::pow(_views_0_pose[0], Scalar(2)); - const Scalar _tmp6 = 1 - 2 * std::pow(_views_0_pose[2], Scalar(2)); - const Scalar _tmp7 = _tmp5 + _tmp6; - const Scalar _tmp8 = 2 * _views_0_pose[1] * _views_0_pose[2]; - const Scalar _tmp9 = _tmp2 * _views_0_pose[3]; - const Scalar _tmp10 = _tmp8 + _tmp9; + // Intermediate terms (2240) + const Scalar _tmp0 = 2 * _views_0_pose[2]; + const Scalar _tmp1 = _tmp0 * _views_0_pose[3]; + const Scalar _tmp2 = 2 * _views_0_pose[0] * _views_0_pose[1]; + const Scalar _tmp3 = -_tmp1 + _tmp2; + const Scalar _tmp4 = -2 * std::pow(_views_0_pose[0], Scalar(2)); + const Scalar _tmp5 = 1 - 2 * std::pow(_views_0_pose[2], Scalar(2)); + const Scalar _tmp6 = _tmp4 + _tmp5; + const Scalar _tmp7 = _tmp0 * _views_0_pose[1]; + const Scalar _tmp8 = 2 * _views_0_pose[3]; + const Scalar _tmp9 = _tmp8 * _views_0_pose[0]; + const Scalar _tmp10 = _tmp7 + _tmp9; const Scalar _tmp11 = -_priors_0_1_target_T_src[5] - _tmp10 * _views_0_pose[6] + - _tmp10 * _views_1_pose[6] - _tmp4 * _views_0_pose[4] + - _tmp4 * _views_1_pose[4] - _tmp7 * _views_0_pose[5] + - _tmp7 * _views_1_pose[5]; + _tmp10 * _views_1_pose[6] - _tmp3 * _views_0_pose[4] + + _tmp3 * _views_1_pose[4] - _tmp6 * _views_0_pose[5] + + _tmp6 * _views_1_pose[5]; const Scalar _tmp12 = _views_0_pose[0] * _views_1_pose[0]; const Scalar _tmp13 = _views_0_pose[2] * _views_1_pose[2]; const Scalar _tmp14 = _views_0_pose[1] * _views_1_pose[1]; const Scalar _tmp15 = _views_0_pose[3] * _views_1_pose[3]; const Scalar _tmp16 = _tmp12 + _tmp13 + _tmp14 + _tmp15; - const Scalar _tmp17 = _views_0_pose[0] * _views_1_pose[3]; - const Scalar _tmp18 = _views_0_pose[3] * _views_1_pose[0]; - const Scalar _tmp19 = _views_0_pose[1] * _views_1_pose[2]; - const Scalar _tmp20 = _views_0_pose[2] * _views_1_pose[1]; - const Scalar _tmp21 = -_tmp17 + _tmp18 - _tmp19 + _tmp20; - const Scalar _tmp22 = _views_0_pose[1] * _views_1_pose[3]; - const Scalar _tmp23 = _views_0_pose[2] * _views_1_pose[0]; - const Scalar _tmp24 = _views_0_pose[0] * _views_1_pose[2]; - const Scalar _tmp25 = _views_0_pose[3] * _views_1_pose[1]; - const Scalar _tmp26 = -_tmp22 - _tmp23 + _tmp24 + _tmp25; - const Scalar _tmp27 = _views_0_pose[2] * _views_1_pose[3]; - const Scalar _tmp28 = _views_0_pose[1] * _views_1_pose[0]; - const Scalar _tmp29 = _views_0_pose[3] * _views_1_pose[2]; - const Scalar _tmp30 = _views_0_pose[0] * _views_1_pose[1]; - const Scalar _tmp31 = -_tmp27 + _tmp28 + _tmp29 - _tmp30; - const Scalar _tmp32 = _priors_0_1_target_T_src[0] * _tmp31 - + const Scalar _tmp17 = _priors_0_1_target_T_src[3] * _tmp16; + const Scalar _tmp18 = _views_0_pose[0] * _views_1_pose[3]; + const Scalar _tmp19 = _views_0_pose[3] * _views_1_pose[0]; + const Scalar _tmp20 = _views_0_pose[1] * _views_1_pose[2]; + const Scalar _tmp21 = _views_0_pose[2] * _views_1_pose[1]; + const Scalar _tmp22 = -_tmp18 + _tmp19 - _tmp20 + _tmp21; + const Scalar _tmp23 = _priors_0_1_target_T_src[0] * _tmp22; + const Scalar _tmp24 = _views_0_pose[1] * _views_1_pose[3]; + const Scalar _tmp25 = _views_0_pose[2] * _views_1_pose[0]; + const Scalar _tmp26 = _views_0_pose[0] * _views_1_pose[2]; + const Scalar _tmp27 = _views_0_pose[3] * _views_1_pose[1]; + const Scalar _tmp28 = -_tmp24 - _tmp25 + _tmp26 + _tmp27; + const Scalar _tmp29 = _priors_0_1_target_T_src[1] * _tmp28; + const Scalar _tmp30 = _views_0_pose[2] * _views_1_pose[3]; + const Scalar _tmp31 = _views_0_pose[1] * _views_1_pose[0]; + const Scalar _tmp32 = _views_0_pose[3] * _views_1_pose[2]; + const Scalar _tmp33 = _views_0_pose[0] * _views_1_pose[1]; + const Scalar _tmp34 = -_tmp30 + _tmp31 + _tmp32 - _tmp33; + const Scalar _tmp35 = _priors_0_1_target_T_src[2] * _tmp34; + const Scalar _tmp36 = -_tmp23 - _tmp29 - _tmp35; + const Scalar _tmp37 = _tmp17 - _tmp36; + const Scalar _tmp38 = 2 * std::min(0, (((_tmp37) > 0) - ((_tmp37) < 0))) + 1; + const Scalar _tmp39 = 2 * _tmp38; + const Scalar _tmp40 = _tmp39 * priors_0_1_sqrt_info(0, 1); + const Scalar _tmp41 = _priors_0_1_target_T_src[0] * _tmp34 - _priors_0_1_target_T_src[1] * _tmp16 - - _priors_0_1_target_T_src[2] * _tmp21 + _priors_0_1_target_T_src[3] * _tmp26; - const Scalar _tmp33 = _priors_0_1_target_T_src[3] * _tmp16; - const Scalar _tmp34 = _priors_0_1_target_T_src[0] * _tmp21; - const Scalar _tmp35 = _priors_0_1_target_T_src[1] * _tmp26; - const Scalar _tmp36 = _priors_0_1_target_T_src[2] * _tmp31; - const Scalar _tmp37 = -_tmp34 - _tmp35 - _tmp36; - const Scalar _tmp38 = _tmp33 - _tmp37; - const Scalar _tmp39 = 2 * std::min(0, (((_tmp38) > 0) - ((_tmp38) < 0))) + 1; - const Scalar _tmp40 = 2 * _tmp39; - const Scalar _tmp41 = -epsilon; - const Scalar _tmp42 = _tmp41 + 1; - const Scalar _tmp43 = std::min(_tmp42, std::fabs(_tmp38)); - const Scalar _tmp44 = std::acos(_tmp43) / std::sqrt(Scalar(1 - std::pow(_tmp43, Scalar(2)))); - const Scalar _tmp45 = _tmp40 * _tmp44; - const Scalar _tmp46 = _tmp32 * _tmp45; + _priors_0_1_target_T_src[2] * _tmp22 + _priors_0_1_target_T_src[3] * _tmp28; + const Scalar _tmp42 = -epsilon; + const Scalar _tmp43 = _tmp42 + 1; + const Scalar _tmp44 = std::min(_tmp43, std::fabs(_tmp37)); + const Scalar _tmp45 = std::acos(_tmp44) / std::sqrt(Scalar(1 - std::pow(_tmp44, Scalar(2)))); + const Scalar _tmp46 = _tmp41 * _tmp45; const Scalar _tmp47 = -2 * std::pow(_views_0_pose[1], Scalar(2)); - const Scalar _tmp48 = _tmp47 + _tmp5 + 1; - const Scalar _tmp49 = _tmp2 * _views_0_pose[2]; - const Scalar _tmp50 = _tmp0 * _views_0_pose[1]; + const Scalar _tmp48 = _tmp4 + _tmp47 + 1; + const Scalar _tmp49 = _tmp0 * _views_0_pose[0]; + const Scalar _tmp50 = _tmp8 * _views_0_pose[1]; const Scalar _tmp51 = _tmp49 + _tmp50; - const Scalar _tmp52 = _tmp8 - _tmp9; + const Scalar _tmp52 = _tmp7 - _tmp9; const Scalar _tmp53 = -_priors_0_1_target_T_src[6] - _tmp48 * _views_0_pose[6] + _tmp48 * _views_1_pose[6] - _tmp51 * _views_0_pose[4] + _tmp51 * _views_1_pose[4] - _tmp52 * _views_0_pose[5] + _tmp52 * _views_1_pose[5]; const Scalar _tmp54 = -_priors_0_1_target_T_src[0] * _tmp16 - - _priors_0_1_target_T_src[1] * _tmp31 + - _priors_0_1_target_T_src[2] * _tmp26 + _priors_0_1_target_T_src[3] * _tmp21; - const Scalar _tmp55 = _tmp44 * _tmp54; - const Scalar _tmp56 = _tmp40 * _tmp55; - const Scalar _tmp57 = -_priors_0_1_target_T_src[0] * _tmp26 + - _priors_0_1_target_T_src[1] * _tmp21 - - _priors_0_1_target_T_src[2] * _tmp16 + _priors_0_1_target_T_src[3] * _tmp31; - const Scalar _tmp58 = _tmp45 * _tmp57; - const Scalar _tmp59 = _tmp1 + _tmp3; - const Scalar _tmp60 = _tmp47 + _tmp6; + _priors_0_1_target_T_src[1] * _tmp34 + + _priors_0_1_target_T_src[2] * _tmp28 + _priors_0_1_target_T_src[3] * _tmp22; + const Scalar _tmp55 = _tmp39 * priors_0_1_sqrt_info(0, 0); + const Scalar _tmp56 = -_priors_0_1_target_T_src[0] * _tmp28 + + _priors_0_1_target_T_src[1] * _tmp22 - + _priors_0_1_target_T_src[2] * _tmp16 + _priors_0_1_target_T_src[3] * _tmp34; + const Scalar _tmp57 = _tmp45 * _tmp56; + const Scalar _tmp58 = _tmp39 * _tmp57; + const Scalar _tmp59 = _tmp1 + _tmp2; + const Scalar _tmp60 = _tmp47 + _tmp5; const Scalar _tmp61 = _tmp49 - _tmp50; const Scalar _tmp62 = -_priors_0_1_target_T_src[4] - _tmp59 * _views_0_pose[5] + _tmp59 * _views_1_pose[5] - _tmp60 * _views_0_pose[4] + _tmp60 * _views_1_pose[4] - _tmp61 * _views_0_pose[6] + _tmp61 * _views_1_pose[6]; - const Scalar _tmp63 = _tmp11 * priors_0_1_sqrt_info(0, 4) + _tmp46 * priors_0_1_sqrt_info(0, 1) + - _tmp53 * priors_0_1_sqrt_info(0, 5) + _tmp56 * priors_0_1_sqrt_info(0, 0) + + const Scalar _tmp63 = _tmp11 * priors_0_1_sqrt_info(0, 4) + _tmp40 * _tmp46 + + _tmp45 * _tmp54 * _tmp55 + _tmp53 * priors_0_1_sqrt_info(0, 5) + _tmp58 * priors_0_1_sqrt_info(0, 2) + _tmp62 * priors_0_1_sqrt_info(0, 3); - const Scalar _tmp64 = _tmp40 * priors_0_1_sqrt_info(1, 0); - const Scalar _tmp65 = _tmp11 * priors_0_1_sqrt_info(1, 4) + _tmp46 * priors_0_1_sqrt_info(1, 1) + - _tmp53 * priors_0_1_sqrt_info(1, 5) + _tmp55 * _tmp64 + - _tmp58 * priors_0_1_sqrt_info(1, 2) + _tmp62 * priors_0_1_sqrt_info(1, 3); - const Scalar _tmp66 = _tmp57 * priors_0_1_sqrt_info(2, 2); - const Scalar _tmp67 = _tmp11 * priors_0_1_sqrt_info(2, 4) + _tmp45 * _tmp66 + - _tmp46 * priors_0_1_sqrt_info(2, 1) + _tmp53 * priors_0_1_sqrt_info(2, 5) + - _tmp56 * priors_0_1_sqrt_info(2, 0) + _tmp62 * priors_0_1_sqrt_info(2, 3); - const Scalar _tmp68 = _tmp11 * priors_0_1_sqrt_info(3, 4) + _tmp46 * priors_0_1_sqrt_info(3, 1) + - _tmp53 * priors_0_1_sqrt_info(3, 5) + _tmp56 * priors_0_1_sqrt_info(3, 0) + - _tmp58 * priors_0_1_sqrt_info(3, 2) + _tmp62 * priors_0_1_sqrt_info(3, 3); - const Scalar _tmp69 = _tmp11 * priors_0_1_sqrt_info(4, 4) + _tmp46 * priors_0_1_sqrt_info(4, 1) + - _tmp53 * priors_0_1_sqrt_info(4, 5) + _tmp56 * priors_0_1_sqrt_info(4, 0) + - _tmp58 * priors_0_1_sqrt_info(4, 2) + _tmp62 * priors_0_1_sqrt_info(4, 3); - const Scalar _tmp70 = _tmp11 * priors_0_1_sqrt_info(5, 4) + _tmp46 * priors_0_1_sqrt_info(5, 1) + - _tmp53 * priors_0_1_sqrt_info(5, 5) + _tmp56 * priors_0_1_sqrt_info(5, 0) + - _tmp58 * priors_0_1_sqrt_info(5, 2) + _tmp62 * priors_0_1_sqrt_info(5, 3); - const Scalar _tmp71 = std::pow(_views_1_pose[2], Scalar(2)); - const Scalar _tmp72 = 2 * _tmp71; - const Scalar _tmp73 = -_tmp72; - const Scalar _tmp74 = std::pow(_views_1_pose[1], Scalar(2)); - const Scalar _tmp75 = 2 * _tmp74; - const Scalar _tmp76 = -_tmp75; - const Scalar _tmp77 = _tmp73 + _tmp76 + 1; - const Scalar _tmp78 = 2 * _views_1_pose[0]; - const Scalar _tmp79 = _tmp78 * _views_1_pose[2]; - const Scalar _tmp80 = 2 * _views_1_pose[1]; - const Scalar _tmp81 = _tmp80 * _views_1_pose[3]; - const Scalar _tmp82 = -_tmp81; - const Scalar _tmp83 = _tmp79 + _tmp82; - const Scalar _tmp84 = _tmp83 * _views_1_pose[6]; - const Scalar _tmp85 = _tmp78 * _views_1_pose[1]; - const Scalar _tmp86 = 2 * _views_1_pose[2] * _views_1_pose[3]; - const Scalar _tmp87 = _tmp85 + _tmp86; - const Scalar _tmp88 = _tmp87 * _views_1_pose[5]; - const Scalar _tmp89 = _tmp83 * _views_0_pose[6] + _tmp87 * _views_0_pose[5]; - const Scalar _tmp90 = -_priors_1_0_target_T_src[4] + _tmp77 * _views_0_pose[4] - - _tmp77 * _views_1_pose[4] - _tmp84 - _tmp88 + _tmp89; - const Scalar _tmp91 = std::pow(_views_1_pose[0], Scalar(2)); - const Scalar _tmp92 = 2 * _tmp91; - const Scalar _tmp93 = 1 - _tmp92; - const Scalar _tmp94 = _tmp73 + _tmp93; - const Scalar _tmp95 = _tmp78 * _views_1_pose[3]; - const Scalar _tmp96 = _tmp80 * _views_1_pose[2]; - const Scalar _tmp97 = _tmp95 + _tmp96; - const Scalar _tmp98 = _tmp97 * _views_1_pose[6]; - const Scalar _tmp99 = -_tmp86; - const Scalar _tmp100 = _tmp85 + _tmp99; - const Scalar _tmp101 = _tmp100 * _views_1_pose[4]; - const Scalar _tmp102 = _tmp100 * _views_0_pose[4] + _tmp97 * _views_0_pose[6]; - const Scalar _tmp103 = -_priors_1_0_target_T_src[5] - _tmp101 + _tmp102 + - _tmp94 * _views_0_pose[5] - _tmp94 * _views_1_pose[5] - _tmp98; - const Scalar _tmp104 = _tmp76 + _tmp93; - const Scalar _tmp105 = -_tmp95; - const Scalar _tmp106 = _tmp105 + _tmp96; - const Scalar _tmp107 = _tmp106 * _views_1_pose[5]; - const Scalar _tmp108 = _tmp79 + _tmp81; - const Scalar _tmp109 = _tmp108 * _views_1_pose[4]; - const Scalar _tmp110 = _tmp106 * _views_0_pose[5] + _tmp108 * _views_0_pose[4]; - const Scalar _tmp111 = -_priors_1_0_target_T_src[6] + _tmp104 * _views_0_pose[6] - - _tmp104 * _views_1_pose[6] - _tmp107 - _tmp109 + _tmp110; - const Scalar _tmp112 = _priors_1_0_target_T_src[3] * _tmp16; - const Scalar _tmp113 = _tmp27 - _tmp28 - _tmp29 + _tmp30; - const Scalar _tmp114 = _priors_1_0_target_T_src[2] * _tmp113; - const Scalar _tmp115 = _tmp22 + _tmp23 - _tmp24 - _tmp25; - const Scalar _tmp116 = _priors_1_0_target_T_src[1] * _tmp115; - const Scalar _tmp117 = _tmp17 - _tmp18 + _tmp19 - _tmp20; - const Scalar _tmp118 = _priors_1_0_target_T_src[0] * _tmp117; - const Scalar _tmp119 = -_tmp114 - _tmp116 - _tmp118; - const Scalar _tmp120 = _tmp112 - _tmp119; - const Scalar _tmp121 = 2 * std::min(0, (((_tmp120) > 0) - ((_tmp120) < 0))) + 1; - const Scalar _tmp122 = 2 * _tmp121; - const Scalar _tmp123 = - -_priors_1_0_target_T_src[0] * _tmp115 + _priors_1_0_target_T_src[1] * _tmp117 - - _priors_1_0_target_T_src[2] * _tmp16 + _priors_1_0_target_T_src[3] * _tmp113; - const Scalar _tmp124 = std::min(_tmp42, std::fabs(_tmp120)); - const Scalar _tmp125 = std::acos(_tmp124) / std::sqrt(Scalar(1 - std::pow(_tmp124, Scalar(2)))); - const Scalar _tmp126 = _tmp123 * _tmp125; - const Scalar _tmp127 = _tmp122 * _tmp126; + const Scalar _tmp64 = _tmp39 * priors_0_1_sqrt_info(1, 1); + const Scalar _tmp65 = _tmp11 * priors_0_1_sqrt_info(1, 4) + _tmp46 * _tmp64 + + _tmp53 * priors_0_1_sqrt_info(1, 5) + _tmp58 * priors_0_1_sqrt_info(1, 2) + + _tmp62 * priors_0_1_sqrt_info(1, 3); + const Scalar _tmp66 = _tmp39 * priors_0_1_sqrt_info(2, 2); + const Scalar _tmp67 = _tmp11 * priors_0_1_sqrt_info(2, 4) + _tmp53 * priors_0_1_sqrt_info(2, 5) + + _tmp57 * _tmp66 + _tmp62 * priors_0_1_sqrt_info(2, 3); + const Scalar _tmp68 = _tmp11 * priors_0_1_sqrt_info(3, 4) + _tmp53 * priors_0_1_sqrt_info(3, 5) + + _tmp62 * priors_0_1_sqrt_info(3, 3); + const Scalar _tmp69 = _tmp11 * priors_0_1_sqrt_info(4, 4) + _tmp53 * priors_0_1_sqrt_info(4, 5); + const Scalar _tmp70 = std::pow(_views_1_pose[2], Scalar(2)); + const Scalar _tmp71 = 2 * _tmp70; + const Scalar _tmp72 = -_tmp71; + const Scalar _tmp73 = std::pow(_views_1_pose[1], Scalar(2)); + const Scalar _tmp74 = 2 * _tmp73; + const Scalar _tmp75 = -_tmp74; + const Scalar _tmp76 = _tmp72 + _tmp75 + 1; + const Scalar _tmp77 = 2 * _views_1_pose[2]; + const Scalar _tmp78 = _tmp77 * _views_1_pose[0]; + const Scalar _tmp79 = 2 * _views_1_pose[1] * _views_1_pose[3]; + const Scalar _tmp80 = -_tmp79; + const Scalar _tmp81 = _tmp78 + _tmp80; + const Scalar _tmp82 = _tmp81 * _views_1_pose[6]; + const Scalar _tmp83 = 2 * _views_1_pose[0]; + const Scalar _tmp84 = _tmp83 * _views_1_pose[1]; + const Scalar _tmp85 = _tmp77 * _views_1_pose[3]; + const Scalar _tmp86 = _tmp84 + _tmp85; + const Scalar _tmp87 = _tmp86 * _views_1_pose[5]; + const Scalar _tmp88 = _tmp81 * _views_0_pose[6] + _tmp86 * _views_0_pose[5]; + const Scalar _tmp89 = -_priors_1_0_target_T_src[4] + _tmp76 * _views_0_pose[4] - + _tmp76 * _views_1_pose[4] - _tmp82 - _tmp87 + _tmp88; + const Scalar _tmp90 = std::pow(_views_1_pose[0], Scalar(2)); + const Scalar _tmp91 = 2 * _tmp90; + const Scalar _tmp92 = 1 - _tmp91; + const Scalar _tmp93 = _tmp72 + _tmp92; + const Scalar _tmp94 = _tmp83 * _views_1_pose[3]; + const Scalar _tmp95 = _tmp77 * _views_1_pose[1]; + const Scalar _tmp96 = _tmp94 + _tmp95; + const Scalar _tmp97 = _tmp96 * _views_1_pose[6]; + const Scalar _tmp98 = -_tmp85; + const Scalar _tmp99 = _tmp84 + _tmp98; + const Scalar _tmp100 = _tmp99 * _views_1_pose[4]; + const Scalar _tmp101 = _tmp96 * _views_0_pose[6] + _tmp99 * _views_0_pose[4]; + const Scalar _tmp102 = -_priors_1_0_target_T_src[5] - _tmp100 + _tmp101 + + _tmp93 * _views_0_pose[5] - _tmp93 * _views_1_pose[5] - _tmp97; + const Scalar _tmp103 = _tmp75 + _tmp92; + const Scalar _tmp104 = -_tmp94; + const Scalar _tmp105 = _tmp104 + _tmp95; + const Scalar _tmp106 = _tmp105 * _views_1_pose[5]; + const Scalar _tmp107 = _tmp78 + _tmp79; + const Scalar _tmp108 = _tmp107 * _views_1_pose[4]; + const Scalar _tmp109 = _tmp105 * _views_0_pose[5] + _tmp107 * _views_0_pose[4]; + const Scalar _tmp110 = -_priors_1_0_target_T_src[6] + _tmp103 * _views_0_pose[6] - + _tmp103 * _views_1_pose[6] - _tmp106 - _tmp108 + _tmp109; + const Scalar _tmp111 = _priors_1_0_target_T_src[3] * _tmp16; + const Scalar _tmp112 = _tmp30 - _tmp31 - _tmp32 + _tmp33; + const Scalar _tmp113 = _priors_1_0_target_T_src[2] * _tmp112; + const Scalar _tmp114 = _tmp24 + _tmp25 - _tmp26 - _tmp27; + const Scalar _tmp115 = _priors_1_0_target_T_src[1] * _tmp114; + const Scalar _tmp116 = _tmp18 - _tmp19 + _tmp20 - _tmp21; + const Scalar _tmp117 = _priors_1_0_target_T_src[0] * _tmp116; + const Scalar _tmp118 = -_tmp113 - _tmp115 - _tmp117; + const Scalar _tmp119 = _tmp111 - _tmp118; + const Scalar _tmp120 = 2 * std::min(0, (((_tmp119) > 0) - ((_tmp119) < 0))) + 1; + const Scalar _tmp121 = 2 * _tmp120; + const Scalar _tmp122 = + -_priors_1_0_target_T_src[0] * _tmp114 + _priors_1_0_target_T_src[1] * _tmp116 - + _priors_1_0_target_T_src[2] * _tmp16 + _priors_1_0_target_T_src[3] * _tmp112; + const Scalar _tmp123 = std::min(_tmp43, std::fabs(_tmp119)); + const Scalar _tmp124 = std::acos(_tmp123) / std::sqrt(Scalar(1 - std::pow(_tmp123, Scalar(2)))); + const Scalar _tmp125 = _tmp122 * _tmp124; + const Scalar _tmp126 = _tmp121 * _tmp125; + const Scalar _tmp127 = _tmp121 * _tmp124; const Scalar _tmp128 = - _priors_1_0_target_T_src[0] * _tmp113 - _priors_1_0_target_T_src[1] * _tmp16 - - _priors_1_0_target_T_src[2] * _tmp117 + _priors_1_0_target_T_src[3] * _tmp115; - const Scalar _tmp129 = _tmp122 * _tmp125; - const Scalar _tmp130 = _tmp128 * _tmp129; - const Scalar _tmp131 = - -_priors_1_0_target_T_src[0] * _tmp16 - _priors_1_0_target_T_src[1] * _tmp113 + - _priors_1_0_target_T_src[2] * _tmp115 + _priors_1_0_target_T_src[3] * _tmp117; - const Scalar _tmp132 = _tmp129 * _tmp131; - const Scalar _tmp133 = - _tmp103 * priors_1_0_sqrt_info(0, 4) + _tmp111 * priors_1_0_sqrt_info(0, 5) + - _tmp127 * priors_1_0_sqrt_info(0, 2) + _tmp130 * priors_1_0_sqrt_info(0, 1) + - _tmp132 * priors_1_0_sqrt_info(0, 0) + _tmp90 * priors_1_0_sqrt_info(0, 3); - const Scalar _tmp134 = - _tmp103 * priors_1_0_sqrt_info(1, 4) + _tmp111 * priors_1_0_sqrt_info(1, 5) + - _tmp127 * priors_1_0_sqrt_info(1, 2) + _tmp130 * priors_1_0_sqrt_info(1, 1) + - _tmp132 * priors_1_0_sqrt_info(1, 0) + _tmp90 * priors_1_0_sqrt_info(1, 3); - const Scalar _tmp135 = _tmp128 * priors_1_0_sqrt_info(2, 1); - const Scalar _tmp136 = _tmp122 * priors_1_0_sqrt_info(2, 0); - const Scalar _tmp137 = _tmp103 * priors_1_0_sqrt_info(2, 4) + - _tmp111 * priors_1_0_sqrt_info(2, 5) + _tmp125 * _tmp131 * _tmp136 + - _tmp127 * priors_1_0_sqrt_info(2, 2) + _tmp129 * _tmp135 + - _tmp90 * priors_1_0_sqrt_info(2, 3); - const Scalar _tmp138 = _tmp122 * priors_1_0_sqrt_info(3, 2); - const Scalar _tmp139 = _tmp103 * priors_1_0_sqrt_info(3, 4) + - _tmp111 * priors_1_0_sqrt_info(3, 5) + _tmp126 * _tmp138 + - _tmp130 * priors_1_0_sqrt_info(3, 1) + - _tmp132 * priors_1_0_sqrt_info(3, 0) + _tmp90 * priors_1_0_sqrt_info(3, 3); - const Scalar _tmp140 = - _tmp103 * priors_1_0_sqrt_info(4, 4) + _tmp111 * priors_1_0_sqrt_info(4, 5) + - _tmp127 * priors_1_0_sqrt_info(4, 2) + _tmp130 * priors_1_0_sqrt_info(4, 1) + - _tmp132 * priors_1_0_sqrt_info(4, 0) + _tmp90 * priors_1_0_sqrt_info(4, 3); - const Scalar _tmp141 = - _tmp103 * priors_1_0_sqrt_info(5, 4) + _tmp111 * priors_1_0_sqrt_info(5, 5) + - _tmp127 * priors_1_0_sqrt_info(5, 2) + _tmp130 * priors_1_0_sqrt_info(5, 1) + - _tmp132 * priors_1_0_sqrt_info(5, 0) + _tmp90 * priors_1_0_sqrt_info(5, 3); - const Scalar _tmp142 = Scalar(1.0) / (views_0_calibration(1, 0)); - const Scalar _tmp143 = _tmp142 * _tmp4; - const Scalar _tmp144 = -views_0_calibration(2, 0); - const Scalar _tmp145 = _tmp144 + matches_0_0_source_coords(0, 0); - const Scalar _tmp146 = std::pow(views_0_calibration(0, 0), Scalar(-2)); - const Scalar _tmp147 = -views_0_calibration(3, 0); - const Scalar _tmp148 = _tmp147 + matches_0_0_source_coords(1, 0); - const Scalar _tmp149 = std::pow(views_0_calibration(1, 0), Scalar(-2)); - const Scalar _tmp150 = epsilon + 1; - const Scalar _tmp151 = std::pow(Scalar(std::pow(_tmp145, Scalar(2)) * _tmp146 + - std::pow(_tmp148, Scalar(2)) * _tmp149 + _tmp150), + _priors_1_0_target_T_src[0] * _tmp112 - _priors_1_0_target_T_src[1] * _tmp16 - + _priors_1_0_target_T_src[2] * _tmp116 + _priors_1_0_target_T_src[3] * _tmp114; + const Scalar _tmp129 = _tmp128 * priors_1_0_sqrt_info(0, 1); + const Scalar _tmp130 = + priors_1_0_sqrt_info(0, 0) * + (-_priors_1_0_target_T_src[0] * _tmp16 - _priors_1_0_target_T_src[1] * _tmp112 + + _priors_1_0_target_T_src[2] * _tmp114 + _priors_1_0_target_T_src[3] * _tmp116); + const Scalar _tmp131 = _tmp102 * priors_1_0_sqrt_info(0, 4) + + _tmp110 * priors_1_0_sqrt_info(0, 5) + + _tmp126 * priors_1_0_sqrt_info(0, 2) + _tmp127 * _tmp129 + + _tmp127 * _tmp130 + _tmp89 * priors_1_0_sqrt_info(0, 3); + const Scalar _tmp132 = _tmp128 * priors_1_0_sqrt_info(1, 1); + const Scalar _tmp133 = _tmp121 * priors_1_0_sqrt_info(1, 2); + const Scalar _tmp134 = _tmp102 * priors_1_0_sqrt_info(1, 4) + + _tmp110 * priors_1_0_sqrt_info(1, 5) + _tmp125 * _tmp133 + + _tmp127 * _tmp132 + _tmp89 * priors_1_0_sqrt_info(1, 3); + const Scalar _tmp135 = _tmp102 * priors_1_0_sqrt_info(2, 4) + + _tmp110 * priors_1_0_sqrt_info(2, 5) + + _tmp126 * priors_1_0_sqrt_info(2, 2) + _tmp89 * priors_1_0_sqrt_info(2, 3); + const Scalar _tmp136 = _tmp102 * priors_1_0_sqrt_info(3, 4) + + _tmp110 * priors_1_0_sqrt_info(3, 5) + _tmp89 * priors_1_0_sqrt_info(3, 3); + const Scalar _tmp137 = + _tmp102 * priors_1_0_sqrt_info(4, 4) + _tmp110 * priors_1_0_sqrt_info(4, 5); + const Scalar _tmp138 = Scalar(1.0) / (views_0_calibration(1, 0)); + const Scalar _tmp139 = _tmp138 * _tmp3; + const Scalar _tmp140 = -views_0_calibration(2, 0); + const Scalar _tmp141 = _tmp140 + matches_0_0_source_coords(0, 0); + const Scalar _tmp142 = std::pow(views_0_calibration(0, 0), Scalar(-2)); + const Scalar _tmp143 = -views_0_calibration(3, 0); + const Scalar _tmp144 = _tmp143 + matches_0_0_source_coords(1, 0); + const Scalar _tmp145 = std::pow(views_0_calibration(1, 0), Scalar(-2)); + const Scalar _tmp146 = epsilon + 1; + const Scalar _tmp147 = std::pow(Scalar(std::pow(_tmp141, Scalar(2)) * _tmp142 + + std::pow(_tmp144, Scalar(2)) * _tmp145 + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp152 = _tmp148 * _tmp151; - const Scalar _tmp153 = _views_0_pose[4] - _views_1_pose[4]; - const Scalar _tmp154 = Scalar(1.0) / (views_0_calibration(0, 0)); - const Scalar _tmp155 = _tmp154 * _tmp60; - const Scalar _tmp156 = _tmp145 * _tmp151; + const Scalar _tmp148 = _tmp144 * _tmp147; + const Scalar _tmp149 = _views_0_pose[4] - _views_1_pose[4]; + const Scalar _tmp150 = Scalar(1.0) / (views_0_calibration(0, 0)); + const Scalar _tmp151 = _tmp150 * _tmp60; + const Scalar _tmp152 = _tmp141 * _tmp147; + const Scalar _tmp153 = + _tmp139 * _tmp148 + _tmp147 * _tmp51 + _tmp149 * landmarks_0_ + _tmp151 * _tmp152; + const Scalar _tmp154 = _tmp150 * _tmp59; + const Scalar _tmp155 = _tmp138 * _tmp6; + const Scalar _tmp156 = _views_0_pose[5] - _views_1_pose[5]; const Scalar _tmp157 = - _tmp143 * _tmp152 + _tmp151 * _tmp51 + _tmp153 * landmarks_0_ + _tmp155 * _tmp156; - const Scalar _tmp158 = _tmp154 * _tmp59; - const Scalar _tmp159 = _tmp142 * _tmp7; - const Scalar _tmp160 = _views_0_pose[5] - _views_1_pose[5]; + _tmp147 * _tmp52 + _tmp148 * _tmp155 + _tmp152 * _tmp154 + _tmp156 * landmarks_0_; + const Scalar _tmp158 = _tmp150 * _tmp61; + const Scalar _tmp159 = _tmp10 * _tmp138; + const Scalar _tmp160 = _views_0_pose[6] - _views_1_pose[6]; const Scalar _tmp161 = - _tmp151 * _tmp52 + _tmp152 * _tmp159 + _tmp156 * _tmp158 + _tmp160 * landmarks_0_; - const Scalar _tmp162 = _tmp154 * _tmp61; - const Scalar _tmp163 = _tmp10 * _tmp142; - const Scalar _tmp164 = _views_0_pose[6] - _views_1_pose[6]; - const Scalar _tmp165 = - _tmp151 * _tmp48 + _tmp152 * _tmp163 + _tmp156 * _tmp162 + _tmp164 * landmarks_0_; - const Scalar _tmp166 = _tmp161 * _tmp87 + _tmp165 * _tmp83; - const Scalar _tmp167 = _tmp157 * _tmp77 + _tmp166; - const Scalar _tmp168 = _tmp106 * _tmp161 + _tmp108 * _tmp157; - const Scalar _tmp169 = _tmp104 * _tmp165 + _tmp168; - const Scalar _tmp170 = std::max(_tmp169, epsilon); - const Scalar _tmp171 = Scalar(1.0) / (_tmp170); - const Scalar _tmp172 = _tmp171 * views_1_calibration(0, 0); + _tmp147 * _tmp48 + _tmp148 * _tmp159 + _tmp152 * _tmp158 + _tmp160 * landmarks_0_; + const Scalar _tmp162 = _tmp157 * _tmp86 + _tmp161 * _tmp81; + const Scalar _tmp163 = _tmp153 * _tmp76 + _tmp162; + const Scalar _tmp164 = _tmp105 * _tmp157 + _tmp107 * _tmp153; + const Scalar _tmp165 = _tmp103 * _tmp161 + _tmp164; + const Scalar _tmp166 = std::max(_tmp165, epsilon); + const Scalar _tmp167 = Scalar(1.0) / (_tmp166); + const Scalar _tmp168 = _tmp167 * views_1_calibration(0, 0); + const Scalar _tmp169 = + _tmp163 * _tmp168 - matches_0_0_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp170 = _tmp153 * _tmp99 + _tmp161 * _tmp96; + const Scalar _tmp171 = _tmp157 * _tmp93 + _tmp170; + const Scalar _tmp172 = _tmp167 * views_1_calibration(1, 0); const Scalar _tmp173 = - _tmp167 * _tmp172 - matches_0_0_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp174 = _tmp100 * _tmp157 + _tmp165 * _tmp97; - const Scalar _tmp175 = _tmp161 * _tmp94 + _tmp174; - const Scalar _tmp176 = _tmp171 * views_1_calibration(1, 0); - const Scalar _tmp177 = - _tmp175 * _tmp176 - matches_0_0_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp178 = std::pow(_tmp173, Scalar(2)) + std::pow(_tmp177, Scalar(2)) + epsilon; - const Scalar _tmp179 = std::pow(_tmp178, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp180 = std::sqrt(matches_0_0_weight); - const Scalar _tmp181 = std::pow(costs_reprojection_error_gnc_scale, Scalar(-2)); - const Scalar _tmp182 = Scalar(1.0) / (_tmp150 - costs_reprojection_error_gnc_mu); - const Scalar _tmp183 = epsilon + std::fabs(_tmp182); - const Scalar _tmp184 = _tmp181 / _tmp183; - const Scalar _tmp185 = _tmp178 * _tmp184 + 1; - const Scalar _tmp186 = 2 - _tmp182; - const Scalar _tmp187 = - _tmp186 + epsilon * (2 * std::min(0, (((_tmp186) > 0) - ((_tmp186) < 0))) + 1); - const Scalar _tmp188 = (Scalar(1) / Scalar(2)) * _tmp187; - const Scalar _tmp189 = 2 * _tmp183 / _tmp187; - const Scalar _tmp190 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp185, _tmp188) - 1))); - const Scalar _tmp191 = std::max(0, (((_tmp169) > 0) - ((_tmp169) < 0))); - const Scalar _tmp192 = _tmp180 * _tmp190 * _tmp191; - const Scalar _tmp193 = _tmp179 * _tmp192; - const Scalar _tmp194 = _tmp173 * _tmp193; - const Scalar _tmp195 = _tmp177 * _tmp193; - const Scalar _tmp196 = _tmp144 + matches_0_1_source_coords(0, 0); - const Scalar _tmp197 = _tmp147 + matches_0_1_source_coords(1, 0); - const Scalar _tmp198 = std::pow(Scalar(_tmp146 * std::pow(_tmp196, Scalar(2)) + - _tmp149 * std::pow(_tmp197, Scalar(2)) + _tmp150), + _tmp171 * _tmp172 - matches_0_0_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp174 = std::pow(_tmp169, Scalar(2)) + std::pow(_tmp173, Scalar(2)) + epsilon; + const Scalar _tmp175 = std::pow(_tmp174, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp176 = std::sqrt(matches_0_0_weight); + const Scalar _tmp177 = std::pow(costs_reprojection_error_gnc_scale, Scalar(-2)); + const Scalar _tmp178 = Scalar(1.0) / (_tmp146 - costs_reprojection_error_gnc_mu); + const Scalar _tmp179 = epsilon + std::fabs(_tmp178); + const Scalar _tmp180 = _tmp177 / _tmp179; + const Scalar _tmp181 = _tmp174 * _tmp180 + 1; + const Scalar _tmp182 = 2 - _tmp178; + const Scalar _tmp183 = + _tmp182 + epsilon * (2 * std::min(0, (((_tmp182) > 0) - ((_tmp182) < 0))) + 1); + const Scalar _tmp184 = (Scalar(1) / Scalar(2)) * _tmp183; + const Scalar _tmp185 = 2 * _tmp179 / _tmp183; + const Scalar _tmp186 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp181, _tmp184) - 1))); + const Scalar _tmp187 = std::max(0, (((_tmp165) > 0) - ((_tmp165) < 0))); + const Scalar _tmp188 = _tmp176 * _tmp186 * _tmp187; + const Scalar _tmp189 = _tmp175 * _tmp188; + const Scalar _tmp190 = _tmp169 * _tmp189; + const Scalar _tmp191 = _tmp173 * _tmp189; + const Scalar _tmp192 = _tmp140 + matches_0_1_source_coords(0, 0); + const Scalar _tmp193 = _tmp143 + matches_0_1_source_coords(1, 0); + const Scalar _tmp194 = std::pow(Scalar(_tmp142 * std::pow(_tmp192, Scalar(2)) + + _tmp145 * std::pow(_tmp193, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp199 = _tmp197 * _tmp198; - const Scalar _tmp200 = _tmp196 * _tmp198; - const Scalar _tmp201 = - _tmp143 * _tmp199 + _tmp153 * landmarks_1_ + _tmp155 * _tmp200 + _tmp198 * _tmp51; - const Scalar _tmp202 = - _tmp158 * _tmp200 + _tmp159 * _tmp199 + _tmp160 * landmarks_1_ + _tmp198 * _tmp52; - const Scalar _tmp203 = - _tmp162 * _tmp200 + _tmp163 * _tmp199 + _tmp164 * landmarks_1_ + _tmp198 * _tmp48; - const Scalar _tmp204 = _tmp202 * _tmp87 + _tmp203 * _tmp83; - const Scalar _tmp205 = _tmp201 * _tmp77 + _tmp204; - const Scalar _tmp206 = _tmp106 * _tmp202 + _tmp108 * _tmp201; - const Scalar _tmp207 = _tmp104 * _tmp203 + _tmp206; - const Scalar _tmp208 = std::max(_tmp207, epsilon); - const Scalar _tmp209 = Scalar(1.0) / (_tmp208); - const Scalar _tmp210 = _tmp209 * views_1_calibration(0, 0); + const Scalar _tmp195 = _tmp193 * _tmp194; + const Scalar _tmp196 = _tmp192 * _tmp194; + const Scalar _tmp197 = + _tmp139 * _tmp195 + _tmp149 * landmarks_1_ + _tmp151 * _tmp196 + _tmp194 * _tmp51; + const Scalar _tmp198 = + _tmp154 * _tmp196 + _tmp155 * _tmp195 + _tmp156 * landmarks_1_ + _tmp194 * _tmp52; + const Scalar _tmp199 = + _tmp158 * _tmp196 + _tmp159 * _tmp195 + _tmp160 * landmarks_1_ + _tmp194 * _tmp48; + const Scalar _tmp200 = _tmp198 * _tmp86 + _tmp199 * _tmp81; + const Scalar _tmp201 = _tmp197 * _tmp76 + _tmp200; + const Scalar _tmp202 = _tmp105 * _tmp198 + _tmp107 * _tmp197; + const Scalar _tmp203 = _tmp103 * _tmp199 + _tmp202; + const Scalar _tmp204 = std::max(_tmp203, epsilon); + const Scalar _tmp205 = Scalar(1.0) / (_tmp204); + const Scalar _tmp206 = _tmp205 * views_1_calibration(0, 0); + const Scalar _tmp207 = + _tmp201 * _tmp206 - matches_0_1_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp208 = _tmp197 * _tmp99 + _tmp199 * _tmp96; + const Scalar _tmp209 = _tmp198 * _tmp93 + _tmp208; + const Scalar _tmp210 = _tmp205 * views_1_calibration(1, 0); const Scalar _tmp211 = - _tmp205 * _tmp210 - matches_0_1_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp212 = _tmp100 * _tmp201 + _tmp203 * _tmp97; - const Scalar _tmp213 = _tmp202 * _tmp94 + _tmp212; - const Scalar _tmp214 = _tmp209 * views_1_calibration(1, 0); - const Scalar _tmp215 = - _tmp213 * _tmp214 - matches_0_1_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp216 = std::pow(_tmp211, Scalar(2)) + std::pow(_tmp215, Scalar(2)) + epsilon; - const Scalar _tmp217 = std::pow(_tmp216, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp218 = _tmp184 * _tmp216 + 1; - const Scalar _tmp219 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp218, _tmp188) - 1))); - const Scalar _tmp220 = std::sqrt(matches_0_1_weight); - const Scalar _tmp221 = std::max(0, (((_tmp207) > 0) - ((_tmp207) < 0))); - const Scalar _tmp222 = _tmp219 * _tmp220 * _tmp221; - const Scalar _tmp223 = _tmp217 * _tmp222; - const Scalar _tmp224 = _tmp211 * _tmp223; - const Scalar _tmp225 = _tmp215 * _tmp223; - const Scalar _tmp226 = _tmp147 + matches_0_2_source_coords(1, 0); - const Scalar _tmp227 = _tmp144 + matches_0_2_source_coords(0, 0); - const Scalar _tmp228 = std::pow(Scalar(_tmp146 * std::pow(_tmp227, Scalar(2)) + - _tmp149 * std::pow(_tmp226, Scalar(2)) + _tmp150), + _tmp209 * _tmp210 - matches_0_1_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp212 = std::pow(_tmp207, Scalar(2)) + std::pow(_tmp211, Scalar(2)) + epsilon; + const Scalar _tmp213 = std::pow(_tmp212, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp214 = _tmp180 * _tmp212 + 1; + const Scalar _tmp215 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp214, _tmp184) - 1))); + const Scalar _tmp216 = std::sqrt(matches_0_1_weight); + const Scalar _tmp217 = std::max(0, (((_tmp203) > 0) - ((_tmp203) < 0))); + const Scalar _tmp218 = _tmp215 * _tmp216 * _tmp217; + const Scalar _tmp219 = _tmp213 * _tmp218; + const Scalar _tmp220 = _tmp207 * _tmp219; + const Scalar _tmp221 = _tmp211 * _tmp219; + const Scalar _tmp222 = _tmp143 + matches_0_2_source_coords(1, 0); + const Scalar _tmp223 = _tmp140 + matches_0_2_source_coords(0, 0); + const Scalar _tmp224 = std::pow(Scalar(_tmp142 * std::pow(_tmp223, Scalar(2)) + + _tmp145 * std::pow(_tmp222, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp229 = _tmp226 * _tmp228; - const Scalar _tmp230 = _tmp227 * _tmp228; - const Scalar _tmp231 = - _tmp143 * _tmp229 + _tmp153 * landmarks_2_ + _tmp155 * _tmp230 + _tmp228 * _tmp51; - const Scalar _tmp232 = - _tmp162 * _tmp230 + _tmp163 * _tmp229 + _tmp164 * landmarks_2_ + _tmp228 * _tmp48; - const Scalar _tmp233 = - _tmp158 * _tmp230 + _tmp159 * _tmp229 + _tmp160 * landmarks_2_ + _tmp228 * _tmp52; - const Scalar _tmp234 = _tmp232 * _tmp83 + _tmp233 * _tmp87; - const Scalar _tmp235 = _tmp231 * _tmp77 + _tmp234; - const Scalar _tmp236 = _tmp106 * _tmp233 + _tmp108 * _tmp231; - const Scalar _tmp237 = _tmp104 * _tmp232 + _tmp236; - const Scalar _tmp238 = std::max(_tmp237, epsilon); - const Scalar _tmp239 = Scalar(1.0) / (_tmp238); - const Scalar _tmp240 = _tmp239 * views_1_calibration(0, 0); + const Scalar _tmp225 = _tmp222 * _tmp224; + const Scalar _tmp226 = _tmp223 * _tmp224; + const Scalar _tmp227 = + _tmp139 * _tmp225 + _tmp149 * landmarks_2_ + _tmp151 * _tmp226 + _tmp224 * _tmp51; + const Scalar _tmp228 = + _tmp158 * _tmp226 + _tmp159 * _tmp225 + _tmp160 * landmarks_2_ + _tmp224 * _tmp48; + const Scalar _tmp229 = + _tmp154 * _tmp226 + _tmp155 * _tmp225 + _tmp156 * landmarks_2_ + _tmp224 * _tmp52; + const Scalar _tmp230 = _tmp228 * _tmp81 + _tmp229 * _tmp86; + const Scalar _tmp231 = _tmp227 * _tmp76 + _tmp230; + const Scalar _tmp232 = _tmp105 * _tmp229 + _tmp107 * _tmp227; + const Scalar _tmp233 = _tmp103 * _tmp228 + _tmp232; + const Scalar _tmp234 = std::max(_tmp233, epsilon); + const Scalar _tmp235 = Scalar(1.0) / (_tmp234); + const Scalar _tmp236 = _tmp235 * views_1_calibration(0, 0); + const Scalar _tmp237 = + _tmp231 * _tmp236 - matches_0_2_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp238 = _tmp227 * _tmp99 + _tmp228 * _tmp96; + const Scalar _tmp239 = _tmp229 * _tmp93 + _tmp238; + const Scalar _tmp240 = _tmp235 * views_1_calibration(1, 0); const Scalar _tmp241 = - _tmp235 * _tmp240 - matches_0_2_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp242 = _tmp100 * _tmp231 + _tmp232 * _tmp97; - const Scalar _tmp243 = _tmp233 * _tmp94 + _tmp242; - const Scalar _tmp244 = _tmp239 * views_1_calibration(1, 0); - const Scalar _tmp245 = - _tmp243 * _tmp244 - matches_0_2_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp246 = std::pow(_tmp241, Scalar(2)) + std::pow(_tmp245, Scalar(2)) + epsilon; - const Scalar _tmp247 = std::pow(_tmp246, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp248 = std::max(0, (((_tmp237) > 0) - ((_tmp237) < 0))); - const Scalar _tmp249 = std::sqrt(matches_0_2_weight); - const Scalar _tmp250 = _tmp184 * _tmp246 + 1; - const Scalar _tmp251 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp250, _tmp188) - 1))); - const Scalar _tmp252 = _tmp248 * _tmp249 * _tmp251; - const Scalar _tmp253 = _tmp247 * _tmp252; - const Scalar _tmp254 = _tmp241 * _tmp253; - const Scalar _tmp255 = _tmp245 * _tmp253; - const Scalar _tmp256 = _tmp144 + matches_0_3_source_coords(0, 0); - const Scalar _tmp257 = _tmp147 + matches_0_3_source_coords(1, 0); - const Scalar _tmp258 = std::pow(Scalar(_tmp146 * std::pow(_tmp256, Scalar(2)) + - _tmp149 * std::pow(_tmp257, Scalar(2)) + _tmp150), + _tmp239 * _tmp240 - matches_0_2_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp242 = std::pow(_tmp237, Scalar(2)) + std::pow(_tmp241, Scalar(2)) + epsilon; + const Scalar _tmp243 = std::pow(_tmp242, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp244 = std::max(0, (((_tmp233) > 0) - ((_tmp233) < 0))); + const Scalar _tmp245 = std::sqrt(matches_0_2_weight); + const Scalar _tmp246 = _tmp180 * _tmp242 + 1; + const Scalar _tmp247 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp246, _tmp184) - 1))); + const Scalar _tmp248 = _tmp244 * _tmp245 * _tmp247; + const Scalar _tmp249 = _tmp243 * _tmp248; + const Scalar _tmp250 = _tmp237 * _tmp249; + const Scalar _tmp251 = _tmp241 * _tmp249; + const Scalar _tmp252 = _tmp140 + matches_0_3_source_coords(0, 0); + const Scalar _tmp253 = _tmp143 + matches_0_3_source_coords(1, 0); + const Scalar _tmp254 = std::pow(Scalar(_tmp142 * std::pow(_tmp252, Scalar(2)) + + _tmp145 * std::pow(_tmp253, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp259 = _tmp257 * _tmp258; - const Scalar _tmp260 = _tmp256 * _tmp258; - const Scalar _tmp261 = - _tmp143 * _tmp259 + _tmp153 * landmarks_3_ + _tmp155 * _tmp260 + _tmp258 * _tmp51; - const Scalar _tmp262 = - _tmp158 * _tmp260 + _tmp159 * _tmp259 + _tmp160 * landmarks_3_ + _tmp258 * _tmp52; - const Scalar _tmp263 = - _tmp162 * _tmp260 + _tmp163 * _tmp259 + _tmp164 * landmarks_3_ + _tmp258 * _tmp48; - const Scalar _tmp264 = _tmp262 * _tmp87 + _tmp263 * _tmp83; - const Scalar _tmp265 = _tmp261 * _tmp77 + _tmp264; - const Scalar _tmp266 = _tmp106 * _tmp262 + _tmp108 * _tmp261; - const Scalar _tmp267 = _tmp104 * _tmp263 + _tmp266; - const Scalar _tmp268 = std::max(_tmp267, epsilon); - const Scalar _tmp269 = Scalar(1.0) / (_tmp268); - const Scalar _tmp270 = _tmp269 * views_1_calibration(0, 0); + const Scalar _tmp255 = _tmp253 * _tmp254; + const Scalar _tmp256 = _tmp252 * _tmp254; + const Scalar _tmp257 = + _tmp139 * _tmp255 + _tmp149 * landmarks_3_ + _tmp151 * _tmp256 + _tmp254 * _tmp51; + const Scalar _tmp258 = + _tmp154 * _tmp256 + _tmp155 * _tmp255 + _tmp156 * landmarks_3_ + _tmp254 * _tmp52; + const Scalar _tmp259 = + _tmp158 * _tmp256 + _tmp159 * _tmp255 + _tmp160 * landmarks_3_ + _tmp254 * _tmp48; + const Scalar _tmp260 = _tmp258 * _tmp86 + _tmp259 * _tmp81; + const Scalar _tmp261 = _tmp257 * _tmp76 + _tmp260; + const Scalar _tmp262 = _tmp105 * _tmp258 + _tmp107 * _tmp257; + const Scalar _tmp263 = _tmp103 * _tmp259 + _tmp262; + const Scalar _tmp264 = std::max(_tmp263, epsilon); + const Scalar _tmp265 = Scalar(1.0) / (_tmp264); + const Scalar _tmp266 = _tmp265 * views_1_calibration(0, 0); + const Scalar _tmp267 = + _tmp261 * _tmp266 - matches_0_3_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp268 = _tmp257 * _tmp99 + _tmp259 * _tmp96; + const Scalar _tmp269 = _tmp258 * _tmp93 + _tmp268; + const Scalar _tmp270 = _tmp265 * views_1_calibration(1, 0); const Scalar _tmp271 = - _tmp265 * _tmp270 - matches_0_3_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp272 = _tmp100 * _tmp261 + _tmp263 * _tmp97; - const Scalar _tmp273 = _tmp262 * _tmp94 + _tmp272; - const Scalar _tmp274 = _tmp269 * views_1_calibration(1, 0); - const Scalar _tmp275 = - _tmp273 * _tmp274 - matches_0_3_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp276 = std::pow(_tmp271, Scalar(2)) + std::pow(_tmp275, Scalar(2)) + epsilon; - const Scalar _tmp277 = std::pow(_tmp276, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp278 = std::sqrt(matches_0_3_weight); - const Scalar _tmp279 = _tmp184 * _tmp276 + 1; - const Scalar _tmp280 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp279, _tmp188) - 1))); - const Scalar _tmp281 = std::max(0, (((_tmp267) > 0) - ((_tmp267) < 0))); - const Scalar _tmp282 = _tmp278 * _tmp280 * _tmp281; - const Scalar _tmp283 = _tmp277 * _tmp282; - const Scalar _tmp284 = _tmp271 * _tmp283; - const Scalar _tmp285 = _tmp275 * _tmp283; - const Scalar _tmp286 = _tmp147 + matches_0_4_source_coords(1, 0); - const Scalar _tmp287 = _tmp144 + matches_0_4_source_coords(0, 0); - const Scalar _tmp288 = std::pow(Scalar(_tmp146 * std::pow(_tmp287, Scalar(2)) + - _tmp149 * std::pow(_tmp286, Scalar(2)) + _tmp150), + _tmp269 * _tmp270 - matches_0_3_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp272 = std::pow(_tmp267, Scalar(2)) + std::pow(_tmp271, Scalar(2)) + epsilon; + const Scalar _tmp273 = std::pow(_tmp272, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp274 = std::sqrt(matches_0_3_weight); + const Scalar _tmp275 = _tmp180 * _tmp272 + 1; + const Scalar _tmp276 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp275, _tmp184) - 1))); + const Scalar _tmp277 = std::max(0, (((_tmp263) > 0) - ((_tmp263) < 0))); + const Scalar _tmp278 = _tmp274 * _tmp276 * _tmp277; + const Scalar _tmp279 = _tmp273 * _tmp278; + const Scalar _tmp280 = _tmp267 * _tmp279; + const Scalar _tmp281 = _tmp271 * _tmp279; + const Scalar _tmp282 = _tmp143 + matches_0_4_source_coords(1, 0); + const Scalar _tmp283 = _tmp140 + matches_0_4_source_coords(0, 0); + const Scalar _tmp284 = std::pow(Scalar(_tmp142 * std::pow(_tmp283, Scalar(2)) + + _tmp145 * std::pow(_tmp282, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp289 = _tmp286 * _tmp288; - const Scalar _tmp290 = _tmp287 * _tmp288; - const Scalar _tmp291 = - _tmp143 * _tmp289 + _tmp153 * landmarks_4_ + _tmp155 * _tmp290 + _tmp288 * _tmp51; - const Scalar _tmp292 = - _tmp162 * _tmp290 + _tmp163 * _tmp289 + _tmp164 * landmarks_4_ + _tmp288 * _tmp48; - const Scalar _tmp293 = - _tmp158 * _tmp290 + _tmp159 * _tmp289 + _tmp160 * landmarks_4_ + _tmp288 * _tmp52; - const Scalar _tmp294 = _tmp292 * _tmp83 + _tmp293 * _tmp87; - const Scalar _tmp295 = _tmp291 * _tmp77 + _tmp294; - const Scalar _tmp296 = _tmp106 * _tmp293 + _tmp108 * _tmp291; - const Scalar _tmp297 = _tmp104 * _tmp292 + _tmp296; - const Scalar _tmp298 = std::max(_tmp297, epsilon); - const Scalar _tmp299 = Scalar(1.0) / (_tmp298); - const Scalar _tmp300 = _tmp299 * views_1_calibration(0, 0); + const Scalar _tmp285 = _tmp282 * _tmp284; + const Scalar _tmp286 = _tmp283 * _tmp284; + const Scalar _tmp287 = + _tmp139 * _tmp285 + _tmp149 * landmarks_4_ + _tmp151 * _tmp286 + _tmp284 * _tmp51; + const Scalar _tmp288 = + _tmp158 * _tmp286 + _tmp159 * _tmp285 + _tmp160 * landmarks_4_ + _tmp284 * _tmp48; + const Scalar _tmp289 = + _tmp154 * _tmp286 + _tmp155 * _tmp285 + _tmp156 * landmarks_4_ + _tmp284 * _tmp52; + const Scalar _tmp290 = _tmp288 * _tmp81 + _tmp289 * _tmp86; + const Scalar _tmp291 = _tmp287 * _tmp76 + _tmp290; + const Scalar _tmp292 = _tmp105 * _tmp289 + _tmp107 * _tmp287; + const Scalar _tmp293 = _tmp103 * _tmp288 + _tmp292; + const Scalar _tmp294 = std::max(_tmp293, epsilon); + const Scalar _tmp295 = Scalar(1.0) / (_tmp294); + const Scalar _tmp296 = _tmp295 * views_1_calibration(0, 0); + const Scalar _tmp297 = + _tmp291 * _tmp296 - matches_0_4_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp298 = _tmp287 * _tmp99 + _tmp288 * _tmp96; + const Scalar _tmp299 = _tmp289 * _tmp93 + _tmp298; + const Scalar _tmp300 = _tmp295 * views_1_calibration(1, 0); const Scalar _tmp301 = - _tmp295 * _tmp300 - matches_0_4_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp302 = _tmp100 * _tmp291 + _tmp292 * _tmp97; - const Scalar _tmp303 = _tmp293 * _tmp94 + _tmp302; - const Scalar _tmp304 = _tmp299 * views_1_calibration(1, 0); - const Scalar _tmp305 = - _tmp303 * _tmp304 - matches_0_4_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp306 = std::pow(_tmp301, Scalar(2)) + std::pow(_tmp305, Scalar(2)) + epsilon; - const Scalar _tmp307 = std::pow(_tmp306, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp308 = std::sqrt(matches_0_4_weight); - const Scalar _tmp309 = std::max(0, (((_tmp297) > 0) - ((_tmp297) < 0))); - const Scalar _tmp310 = _tmp184 * _tmp306 + 1; - const Scalar _tmp311 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp310, _tmp188) - 1))); - const Scalar _tmp312 = _tmp308 * _tmp309 * _tmp311; - const Scalar _tmp313 = _tmp307 * _tmp312; - const Scalar _tmp314 = _tmp301 * _tmp313; - const Scalar _tmp315 = _tmp305 * _tmp313; - const Scalar _tmp316 = _tmp147 + matches_0_5_source_coords(1, 0); - const Scalar _tmp317 = _tmp144 + matches_0_5_source_coords(0, 0); - const Scalar _tmp318 = std::pow(Scalar(_tmp146 * std::pow(_tmp317, Scalar(2)) + - _tmp149 * std::pow(_tmp316, Scalar(2)) + _tmp150), + _tmp299 * _tmp300 - matches_0_4_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp302 = std::pow(_tmp297, Scalar(2)) + std::pow(_tmp301, Scalar(2)) + epsilon; + const Scalar _tmp303 = std::pow(_tmp302, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp304 = std::sqrt(matches_0_4_weight); + const Scalar _tmp305 = std::max(0, (((_tmp293) > 0) - ((_tmp293) < 0))); + const Scalar _tmp306 = _tmp180 * _tmp302 + 1; + const Scalar _tmp307 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp306, _tmp184) - 1))); + const Scalar _tmp308 = _tmp304 * _tmp305 * _tmp307; + const Scalar _tmp309 = _tmp303 * _tmp308; + const Scalar _tmp310 = _tmp297 * _tmp309; + const Scalar _tmp311 = _tmp301 * _tmp309; + const Scalar _tmp312 = _tmp143 + matches_0_5_source_coords(1, 0); + const Scalar _tmp313 = _tmp140 + matches_0_5_source_coords(0, 0); + const Scalar _tmp314 = std::pow(Scalar(_tmp142 * std::pow(_tmp313, Scalar(2)) + + _tmp145 * std::pow(_tmp312, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp319 = _tmp316 * _tmp318; - const Scalar _tmp320 = _tmp317 * _tmp318; - const Scalar _tmp321 = - _tmp143 * _tmp319 + _tmp153 * landmarks_5_ + _tmp155 * _tmp320 + _tmp318 * _tmp51; - const Scalar _tmp322 = - _tmp158 * _tmp320 + _tmp159 * _tmp319 + _tmp160 * landmarks_5_ + _tmp318 * _tmp52; - const Scalar _tmp323 = - _tmp162 * _tmp320 + _tmp163 * _tmp319 + _tmp164 * landmarks_5_ + _tmp318 * _tmp48; - const Scalar _tmp324 = _tmp322 * _tmp87 + _tmp323 * _tmp83; - const Scalar _tmp325 = _tmp321 * _tmp77 + _tmp324; - const Scalar _tmp326 = _tmp106 * _tmp322 + _tmp108 * _tmp321; - const Scalar _tmp327 = _tmp104 * _tmp323 + _tmp326; - const Scalar _tmp328 = std::max(_tmp327, epsilon); - const Scalar _tmp329 = Scalar(1.0) / (_tmp328); - const Scalar _tmp330 = _tmp329 * views_1_calibration(0, 0); + const Scalar _tmp315 = _tmp312 * _tmp314; + const Scalar _tmp316 = _tmp313 * _tmp314; + const Scalar _tmp317 = + _tmp139 * _tmp315 + _tmp149 * landmarks_5_ + _tmp151 * _tmp316 + _tmp314 * _tmp51; + const Scalar _tmp318 = + _tmp154 * _tmp316 + _tmp155 * _tmp315 + _tmp156 * landmarks_5_ + _tmp314 * _tmp52; + const Scalar _tmp319 = + _tmp158 * _tmp316 + _tmp159 * _tmp315 + _tmp160 * landmarks_5_ + _tmp314 * _tmp48; + const Scalar _tmp320 = _tmp318 * _tmp86 + _tmp319 * _tmp81; + const Scalar _tmp321 = _tmp317 * _tmp76 + _tmp320; + const Scalar _tmp322 = _tmp105 * _tmp318 + _tmp107 * _tmp317; + const Scalar _tmp323 = _tmp103 * _tmp319 + _tmp322; + const Scalar _tmp324 = std::max(_tmp323, epsilon); + const Scalar _tmp325 = Scalar(1.0) / (_tmp324); + const Scalar _tmp326 = _tmp325 * views_1_calibration(0, 0); + const Scalar _tmp327 = + _tmp321 * _tmp326 - matches_0_5_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp328 = _tmp317 * _tmp99 + _tmp319 * _tmp96; + const Scalar _tmp329 = _tmp318 * _tmp93 + _tmp328; + const Scalar _tmp330 = _tmp325 * views_1_calibration(1, 0); const Scalar _tmp331 = - _tmp325 * _tmp330 - matches_0_5_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp332 = _tmp100 * _tmp321 + _tmp323 * _tmp97; - const Scalar _tmp333 = _tmp322 * _tmp94 + _tmp332; - const Scalar _tmp334 = _tmp329 * views_1_calibration(1, 0); - const Scalar _tmp335 = - _tmp333 * _tmp334 - matches_0_5_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp336 = std::pow(_tmp331, Scalar(2)) + std::pow(_tmp335, Scalar(2)) + epsilon; - const Scalar _tmp337 = std::pow(_tmp336, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp338 = std::sqrt(matches_0_5_weight); - const Scalar _tmp339 = std::max(0, (((_tmp327) > 0) - ((_tmp327) < 0))); - const Scalar _tmp340 = _tmp184 * _tmp336 + 1; - const Scalar _tmp341 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp340, _tmp188) - 1))); - const Scalar _tmp342 = _tmp338 * _tmp339 * _tmp341; - const Scalar _tmp343 = _tmp337 * _tmp342; - const Scalar _tmp344 = _tmp331 * _tmp343; - const Scalar _tmp345 = _tmp335 * _tmp343; - const Scalar _tmp346 = _tmp144 + matches_0_6_source_coords(0, 0); - const Scalar _tmp347 = _tmp147 + matches_0_6_source_coords(1, 0); - const Scalar _tmp348 = std::pow(Scalar(_tmp146 * std::pow(_tmp346, Scalar(2)) + - _tmp149 * std::pow(_tmp347, Scalar(2)) + _tmp150), + _tmp329 * _tmp330 - matches_0_5_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp332 = std::pow(_tmp327, Scalar(2)) + std::pow(_tmp331, Scalar(2)) + epsilon; + const Scalar _tmp333 = std::pow(_tmp332, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp334 = std::sqrt(matches_0_5_weight); + const Scalar _tmp335 = std::max(0, (((_tmp323) > 0) - ((_tmp323) < 0))); + const Scalar _tmp336 = _tmp180 * _tmp332 + 1; + const Scalar _tmp337 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp336, _tmp184) - 1))); + const Scalar _tmp338 = _tmp334 * _tmp335 * _tmp337; + const Scalar _tmp339 = _tmp333 * _tmp338; + const Scalar _tmp340 = _tmp327 * _tmp339; + const Scalar _tmp341 = _tmp331 * _tmp339; + const Scalar _tmp342 = _tmp140 + matches_0_6_source_coords(0, 0); + const Scalar _tmp343 = _tmp143 + matches_0_6_source_coords(1, 0); + const Scalar _tmp344 = std::pow(Scalar(_tmp142 * std::pow(_tmp342, Scalar(2)) + + _tmp145 * std::pow(_tmp343, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp349 = _tmp347 * _tmp348; - const Scalar _tmp350 = _tmp346 * _tmp348; - const Scalar _tmp351 = - _tmp143 * _tmp349 + _tmp153 * landmarks_6_ + _tmp155 * _tmp350 + _tmp348 * _tmp51; - const Scalar _tmp352 = - _tmp158 * _tmp350 + _tmp159 * _tmp349 + _tmp160 * landmarks_6_ + _tmp348 * _tmp52; - const Scalar _tmp353 = - _tmp162 * _tmp350 + _tmp163 * _tmp349 + _tmp164 * landmarks_6_ + _tmp348 * _tmp48; - const Scalar _tmp354 = _tmp352 * _tmp87 + _tmp353 * _tmp83; - const Scalar _tmp355 = _tmp351 * _tmp77 + _tmp354; - const Scalar _tmp356 = _tmp106 * _tmp352 + _tmp108 * _tmp351; - const Scalar _tmp357 = _tmp104 * _tmp353 + _tmp356; - const Scalar _tmp358 = std::max(_tmp357, epsilon); - const Scalar _tmp359 = Scalar(1.0) / (_tmp358); - const Scalar _tmp360 = _tmp359 * views_1_calibration(0, 0); + const Scalar _tmp345 = _tmp343 * _tmp344; + const Scalar _tmp346 = _tmp342 * _tmp344; + const Scalar _tmp347 = + _tmp139 * _tmp345 + _tmp149 * landmarks_6_ + _tmp151 * _tmp346 + _tmp344 * _tmp51; + const Scalar _tmp348 = + _tmp154 * _tmp346 + _tmp155 * _tmp345 + _tmp156 * landmarks_6_ + _tmp344 * _tmp52; + const Scalar _tmp349 = + _tmp158 * _tmp346 + _tmp159 * _tmp345 + _tmp160 * landmarks_6_ + _tmp344 * _tmp48; + const Scalar _tmp350 = _tmp348 * _tmp86 + _tmp349 * _tmp81; + const Scalar _tmp351 = _tmp347 * _tmp76 + _tmp350; + const Scalar _tmp352 = _tmp105 * _tmp348 + _tmp107 * _tmp347; + const Scalar _tmp353 = _tmp103 * _tmp349 + _tmp352; + const Scalar _tmp354 = std::max(_tmp353, epsilon); + const Scalar _tmp355 = Scalar(1.0) / (_tmp354); + const Scalar _tmp356 = _tmp355 * views_1_calibration(0, 0); + const Scalar _tmp357 = + _tmp351 * _tmp356 - matches_0_6_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp358 = _tmp347 * _tmp99 + _tmp349 * _tmp96; + const Scalar _tmp359 = _tmp348 * _tmp93 + _tmp358; + const Scalar _tmp360 = _tmp355 * views_1_calibration(1, 0); const Scalar _tmp361 = - _tmp355 * _tmp360 - matches_0_6_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp362 = _tmp100 * _tmp351 + _tmp353 * _tmp97; - const Scalar _tmp363 = _tmp352 * _tmp94 + _tmp362; - const Scalar _tmp364 = _tmp359 * views_1_calibration(1, 0); - const Scalar _tmp365 = - _tmp363 * _tmp364 - matches_0_6_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp366 = std::pow(_tmp361, Scalar(2)) + std::pow(_tmp365, Scalar(2)) + epsilon; - const Scalar _tmp367 = std::pow(_tmp366, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp368 = std::sqrt(matches_0_6_weight); - const Scalar _tmp369 = std::max(0, (((_tmp357) > 0) - ((_tmp357) < 0))); - const Scalar _tmp370 = _tmp184 * _tmp366 + 1; - const Scalar _tmp371 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp370, _tmp188) - 1))); - const Scalar _tmp372 = _tmp368 * _tmp369 * _tmp371; - const Scalar _tmp373 = _tmp367 * _tmp372; - const Scalar _tmp374 = _tmp361 * _tmp373; - const Scalar _tmp375 = _tmp365 * _tmp373; - const Scalar _tmp376 = _tmp147 + matches_0_7_source_coords(1, 0); - const Scalar _tmp377 = _tmp144 + matches_0_7_source_coords(0, 0); - const Scalar _tmp378 = std::pow(Scalar(_tmp146 * std::pow(_tmp377, Scalar(2)) + - _tmp149 * std::pow(_tmp376, Scalar(2)) + _tmp150), + _tmp359 * _tmp360 - matches_0_6_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp362 = std::pow(_tmp357, Scalar(2)) + std::pow(_tmp361, Scalar(2)) + epsilon; + const Scalar _tmp363 = std::pow(_tmp362, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp364 = std::sqrt(matches_0_6_weight); + const Scalar _tmp365 = std::max(0, (((_tmp353) > 0) - ((_tmp353) < 0))); + const Scalar _tmp366 = _tmp180 * _tmp362 + 1; + const Scalar _tmp367 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp366, _tmp184) - 1))); + const Scalar _tmp368 = _tmp364 * _tmp365 * _tmp367; + const Scalar _tmp369 = _tmp363 * _tmp368; + const Scalar _tmp370 = _tmp357 * _tmp369; + const Scalar _tmp371 = _tmp361 * _tmp369; + const Scalar _tmp372 = _tmp143 + matches_0_7_source_coords(1, 0); + const Scalar _tmp373 = _tmp140 + matches_0_7_source_coords(0, 0); + const Scalar _tmp374 = std::pow(Scalar(_tmp142 * std::pow(_tmp373, Scalar(2)) + + _tmp145 * std::pow(_tmp372, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp379 = _tmp376 * _tmp378; - const Scalar _tmp380 = _tmp377 * _tmp378; - const Scalar _tmp381 = - _tmp143 * _tmp379 + _tmp153 * landmarks_7_ + _tmp155 * _tmp380 + _tmp378 * _tmp51; - const Scalar _tmp382 = - _tmp158 * _tmp380 + _tmp159 * _tmp379 + _tmp160 * landmarks_7_ + _tmp378 * _tmp52; - const Scalar _tmp383 = - _tmp162 * _tmp380 + _tmp163 * _tmp379 + _tmp164 * landmarks_7_ + _tmp378 * _tmp48; - const Scalar _tmp384 = _tmp382 * _tmp87 + _tmp383 * _tmp83; - const Scalar _tmp385 = _tmp381 * _tmp77 + _tmp384; - const Scalar _tmp386 = _tmp106 * _tmp382 + _tmp108 * _tmp381; - const Scalar _tmp387 = _tmp104 * _tmp383 + _tmp386; - const Scalar _tmp388 = std::max(_tmp387, epsilon); - const Scalar _tmp389 = Scalar(1.0) / (_tmp388); - const Scalar _tmp390 = _tmp389 * views_1_calibration(0, 0); + const Scalar _tmp375 = _tmp372 * _tmp374; + const Scalar _tmp376 = _tmp373 * _tmp374; + const Scalar _tmp377 = + _tmp139 * _tmp375 + _tmp149 * landmarks_7_ + _tmp151 * _tmp376 + _tmp374 * _tmp51; + const Scalar _tmp378 = + _tmp154 * _tmp376 + _tmp155 * _tmp375 + _tmp156 * landmarks_7_ + _tmp374 * _tmp52; + const Scalar _tmp379 = + _tmp158 * _tmp376 + _tmp159 * _tmp375 + _tmp160 * landmarks_7_ + _tmp374 * _tmp48; + const Scalar _tmp380 = _tmp378 * _tmp86 + _tmp379 * _tmp81; + const Scalar _tmp381 = _tmp377 * _tmp76 + _tmp380; + const Scalar _tmp382 = _tmp105 * _tmp378 + _tmp107 * _tmp377; + const Scalar _tmp383 = _tmp103 * _tmp379 + _tmp382; + const Scalar _tmp384 = std::max(_tmp383, epsilon); + const Scalar _tmp385 = Scalar(1.0) / (_tmp384); + const Scalar _tmp386 = _tmp385 * views_1_calibration(0, 0); + const Scalar _tmp387 = + _tmp381 * _tmp386 - matches_0_7_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp388 = _tmp377 * _tmp99 + _tmp379 * _tmp96; + const Scalar _tmp389 = _tmp378 * _tmp93 + _tmp388; + const Scalar _tmp390 = _tmp385 * views_1_calibration(1, 0); const Scalar _tmp391 = - _tmp385 * _tmp390 - matches_0_7_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp392 = _tmp100 * _tmp381 + _tmp383 * _tmp97; - const Scalar _tmp393 = _tmp382 * _tmp94 + _tmp392; - const Scalar _tmp394 = _tmp389 * views_1_calibration(1, 0); - const Scalar _tmp395 = - _tmp393 * _tmp394 - matches_0_7_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp396 = std::pow(_tmp391, Scalar(2)) + std::pow(_tmp395, Scalar(2)) + epsilon; - const Scalar _tmp397 = std::pow(_tmp396, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp398 = std::sqrt(matches_0_7_weight); - const Scalar _tmp399 = std::max(0, (((_tmp387) > 0) - ((_tmp387) < 0))); - const Scalar _tmp400 = _tmp184 * _tmp396 + 1; - const Scalar _tmp401 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp400, _tmp188) - 1))); - const Scalar _tmp402 = _tmp398 * _tmp399 * _tmp401; - const Scalar _tmp403 = _tmp397 * _tmp402; - const Scalar _tmp404 = _tmp391 * _tmp403; - const Scalar _tmp405 = _tmp395 * _tmp403; - const Scalar _tmp406 = _tmp147 + matches_0_8_source_coords(1, 0); - const Scalar _tmp407 = _tmp144 + matches_0_8_source_coords(0, 0); - const Scalar _tmp408 = std::pow(Scalar(_tmp146 * std::pow(_tmp407, Scalar(2)) + - _tmp149 * std::pow(_tmp406, Scalar(2)) + _tmp150), + _tmp389 * _tmp390 - matches_0_7_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp392 = std::pow(_tmp387, Scalar(2)) + std::pow(_tmp391, Scalar(2)) + epsilon; + const Scalar _tmp393 = std::pow(_tmp392, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp394 = std::sqrt(matches_0_7_weight); + const Scalar _tmp395 = std::max(0, (((_tmp383) > 0) - ((_tmp383) < 0))); + const Scalar _tmp396 = _tmp180 * _tmp392 + 1; + const Scalar _tmp397 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp396, _tmp184) - 1))); + const Scalar _tmp398 = _tmp394 * _tmp395 * _tmp397; + const Scalar _tmp399 = _tmp393 * _tmp398; + const Scalar _tmp400 = _tmp387 * _tmp399; + const Scalar _tmp401 = _tmp391 * _tmp399; + const Scalar _tmp402 = _tmp143 + matches_0_8_source_coords(1, 0); + const Scalar _tmp403 = _tmp140 + matches_0_8_source_coords(0, 0); + const Scalar _tmp404 = std::pow(Scalar(_tmp142 * std::pow(_tmp403, Scalar(2)) + + _tmp145 * std::pow(_tmp402, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp409 = _tmp406 * _tmp408; - const Scalar _tmp410 = _tmp407 * _tmp408; - const Scalar _tmp411 = - _tmp143 * _tmp409 + _tmp153 * landmarks_8_ + _tmp155 * _tmp410 + _tmp408 * _tmp51; - const Scalar _tmp412 = - _tmp158 * _tmp410 + _tmp159 * _tmp409 + _tmp160 * landmarks_8_ + _tmp408 * _tmp52; - const Scalar _tmp413 = - _tmp162 * _tmp410 + _tmp163 * _tmp409 + _tmp164 * landmarks_8_ + _tmp408 * _tmp48; - const Scalar _tmp414 = _tmp412 * _tmp87 + _tmp413 * _tmp83; - const Scalar _tmp415 = _tmp411 * _tmp77 + _tmp414; - const Scalar _tmp416 = _tmp106 * _tmp412 + _tmp108 * _tmp411; - const Scalar _tmp417 = _tmp104 * _tmp413 + _tmp416; - const Scalar _tmp418 = std::max(_tmp417, epsilon); - const Scalar _tmp419 = Scalar(1.0) / (_tmp418); - const Scalar _tmp420 = _tmp419 * views_1_calibration(0, 0); + const Scalar _tmp405 = _tmp402 * _tmp404; + const Scalar _tmp406 = _tmp403 * _tmp404; + const Scalar _tmp407 = + _tmp139 * _tmp405 + _tmp149 * landmarks_8_ + _tmp151 * _tmp406 + _tmp404 * _tmp51; + const Scalar _tmp408 = + _tmp154 * _tmp406 + _tmp155 * _tmp405 + _tmp156 * landmarks_8_ + _tmp404 * _tmp52; + const Scalar _tmp409 = + _tmp158 * _tmp406 + _tmp159 * _tmp405 + _tmp160 * landmarks_8_ + _tmp404 * _tmp48; + const Scalar _tmp410 = _tmp408 * _tmp86 + _tmp409 * _tmp81; + const Scalar _tmp411 = _tmp407 * _tmp76 + _tmp410; + const Scalar _tmp412 = _tmp105 * _tmp408 + _tmp107 * _tmp407; + const Scalar _tmp413 = _tmp103 * _tmp409 + _tmp412; + const Scalar _tmp414 = std::max(_tmp413, epsilon); + const Scalar _tmp415 = Scalar(1.0) / (_tmp414); + const Scalar _tmp416 = _tmp415 * views_1_calibration(0, 0); + const Scalar _tmp417 = + _tmp411 * _tmp416 - matches_0_8_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp418 = _tmp407 * _tmp99 + _tmp409 * _tmp96; + const Scalar _tmp419 = _tmp408 * _tmp93 + _tmp418; + const Scalar _tmp420 = _tmp415 * views_1_calibration(1, 0); const Scalar _tmp421 = - _tmp415 * _tmp420 - matches_0_8_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp422 = _tmp100 * _tmp411 + _tmp413 * _tmp97; - const Scalar _tmp423 = _tmp412 * _tmp94 + _tmp422; - const Scalar _tmp424 = _tmp419 * views_1_calibration(1, 0); - const Scalar _tmp425 = - _tmp423 * _tmp424 - matches_0_8_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp426 = std::pow(_tmp421, Scalar(2)) + std::pow(_tmp425, Scalar(2)) + epsilon; - const Scalar _tmp427 = std::pow(_tmp426, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp428 = std::sqrt(matches_0_8_weight); - const Scalar _tmp429 = _tmp184 * _tmp426 + 1; - const Scalar _tmp430 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp429, _tmp188) - 1))); - const Scalar _tmp431 = std::max(0, (((_tmp417) > 0) - ((_tmp417) < 0))); - const Scalar _tmp432 = _tmp428 * _tmp430 * _tmp431; - const Scalar _tmp433 = _tmp427 * _tmp432; - const Scalar _tmp434 = _tmp421 * _tmp433; - const Scalar _tmp435 = _tmp425 * _tmp433; - const Scalar _tmp436 = _tmp147 + matches_0_9_source_coords(1, 0); - const Scalar _tmp437 = _tmp144 + matches_0_9_source_coords(0, 0); - const Scalar _tmp438 = std::pow(Scalar(_tmp146 * std::pow(_tmp437, Scalar(2)) + - _tmp149 * std::pow(_tmp436, Scalar(2)) + _tmp150), + _tmp419 * _tmp420 - matches_0_8_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp422 = std::pow(_tmp417, Scalar(2)) + std::pow(_tmp421, Scalar(2)) + epsilon; + const Scalar _tmp423 = std::pow(_tmp422, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp424 = std::sqrt(matches_0_8_weight); + const Scalar _tmp425 = _tmp180 * _tmp422 + 1; + const Scalar _tmp426 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp425, _tmp184) - 1))); + const Scalar _tmp427 = std::max(0, (((_tmp413) > 0) - ((_tmp413) < 0))); + const Scalar _tmp428 = _tmp424 * _tmp426 * _tmp427; + const Scalar _tmp429 = _tmp423 * _tmp428; + const Scalar _tmp430 = _tmp417 * _tmp429; + const Scalar _tmp431 = _tmp421 * _tmp429; + const Scalar _tmp432 = _tmp143 + matches_0_9_source_coords(1, 0); + const Scalar _tmp433 = _tmp140 + matches_0_9_source_coords(0, 0); + const Scalar _tmp434 = std::pow(Scalar(_tmp142 * std::pow(_tmp433, Scalar(2)) + + _tmp145 * std::pow(_tmp432, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp439 = _tmp436 * _tmp438; - const Scalar _tmp440 = _tmp437 * _tmp438; - const Scalar _tmp441 = - _tmp143 * _tmp439 + _tmp153 * landmarks_9_ + _tmp155 * _tmp440 + _tmp438 * _tmp51; - const Scalar _tmp442 = - _tmp158 * _tmp440 + _tmp159 * _tmp439 + _tmp160 * landmarks_9_ + _tmp438 * _tmp52; - const Scalar _tmp443 = - _tmp162 * _tmp440 + _tmp163 * _tmp439 + _tmp164 * landmarks_9_ + _tmp438 * _tmp48; - const Scalar _tmp444 = _tmp442 * _tmp87 + _tmp443 * _tmp83; - const Scalar _tmp445 = _tmp441 * _tmp77 + _tmp444; - const Scalar _tmp446 = _tmp106 * _tmp442 + _tmp108 * _tmp441; - const Scalar _tmp447 = _tmp104 * _tmp443 + _tmp446; - const Scalar _tmp448 = std::max(_tmp447, epsilon); - const Scalar _tmp449 = Scalar(1.0) / (_tmp448); - const Scalar _tmp450 = _tmp449 * views_1_calibration(0, 0); + const Scalar _tmp435 = _tmp432 * _tmp434; + const Scalar _tmp436 = _tmp433 * _tmp434; + const Scalar _tmp437 = + _tmp139 * _tmp435 + _tmp149 * landmarks_9_ + _tmp151 * _tmp436 + _tmp434 * _tmp51; + const Scalar _tmp438 = + _tmp154 * _tmp436 + _tmp155 * _tmp435 + _tmp156 * landmarks_9_ + _tmp434 * _tmp52; + const Scalar _tmp439 = + _tmp158 * _tmp436 + _tmp159 * _tmp435 + _tmp160 * landmarks_9_ + _tmp434 * _tmp48; + const Scalar _tmp440 = _tmp438 * _tmp86 + _tmp439 * _tmp81; + const Scalar _tmp441 = _tmp437 * _tmp76 + _tmp440; + const Scalar _tmp442 = _tmp105 * _tmp438 + _tmp107 * _tmp437; + const Scalar _tmp443 = _tmp103 * _tmp439 + _tmp442; + const Scalar _tmp444 = std::max(_tmp443, epsilon); + const Scalar _tmp445 = Scalar(1.0) / (_tmp444); + const Scalar _tmp446 = _tmp445 * views_1_calibration(0, 0); + const Scalar _tmp447 = + _tmp441 * _tmp446 - matches_0_9_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp448 = _tmp437 * _tmp99 + _tmp439 * _tmp96; + const Scalar _tmp449 = _tmp438 * _tmp93 + _tmp448; + const Scalar _tmp450 = _tmp445 * views_1_calibration(1, 0); const Scalar _tmp451 = - _tmp445 * _tmp450 - matches_0_9_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp452 = _tmp100 * _tmp441 + _tmp443 * _tmp97; - const Scalar _tmp453 = _tmp442 * _tmp94 + _tmp452; - const Scalar _tmp454 = _tmp449 * views_1_calibration(1, 0); - const Scalar _tmp455 = - _tmp453 * _tmp454 - matches_0_9_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp456 = std::pow(_tmp451, Scalar(2)) + std::pow(_tmp455, Scalar(2)) + epsilon; - const Scalar _tmp457 = std::pow(_tmp456, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp458 = std::sqrt(matches_0_9_weight); - const Scalar _tmp459 = std::max(0, (((_tmp447) > 0) - ((_tmp447) < 0))); - const Scalar _tmp460 = _tmp184 * _tmp456 + 1; - const Scalar _tmp461 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp460, _tmp188) - 1))); - const Scalar _tmp462 = _tmp458 * _tmp459 * _tmp461; - const Scalar _tmp463 = _tmp457 * _tmp462; - const Scalar _tmp464 = _tmp451 * _tmp463; - const Scalar _tmp465 = _tmp455 * _tmp463; - const Scalar _tmp466 = _tmp147 + matches_0_10_source_coords(1, 0); - const Scalar _tmp467 = _tmp144 + matches_0_10_source_coords(0, 0); - const Scalar _tmp468 = std::pow(Scalar(_tmp146 * std::pow(_tmp467, Scalar(2)) + - _tmp149 * std::pow(_tmp466, Scalar(2)) + _tmp150), + _tmp449 * _tmp450 - matches_0_9_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp452 = std::pow(_tmp447, Scalar(2)) + std::pow(_tmp451, Scalar(2)) + epsilon; + const Scalar _tmp453 = std::pow(_tmp452, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp454 = std::sqrt(matches_0_9_weight); + const Scalar _tmp455 = std::max(0, (((_tmp443) > 0) - ((_tmp443) < 0))); + const Scalar _tmp456 = _tmp180 * _tmp452 + 1; + const Scalar _tmp457 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp456, _tmp184) - 1))); + const Scalar _tmp458 = _tmp454 * _tmp455 * _tmp457; + const Scalar _tmp459 = _tmp453 * _tmp458; + const Scalar _tmp460 = _tmp447 * _tmp459; + const Scalar _tmp461 = _tmp451 * _tmp459; + const Scalar _tmp462 = _tmp143 + matches_0_10_source_coords(1, 0); + const Scalar _tmp463 = _tmp140 + matches_0_10_source_coords(0, 0); + const Scalar _tmp464 = std::pow(Scalar(_tmp142 * std::pow(_tmp463, Scalar(2)) + + _tmp145 * std::pow(_tmp462, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp469 = _tmp466 * _tmp468; - const Scalar _tmp470 = _tmp467 * _tmp468; - const Scalar _tmp471 = - _tmp143 * _tmp469 + _tmp153 * landmarks_10_ + _tmp155 * _tmp470 + _tmp468 * _tmp51; - const Scalar _tmp472 = - _tmp162 * _tmp470 + _tmp163 * _tmp469 + _tmp164 * landmarks_10_ + _tmp468 * _tmp48; - const Scalar _tmp473 = - _tmp158 * _tmp470 + _tmp159 * _tmp469 + _tmp160 * landmarks_10_ + _tmp468 * _tmp52; - const Scalar _tmp474 = _tmp472 * _tmp83 + _tmp473 * _tmp87; - const Scalar _tmp475 = _tmp471 * _tmp77 + _tmp474; - const Scalar _tmp476 = _tmp106 * _tmp473 + _tmp108 * _tmp471; - const Scalar _tmp477 = _tmp104 * _tmp472 + _tmp476; - const Scalar _tmp478 = std::max(_tmp477, epsilon); - const Scalar _tmp479 = Scalar(1.0) / (_tmp478); - const Scalar _tmp480 = _tmp479 * views_1_calibration(0, 0); + const Scalar _tmp465 = _tmp462 * _tmp464; + const Scalar _tmp466 = _tmp463 * _tmp464; + const Scalar _tmp467 = + _tmp139 * _tmp465 + _tmp149 * landmarks_10_ + _tmp151 * _tmp466 + _tmp464 * _tmp51; + const Scalar _tmp468 = + _tmp158 * _tmp466 + _tmp159 * _tmp465 + _tmp160 * landmarks_10_ + _tmp464 * _tmp48; + const Scalar _tmp469 = + _tmp154 * _tmp466 + _tmp155 * _tmp465 + _tmp156 * landmarks_10_ + _tmp464 * _tmp52; + const Scalar _tmp470 = _tmp468 * _tmp81 + _tmp469 * _tmp86; + const Scalar _tmp471 = _tmp467 * _tmp76 + _tmp470; + const Scalar _tmp472 = _tmp105 * _tmp469 + _tmp107 * _tmp467; + const Scalar _tmp473 = _tmp103 * _tmp468 + _tmp472; + const Scalar _tmp474 = std::max(_tmp473, epsilon); + const Scalar _tmp475 = Scalar(1.0) / (_tmp474); + const Scalar _tmp476 = _tmp475 * views_1_calibration(0, 0); + const Scalar _tmp477 = + _tmp471 * _tmp476 - matches_0_10_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp478 = _tmp467 * _tmp99 + _tmp468 * _tmp96; + const Scalar _tmp479 = _tmp469 * _tmp93 + _tmp478; + const Scalar _tmp480 = _tmp475 * views_1_calibration(1, 0); const Scalar _tmp481 = - _tmp475 * _tmp480 - matches_0_10_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp482 = _tmp100 * _tmp471 + _tmp472 * _tmp97; - const Scalar _tmp483 = _tmp473 * _tmp94 + _tmp482; - const Scalar _tmp484 = _tmp479 * views_1_calibration(1, 0); - const Scalar _tmp485 = - _tmp483 * _tmp484 - matches_0_10_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp486 = std::pow(_tmp481, Scalar(2)) + std::pow(_tmp485, Scalar(2)) + epsilon; - const Scalar _tmp487 = std::pow(_tmp486, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp488 = std::sqrt(matches_0_10_weight); - const Scalar _tmp489 = _tmp184 * _tmp486 + 1; - const Scalar _tmp490 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp489, _tmp188) - 1))); - const Scalar _tmp491 = std::max(0, (((_tmp477) > 0) - ((_tmp477) < 0))); - const Scalar _tmp492 = _tmp488 * _tmp490 * _tmp491; - const Scalar _tmp493 = _tmp487 * _tmp492; - const Scalar _tmp494 = _tmp481 * _tmp493; - const Scalar _tmp495 = _tmp485 * _tmp493; - const Scalar _tmp496 = _tmp147 + matches_0_11_source_coords(1, 0); - const Scalar _tmp497 = _tmp144 + matches_0_11_source_coords(0, 0); - const Scalar _tmp498 = std::pow(Scalar(_tmp146 * std::pow(_tmp497, Scalar(2)) + - _tmp149 * std::pow(_tmp496, Scalar(2)) + _tmp150), + _tmp479 * _tmp480 - matches_0_10_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp482 = std::pow(_tmp477, Scalar(2)) + std::pow(_tmp481, Scalar(2)) + epsilon; + const Scalar _tmp483 = std::pow(_tmp482, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp484 = std::sqrt(matches_0_10_weight); + const Scalar _tmp485 = _tmp180 * _tmp482 + 1; + const Scalar _tmp486 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp485, _tmp184) - 1))); + const Scalar _tmp487 = std::max(0, (((_tmp473) > 0) - ((_tmp473) < 0))); + const Scalar _tmp488 = _tmp484 * _tmp486 * _tmp487; + const Scalar _tmp489 = _tmp483 * _tmp488; + const Scalar _tmp490 = _tmp477 * _tmp489; + const Scalar _tmp491 = _tmp481 * _tmp489; + const Scalar _tmp492 = _tmp143 + matches_0_11_source_coords(1, 0); + const Scalar _tmp493 = _tmp140 + matches_0_11_source_coords(0, 0); + const Scalar _tmp494 = std::pow(Scalar(_tmp142 * std::pow(_tmp493, Scalar(2)) + + _tmp145 * std::pow(_tmp492, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp499 = _tmp496 * _tmp498; - const Scalar _tmp500 = _tmp497 * _tmp498; - const Scalar _tmp501 = - _tmp143 * _tmp499 + _tmp153 * landmarks_11_ + _tmp155 * _tmp500 + _tmp498 * _tmp51; - const Scalar _tmp502 = - _tmp158 * _tmp500 + _tmp159 * _tmp499 + _tmp160 * landmarks_11_ + _tmp498 * _tmp52; - const Scalar _tmp503 = - _tmp162 * _tmp500 + _tmp163 * _tmp499 + _tmp164 * landmarks_11_ + _tmp48 * _tmp498; - const Scalar _tmp504 = _tmp502 * _tmp87 + _tmp503 * _tmp83; - const Scalar _tmp505 = _tmp501 * _tmp77 + _tmp504; - const Scalar _tmp506 = _tmp106 * _tmp502 + _tmp108 * _tmp501; - const Scalar _tmp507 = _tmp104 * _tmp503 + _tmp506; - const Scalar _tmp508 = std::max(_tmp507, epsilon); - const Scalar _tmp509 = Scalar(1.0) / (_tmp508); - const Scalar _tmp510 = _tmp509 * views_1_calibration(0, 0); + const Scalar _tmp495 = _tmp492 * _tmp494; + const Scalar _tmp496 = _tmp493 * _tmp494; + const Scalar _tmp497 = + _tmp139 * _tmp495 + _tmp149 * landmarks_11_ + _tmp151 * _tmp496 + _tmp494 * _tmp51; + const Scalar _tmp498 = + _tmp154 * _tmp496 + _tmp155 * _tmp495 + _tmp156 * landmarks_11_ + _tmp494 * _tmp52; + const Scalar _tmp499 = + _tmp158 * _tmp496 + _tmp159 * _tmp495 + _tmp160 * landmarks_11_ + _tmp48 * _tmp494; + const Scalar _tmp500 = _tmp498 * _tmp86 + _tmp499 * _tmp81; + const Scalar _tmp501 = _tmp497 * _tmp76 + _tmp500; + const Scalar _tmp502 = _tmp105 * _tmp498 + _tmp107 * _tmp497; + const Scalar _tmp503 = _tmp103 * _tmp499 + _tmp502; + const Scalar _tmp504 = std::max(_tmp503, epsilon); + const Scalar _tmp505 = Scalar(1.0) / (_tmp504); + const Scalar _tmp506 = _tmp505 * views_1_calibration(0, 0); + const Scalar _tmp507 = + _tmp501 * _tmp506 - matches_0_11_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp508 = _tmp497 * _tmp99 + _tmp499 * _tmp96; + const Scalar _tmp509 = _tmp498 * _tmp93 + _tmp508; + const Scalar _tmp510 = _tmp505 * views_1_calibration(1, 0); const Scalar _tmp511 = - _tmp505 * _tmp510 - matches_0_11_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp512 = _tmp100 * _tmp501 + _tmp503 * _tmp97; - const Scalar _tmp513 = _tmp502 * _tmp94 + _tmp512; - const Scalar _tmp514 = _tmp509 * views_1_calibration(1, 0); - const Scalar _tmp515 = - _tmp513 * _tmp514 - matches_0_11_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp516 = std::pow(_tmp511, Scalar(2)) + std::pow(_tmp515, Scalar(2)) + epsilon; - const Scalar _tmp517 = std::pow(_tmp516, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp518 = _tmp184 * _tmp516 + 1; - const Scalar _tmp519 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp518, _tmp188) - 1))); - const Scalar _tmp520 = std::sqrt(matches_0_11_weight); - const Scalar _tmp521 = std::max(0, (((_tmp507) > 0) - ((_tmp507) < 0))); - const Scalar _tmp522 = _tmp519 * _tmp520 * _tmp521; - const Scalar _tmp523 = _tmp517 * _tmp522; - const Scalar _tmp524 = _tmp511 * _tmp523; - const Scalar _tmp525 = _tmp515 * _tmp523; - const Scalar _tmp526 = _tmp147 + matches_0_12_source_coords(1, 0); - const Scalar _tmp527 = _tmp144 + matches_0_12_source_coords(0, 0); - const Scalar _tmp528 = std::pow(Scalar(_tmp146 * std::pow(_tmp527, Scalar(2)) + - _tmp149 * std::pow(_tmp526, Scalar(2)) + _tmp150), + _tmp509 * _tmp510 - matches_0_11_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp512 = std::pow(_tmp507, Scalar(2)) + std::pow(_tmp511, Scalar(2)) + epsilon; + const Scalar _tmp513 = std::pow(_tmp512, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp514 = _tmp180 * _tmp512 + 1; + const Scalar _tmp515 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp514, _tmp184) - 1))); + const Scalar _tmp516 = std::sqrt(matches_0_11_weight); + const Scalar _tmp517 = std::max(0, (((_tmp503) > 0) - ((_tmp503) < 0))); + const Scalar _tmp518 = _tmp515 * _tmp516 * _tmp517; + const Scalar _tmp519 = _tmp513 * _tmp518; + const Scalar _tmp520 = _tmp507 * _tmp519; + const Scalar _tmp521 = _tmp511 * _tmp519; + const Scalar _tmp522 = _tmp143 + matches_0_12_source_coords(1, 0); + const Scalar _tmp523 = _tmp140 + matches_0_12_source_coords(0, 0); + const Scalar _tmp524 = std::pow(Scalar(_tmp142 * std::pow(_tmp523, Scalar(2)) + + _tmp145 * std::pow(_tmp522, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp529 = _tmp526 * _tmp528; - const Scalar _tmp530 = _tmp527 * _tmp528; - const Scalar _tmp531 = - _tmp143 * _tmp529 + _tmp153 * landmarks_12_ + _tmp155 * _tmp530 + _tmp51 * _tmp528; - const Scalar _tmp532 = - _tmp162 * _tmp530 + _tmp163 * _tmp529 + _tmp164 * landmarks_12_ + _tmp48 * _tmp528; - const Scalar _tmp533 = - _tmp158 * _tmp530 + _tmp159 * _tmp529 + _tmp160 * landmarks_12_ + _tmp52 * _tmp528; - const Scalar _tmp534 = _tmp532 * _tmp83 + _tmp533 * _tmp87; - const Scalar _tmp535 = _tmp531 * _tmp77 + _tmp534; - const Scalar _tmp536 = _tmp106 * _tmp533 + _tmp108 * _tmp531; - const Scalar _tmp537 = _tmp104 * _tmp532 + _tmp536; - const Scalar _tmp538 = std::max(_tmp537, epsilon); - const Scalar _tmp539 = Scalar(1.0) / (_tmp538); - const Scalar _tmp540 = _tmp539 * views_1_calibration(0, 0); + const Scalar _tmp525 = _tmp522 * _tmp524; + const Scalar _tmp526 = _tmp523 * _tmp524; + const Scalar _tmp527 = + _tmp139 * _tmp525 + _tmp149 * landmarks_12_ + _tmp151 * _tmp526 + _tmp51 * _tmp524; + const Scalar _tmp528 = + _tmp158 * _tmp526 + _tmp159 * _tmp525 + _tmp160 * landmarks_12_ + _tmp48 * _tmp524; + const Scalar _tmp529 = + _tmp154 * _tmp526 + _tmp155 * _tmp525 + _tmp156 * landmarks_12_ + _tmp52 * _tmp524; + const Scalar _tmp530 = _tmp528 * _tmp81 + _tmp529 * _tmp86; + const Scalar _tmp531 = _tmp527 * _tmp76 + _tmp530; + const Scalar _tmp532 = _tmp105 * _tmp529 + _tmp107 * _tmp527; + const Scalar _tmp533 = _tmp103 * _tmp528 + _tmp532; + const Scalar _tmp534 = std::max(_tmp533, epsilon); + const Scalar _tmp535 = Scalar(1.0) / (_tmp534); + const Scalar _tmp536 = _tmp535 * views_1_calibration(0, 0); + const Scalar _tmp537 = + _tmp531 * _tmp536 - matches_0_12_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp538 = _tmp527 * _tmp99 + _tmp528 * _tmp96; + const Scalar _tmp539 = _tmp529 * _tmp93 + _tmp538; + const Scalar _tmp540 = _tmp535 * views_1_calibration(1, 0); const Scalar _tmp541 = - _tmp535 * _tmp540 - matches_0_12_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp542 = _tmp100 * _tmp531 + _tmp532 * _tmp97; - const Scalar _tmp543 = _tmp533 * _tmp94 + _tmp542; - const Scalar _tmp544 = _tmp539 * views_1_calibration(1, 0); - const Scalar _tmp545 = - _tmp543 * _tmp544 - matches_0_12_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp546 = std::pow(_tmp541, Scalar(2)) + std::pow(_tmp545, Scalar(2)) + epsilon; - const Scalar _tmp547 = std::pow(_tmp546, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp548 = _tmp184 * _tmp546 + 1; - const Scalar _tmp549 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp548, _tmp188) - 1))); - const Scalar _tmp550 = std::sqrt(matches_0_12_weight); - const Scalar _tmp551 = std::max(0, (((_tmp537) > 0) - ((_tmp537) < 0))); - const Scalar _tmp552 = _tmp549 * _tmp550 * _tmp551; - const Scalar _tmp553 = _tmp547 * _tmp552; - const Scalar _tmp554 = _tmp541 * _tmp553; - const Scalar _tmp555 = _tmp545 * _tmp553; - const Scalar _tmp556 = _tmp147 + matches_0_13_source_coords(1, 0); - const Scalar _tmp557 = _tmp144 + matches_0_13_source_coords(0, 0); - const Scalar _tmp558 = std::pow(Scalar(_tmp146 * std::pow(_tmp557, Scalar(2)) + - _tmp149 * std::pow(_tmp556, Scalar(2)) + _tmp150), + _tmp539 * _tmp540 - matches_0_12_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp542 = std::pow(_tmp537, Scalar(2)) + std::pow(_tmp541, Scalar(2)) + epsilon; + const Scalar _tmp543 = std::pow(_tmp542, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp544 = _tmp180 * _tmp542 + 1; + const Scalar _tmp545 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp544, _tmp184) - 1))); + const Scalar _tmp546 = std::sqrt(matches_0_12_weight); + const Scalar _tmp547 = std::max(0, (((_tmp533) > 0) - ((_tmp533) < 0))); + const Scalar _tmp548 = _tmp545 * _tmp546 * _tmp547; + const Scalar _tmp549 = _tmp543 * _tmp548; + const Scalar _tmp550 = _tmp537 * _tmp549; + const Scalar _tmp551 = _tmp541 * _tmp549; + const Scalar _tmp552 = _tmp143 + matches_0_13_source_coords(1, 0); + const Scalar _tmp553 = _tmp140 + matches_0_13_source_coords(0, 0); + const Scalar _tmp554 = std::pow(Scalar(_tmp142 * std::pow(_tmp553, Scalar(2)) + + _tmp145 * std::pow(_tmp552, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp559 = _tmp556 * _tmp558; - const Scalar _tmp560 = _tmp557 * _tmp558; - const Scalar _tmp561 = - _tmp143 * _tmp559 + _tmp153 * landmarks_13_ + _tmp155 * _tmp560 + _tmp51 * _tmp558; - const Scalar _tmp562 = - _tmp158 * _tmp560 + _tmp159 * _tmp559 + _tmp160 * landmarks_13_ + _tmp52 * _tmp558; - const Scalar _tmp563 = - _tmp162 * _tmp560 + _tmp163 * _tmp559 + _tmp164 * landmarks_13_ + _tmp48 * _tmp558; - const Scalar _tmp564 = _tmp562 * _tmp87 + _tmp563 * _tmp83; - const Scalar _tmp565 = _tmp561 * _tmp77 + _tmp564; - const Scalar _tmp566 = _tmp106 * _tmp562 + _tmp108 * _tmp561; - const Scalar _tmp567 = _tmp104 * _tmp563 + _tmp566; - const Scalar _tmp568 = std::max(_tmp567, epsilon); - const Scalar _tmp569 = Scalar(1.0) / (_tmp568); - const Scalar _tmp570 = _tmp569 * views_1_calibration(0, 0); + const Scalar _tmp555 = _tmp552 * _tmp554; + const Scalar _tmp556 = _tmp553 * _tmp554; + const Scalar _tmp557 = + _tmp139 * _tmp555 + _tmp149 * landmarks_13_ + _tmp151 * _tmp556 + _tmp51 * _tmp554; + const Scalar _tmp558 = + _tmp154 * _tmp556 + _tmp155 * _tmp555 + _tmp156 * landmarks_13_ + _tmp52 * _tmp554; + const Scalar _tmp559 = + _tmp158 * _tmp556 + _tmp159 * _tmp555 + _tmp160 * landmarks_13_ + _tmp48 * _tmp554; + const Scalar _tmp560 = _tmp558 * _tmp86 + _tmp559 * _tmp81; + const Scalar _tmp561 = _tmp557 * _tmp76 + _tmp560; + const Scalar _tmp562 = _tmp105 * _tmp558 + _tmp107 * _tmp557; + const Scalar _tmp563 = _tmp103 * _tmp559 + _tmp562; + const Scalar _tmp564 = std::max(_tmp563, epsilon); + const Scalar _tmp565 = Scalar(1.0) / (_tmp564); + const Scalar _tmp566 = _tmp565 * views_1_calibration(0, 0); + const Scalar _tmp567 = + _tmp561 * _tmp566 - matches_0_13_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp568 = _tmp557 * _tmp99 + _tmp559 * _tmp96; + const Scalar _tmp569 = _tmp558 * _tmp93 + _tmp568; + const Scalar _tmp570 = _tmp565 * views_1_calibration(1, 0); const Scalar _tmp571 = - _tmp565 * _tmp570 - matches_0_13_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp572 = _tmp100 * _tmp561 + _tmp563 * _tmp97; - const Scalar _tmp573 = _tmp562 * _tmp94 + _tmp572; - const Scalar _tmp574 = _tmp569 * views_1_calibration(1, 0); - const Scalar _tmp575 = - _tmp573 * _tmp574 - matches_0_13_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp576 = std::pow(_tmp571, Scalar(2)) + std::pow(_tmp575, Scalar(2)) + epsilon; - const Scalar _tmp577 = std::pow(_tmp576, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp578 = _tmp184 * _tmp576 + 1; - const Scalar _tmp579 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp578, _tmp188) - 1))); - const Scalar _tmp580 = std::sqrt(matches_0_13_weight); - const Scalar _tmp581 = std::max(0, (((_tmp567) > 0) - ((_tmp567) < 0))); - const Scalar _tmp582 = _tmp579 * _tmp580 * _tmp581; - const Scalar _tmp583 = _tmp577 * _tmp582; - const Scalar _tmp584 = _tmp571 * _tmp583; - const Scalar _tmp585 = _tmp575 * _tmp583; - const Scalar _tmp586 = _tmp147 + matches_0_14_source_coords(1, 0); - const Scalar _tmp587 = _tmp144 + matches_0_14_source_coords(0, 0); - const Scalar _tmp588 = std::pow(Scalar(_tmp146 * std::pow(_tmp587, Scalar(2)) + - _tmp149 * std::pow(_tmp586, Scalar(2)) + _tmp150), + _tmp569 * _tmp570 - matches_0_13_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp572 = std::pow(_tmp567, Scalar(2)) + std::pow(_tmp571, Scalar(2)) + epsilon; + const Scalar _tmp573 = std::pow(_tmp572, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp574 = _tmp180 * _tmp572 + 1; + const Scalar _tmp575 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp574, _tmp184) - 1))); + const Scalar _tmp576 = std::sqrt(matches_0_13_weight); + const Scalar _tmp577 = std::max(0, (((_tmp563) > 0) - ((_tmp563) < 0))); + const Scalar _tmp578 = _tmp575 * _tmp576 * _tmp577; + const Scalar _tmp579 = _tmp573 * _tmp578; + const Scalar _tmp580 = _tmp567 * _tmp579; + const Scalar _tmp581 = _tmp571 * _tmp579; + const Scalar _tmp582 = _tmp143 + matches_0_14_source_coords(1, 0); + const Scalar _tmp583 = _tmp140 + matches_0_14_source_coords(0, 0); + const Scalar _tmp584 = std::pow(Scalar(_tmp142 * std::pow(_tmp583, Scalar(2)) + + _tmp145 * std::pow(_tmp582, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp589 = _tmp586 * _tmp588; - const Scalar _tmp590 = _tmp587 * _tmp588; - const Scalar _tmp591 = - _tmp143 * _tmp589 + _tmp153 * landmarks_14_ + _tmp155 * _tmp590 + _tmp51 * _tmp588; - const Scalar _tmp592 = - _tmp158 * _tmp590 + _tmp159 * _tmp589 + _tmp160 * landmarks_14_ + _tmp52 * _tmp588; - const Scalar _tmp593 = - _tmp162 * _tmp590 + _tmp163 * _tmp589 + _tmp164 * landmarks_14_ + _tmp48 * _tmp588; - const Scalar _tmp594 = _tmp592 * _tmp87 + _tmp593 * _tmp83; - const Scalar _tmp595 = _tmp591 * _tmp77 + _tmp594; - const Scalar _tmp596 = _tmp106 * _tmp592 + _tmp108 * _tmp591; - const Scalar _tmp597 = _tmp104 * _tmp593 + _tmp596; - const Scalar _tmp598 = std::max(_tmp597, epsilon); - const Scalar _tmp599 = Scalar(1.0) / (_tmp598); - const Scalar _tmp600 = _tmp599 * views_1_calibration(0, 0); + const Scalar _tmp585 = _tmp582 * _tmp584; + const Scalar _tmp586 = _tmp583 * _tmp584; + const Scalar _tmp587 = + _tmp139 * _tmp585 + _tmp149 * landmarks_14_ + _tmp151 * _tmp586 + _tmp51 * _tmp584; + const Scalar _tmp588 = + _tmp154 * _tmp586 + _tmp155 * _tmp585 + _tmp156 * landmarks_14_ + _tmp52 * _tmp584; + const Scalar _tmp589 = + _tmp158 * _tmp586 + _tmp159 * _tmp585 + _tmp160 * landmarks_14_ + _tmp48 * _tmp584; + const Scalar _tmp590 = _tmp588 * _tmp86 + _tmp589 * _tmp81; + const Scalar _tmp591 = _tmp587 * _tmp76 + _tmp590; + const Scalar _tmp592 = _tmp105 * _tmp588 + _tmp107 * _tmp587; + const Scalar _tmp593 = _tmp103 * _tmp589 + _tmp592; + const Scalar _tmp594 = std::max(_tmp593, epsilon); + const Scalar _tmp595 = Scalar(1.0) / (_tmp594); + const Scalar _tmp596 = _tmp595 * views_1_calibration(0, 0); + const Scalar _tmp597 = + _tmp591 * _tmp596 - matches_0_14_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp598 = _tmp587 * _tmp99 + _tmp589 * _tmp96; + const Scalar _tmp599 = _tmp588 * _tmp93 + _tmp598; + const Scalar _tmp600 = _tmp595 * views_1_calibration(1, 0); const Scalar _tmp601 = - _tmp595 * _tmp600 - matches_0_14_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp602 = _tmp100 * _tmp591 + _tmp593 * _tmp97; - const Scalar _tmp603 = _tmp592 * _tmp94 + _tmp602; - const Scalar _tmp604 = _tmp599 * views_1_calibration(1, 0); - const Scalar _tmp605 = - _tmp603 * _tmp604 - matches_0_14_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp606 = std::pow(_tmp601, Scalar(2)) + std::pow(_tmp605, Scalar(2)) + epsilon; - const Scalar _tmp607 = std::pow(_tmp606, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp608 = std::sqrt(matches_0_14_weight); - const Scalar _tmp609 = _tmp184 * _tmp606 + 1; - const Scalar _tmp610 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp609, _tmp188) - 1))); - const Scalar _tmp611 = std::max(0, (((_tmp597) > 0) - ((_tmp597) < 0))); - const Scalar _tmp612 = _tmp608 * _tmp610 * _tmp611; - const Scalar _tmp613 = _tmp607 * _tmp612; - const Scalar _tmp614 = _tmp601 * _tmp613; - const Scalar _tmp615 = _tmp605 * _tmp613; - const Scalar _tmp616 = _tmp147 + matches_0_15_source_coords(1, 0); - const Scalar _tmp617 = _tmp144 + matches_0_15_source_coords(0, 0); - const Scalar _tmp618 = std::pow(Scalar(_tmp146 * std::pow(_tmp617, Scalar(2)) + - _tmp149 * std::pow(_tmp616, Scalar(2)) + _tmp150), + _tmp599 * _tmp600 - matches_0_14_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp602 = std::pow(_tmp597, Scalar(2)) + std::pow(_tmp601, Scalar(2)) + epsilon; + const Scalar _tmp603 = std::pow(_tmp602, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp604 = std::sqrt(matches_0_14_weight); + const Scalar _tmp605 = _tmp180 * _tmp602 + 1; + const Scalar _tmp606 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp605, _tmp184) - 1))); + const Scalar _tmp607 = std::max(0, (((_tmp593) > 0) - ((_tmp593) < 0))); + const Scalar _tmp608 = _tmp604 * _tmp606 * _tmp607; + const Scalar _tmp609 = _tmp603 * _tmp608; + const Scalar _tmp610 = _tmp597 * _tmp609; + const Scalar _tmp611 = _tmp601 * _tmp609; + const Scalar _tmp612 = _tmp143 + matches_0_15_source_coords(1, 0); + const Scalar _tmp613 = _tmp140 + matches_0_15_source_coords(0, 0); + const Scalar _tmp614 = std::pow(Scalar(_tmp142 * std::pow(_tmp613, Scalar(2)) + + _tmp145 * std::pow(_tmp612, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp619 = _tmp616 * _tmp618; - const Scalar _tmp620 = _tmp617 * _tmp618; - const Scalar _tmp621 = - _tmp143 * _tmp619 + _tmp153 * landmarks_15_ + _tmp155 * _tmp620 + _tmp51 * _tmp618; - const Scalar _tmp622 = - _tmp162 * _tmp620 + _tmp163 * _tmp619 + _tmp164 * landmarks_15_ + _tmp48 * _tmp618; - const Scalar _tmp623 = - _tmp158 * _tmp620 + _tmp159 * _tmp619 + _tmp160 * landmarks_15_ + _tmp52 * _tmp618; - const Scalar _tmp624 = _tmp622 * _tmp83 + _tmp623 * _tmp87; - const Scalar _tmp625 = _tmp621 * _tmp77 + _tmp624; - const Scalar _tmp626 = _tmp106 * _tmp623 + _tmp108 * _tmp621; - const Scalar _tmp627 = _tmp104 * _tmp622 + _tmp626; - const Scalar _tmp628 = std::max(_tmp627, epsilon); - const Scalar _tmp629 = Scalar(1.0) / (_tmp628); - const Scalar _tmp630 = _tmp629 * views_1_calibration(0, 0); + const Scalar _tmp615 = _tmp612 * _tmp614; + const Scalar _tmp616 = _tmp613 * _tmp614; + const Scalar _tmp617 = + _tmp139 * _tmp615 + _tmp149 * landmarks_15_ + _tmp151 * _tmp616 + _tmp51 * _tmp614; + const Scalar _tmp618 = + _tmp158 * _tmp616 + _tmp159 * _tmp615 + _tmp160 * landmarks_15_ + _tmp48 * _tmp614; + const Scalar _tmp619 = + _tmp154 * _tmp616 + _tmp155 * _tmp615 + _tmp156 * landmarks_15_ + _tmp52 * _tmp614; + const Scalar _tmp620 = _tmp618 * _tmp81 + _tmp619 * _tmp86; + const Scalar _tmp621 = _tmp617 * _tmp76 + _tmp620; + const Scalar _tmp622 = _tmp105 * _tmp619 + _tmp107 * _tmp617; + const Scalar _tmp623 = _tmp103 * _tmp618 + _tmp622; + const Scalar _tmp624 = std::max(_tmp623, epsilon); + const Scalar _tmp625 = Scalar(1.0) / (_tmp624); + const Scalar _tmp626 = _tmp625 * views_1_calibration(0, 0); + const Scalar _tmp627 = + _tmp621 * _tmp626 - matches_0_15_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp628 = _tmp617 * _tmp99 + _tmp618 * _tmp96; + const Scalar _tmp629 = _tmp619 * _tmp93 + _tmp628; + const Scalar _tmp630 = _tmp625 * views_1_calibration(1, 0); const Scalar _tmp631 = - _tmp625 * _tmp630 - matches_0_15_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp632 = _tmp100 * _tmp621 + _tmp622 * _tmp97; - const Scalar _tmp633 = _tmp623 * _tmp94 + _tmp632; - const Scalar _tmp634 = _tmp629 * views_1_calibration(1, 0); - const Scalar _tmp635 = - _tmp633 * _tmp634 - matches_0_15_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp636 = std::pow(_tmp631, Scalar(2)) + std::pow(_tmp635, Scalar(2)) + epsilon; - const Scalar _tmp637 = std::pow(_tmp636, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp638 = _tmp184 * _tmp636 + 1; - const Scalar _tmp639 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp638, _tmp188) - 1))); - const Scalar _tmp640 = std::sqrt(matches_0_15_weight); - const Scalar _tmp641 = std::max(0, (((_tmp627) > 0) - ((_tmp627) < 0))); - const Scalar _tmp642 = _tmp639 * _tmp640 * _tmp641; - const Scalar _tmp643 = _tmp637 * _tmp642; - const Scalar _tmp644 = _tmp631 * _tmp643; - const Scalar _tmp645 = _tmp635 * _tmp643; - const Scalar _tmp646 = _tmp147 + matches_0_16_source_coords(1, 0); - const Scalar _tmp647 = _tmp144 + matches_0_16_source_coords(0, 0); - const Scalar _tmp648 = std::pow(Scalar(_tmp146 * std::pow(_tmp647, Scalar(2)) + - _tmp149 * std::pow(_tmp646, Scalar(2)) + _tmp150), + _tmp629 * _tmp630 - matches_0_15_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp632 = std::pow(_tmp627, Scalar(2)) + std::pow(_tmp631, Scalar(2)) + epsilon; + const Scalar _tmp633 = std::pow(_tmp632, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp634 = _tmp180 * _tmp632 + 1; + const Scalar _tmp635 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp634, _tmp184) - 1))); + const Scalar _tmp636 = std::sqrt(matches_0_15_weight); + const Scalar _tmp637 = std::max(0, (((_tmp623) > 0) - ((_tmp623) < 0))); + const Scalar _tmp638 = _tmp635 * _tmp636 * _tmp637; + const Scalar _tmp639 = _tmp633 * _tmp638; + const Scalar _tmp640 = _tmp627 * _tmp639; + const Scalar _tmp641 = _tmp631 * _tmp639; + const Scalar _tmp642 = _tmp143 + matches_0_16_source_coords(1, 0); + const Scalar _tmp643 = _tmp140 + matches_0_16_source_coords(0, 0); + const Scalar _tmp644 = std::pow(Scalar(_tmp142 * std::pow(_tmp643, Scalar(2)) + + _tmp145 * std::pow(_tmp642, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp649 = _tmp646 * _tmp648; - const Scalar _tmp650 = _tmp647 * _tmp648; - const Scalar _tmp651 = - _tmp143 * _tmp649 + _tmp153 * landmarks_16_ + _tmp155 * _tmp650 + _tmp51 * _tmp648; - const Scalar _tmp652 = - _tmp158 * _tmp650 + _tmp159 * _tmp649 + _tmp160 * landmarks_16_ + _tmp52 * _tmp648; - const Scalar _tmp653 = - _tmp162 * _tmp650 + _tmp163 * _tmp649 + _tmp164 * landmarks_16_ + _tmp48 * _tmp648; - const Scalar _tmp654 = _tmp652 * _tmp87 + _tmp653 * _tmp83; - const Scalar _tmp655 = _tmp651 * _tmp77 + _tmp654; - const Scalar _tmp656 = _tmp106 * _tmp652 + _tmp108 * _tmp651; - const Scalar _tmp657 = _tmp104 * _tmp653 + _tmp656; - const Scalar _tmp658 = std::max(_tmp657, epsilon); - const Scalar _tmp659 = Scalar(1.0) / (_tmp658); - const Scalar _tmp660 = _tmp659 * views_1_calibration(0, 0); + const Scalar _tmp645 = _tmp642 * _tmp644; + const Scalar _tmp646 = _tmp643 * _tmp644; + const Scalar _tmp647 = + _tmp139 * _tmp645 + _tmp149 * landmarks_16_ + _tmp151 * _tmp646 + _tmp51 * _tmp644; + const Scalar _tmp648 = + _tmp154 * _tmp646 + _tmp155 * _tmp645 + _tmp156 * landmarks_16_ + _tmp52 * _tmp644; + const Scalar _tmp649 = + _tmp158 * _tmp646 + _tmp159 * _tmp645 + _tmp160 * landmarks_16_ + _tmp48 * _tmp644; + const Scalar _tmp650 = _tmp648 * _tmp86 + _tmp649 * _tmp81; + const Scalar _tmp651 = _tmp647 * _tmp76 + _tmp650; + const Scalar _tmp652 = _tmp105 * _tmp648 + _tmp107 * _tmp647; + const Scalar _tmp653 = _tmp103 * _tmp649 + _tmp652; + const Scalar _tmp654 = std::max(_tmp653, epsilon); + const Scalar _tmp655 = Scalar(1.0) / (_tmp654); + const Scalar _tmp656 = _tmp655 * views_1_calibration(0, 0); + const Scalar _tmp657 = + _tmp651 * _tmp656 - matches_0_16_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp658 = _tmp647 * _tmp99 + _tmp649 * _tmp96; + const Scalar _tmp659 = _tmp648 * _tmp93 + _tmp658; + const Scalar _tmp660 = _tmp655 * views_1_calibration(1, 0); const Scalar _tmp661 = - _tmp655 * _tmp660 - matches_0_16_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp662 = _tmp100 * _tmp651 + _tmp653 * _tmp97; - const Scalar _tmp663 = _tmp652 * _tmp94 + _tmp662; - const Scalar _tmp664 = _tmp659 * views_1_calibration(1, 0); - const Scalar _tmp665 = - _tmp663 * _tmp664 - matches_0_16_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp666 = std::pow(_tmp661, Scalar(2)) + std::pow(_tmp665, Scalar(2)) + epsilon; - const Scalar _tmp667 = std::pow(_tmp666, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp668 = std::sqrt(matches_0_16_weight); - const Scalar _tmp669 = std::max(0, (((_tmp657) > 0) - ((_tmp657) < 0))); - const Scalar _tmp670 = _tmp184 * _tmp666 + 1; - const Scalar _tmp671 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp670, _tmp188) - 1))); - const Scalar _tmp672 = _tmp668 * _tmp669 * _tmp671; - const Scalar _tmp673 = _tmp667 * _tmp672; - const Scalar _tmp674 = _tmp661 * _tmp673; - const Scalar _tmp675 = _tmp665 * _tmp673; - const Scalar _tmp676 = _tmp147 + matches_0_17_source_coords(1, 0); - const Scalar _tmp677 = _tmp144 + matches_0_17_source_coords(0, 0); - const Scalar _tmp678 = std::pow(Scalar(_tmp146 * std::pow(_tmp677, Scalar(2)) + - _tmp149 * std::pow(_tmp676, Scalar(2)) + _tmp150), + _tmp659 * _tmp660 - matches_0_16_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp662 = std::pow(_tmp657, Scalar(2)) + std::pow(_tmp661, Scalar(2)) + epsilon; + const Scalar _tmp663 = std::pow(_tmp662, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp664 = std::sqrt(matches_0_16_weight); + const Scalar _tmp665 = std::max(0, (((_tmp653) > 0) - ((_tmp653) < 0))); + const Scalar _tmp666 = _tmp180 * _tmp662 + 1; + const Scalar _tmp667 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp666, _tmp184) - 1))); + const Scalar _tmp668 = _tmp664 * _tmp665 * _tmp667; + const Scalar _tmp669 = _tmp663 * _tmp668; + const Scalar _tmp670 = _tmp657 * _tmp669; + const Scalar _tmp671 = _tmp661 * _tmp669; + const Scalar _tmp672 = _tmp143 + matches_0_17_source_coords(1, 0); + const Scalar _tmp673 = _tmp140 + matches_0_17_source_coords(0, 0); + const Scalar _tmp674 = std::pow(Scalar(_tmp142 * std::pow(_tmp673, Scalar(2)) + + _tmp145 * std::pow(_tmp672, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp679 = _tmp676 * _tmp678; - const Scalar _tmp680 = _tmp677 * _tmp678; - const Scalar _tmp681 = - _tmp143 * _tmp679 + _tmp153 * landmarks_17_ + _tmp155 * _tmp680 + _tmp51 * _tmp678; - const Scalar _tmp682 = - _tmp158 * _tmp680 + _tmp159 * _tmp679 + _tmp160 * landmarks_17_ + _tmp52 * _tmp678; - const Scalar _tmp683 = - _tmp162 * _tmp680 + _tmp163 * _tmp679 + _tmp164 * landmarks_17_ + _tmp48 * _tmp678; - const Scalar _tmp684 = _tmp682 * _tmp87 + _tmp683 * _tmp83; - const Scalar _tmp685 = _tmp681 * _tmp77 + _tmp684; - const Scalar _tmp686 = _tmp106 * _tmp682 + _tmp108 * _tmp681; - const Scalar _tmp687 = _tmp104 * _tmp683 + _tmp686; - const Scalar _tmp688 = std::max(_tmp687, epsilon); - const Scalar _tmp689 = Scalar(1.0) / (_tmp688); - const Scalar _tmp690 = _tmp689 * views_1_calibration(0, 0); + const Scalar _tmp675 = _tmp672 * _tmp674; + const Scalar _tmp676 = _tmp673 * _tmp674; + const Scalar _tmp677 = + _tmp139 * _tmp675 + _tmp149 * landmarks_17_ + _tmp151 * _tmp676 + _tmp51 * _tmp674; + const Scalar _tmp678 = + _tmp154 * _tmp676 + _tmp155 * _tmp675 + _tmp156 * landmarks_17_ + _tmp52 * _tmp674; + const Scalar _tmp679 = + _tmp158 * _tmp676 + _tmp159 * _tmp675 + _tmp160 * landmarks_17_ + _tmp48 * _tmp674; + const Scalar _tmp680 = _tmp678 * _tmp86 + _tmp679 * _tmp81; + const Scalar _tmp681 = _tmp677 * _tmp76 + _tmp680; + const Scalar _tmp682 = _tmp105 * _tmp678 + _tmp107 * _tmp677; + const Scalar _tmp683 = _tmp103 * _tmp679 + _tmp682; + const Scalar _tmp684 = std::max(_tmp683, epsilon); + const Scalar _tmp685 = Scalar(1.0) / (_tmp684); + const Scalar _tmp686 = _tmp685 * views_1_calibration(0, 0); + const Scalar _tmp687 = + _tmp681 * _tmp686 - matches_0_17_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp688 = _tmp677 * _tmp99 + _tmp679 * _tmp96; + const Scalar _tmp689 = _tmp678 * _tmp93 + _tmp688; + const Scalar _tmp690 = _tmp685 * views_1_calibration(1, 0); const Scalar _tmp691 = - _tmp685 * _tmp690 - matches_0_17_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp692 = _tmp100 * _tmp681 + _tmp683 * _tmp97; - const Scalar _tmp693 = _tmp682 * _tmp94 + _tmp692; - const Scalar _tmp694 = _tmp689 * views_1_calibration(1, 0); - const Scalar _tmp695 = - _tmp693 * _tmp694 - matches_0_17_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp696 = std::pow(_tmp691, Scalar(2)) + std::pow(_tmp695, Scalar(2)) + epsilon; - const Scalar _tmp697 = std::pow(_tmp696, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp698 = std::sqrt(matches_0_17_weight); - const Scalar _tmp699 = std::max(0, (((_tmp687) > 0) - ((_tmp687) < 0))); - const Scalar _tmp700 = _tmp184 * _tmp696 + 1; - const Scalar _tmp701 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp700, _tmp188) - 1))); - const Scalar _tmp702 = _tmp698 * _tmp699 * _tmp701; - const Scalar _tmp703 = _tmp697 * _tmp702; - const Scalar _tmp704 = _tmp691 * _tmp703; - const Scalar _tmp705 = _tmp695 * _tmp703; - const Scalar _tmp706 = _tmp147 + matches_0_18_source_coords(1, 0); - const Scalar _tmp707 = _tmp144 + matches_0_18_source_coords(0, 0); - const Scalar _tmp708 = std::pow(Scalar(_tmp146 * std::pow(_tmp707, Scalar(2)) + - _tmp149 * std::pow(_tmp706, Scalar(2)) + _tmp150), + _tmp689 * _tmp690 - matches_0_17_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp692 = std::pow(_tmp687, Scalar(2)) + std::pow(_tmp691, Scalar(2)) + epsilon; + const Scalar _tmp693 = std::pow(_tmp692, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp694 = std::sqrt(matches_0_17_weight); + const Scalar _tmp695 = std::max(0, (((_tmp683) > 0) - ((_tmp683) < 0))); + const Scalar _tmp696 = _tmp180 * _tmp692 + 1; + const Scalar _tmp697 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp696, _tmp184) - 1))); + const Scalar _tmp698 = _tmp694 * _tmp695 * _tmp697; + const Scalar _tmp699 = _tmp693 * _tmp698; + const Scalar _tmp700 = _tmp687 * _tmp699; + const Scalar _tmp701 = _tmp691 * _tmp699; + const Scalar _tmp702 = _tmp143 + matches_0_18_source_coords(1, 0); + const Scalar _tmp703 = _tmp140 + matches_0_18_source_coords(0, 0); + const Scalar _tmp704 = std::pow(Scalar(_tmp142 * std::pow(_tmp703, Scalar(2)) + + _tmp145 * std::pow(_tmp702, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp709 = _tmp706 * _tmp708; - const Scalar _tmp710 = _tmp707 * _tmp708; - const Scalar _tmp711 = - _tmp143 * _tmp709 + _tmp153 * landmarks_18_ + _tmp155 * _tmp710 + _tmp51 * _tmp708; - const Scalar _tmp712 = - _tmp162 * _tmp710 + _tmp163 * _tmp709 + _tmp164 * landmarks_18_ + _tmp48 * _tmp708; - const Scalar _tmp713 = - _tmp158 * _tmp710 + _tmp159 * _tmp709 + _tmp160 * landmarks_18_ + _tmp52 * _tmp708; - const Scalar _tmp714 = _tmp712 * _tmp83 + _tmp713 * _tmp87; - const Scalar _tmp715 = _tmp711 * _tmp77 + _tmp714; - const Scalar _tmp716 = _tmp106 * _tmp713 + _tmp108 * _tmp711; - const Scalar _tmp717 = _tmp104 * _tmp712 + _tmp716; - const Scalar _tmp718 = std::max(_tmp717, epsilon); - const Scalar _tmp719 = Scalar(1.0) / (_tmp718); - const Scalar _tmp720 = _tmp719 * views_1_calibration(0, 0); + const Scalar _tmp705 = _tmp702 * _tmp704; + const Scalar _tmp706 = _tmp703 * _tmp704; + const Scalar _tmp707 = + _tmp139 * _tmp705 + _tmp149 * landmarks_18_ + _tmp151 * _tmp706 + _tmp51 * _tmp704; + const Scalar _tmp708 = + _tmp158 * _tmp706 + _tmp159 * _tmp705 + _tmp160 * landmarks_18_ + _tmp48 * _tmp704; + const Scalar _tmp709 = + _tmp154 * _tmp706 + _tmp155 * _tmp705 + _tmp156 * landmarks_18_ + _tmp52 * _tmp704; + const Scalar _tmp710 = _tmp708 * _tmp81 + _tmp709 * _tmp86; + const Scalar _tmp711 = _tmp707 * _tmp76 + _tmp710; + const Scalar _tmp712 = _tmp105 * _tmp709 + _tmp107 * _tmp707; + const Scalar _tmp713 = _tmp103 * _tmp708 + _tmp712; + const Scalar _tmp714 = std::max(_tmp713, epsilon); + const Scalar _tmp715 = Scalar(1.0) / (_tmp714); + const Scalar _tmp716 = _tmp715 * views_1_calibration(0, 0); + const Scalar _tmp717 = + _tmp711 * _tmp716 - matches_0_18_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp718 = _tmp707 * _tmp99 + _tmp708 * _tmp96; + const Scalar _tmp719 = _tmp709 * _tmp93 + _tmp718; + const Scalar _tmp720 = _tmp715 * views_1_calibration(1, 0); const Scalar _tmp721 = - _tmp715 * _tmp720 - matches_0_18_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp722 = _tmp100 * _tmp711 + _tmp712 * _tmp97; - const Scalar _tmp723 = _tmp713 * _tmp94 + _tmp722; - const Scalar _tmp724 = _tmp719 * views_1_calibration(1, 0); - const Scalar _tmp725 = - _tmp723 * _tmp724 - matches_0_18_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp726 = std::pow(_tmp721, Scalar(2)) + std::pow(_tmp725, Scalar(2)) + epsilon; - const Scalar _tmp727 = std::pow(_tmp726, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp728 = std::max(0, (((_tmp717) > 0) - ((_tmp717) < 0))); - const Scalar _tmp729 = std::sqrt(matches_0_18_weight); - const Scalar _tmp730 = _tmp184 * _tmp726 + 1; - const Scalar _tmp731 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp730, _tmp188) - 1))); - const Scalar _tmp732 = _tmp728 * _tmp729 * _tmp731; - const Scalar _tmp733 = _tmp727 * _tmp732; - const Scalar _tmp734 = _tmp721 * _tmp733; - const Scalar _tmp735 = _tmp725 * _tmp733; - const Scalar _tmp736 = _tmp147 + matches_0_19_source_coords(1, 0); - const Scalar _tmp737 = _tmp144 + matches_0_19_source_coords(0, 0); - const Scalar _tmp738 = std::pow(Scalar(_tmp146 * std::pow(_tmp737, Scalar(2)) + - _tmp149 * std::pow(_tmp736, Scalar(2)) + _tmp150), + _tmp719 * _tmp720 - matches_0_18_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp722 = std::pow(_tmp717, Scalar(2)) + std::pow(_tmp721, Scalar(2)) + epsilon; + const Scalar _tmp723 = std::pow(_tmp722, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp724 = std::max(0, (((_tmp713) > 0) - ((_tmp713) < 0))); + const Scalar _tmp725 = std::sqrt(matches_0_18_weight); + const Scalar _tmp726 = _tmp180 * _tmp722 + 1; + const Scalar _tmp727 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp726, _tmp184) - 1))); + const Scalar _tmp728 = _tmp724 * _tmp725 * _tmp727; + const Scalar _tmp729 = _tmp723 * _tmp728; + const Scalar _tmp730 = _tmp717 * _tmp729; + const Scalar _tmp731 = _tmp721 * _tmp729; + const Scalar _tmp732 = _tmp143 + matches_0_19_source_coords(1, 0); + const Scalar _tmp733 = _tmp140 + matches_0_19_source_coords(0, 0); + const Scalar _tmp734 = std::pow(Scalar(_tmp142 * std::pow(_tmp733, Scalar(2)) + + _tmp145 * std::pow(_tmp732, Scalar(2)) + _tmp146), Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp739 = _tmp736 * _tmp738; - const Scalar _tmp740 = _tmp737 * _tmp738; - const Scalar _tmp741 = - _tmp143 * _tmp739 + _tmp153 * landmarks_19_ + _tmp155 * _tmp740 + _tmp51 * _tmp738; - const Scalar _tmp742 = - _tmp162 * _tmp740 + _tmp163 * _tmp739 + _tmp164 * landmarks_19_ + _tmp48 * _tmp738; - const Scalar _tmp743 = - _tmp158 * _tmp740 + _tmp159 * _tmp739 + _tmp160 * landmarks_19_ + _tmp52 * _tmp738; - const Scalar _tmp744 = _tmp742 * _tmp83 + _tmp743 * _tmp87; - const Scalar _tmp745 = _tmp741 * _tmp77 + _tmp744; - const Scalar _tmp746 = _tmp106 * _tmp743 + _tmp108 * _tmp741; - const Scalar _tmp747 = _tmp104 * _tmp742 + _tmp746; - const Scalar _tmp748 = std::max(_tmp747, epsilon); - const Scalar _tmp749 = Scalar(1.0) / (_tmp748); - const Scalar _tmp750 = _tmp749 * views_1_calibration(0, 0); + const Scalar _tmp735 = _tmp732 * _tmp734; + const Scalar _tmp736 = _tmp733 * _tmp734; + const Scalar _tmp737 = + _tmp139 * _tmp735 + _tmp149 * landmarks_19_ + _tmp151 * _tmp736 + _tmp51 * _tmp734; + const Scalar _tmp738 = + _tmp158 * _tmp736 + _tmp159 * _tmp735 + _tmp160 * landmarks_19_ + _tmp48 * _tmp734; + const Scalar _tmp739 = + _tmp154 * _tmp736 + _tmp155 * _tmp735 + _tmp156 * landmarks_19_ + _tmp52 * _tmp734; + const Scalar _tmp740 = _tmp738 * _tmp81 + _tmp739 * _tmp86; + const Scalar _tmp741 = _tmp737 * _tmp76 + _tmp740; + const Scalar _tmp742 = _tmp105 * _tmp739 + _tmp107 * _tmp737; + const Scalar _tmp743 = _tmp103 * _tmp738 + _tmp742; + const Scalar _tmp744 = std::max(_tmp743, epsilon); + const Scalar _tmp745 = Scalar(1.0) / (_tmp744); + const Scalar _tmp746 = _tmp745 * views_1_calibration(0, 0); + const Scalar _tmp747 = + _tmp741 * _tmp746 - matches_0_19_target_coords(0, 0) + views_1_calibration(2, 0); + const Scalar _tmp748 = _tmp737 * _tmp99 + _tmp738 * _tmp96; + const Scalar _tmp749 = _tmp739 * _tmp93 + _tmp748; + const Scalar _tmp750 = _tmp745 * views_1_calibration(1, 0); const Scalar _tmp751 = - _tmp745 * _tmp750 - matches_0_19_target_coords(0, 0) + views_1_calibration(2, 0); - const Scalar _tmp752 = _tmp100 * _tmp741 + _tmp742 * _tmp97; - const Scalar _tmp753 = _tmp743 * _tmp94 + _tmp752; - const Scalar _tmp754 = _tmp749 * views_1_calibration(1, 0); - const Scalar _tmp755 = - _tmp753 * _tmp754 - matches_0_19_target_coords(1, 0) + views_1_calibration(3, 0); - const Scalar _tmp756 = std::pow(_tmp751, Scalar(2)) + std::pow(_tmp755, Scalar(2)) + epsilon; - const Scalar _tmp757 = std::pow(_tmp756, Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp758 = std::sqrt(matches_0_19_weight); - const Scalar _tmp759 = std::max(0, (((_tmp747) > 0) - ((_tmp747) < 0))); - const Scalar _tmp760 = _tmp184 * _tmp756 + 1; - const Scalar _tmp761 = std::sqrt(Scalar(_tmp189 * (std::pow(_tmp760, _tmp188) - 1))); - const Scalar _tmp762 = _tmp758 * _tmp759 * _tmp761; - const Scalar _tmp763 = _tmp757 * _tmp762; - const Scalar _tmp764 = _tmp751 * _tmp763; - const Scalar _tmp765 = _tmp755 * _tmp763; - const Scalar _tmp766 = landmarks_0_ - matches_0_0_inverse_range_prior; - const Scalar _tmp767 = epsilon + matches_0_0_inverse_range_prior_sigma; - const Scalar _tmp768 = matches_0_0_weight / _tmp767; - const Scalar _tmp769 = landmarks_1_ - matches_0_1_inverse_range_prior; - const Scalar _tmp770 = epsilon + matches_0_1_inverse_range_prior_sigma; - const Scalar _tmp771 = matches_0_1_weight / _tmp770; - const Scalar _tmp772 = landmarks_2_ - matches_0_2_inverse_range_prior; - const Scalar _tmp773 = epsilon + matches_0_2_inverse_range_prior_sigma; - const Scalar _tmp774 = matches_0_2_weight / _tmp773; - const Scalar _tmp775 = landmarks_3_ - matches_0_3_inverse_range_prior; - const Scalar _tmp776 = epsilon + matches_0_3_inverse_range_prior_sigma; - const Scalar _tmp777 = matches_0_3_weight / _tmp776; - const Scalar _tmp778 = landmarks_4_ - matches_0_4_inverse_range_prior; - const Scalar _tmp779 = epsilon + matches_0_4_inverse_range_prior_sigma; - const Scalar _tmp780 = matches_0_4_weight / _tmp779; - const Scalar _tmp781 = landmarks_5_ - matches_0_5_inverse_range_prior; - const Scalar _tmp782 = epsilon + matches_0_5_inverse_range_prior_sigma; - const Scalar _tmp783 = matches_0_5_weight / _tmp782; - const Scalar _tmp784 = landmarks_6_ - matches_0_6_inverse_range_prior; - const Scalar _tmp785 = epsilon + matches_0_6_inverse_range_prior_sigma; - const Scalar _tmp786 = matches_0_6_weight / _tmp785; - const Scalar _tmp787 = landmarks_7_ - matches_0_7_inverse_range_prior; - const Scalar _tmp788 = epsilon + matches_0_7_inverse_range_prior_sigma; - const Scalar _tmp789 = matches_0_7_weight / _tmp788; - const Scalar _tmp790 = landmarks_8_ - matches_0_8_inverse_range_prior; - const Scalar _tmp791 = epsilon + matches_0_8_inverse_range_prior_sigma; - const Scalar _tmp792 = matches_0_8_weight / _tmp791; - const Scalar _tmp793 = landmarks_9_ - matches_0_9_inverse_range_prior; - const Scalar _tmp794 = epsilon + matches_0_9_inverse_range_prior_sigma; - const Scalar _tmp795 = matches_0_9_weight / _tmp794; - const Scalar _tmp796 = landmarks_10_ - matches_0_10_inverse_range_prior; - const Scalar _tmp797 = epsilon + matches_0_10_inverse_range_prior_sigma; - const Scalar _tmp798 = matches_0_10_weight / _tmp797; - const Scalar _tmp799 = landmarks_11_ - matches_0_11_inverse_range_prior; - const Scalar _tmp800 = epsilon + matches_0_11_inverse_range_prior_sigma; - const Scalar _tmp801 = matches_0_11_weight / _tmp800; - const Scalar _tmp802 = landmarks_12_ - matches_0_12_inverse_range_prior; - const Scalar _tmp803 = epsilon + matches_0_12_inverse_range_prior_sigma; - const Scalar _tmp804 = matches_0_12_weight / _tmp803; - const Scalar _tmp805 = landmarks_13_ - matches_0_13_inverse_range_prior; - const Scalar _tmp806 = epsilon + matches_0_13_inverse_range_prior_sigma; - const Scalar _tmp807 = matches_0_13_weight / _tmp806; - const Scalar _tmp808 = landmarks_14_ - matches_0_14_inverse_range_prior; - const Scalar _tmp809 = epsilon + matches_0_14_inverse_range_prior_sigma; - const Scalar _tmp810 = matches_0_14_weight / _tmp809; - const Scalar _tmp811 = landmarks_15_ - matches_0_15_inverse_range_prior; - const Scalar _tmp812 = epsilon + matches_0_15_inverse_range_prior_sigma; - const Scalar _tmp813 = matches_0_15_weight / _tmp812; - const Scalar _tmp814 = landmarks_16_ - matches_0_16_inverse_range_prior; - const Scalar _tmp815 = epsilon + matches_0_16_inverse_range_prior_sigma; - const Scalar _tmp816 = matches_0_16_weight / _tmp815; - const Scalar _tmp817 = landmarks_17_ - matches_0_17_inverse_range_prior; - const Scalar _tmp818 = epsilon + matches_0_17_inverse_range_prior_sigma; - const Scalar _tmp819 = matches_0_17_weight / _tmp818; - const Scalar _tmp820 = landmarks_18_ - matches_0_18_inverse_range_prior; - const Scalar _tmp821 = epsilon + matches_0_18_inverse_range_prior_sigma; - const Scalar _tmp822 = matches_0_18_weight / _tmp821; - const Scalar _tmp823 = landmarks_19_ - matches_0_19_inverse_range_prior; - const Scalar _tmp824 = epsilon + matches_0_19_inverse_range_prior_sigma; - const Scalar _tmp825 = matches_0_19_weight / _tmp824; - const Scalar _tmp826 = (Scalar(1) / Scalar(2)) * _tmp17; - const Scalar _tmp827 = (Scalar(1) / Scalar(2)) * _tmp18; - const Scalar _tmp828 = (Scalar(1) / Scalar(2)) * _tmp19; - const Scalar _tmp829 = (Scalar(1) / Scalar(2)) * _tmp20; - const Scalar _tmp830 = _tmp826 - _tmp827 + _tmp828 - _tmp829; - const Scalar _tmp831 = _priors_0_1_target_T_src[3] * _tmp830; - const Scalar _tmp832 = (Scalar(1) / Scalar(2)) * _tmp27; - const Scalar _tmp833 = (Scalar(1) / Scalar(2)) * _tmp28; - const Scalar _tmp834 = (Scalar(1) / Scalar(2)) * _tmp29; - const Scalar _tmp835 = (Scalar(1) / Scalar(2)) * _tmp30; - const Scalar _tmp836 = -_tmp832 + _tmp833 + _tmp834 - _tmp835; - const Scalar _tmp837 = (Scalar(1) / Scalar(2)) * _tmp15; - const Scalar _tmp838 = (Scalar(1) / Scalar(2)) * _tmp12; - const Scalar _tmp839 = (Scalar(1) / Scalar(2)) * _tmp13; - const Scalar _tmp840 = (Scalar(1) / Scalar(2)) * _tmp14; - const Scalar _tmp841 = _tmp837 + _tmp838 + _tmp839 + _tmp840; - const Scalar _tmp842 = _priors_0_1_target_T_src[0] * _tmp841; - const Scalar _tmp843 = (Scalar(1) / Scalar(2)) * _tmp22; - const Scalar _tmp844 = (Scalar(1) / Scalar(2)) * _tmp23; - const Scalar _tmp845 = (Scalar(1) / Scalar(2)) * _tmp24; - const Scalar _tmp846 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp847 = _tmp843 + _tmp844 - _tmp845 - _tmp846; - const Scalar _tmp848 = -_priors_0_1_target_T_src[2] * _tmp847 - _tmp842; - const Scalar _tmp849 = -_priors_0_1_target_T_src[1] * _tmp836 - _tmp831 + _tmp848; - const Scalar _tmp850 = std::fabs(_tmp33 + _tmp34 + _tmp35 + _tmp36); - const Scalar _tmp851 = std::min(_tmp42, _tmp850); - const Scalar _tmp852 = std::acos(_tmp851); - const Scalar _tmp853 = 1 - std::pow(_tmp851, Scalar(2)); - const Scalar _tmp854 = _tmp39 * ((((_tmp42 - _tmp850) > 0) - ((_tmp42 - _tmp850) < 0)) + 1) * - (((-_tmp33 + _tmp37) > 0) - ((-_tmp33 + _tmp37) < 0)); - const Scalar _tmp855 = _tmp851 * _tmp852 * _tmp854 / (_tmp853 * std::sqrt(_tmp853)); - const Scalar _tmp856 = _tmp849 * _tmp855; - const Scalar _tmp857 = _tmp57 * _tmp856; - const Scalar _tmp858 = _priors_0_1_target_T_src[3] * _tmp841; - const Scalar _tmp859 = -_priors_0_1_target_T_src[1] * _tmp847; - const Scalar _tmp860 = -_priors_0_1_target_T_src[0] * _tmp830; - const Scalar _tmp861 = _tmp852 / std::sqrt(_tmp853); - const Scalar _tmp862 = - _tmp861 * (_priors_0_1_target_T_src[2] * _tmp836 + _tmp858 + _tmp859 + _tmp860); - const Scalar _tmp863 = _tmp40 * _tmp862; - const Scalar _tmp864 = _priors_0_1_target_T_src[0] * _tmp847; - const Scalar _tmp865 = _priors_0_1_target_T_src[2] * _tmp841; - const Scalar _tmp866 = -_priors_0_1_target_T_src[1] * _tmp830 - _tmp865; - const Scalar _tmp867 = _tmp40 * _tmp861; - const Scalar _tmp868 = _tmp867 * (_priors_0_1_target_T_src[3] * _tmp836 + _tmp864 + _tmp866); - const Scalar _tmp869 = _tmp854 / _tmp853; - const Scalar _tmp870 = _tmp849 * _tmp869; - const Scalar _tmp871 = _tmp57 * _tmp870; - const Scalar _tmp872 = _tmp54 * _tmp870; - const Scalar _tmp873 = _tmp54 * _tmp856; - const Scalar _tmp874 = _tmp32 * _tmp855; - const Scalar _tmp875 = _tmp849 * _tmp874; - const Scalar _tmp876 = _priors_0_1_target_T_src[1] * _tmp841; - const Scalar _tmp877 = _priors_0_1_target_T_src[3] * _tmp847; - const Scalar _tmp878 = _priors_0_1_target_T_src[2] * _tmp830; - const Scalar _tmp879 = - _tmp867 * (-_priors_0_1_target_T_src[0] * _tmp836 + _tmp876 + _tmp877 - _tmp878); - const Scalar _tmp880 = _tmp32 * _tmp869; - const Scalar _tmp881 = _tmp849 * _tmp880; - const Scalar _tmp882 = - _tmp857 * priors_0_1_sqrt_info(4, 2) + _tmp863 * priors_0_1_sqrt_info(4, 0) + - _tmp868 * priors_0_1_sqrt_info(4, 1) - _tmp871 * priors_0_1_sqrt_info(4, 2) - - _tmp872 * priors_0_1_sqrt_info(4, 0) + _tmp873 * priors_0_1_sqrt_info(4, 0) + - _tmp875 * priors_0_1_sqrt_info(4, 1) + _tmp879 * priors_0_1_sqrt_info(4, 2) - - _tmp881 * priors_0_1_sqrt_info(4, 1); - const Scalar _tmp883 = -_tmp74; - const Scalar _tmp884 = _tmp71 + _tmp883; - const Scalar _tmp885 = -_tmp91; - const Scalar _tmp886 = std::pow(_views_1_pose[3], Scalar(2)); - const Scalar _tmp887 = _tmp885 + _tmp886; - const Scalar _tmp888 = _tmp884 + _tmp887; - const Scalar _tmp889 = -_tmp85; - const Scalar _tmp890 = _tmp86 + _tmp889; - const Scalar _tmp891 = -_tmp886; - const Scalar _tmp892 = _tmp891 + _tmp91; - const Scalar _tmp893 = _tmp884 + _tmp892; - const Scalar _tmp894 = -_tmp96; - const Scalar _tmp895 = _tmp105 + _tmp894; - const Scalar _tmp896 = - ((((_tmp41 + _tmp597) > 0) - ((_tmp41 + _tmp597) < 0)) + 1) / std::pow(_tmp598, Scalar(2)); - const Scalar _tmp897 = _tmp896 * (_tmp591 * _tmp890 + _tmp592 * _tmp893 + _tmp593 * _tmp895); - const Scalar _tmp898 = (Scalar(1) / Scalar(2)) * views_1_calibration(1, 0); - const Scalar _tmp899 = _tmp603 * _tmp898; - const Scalar _tmp900 = _tmp604 * (_tmp593 * _tmp888 + _tmp596) - _tmp897 * _tmp899; - const Scalar _tmp901 = 2 * _tmp605; - const Scalar _tmp902 = - -_tmp595 * _tmp601 * _tmp897 * views_1_calibration(0, 0) + _tmp900 * _tmp901; - const Scalar _tmp903 = _tmp612 / (_tmp606 * std::sqrt(_tmp606)); - const Scalar _tmp904 = (Scalar(1) / Scalar(2)) * _tmp605; - const Scalar _tmp905 = _tmp903 * _tmp904; - const Scalar _tmp906 = _tmp188 - 1; - const Scalar _tmp907 = - _tmp181 * _tmp607 * _tmp608 * std::pow(_tmp609, _tmp906) * _tmp611 / _tmp610; - const Scalar _tmp908 = _tmp904 * _tmp907; - const Scalar _tmp909 = _tmp613 * _tmp900 - _tmp902 * _tmp905 + _tmp902 * _tmp908; - const Scalar _tmp910 = _tmp501 * _tmp890 + _tmp502 * _tmp893 + _tmp503 * _tmp895; - const Scalar _tmp911 = - ((((_tmp41 + _tmp507) > 0) - ((_tmp41 + _tmp507) < 0)) + 1) / std::pow(_tmp508, Scalar(2)); - const Scalar _tmp912 = _tmp505 * _tmp911; - const Scalar _tmp913 = _tmp513 * _tmp911; - const Scalar _tmp914 = _tmp898 * _tmp913; - const Scalar _tmp915 = _tmp514 * (_tmp503 * _tmp888 + _tmp506) - _tmp910 * _tmp914; - const Scalar _tmp916 = 2 * _tmp515; - const Scalar _tmp917 = - -_tmp511 * _tmp910 * _tmp912 * views_1_calibration(0, 0) + _tmp915 * _tmp916; - const Scalar _tmp918 = _tmp522 / (_tmp516 * std::sqrt(_tmp516)); - const Scalar _tmp919 = (Scalar(1) / Scalar(2)) * _tmp918; - const Scalar _tmp920 = _tmp511 * _tmp919; - const Scalar _tmp921 = - _tmp181 * _tmp517 * std::pow(_tmp518, _tmp906) * _tmp520 * _tmp521 / _tmp519; - const Scalar _tmp922 = (Scalar(1) / Scalar(2)) * _tmp921; - const Scalar _tmp923 = _tmp511 * _tmp922; - const Scalar _tmp924 = (Scalar(1) / Scalar(2)) * views_1_calibration(0, 0); - const Scalar _tmp925 = _tmp505 * _tmp924; - const Scalar _tmp926 = _tmp911 * _tmp925; - const Scalar _tmp927 = -_tmp523 * _tmp910 * _tmp926 - _tmp917 * _tmp920 + _tmp917 * _tmp923; - const Scalar _tmp928 = _tmp515 * _tmp919; - const Scalar _tmp929 = _tmp515 * _tmp922; - const Scalar _tmp930 = _tmp523 * _tmp915 - _tmp917 * _tmp928 + _tmp917 * _tmp929; - const Scalar _tmp931 = -_tmp837 - _tmp838 - _tmp839 - _tmp840; - const Scalar _tmp932 = _priors_1_0_target_T_src[0] * _tmp931; - const Scalar _tmp933 = -_tmp932; - const Scalar _tmp934 = -_tmp843 - _tmp844 + _tmp845 + _tmp846; - const Scalar _tmp935 = _priors_1_0_target_T_src[3] * _tmp830; - const Scalar _tmp936 = _tmp832 - _tmp833 - _tmp834 + _tmp835; - const Scalar _tmp937 = -_priors_1_0_target_T_src[1] * _tmp936; - const Scalar _tmp938 = -_priors_1_0_target_T_src[2] * _tmp934 + _tmp933 - _tmp935 + _tmp937; - const Scalar _tmp939 = std::fabs(_tmp112 + _tmp114 + _tmp116 + _tmp118); - const Scalar _tmp940 = std::min(_tmp42, _tmp939); - const Scalar _tmp941 = 1 - std::pow(_tmp940, Scalar(2)); - const Scalar _tmp942 = _tmp121 * ((((_tmp42 - _tmp939) > 0) - ((_tmp42 - _tmp939) < 0)) + 1) * - (((-_tmp112 + _tmp119) > 0) - ((-_tmp112 + _tmp119) < 0)); - const Scalar _tmp943 = _tmp942 / _tmp941; - const Scalar _tmp944 = _tmp938 * _tmp943; - const Scalar _tmp945 = _tmp131 * _tmp944; - const Scalar _tmp946 = _priors_1_0_target_T_src[2] * _tmp931; - const Scalar _tmp947 = -_tmp946; - const Scalar _tmp948 = -_priors_1_0_target_T_src[1] * _tmp830; - const Scalar _tmp949 = _priors_1_0_target_T_src[3] * _tmp936; - const Scalar _tmp950 = std::acos(_tmp940); - const Scalar _tmp951 = _tmp950 / std::sqrt(_tmp941); - const Scalar _tmp952 = _tmp122 * _tmp951; + _tmp749 * _tmp750 - matches_0_19_target_coords(1, 0) + views_1_calibration(3, 0); + const Scalar _tmp752 = std::pow(_tmp747, Scalar(2)) + std::pow(_tmp751, Scalar(2)) + epsilon; + const Scalar _tmp753 = std::pow(_tmp752, Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp754 = std::sqrt(matches_0_19_weight); + const Scalar _tmp755 = std::max(0, (((_tmp743) > 0) - ((_tmp743) < 0))); + const Scalar _tmp756 = _tmp180 * _tmp752 + 1; + const Scalar _tmp757 = std::sqrt(Scalar(_tmp185 * (std::pow(_tmp756, _tmp184) - 1))); + const Scalar _tmp758 = _tmp754 * _tmp755 * _tmp757; + const Scalar _tmp759 = _tmp753 * _tmp758; + const Scalar _tmp760 = _tmp747 * _tmp759; + const Scalar _tmp761 = _tmp751 * _tmp759; + const Scalar _tmp762 = landmarks_0_ - matches_0_0_inverse_range_prior; + const Scalar _tmp763 = epsilon + matches_0_0_inverse_range_prior_sigma; + const Scalar _tmp764 = matches_0_0_weight / _tmp763; + const Scalar _tmp765 = landmarks_1_ - matches_0_1_inverse_range_prior; + const Scalar _tmp766 = epsilon + matches_0_1_inverse_range_prior_sigma; + const Scalar _tmp767 = matches_0_1_weight / _tmp766; + const Scalar _tmp768 = landmarks_2_ - matches_0_2_inverse_range_prior; + const Scalar _tmp769 = epsilon + matches_0_2_inverse_range_prior_sigma; + const Scalar _tmp770 = matches_0_2_weight / _tmp769; + const Scalar _tmp771 = landmarks_3_ - matches_0_3_inverse_range_prior; + const Scalar _tmp772 = epsilon + matches_0_3_inverse_range_prior_sigma; + const Scalar _tmp773 = matches_0_3_weight / _tmp772; + const Scalar _tmp774 = landmarks_4_ - matches_0_4_inverse_range_prior; + const Scalar _tmp775 = epsilon + matches_0_4_inverse_range_prior_sigma; + const Scalar _tmp776 = matches_0_4_weight / _tmp775; + const Scalar _tmp777 = landmarks_5_ - matches_0_5_inverse_range_prior; + const Scalar _tmp778 = epsilon + matches_0_5_inverse_range_prior_sigma; + const Scalar _tmp779 = matches_0_5_weight / _tmp778; + const Scalar _tmp780 = landmarks_6_ - matches_0_6_inverse_range_prior; + const Scalar _tmp781 = epsilon + matches_0_6_inverse_range_prior_sigma; + const Scalar _tmp782 = matches_0_6_weight / _tmp781; + const Scalar _tmp783 = landmarks_7_ - matches_0_7_inverse_range_prior; + const Scalar _tmp784 = epsilon + matches_0_7_inverse_range_prior_sigma; + const Scalar _tmp785 = matches_0_7_weight / _tmp784; + const Scalar _tmp786 = landmarks_8_ - matches_0_8_inverse_range_prior; + const Scalar _tmp787 = epsilon + matches_0_8_inverse_range_prior_sigma; + const Scalar _tmp788 = matches_0_8_weight / _tmp787; + const Scalar _tmp789 = landmarks_9_ - matches_0_9_inverse_range_prior; + const Scalar _tmp790 = epsilon + matches_0_9_inverse_range_prior_sigma; + const Scalar _tmp791 = matches_0_9_weight / _tmp790; + const Scalar _tmp792 = landmarks_10_ - matches_0_10_inverse_range_prior; + const Scalar _tmp793 = epsilon + matches_0_10_inverse_range_prior_sigma; + const Scalar _tmp794 = matches_0_10_weight / _tmp793; + const Scalar _tmp795 = landmarks_11_ - matches_0_11_inverse_range_prior; + const Scalar _tmp796 = epsilon + matches_0_11_inverse_range_prior_sigma; + const Scalar _tmp797 = matches_0_11_weight / _tmp796; + const Scalar _tmp798 = landmarks_12_ - matches_0_12_inverse_range_prior; + const Scalar _tmp799 = epsilon + matches_0_12_inverse_range_prior_sigma; + const Scalar _tmp800 = matches_0_12_weight / _tmp799; + const Scalar _tmp801 = landmarks_13_ - matches_0_13_inverse_range_prior; + const Scalar _tmp802 = epsilon + matches_0_13_inverse_range_prior_sigma; + const Scalar _tmp803 = matches_0_13_weight / _tmp802; + const Scalar _tmp804 = landmarks_14_ - matches_0_14_inverse_range_prior; + const Scalar _tmp805 = epsilon + matches_0_14_inverse_range_prior_sigma; + const Scalar _tmp806 = matches_0_14_weight / _tmp805; + const Scalar _tmp807 = landmarks_15_ - matches_0_15_inverse_range_prior; + const Scalar _tmp808 = epsilon + matches_0_15_inverse_range_prior_sigma; + const Scalar _tmp809 = matches_0_15_weight / _tmp808; + const Scalar _tmp810 = landmarks_16_ - matches_0_16_inverse_range_prior; + const Scalar _tmp811 = epsilon + matches_0_16_inverse_range_prior_sigma; + const Scalar _tmp812 = matches_0_16_weight / _tmp811; + const Scalar _tmp813 = landmarks_17_ - matches_0_17_inverse_range_prior; + const Scalar _tmp814 = epsilon + matches_0_17_inverse_range_prior_sigma; + const Scalar _tmp815 = matches_0_17_weight / _tmp814; + const Scalar _tmp816 = landmarks_18_ - matches_0_18_inverse_range_prior; + const Scalar _tmp817 = epsilon + matches_0_18_inverse_range_prior_sigma; + const Scalar _tmp818 = matches_0_18_weight / _tmp817; + const Scalar _tmp819 = landmarks_19_ - matches_0_19_inverse_range_prior; + const Scalar _tmp820 = epsilon + matches_0_19_inverse_range_prior_sigma; + const Scalar _tmp821 = matches_0_19_weight / _tmp820; + const Scalar _tmp822 = -_tmp73; + const Scalar _tmp823 = _tmp70 + _tmp822; + const Scalar _tmp824 = -_tmp90; + const Scalar _tmp825 = std::pow(_views_1_pose[3], Scalar(2)); + const Scalar _tmp826 = _tmp824 + _tmp825; + const Scalar _tmp827 = _tmp823 + _tmp826; + const Scalar _tmp828 = -_tmp84; + const Scalar _tmp829 = _tmp828 + _tmp85; + const Scalar _tmp830 = -_tmp825; + const Scalar _tmp831 = _tmp830 + _tmp90; + const Scalar _tmp832 = _tmp823 + _tmp831; + const Scalar _tmp833 = -_tmp95; + const Scalar _tmp834 = _tmp104 + _tmp833; + const Scalar _tmp835 = _tmp587 * _tmp829 + _tmp588 * _tmp832 + _tmp589 * _tmp834; + const Scalar _tmp836 = (Scalar(1) / Scalar(2)) * views_1_calibration(1, 0); + const Scalar _tmp837 = + ((((_tmp42 + _tmp593) > 0) - ((_tmp42 + _tmp593) < 0)) + 1) / std::pow(_tmp594, Scalar(2)); + const Scalar _tmp838 = _tmp599 * _tmp837; + const Scalar _tmp839 = _tmp836 * _tmp838; + const Scalar _tmp840 = _tmp600 * (_tmp589 * _tmp827 + _tmp592) - _tmp835 * _tmp839; + const Scalar _tmp841 = _tmp591 * _tmp837; + const Scalar _tmp842 = 2 * _tmp601; + const Scalar _tmp843 = + -_tmp597 * _tmp835 * _tmp841 * views_1_calibration(0, 0) + _tmp840 * _tmp842; + const Scalar _tmp844 = _tmp608 / (_tmp602 * std::sqrt(_tmp602)); + const Scalar _tmp845 = (Scalar(1) / Scalar(2)) * _tmp844; + const Scalar _tmp846 = _tmp601 * _tmp845; + const Scalar _tmp847 = (Scalar(1) / Scalar(2)) * _tmp177; + const Scalar _tmp848 = _tmp184 - 1; + const Scalar _tmp849 = _tmp603 * _tmp604 * std::pow(_tmp605, _tmp848) * _tmp607 / _tmp606; + const Scalar _tmp850 = _tmp847 * _tmp849; + const Scalar _tmp851 = _tmp601 * _tmp850; + const Scalar _tmp852 = _tmp609 * _tmp840 - _tmp843 * _tmp846 + _tmp843 * _tmp851; + const Scalar _tmp853 = _tmp497 * _tmp829 + _tmp498 * _tmp832 + _tmp499 * _tmp834; + const Scalar _tmp854 = + ((((_tmp42 + _tmp503) > 0) - ((_tmp42 + _tmp503) < 0)) + 1) / std::pow(_tmp504, Scalar(2)); + const Scalar _tmp855 = _tmp501 * _tmp854; + const Scalar _tmp856 = _tmp509 * _tmp854; + const Scalar _tmp857 = _tmp836 * _tmp856; + const Scalar _tmp858 = _tmp510 * (_tmp499 * _tmp827 + _tmp502) - _tmp853 * _tmp857; + const Scalar _tmp859 = 2 * _tmp511; + const Scalar _tmp860 = + -_tmp507 * _tmp853 * _tmp855 * views_1_calibration(0, 0) + _tmp858 * _tmp859; + const Scalar _tmp861 = (Scalar(1) / Scalar(2)) * _tmp507; + const Scalar _tmp862 = _tmp518 / (_tmp512 * std::sqrt(_tmp512)); + const Scalar _tmp863 = _tmp861 * _tmp862; + const Scalar _tmp864 = + _tmp177 * _tmp513 * std::pow(_tmp514, _tmp848) * _tmp516 * _tmp517 / _tmp515; + const Scalar _tmp865 = _tmp861 * _tmp864; + const Scalar _tmp866 = (Scalar(1) / Scalar(2)) * views_1_calibration(0, 0); + const Scalar _tmp867 = _tmp855 * _tmp866; + const Scalar _tmp868 = -_tmp519 * _tmp853 * _tmp867 - _tmp860 * _tmp863 + _tmp860 * _tmp865; + const Scalar _tmp869 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp870 = (Scalar(1) / Scalar(2)) * _tmp12; + const Scalar _tmp871 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp872 = (Scalar(1) / Scalar(2)) * _tmp14; + const Scalar _tmp873 = -_tmp869 - _tmp870 - _tmp871 - _tmp872; + const Scalar _tmp874 = _priors_1_0_target_T_src[2] * _tmp873; + const Scalar _tmp875 = -_tmp874; + const Scalar _tmp876 = (Scalar(1) / Scalar(2)) * _tmp24; + const Scalar _tmp877 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp878 = (Scalar(1) / Scalar(2)) * _tmp26; + const Scalar _tmp879 = (Scalar(1) / Scalar(2)) * _tmp27; + const Scalar _tmp880 = -_tmp876 - _tmp877 + _tmp878 + _tmp879; + const Scalar _tmp881 = (Scalar(1) / Scalar(2)) * _tmp18; + const Scalar _tmp882 = (Scalar(1) / Scalar(2)) * _tmp19; + const Scalar _tmp883 = (Scalar(1) / Scalar(2)) * _tmp20; + const Scalar _tmp884 = (Scalar(1) / Scalar(2)) * _tmp21; + const Scalar _tmp885 = _tmp881 - _tmp882 + _tmp883 - _tmp884; + const Scalar _tmp886 = -_priors_1_0_target_T_src[1] * _tmp885; + const Scalar _tmp887 = (Scalar(1) / Scalar(2)) * _tmp30; + const Scalar _tmp888 = (Scalar(1) / Scalar(2)) * _tmp31; + const Scalar _tmp889 = (Scalar(1) / Scalar(2)) * _tmp32; + const Scalar _tmp890 = (Scalar(1) / Scalar(2)) * _tmp33; + const Scalar _tmp891 = _tmp887 - _tmp888 - _tmp889 + _tmp890; + const Scalar _tmp892 = _priors_1_0_target_T_src[3] * _tmp891; + const Scalar _tmp893 = std::fabs(_tmp111 + _tmp113 + _tmp115 + _tmp117); + const Scalar _tmp894 = std::min(_tmp43, _tmp893); + const Scalar _tmp895 = std::acos(_tmp894); + const Scalar _tmp896 = 1 - std::pow(_tmp894, Scalar(2)); + const Scalar _tmp897 = _tmp895 / std::sqrt(_tmp896); + const Scalar _tmp898 = _tmp121 * _tmp897; + const Scalar _tmp899 = + _tmp898 * (_priors_1_0_target_T_src[0] * _tmp880 + _tmp875 + _tmp886 + _tmp892); + const Scalar _tmp900 = _priors_1_0_target_T_src[1] * _tmp873; + const Scalar _tmp901 = -_priors_1_0_target_T_src[2] * _tmp885; + const Scalar _tmp902 = -_priors_1_0_target_T_src[0] * _tmp891; + const Scalar _tmp903 = _priors_1_0_target_T_src[3] * _tmp880 + _tmp900 + _tmp901 + _tmp902; + const Scalar _tmp904 = _tmp133 * _tmp897; + const Scalar _tmp905 = + -_tmp106 - _tmp108 + _tmp109 + _tmp827 * _views_0_pose[6] - _tmp827 * _views_1_pose[6]; + const Scalar _tmp906 = _tmp829 * _views_0_pose[4] - _tmp829 * _views_1_pose[4] + + _tmp832 * _views_0_pose[5] - _tmp832 * _views_1_pose[5] + + _tmp834 * _views_0_pose[6] - _tmp834 * _views_1_pose[6]; + const Scalar _tmp907 = _priors_1_0_target_T_src[0] * _tmp873; + const Scalar _tmp908 = -_tmp907; + const Scalar _tmp909 = _priors_1_0_target_T_src[3] * _tmp885; + const Scalar _tmp910 = -_priors_1_0_target_T_src[1] * _tmp891; + const Scalar _tmp911 = -_priors_1_0_target_T_src[2] * _tmp880 + _tmp908 - _tmp909 + _tmp910; + const Scalar _tmp912 = _tmp120 * ((((_tmp43 - _tmp893) > 0) - ((_tmp43 - _tmp893) < 0)) + 1) * + (((-_tmp111 + _tmp118) > 0) - ((-_tmp111 + _tmp118) < 0)); + const Scalar _tmp913 = _tmp894 * _tmp895 * _tmp912 / (_tmp896 * std::sqrt(_tmp896)); + const Scalar _tmp914 = _tmp911 * _tmp913; + const Scalar _tmp915 = _tmp122 * priors_1_0_sqrt_info(1, 2); + const Scalar _tmp916 = _tmp912 / _tmp896; + const Scalar _tmp917 = _tmp911 * _tmp916; + const Scalar _tmp918 = + _tmp132 * _tmp914 - _tmp132 * _tmp917 + _tmp899 * priors_1_0_sqrt_info(1, 1) + + _tmp903 * _tmp904 + _tmp905 * priors_1_0_sqrt_info(1, 4) + + _tmp906 * priors_1_0_sqrt_info(1, 5) + _tmp914 * _tmp915 - _tmp915 * _tmp917; + const Scalar _tmp919 = (Scalar(1) / Scalar(2)) * _tmp511; + const Scalar _tmp920 = _tmp862 * _tmp919; + const Scalar _tmp921 = _tmp864 * _tmp919; + const Scalar _tmp922 = _tmp519 * _tmp858 - _tmp860 * _tmp920 + _tmp860 * _tmp921; + const Scalar _tmp923 = _tmp557 * _tmp829 + _tmp558 * _tmp832 + _tmp559 * _tmp834; + const Scalar _tmp924 = + ((((_tmp42 + _tmp563) > 0) - ((_tmp42 + _tmp563) < 0)) + 1) / std::pow(_tmp564, Scalar(2)); + const Scalar _tmp925 = _tmp561 * _tmp924; + const Scalar _tmp926 = _tmp569 * _tmp924; + const Scalar _tmp927 = _tmp836 * _tmp926; + const Scalar _tmp928 = _tmp570 * (_tmp559 * _tmp827 + _tmp562) - _tmp923 * _tmp927; + const Scalar _tmp929 = 2 * _tmp571; + const Scalar _tmp930 = + -_tmp567 * _tmp923 * _tmp925 * views_1_calibration(0, 0) + _tmp928 * _tmp929; + const Scalar _tmp931 = _tmp578 / (_tmp572 * std::sqrt(_tmp572)); + const Scalar _tmp932 = (Scalar(1) / Scalar(2)) * _tmp931; + const Scalar _tmp933 = _tmp930 * _tmp932; + const Scalar _tmp934 = _tmp573 * std::pow(_tmp574, _tmp848) * _tmp576 * _tmp577 / _tmp575; + const Scalar _tmp935 = _tmp847 * _tmp934; + const Scalar _tmp936 = _tmp571 * _tmp935; + const Scalar _tmp937 = -_tmp571 * _tmp933 + _tmp579 * _tmp928 + _tmp930 * _tmp936; + const Scalar _tmp938 = _tmp197 * _tmp829 + _tmp198 * _tmp832 + _tmp199 * _tmp834; + const Scalar _tmp939 = + ((((_tmp203 + _tmp42) > 0) - ((_tmp203 + _tmp42) < 0)) + 1) / std::pow(_tmp204, Scalar(2)); + const Scalar _tmp940 = _tmp201 * _tmp939; + const Scalar _tmp941 = _tmp209 * _tmp939; + const Scalar _tmp942 = _tmp836 * _tmp941; + const Scalar _tmp943 = _tmp210 * (_tmp199 * _tmp827 + _tmp202) - _tmp938 * _tmp942; + const Scalar _tmp944 = 2 * _tmp211; + const Scalar _tmp945 = + -_tmp207 * _tmp938 * _tmp940 * views_1_calibration(0, 0) + _tmp943 * _tmp944; + const Scalar _tmp946 = _tmp218 / (_tmp212 * std::sqrt(_tmp212)); + const Scalar _tmp947 = (Scalar(1) / Scalar(2)) * _tmp211; + const Scalar _tmp948 = _tmp946 * _tmp947; + const Scalar _tmp949 = + _tmp177 * _tmp213 * std::pow(_tmp214, _tmp848) * _tmp216 * _tmp217 / _tmp215; + const Scalar _tmp950 = _tmp947 * _tmp949; + const Scalar _tmp951 = _tmp219 * _tmp943 - _tmp945 * _tmp948 + _tmp945 * _tmp950; + const Scalar _tmp952 = _tmp467 * _tmp829 + _tmp468 * _tmp834 + _tmp469 * _tmp832; const Scalar _tmp953 = - _tmp952 * (_priors_1_0_target_T_src[0] * _tmp934 + _tmp947 + _tmp948 + _tmp949); - const Scalar _tmp954 = _priors_1_0_target_T_src[1] * _tmp931; - const Scalar _tmp955 = -_priors_1_0_target_T_src[2] * _tmp830; - const Scalar _tmp956 = -_priors_1_0_target_T_src[0] * _tmp936; - const Scalar _tmp957 = _priors_1_0_target_T_src[3] * _tmp934 + _tmp954 + _tmp955 + _tmp956; - const Scalar _tmp958 = _tmp952 * _tmp957; - const Scalar _tmp959 = - -_tmp107 - _tmp109 + _tmp110 + _tmp888 * _views_0_pose[6] - _tmp888 * _views_1_pose[6]; - const Scalar _tmp960 = _tmp890 * _views_0_pose[4] - _tmp890 * _views_1_pose[4] + - _tmp893 * _views_0_pose[5] - _tmp893 * _views_1_pose[5] + - _tmp895 * _views_0_pose[6] - _tmp895 * _views_1_pose[6]; - const Scalar _tmp961 = _tmp940 * _tmp942 * _tmp950 / (_tmp941 * std::sqrt(_tmp941)); - const Scalar _tmp962 = _tmp938 * _tmp961; - const Scalar _tmp963 = _tmp131 * priors_1_0_sqrt_info(1, 0); - const Scalar _tmp964 = _tmp128 * priors_1_0_sqrt_info(1, 1); - const Scalar _tmp965 = _tmp123 * _tmp962; - const Scalar _tmp966 = _tmp123 * _tmp944; - const Scalar _tmp967 = _priors_1_0_target_T_src[3] * _tmp931; - const Scalar _tmp968 = _priors_1_0_target_T_src[0] * _tmp830; - const Scalar _tmp969 = _priors_1_0_target_T_src[2] * _tmp936; - const Scalar _tmp970 = - _tmp951 * (-_priors_1_0_target_T_src[1] * _tmp934 + _tmp967 - _tmp968 + _tmp969); - const Scalar _tmp971 = _tmp122 * _tmp970; - const Scalar _tmp972 = - -_tmp944 * _tmp964 - _tmp945 * priors_1_0_sqrt_info(1, 0) + - _tmp953 * priors_1_0_sqrt_info(1, 1) + _tmp958 * priors_1_0_sqrt_info(1, 2) + - _tmp959 * priors_1_0_sqrt_info(1, 4) + _tmp960 * priors_1_0_sqrt_info(1, 5) + - _tmp962 * _tmp963 + _tmp962 * _tmp964 + _tmp965 * priors_1_0_sqrt_info(1, 2) - - _tmp966 * priors_1_0_sqrt_info(1, 2) + _tmp971 * priors_1_0_sqrt_info(1, 0); - const Scalar _tmp973 = _tmp561 * _tmp890 + _tmp562 * _tmp893 + _tmp563 * _tmp895; - const Scalar _tmp974 = - ((((_tmp41 + _tmp567) > 0) - ((_tmp41 + _tmp567) < 0)) + 1) / std::pow(_tmp568, Scalar(2)); - const Scalar _tmp975 = _tmp565 * _tmp974; - const Scalar _tmp976 = _tmp573 * _tmp974; - const Scalar _tmp977 = _tmp898 * _tmp976; - const Scalar _tmp978 = _tmp574 * (_tmp563 * _tmp888 + _tmp566) - _tmp973 * _tmp977; - const Scalar _tmp979 = 2 * _tmp575; - const Scalar _tmp980 = - -_tmp571 * _tmp973 * _tmp975 * views_1_calibration(0, 0) + _tmp978 * _tmp979; - const Scalar _tmp981 = (Scalar(1) / Scalar(2)) * _tmp575; - const Scalar _tmp982 = _tmp582 / (_tmp576 * std::sqrt(_tmp576)); - const Scalar _tmp983 = _tmp981 * _tmp982; - const Scalar _tmp984 = - _tmp181 * _tmp577 * std::pow(_tmp578, _tmp906) * _tmp580 * _tmp581 / _tmp579; - const Scalar _tmp985 = _tmp981 * _tmp984; - const Scalar _tmp986 = _tmp583 * _tmp978 - _tmp980 * _tmp983 + _tmp980 * _tmp985; - const Scalar _tmp987 = _tmp131 * priors_1_0_sqrt_info(2, 0); + ((((_tmp42 + _tmp473) > 0) - ((_tmp42 + _tmp473) < 0)) + 1) / std::pow(_tmp474, Scalar(2)); + const Scalar _tmp954 = _tmp471 * _tmp953; + const Scalar _tmp955 = _tmp952 * _tmp954; + const Scalar _tmp956 = _tmp479 * _tmp953; + const Scalar _tmp957 = _tmp836 * _tmp956; + const Scalar _tmp958 = _tmp480 * (_tmp468 * _tmp827 + _tmp472) - _tmp952 * _tmp957; + const Scalar _tmp959 = 2 * _tmp481; + const Scalar _tmp960 = -_tmp477 * _tmp955 * views_1_calibration(0, 0) + _tmp958 * _tmp959; + const Scalar _tmp961 = (Scalar(1) / Scalar(2)) * _tmp481; + const Scalar _tmp962 = + _tmp177 * _tmp483 * _tmp484 * std::pow(_tmp485, _tmp848) * _tmp487 / _tmp486; + const Scalar _tmp963 = _tmp961 * _tmp962; + const Scalar _tmp964 = _tmp488 / (_tmp482 * std::sqrt(_tmp482)); + const Scalar _tmp965 = _tmp961 * _tmp964; + const Scalar _tmp966 = _tmp489 * _tmp958 + _tmp960 * _tmp963 - _tmp960 * _tmp965; + const Scalar _tmp967 = + _tmp905 * priors_1_0_sqrt_info(4, 4) + _tmp906 * priors_1_0_sqrt_info(4, 5); + const Scalar _tmp968 = + ((((_tmp353 + _tmp42) > 0) - ((_tmp353 + _tmp42) < 0)) + 1) / std::pow(_tmp354, Scalar(2)); + const Scalar _tmp969 = _tmp968 * (_tmp347 * _tmp829 + _tmp348 * _tmp832 + _tmp349 * _tmp834); + const Scalar _tmp970 = _tmp359 * _tmp836; + const Scalar _tmp971 = _tmp360 * (_tmp349 * _tmp827 + _tmp352) - _tmp969 * _tmp970; + const Scalar _tmp972 = 2 * _tmp361; + const Scalar _tmp973 = _tmp351 * _tmp969; + const Scalar _tmp974 = -_tmp357 * _tmp973 * views_1_calibration(0, 0) + _tmp971 * _tmp972; + const Scalar _tmp975 = _tmp368 / (_tmp362 * std::sqrt(_tmp362)); + const Scalar _tmp976 = _tmp974 * _tmp975; + const Scalar _tmp977 = (Scalar(1) / Scalar(2)) * _tmp357; + const Scalar _tmp978 = + _tmp177 * _tmp363 * _tmp364 * _tmp365 * std::pow(_tmp366, _tmp848) / _tmp367; + const Scalar _tmp979 = _tmp977 * _tmp978; + const Scalar _tmp980 = -_tmp369 * _tmp866 * _tmp973 + _tmp974 * _tmp979 - _tmp976 * _tmp977; + const Scalar _tmp981 = _tmp527 * _tmp829 + _tmp528 * _tmp834 + _tmp529 * _tmp832; + const Scalar _tmp982 = + ((((_tmp42 + _tmp533) > 0) - ((_tmp42 + _tmp533) < 0)) + 1) / std::pow(_tmp534, Scalar(2)); + const Scalar _tmp983 = _tmp531 * _tmp982; + const Scalar _tmp984 = _tmp539 * _tmp982; + const Scalar _tmp985 = _tmp836 * _tmp984; + const Scalar _tmp986 = _tmp540 * (_tmp528 * _tmp827 + _tmp532) - _tmp981 * _tmp985; + const Scalar _tmp987 = 2 * _tmp541; const Scalar _tmp988 = - -_tmp135 * _tmp944 + _tmp135 * _tmp962 + _tmp136 * _tmp970 - - _tmp945 * priors_1_0_sqrt_info(2, 0) + _tmp953 * priors_1_0_sqrt_info(2, 1) + - _tmp958 * priors_1_0_sqrt_info(2, 2) + _tmp959 * priors_1_0_sqrt_info(2, 4) + - _tmp960 * priors_1_0_sqrt_info(2, 5) + _tmp962 * _tmp987 + - _tmp965 * priors_1_0_sqrt_info(2, 2) - _tmp966 * priors_1_0_sqrt_info(2, 2); - const Scalar _tmp989 = _tmp201 * _tmp890 + _tmp202 * _tmp893 + _tmp203 * _tmp895; - const Scalar _tmp990 = - ((((_tmp207 + _tmp41) > 0) - ((_tmp207 + _tmp41) < 0)) + 1) / std::pow(_tmp208, Scalar(2)); - const Scalar _tmp991 = _tmp205 * _tmp990; - const Scalar _tmp992 = _tmp213 * _tmp990; - const Scalar _tmp993 = _tmp898 * _tmp992; - const Scalar _tmp994 = _tmp214 * (_tmp203 * _tmp888 + _tmp206) - _tmp989 * _tmp993; - const Scalar _tmp995 = 2 * _tmp215; - const Scalar _tmp996 = - -_tmp211 * _tmp989 * _tmp991 * views_1_calibration(0, 0) + _tmp994 * _tmp995; - const Scalar _tmp997 = (Scalar(1) / Scalar(2)) * _tmp215; - const Scalar _tmp998 = _tmp222 / (_tmp216 * std::sqrt(_tmp216)); - const Scalar _tmp999 = _tmp997 * _tmp998; - const Scalar _tmp1000 = - _tmp181 * _tmp217 * std::pow(_tmp218, _tmp906) * _tmp220 * _tmp221 / _tmp219; - const Scalar _tmp1001 = _tmp1000 * _tmp997; - const Scalar _tmp1002 = _tmp1001 * _tmp996 + _tmp223 * _tmp994 - _tmp996 * _tmp999; - const Scalar _tmp1003 = _tmp471 * _tmp890 + _tmp472 * _tmp895 + _tmp473 * _tmp893; - const Scalar _tmp1004 = - ((((_tmp41 + _tmp477) > 0) - ((_tmp41 + _tmp477) < 0)) + 1) / std::pow(_tmp478, Scalar(2)); - const Scalar _tmp1005 = _tmp1004 * _tmp475; - const Scalar _tmp1006 = _tmp1004 * _tmp483; - const Scalar _tmp1007 = _tmp1006 * _tmp898; - const Scalar _tmp1008 = -_tmp1003 * _tmp1007 + _tmp484 * (_tmp472 * _tmp888 + _tmp476); - const Scalar _tmp1009 = 2 * _tmp485; - const Scalar _tmp1010 = - -_tmp1003 * _tmp1005 * _tmp481 * views_1_calibration(0, 0) + _tmp1008 * _tmp1009; - const Scalar _tmp1011 = (Scalar(1) / Scalar(2)) * _tmp181; - const Scalar _tmp1012 = _tmp487 * _tmp488 * std::pow(_tmp489, _tmp906) * _tmp491 / _tmp490; - const Scalar _tmp1013 = _tmp1011 * _tmp1012; - const Scalar _tmp1014 = _tmp1013 * _tmp485; - const Scalar _tmp1015 = _tmp492 / (_tmp486 * std::sqrt(_tmp486)); - const Scalar _tmp1016 = (Scalar(1) / Scalar(2)) * _tmp1015; - const Scalar _tmp1017 = _tmp1016 * _tmp485; - const Scalar _tmp1018 = _tmp1008 * _tmp493 + _tmp1010 * _tmp1014 - _tmp1010 * _tmp1017; - const Scalar _tmp1019 = _tmp54 * priors_0_1_sqrt_info(2, 0); - const Scalar _tmp1020 = - _tmp1019 * _tmp856 - _tmp1019 * _tmp870 + _tmp66 * _tmp856 - _tmp66 * _tmp870 + - _tmp863 * priors_0_1_sqrt_info(2, 0) + _tmp868 * priors_0_1_sqrt_info(2, 1) + - _tmp875 * priors_0_1_sqrt_info(2, 1) + _tmp879 * priors_0_1_sqrt_info(2, 2) - - _tmp881 * priors_0_1_sqrt_info(2, 1); - const Scalar _tmp1021 = _tmp351 * _tmp890 + _tmp352 * _tmp893 + _tmp353 * _tmp895; - const Scalar _tmp1022 = - ((((_tmp357 + _tmp41) > 0) - ((_tmp357 + _tmp41) < 0)) + 1) / std::pow(_tmp358, Scalar(2)); - const Scalar _tmp1023 = _tmp1022 * _tmp363; - const Scalar _tmp1024 = _tmp1023 * _tmp898; - const Scalar _tmp1025 = -_tmp1021 * _tmp1024 + _tmp364 * (_tmp353 * _tmp888 + _tmp356); - const Scalar _tmp1026 = 2 * _tmp365; - const Scalar _tmp1027 = _tmp1022 * _tmp355; - const Scalar _tmp1028 = - -_tmp1021 * _tmp1027 * _tmp361 * views_1_calibration(0, 0) + _tmp1025 * _tmp1026; - const Scalar _tmp1029 = _tmp372 / (_tmp366 * std::sqrt(_tmp366)); - const Scalar _tmp1030 = _tmp1028 * _tmp1029; - const Scalar _tmp1031 = (Scalar(1) / Scalar(2)) * _tmp361; - const Scalar _tmp1032 = _tmp1027 * _tmp924; - const Scalar _tmp1033 = - _tmp181 * _tmp367 * _tmp368 * _tmp369 * std::pow(_tmp370, _tmp906) / _tmp371; - const Scalar _tmp1034 = _tmp1031 * _tmp1033; + -_tmp537 * _tmp981 * _tmp983 * views_1_calibration(0, 0) + _tmp986 * _tmp987; + const Scalar _tmp989 = (Scalar(1) / Scalar(2)) * _tmp537; + const Scalar _tmp990 = _tmp548 / (_tmp542 * std::sqrt(_tmp542)); + const Scalar _tmp991 = _tmp989 * _tmp990; + const Scalar _tmp992 = _tmp866 * _tmp983; + const Scalar _tmp993 = + _tmp177 * _tmp543 * std::pow(_tmp544, _tmp848) * _tmp546 * _tmp547 / _tmp545; + const Scalar _tmp994 = _tmp989 * _tmp993; + const Scalar _tmp995 = -_tmp549 * _tmp981 * _tmp992 - _tmp988 * _tmp991 + _tmp988 * _tmp994; + const Scalar _tmp996 = _priors_0_1_target_T_src[3] * _tmp885; + const Scalar _tmp997 = -_tmp887 + _tmp888 + _tmp889 - _tmp890; + const Scalar _tmp998 = _tmp869 + _tmp870 + _tmp871 + _tmp872; + const Scalar _tmp999 = _priors_0_1_target_T_src[0] * _tmp998; + const Scalar _tmp1000 = _tmp876 + _tmp877 - _tmp878 - _tmp879; + const Scalar _tmp1001 = -_priors_0_1_target_T_src[2] * _tmp1000 - _tmp999; + const Scalar _tmp1002 = -_priors_0_1_target_T_src[1] * _tmp997 + _tmp1001 - _tmp996; + const Scalar _tmp1003 = std::fabs(_tmp17 + _tmp23 + _tmp29 + _tmp35); + const Scalar _tmp1004 = std::min(_tmp1003, _tmp43); + const Scalar _tmp1005 = std::acos(_tmp1004); + const Scalar _tmp1006 = 1 - std::pow(_tmp1004, Scalar(2)); + const Scalar _tmp1007 = _tmp38 * ((((-_tmp1003 + _tmp43) > 0) - ((-_tmp1003 + _tmp43) < 0)) + 1) * + (((-_tmp17 + _tmp36) > 0) - ((-_tmp17 + _tmp36) < 0)); + const Scalar _tmp1008 = _tmp1004 * _tmp1005 * _tmp1007 / (_tmp1006 * std::sqrt(_tmp1006)); + const Scalar _tmp1009 = _tmp1002 * _tmp1008; + const Scalar _tmp1010 = _tmp1009 * _tmp56; + const Scalar _tmp1011 = _priors_0_1_target_T_src[0] * _tmp1000; + const Scalar _tmp1012 = _priors_0_1_target_T_src[2] * _tmp998; + const Scalar _tmp1013 = -_priors_0_1_target_T_src[1] * _tmp885 - _tmp1012; + const Scalar _tmp1014 = _tmp1005 / std::sqrt(_tmp1006); + const Scalar _tmp1015 = _tmp1014 * (_priors_0_1_target_T_src[3] * _tmp997 + _tmp1011 + _tmp1013); + const Scalar _tmp1016 = _tmp1007 / _tmp1006; + const Scalar _tmp1017 = _tmp1002 * _tmp1016; + const Scalar _tmp1018 = _tmp1017 * _tmp56; + const Scalar _tmp1019 = _tmp1009 * _tmp41; + const Scalar _tmp1020 = _priors_0_1_target_T_src[1] * _tmp998; + const Scalar _tmp1021 = _priors_0_1_target_T_src[3] * _tmp1000; + const Scalar _tmp1022 = _priors_0_1_target_T_src[2] * _tmp885; + const Scalar _tmp1023 = + _tmp1014 * (-_priors_0_1_target_T_src[0] * _tmp997 + _tmp1020 + _tmp1021 - _tmp1022); + const Scalar _tmp1024 = _tmp1023 * _tmp39; + const Scalar _tmp1025 = _tmp1016 * _tmp41; + const Scalar _tmp1026 = _tmp1002 * _tmp1025; + const Scalar _tmp1027 = + _tmp1010 * priors_0_1_sqrt_info(1, 2) + _tmp1015 * _tmp64 - + _tmp1018 * priors_0_1_sqrt_info(1, 2) + _tmp1019 * priors_0_1_sqrt_info(1, 1) + + _tmp1024 * priors_0_1_sqrt_info(1, 2) - _tmp1026 * priors_0_1_sqrt_info(1, 1); + const Scalar _tmp1028 = _tmp257 * _tmp829 + _tmp258 * _tmp832 + _tmp259 * _tmp834; + const Scalar _tmp1029 = + ((((_tmp263 + _tmp42) > 0) - ((_tmp263 + _tmp42) < 0)) + 1) / std::pow(_tmp264, Scalar(2)); + const Scalar _tmp1030 = _tmp1029 * _tmp261; + const Scalar _tmp1031 = _tmp1029 * _tmp269; + const Scalar _tmp1032 = _tmp1031 * _tmp836; + const Scalar _tmp1033 = -_tmp1028 * _tmp1032 + _tmp270 * (_tmp259 * _tmp827 + _tmp262); + const Scalar _tmp1034 = 2 * _tmp271; const Scalar _tmp1035 = - -_tmp1021 * _tmp1032 * _tmp373 + _tmp1028 * _tmp1034 - _tmp1030 * _tmp1031; - const Scalar _tmp1036 = _tmp531 * _tmp890 + _tmp532 * _tmp895 + _tmp533 * _tmp893; + -_tmp1028 * _tmp1030 * _tmp267 * views_1_calibration(0, 0) + _tmp1033 * _tmp1034; + const Scalar _tmp1036 = (Scalar(1) / Scalar(2)) * _tmp267; const Scalar _tmp1037 = - ((((_tmp41 + _tmp537) > 0) - ((_tmp41 + _tmp537) < 0)) + 1) / std::pow(_tmp538, Scalar(2)); - const Scalar _tmp1038 = _tmp1037 * _tmp535; - const Scalar _tmp1039 = _tmp1037 * _tmp543; - const Scalar _tmp1040 = _tmp1039 * _tmp898; - const Scalar _tmp1041 = -_tmp1036 * _tmp1040 + _tmp544 * (_tmp532 * _tmp888 + _tmp536); - const Scalar _tmp1042 = 2 * _tmp545; - const Scalar _tmp1043 = - -_tmp1036 * _tmp1038 * _tmp541 * views_1_calibration(0, 0) + _tmp1041 * _tmp1042; - const Scalar _tmp1044 = _tmp552 / (_tmp546 * std::sqrt(_tmp546)); - const Scalar _tmp1045 = (Scalar(1) / Scalar(2)) * _tmp541; - const Scalar _tmp1046 = _tmp1044 * _tmp1045; - const Scalar _tmp1047 = _tmp1038 * _tmp924; - const Scalar _tmp1048 = - _tmp181 * _tmp547 * std::pow(_tmp548, _tmp906) * _tmp550 * _tmp551 / _tmp549; - const Scalar _tmp1049 = _tmp1045 * _tmp1048; - const Scalar _tmp1050 = - -_tmp1036 * _tmp1047 * _tmp553 - _tmp1043 * _tmp1046 + _tmp1043 * _tmp1049; - const Scalar _tmp1051 = _tmp261 * _tmp890 + _tmp262 * _tmp893 + _tmp263 * _tmp895; - const Scalar _tmp1052 = - ((((_tmp267 + _tmp41) > 0) - ((_tmp267 + _tmp41) < 0)) + 1) / std::pow(_tmp268, Scalar(2)); - const Scalar _tmp1053 = _tmp1052 * _tmp265; - const Scalar _tmp1054 = _tmp1052 * _tmp273; - const Scalar _tmp1055 = _tmp1054 * _tmp898; - const Scalar _tmp1056 = -_tmp1051 * _tmp1055 + _tmp274 * (_tmp263 * _tmp888 + _tmp266); - const Scalar _tmp1057 = 2 * _tmp275; + _tmp177 * _tmp273 * _tmp274 * std::pow(_tmp275, _tmp848) * _tmp277 / _tmp276; + const Scalar _tmp1038 = _tmp1036 * _tmp1037; + const Scalar _tmp1039 = _tmp1030 * _tmp866; + const Scalar _tmp1040 = _tmp278 / (_tmp272 * std::sqrt(_tmp272)); + const Scalar _tmp1041 = _tmp1036 * _tmp1040; + const Scalar _tmp1042 = + -_tmp1028 * _tmp1039 * _tmp279 + _tmp1035 * _tmp1038 - _tmp1035 * _tmp1041; + const Scalar _tmp1043 = _tmp317 * _tmp829 + _tmp318 * _tmp832 + _tmp319 * _tmp834; + const Scalar _tmp1044 = + ((((_tmp323 + _tmp42) > 0) - ((_tmp323 + _tmp42) < 0)) + 1) / std::pow(_tmp324, Scalar(2)); + const Scalar _tmp1045 = _tmp1044 * _tmp329; + const Scalar _tmp1046 = _tmp1045 * _tmp836; + const Scalar _tmp1047 = -_tmp1043 * _tmp1046 + _tmp330 * (_tmp319 * _tmp827 + _tmp322); + const Scalar _tmp1048 = 2 * _tmp331; + const Scalar _tmp1049 = _tmp1044 * _tmp321; + const Scalar _tmp1050 = _tmp1049 * views_1_calibration(0, 0); + const Scalar _tmp1051 = _tmp1043 * _tmp1050; + const Scalar _tmp1052 = _tmp1047 * _tmp1048 - _tmp1051 * _tmp327; + const Scalar _tmp1053 = (Scalar(1) / Scalar(2)) * _tmp327; + const Scalar _tmp1054 = _tmp338 / (_tmp332 * std::sqrt(_tmp332)); + const Scalar _tmp1055 = _tmp1053 * _tmp1054; + const Scalar _tmp1056 = + _tmp177 * _tmp333 * _tmp334 * _tmp335 * std::pow(_tmp336, _tmp848) / _tmp337; + const Scalar _tmp1057 = _tmp1053 * _tmp1056; const Scalar _tmp1058 = - -_tmp1051 * _tmp1053 * _tmp271 * views_1_calibration(0, 0) + _tmp1056 * _tmp1057; + -Scalar(1) / Scalar(2) * _tmp1051 * _tmp339 - _tmp1052 * _tmp1055 + _tmp1052 * _tmp1057; const Scalar _tmp1059 = - _tmp181 * _tmp277 * _tmp278 * std::pow(_tmp279, _tmp906) * _tmp281 / _tmp280; - const Scalar _tmp1060 = (Scalar(1) / Scalar(2)) * _tmp1059; - const Scalar _tmp1061 = _tmp1060 * _tmp271; - const Scalar _tmp1062 = _tmp1053 * _tmp924; - const Scalar _tmp1063 = _tmp282 / (_tmp276 * std::sqrt(_tmp276)); - const Scalar _tmp1064 = (Scalar(1) / Scalar(2)) * _tmp1063; - const Scalar _tmp1065 = _tmp1064 * _tmp271; - const Scalar _tmp1066 = - -_tmp1051 * _tmp1062 * _tmp283 + _tmp1058 * _tmp1061 - _tmp1058 * _tmp1065; + ((((_tmp42 + _tmp683) > 0) - ((_tmp42 + _tmp683) < 0)) + 1) / std::pow(_tmp684, Scalar(2)); + const Scalar _tmp1060 = _tmp1059 * (_tmp677 * _tmp829 + _tmp678 * _tmp832 + _tmp679 * _tmp834); + const Scalar _tmp1061 = -_tmp1060 * _tmp689 * _tmp836 + _tmp690 * (_tmp679 * _tmp827 + _tmp682); + const Scalar _tmp1062 = 2 * _tmp691; + const Scalar _tmp1063 = _tmp681 * views_1_calibration(0, 0); + const Scalar _tmp1064 = _tmp1060 * _tmp1063; + const Scalar _tmp1065 = _tmp1061 * _tmp1062 - _tmp1064 * _tmp687; + const Scalar _tmp1066 = (Scalar(1) / Scalar(2)) * _tmp687; const Scalar _tmp1067 = - ((((_tmp327 + _tmp41) > 0) - ((_tmp327 + _tmp41) < 0)) + 1) / std::pow(_tmp328, Scalar(2)); - const Scalar _tmp1068 = _tmp1067 * (_tmp321 * _tmp890 + _tmp322 * _tmp893 + _tmp323 * _tmp895); - const Scalar _tmp1069 = _tmp333 * _tmp898; - const Scalar _tmp1070 = -_tmp1068 * _tmp1069 + _tmp334 * (_tmp323 * _tmp888 + _tmp326); - const Scalar _tmp1071 = 2 * _tmp335; - const Scalar _tmp1072 = _tmp1068 * _tmp325; - const Scalar _tmp1073 = _tmp1070 * _tmp1071 - _tmp1072 * _tmp331 * views_1_calibration(0, 0); - const Scalar _tmp1074 = _tmp342 / (_tmp336 * std::sqrt(_tmp336)); - const Scalar _tmp1075 = (Scalar(1) / Scalar(2)) * _tmp1074; - const Scalar _tmp1076 = _tmp1075 * _tmp331; - const Scalar _tmp1077 = _tmp337 * _tmp338 * _tmp339 * std::pow(_tmp340, _tmp906) / _tmp341; - const Scalar _tmp1078 = _tmp1011 * _tmp1077; - const Scalar _tmp1079 = _tmp1073 * _tmp1078; - const Scalar _tmp1080 = -_tmp1072 * _tmp343 * _tmp924 - _tmp1073 * _tmp1076 + _tmp1079 * _tmp331; - const Scalar _tmp1081 = _tmp681 * _tmp890 + _tmp682 * _tmp893 + _tmp683 * _tmp895; + _tmp177 * _tmp693 * _tmp694 * _tmp695 * std::pow(_tmp696, _tmp848) / _tmp697; + const Scalar _tmp1068 = _tmp1066 * _tmp1067; + const Scalar _tmp1069 = _tmp698 / (_tmp692 * std::sqrt(_tmp692)); + const Scalar _tmp1070 = _tmp1066 * _tmp1069; + const Scalar _tmp1071 = + -Scalar(1) / Scalar(2) * _tmp1064 * _tmp699 + _tmp1065 * _tmp1068 - _tmp1065 * _tmp1070; + const Scalar _tmp1072 = + ((((_tmp42 + _tmp743) > 0) - ((_tmp42 + _tmp743) < 0)) + 1) / std::pow(_tmp744, Scalar(2)); + const Scalar _tmp1073 = _tmp1072 * (_tmp737 * _tmp829 + _tmp738 * _tmp834 + _tmp739 * _tmp832); + const Scalar _tmp1074 = _tmp749 * _tmp836; + const Scalar _tmp1075 = -_tmp1073 * _tmp1074 + _tmp750 * (_tmp738 * _tmp827 + _tmp742); + const Scalar _tmp1076 = 2 * _tmp751; + const Scalar _tmp1077 = _tmp1073 * _tmp741; + const Scalar _tmp1078 = _tmp1075 * _tmp1076 - _tmp1077 * _tmp747 * views_1_calibration(0, 0); + const Scalar _tmp1079 = (Scalar(1) / Scalar(2)) * _tmp751; + const Scalar _tmp1080 = _tmp758 / (_tmp752 * std::sqrt(_tmp752)); + const Scalar _tmp1081 = _tmp1079 * _tmp1080; const Scalar _tmp1082 = - ((((_tmp41 + _tmp687) > 0) - ((_tmp41 + _tmp687) < 0)) + 1) / std::pow(_tmp688, Scalar(2)); - const Scalar _tmp1083 = _tmp1082 * _tmp693; - const Scalar _tmp1084 = _tmp1083 * _tmp898; - const Scalar _tmp1085 = -_tmp1081 * _tmp1084 + _tmp694 * (_tmp683 * _tmp888 + _tmp686); - const Scalar _tmp1086 = 2 * _tmp695; - const Scalar _tmp1087 = _tmp1082 * _tmp685; - const Scalar _tmp1088 = _tmp1081 * _tmp1087; - const Scalar _tmp1089 = _tmp1085 * _tmp1086 - _tmp1088 * _tmp691 * views_1_calibration(0, 0); - const Scalar _tmp1090 = (Scalar(1) / Scalar(2)) * _tmp691; - const Scalar _tmp1091 = - _tmp181 * _tmp697 * _tmp698 * _tmp699 * std::pow(_tmp700, _tmp906) / _tmp701; - const Scalar _tmp1092 = _tmp1090 * _tmp1091; - const Scalar _tmp1093 = _tmp702 / (_tmp696 * std::sqrt(_tmp696)); - const Scalar _tmp1094 = _tmp1090 * _tmp1093; - const Scalar _tmp1095 = -_tmp1088 * _tmp703 * _tmp924 + _tmp1089 * _tmp1092 - _tmp1089 * _tmp1094; - const Scalar _tmp1096 = - ((((_tmp41 + _tmp747) > 0) - ((_tmp41 + _tmp747) < 0)) + 1) / std::pow(_tmp748, Scalar(2)); - const Scalar _tmp1097 = _tmp1096 * (_tmp741 * _tmp890 + _tmp742 * _tmp895 + _tmp743 * _tmp893); - const Scalar _tmp1098 = _tmp753 * _tmp898; - const Scalar _tmp1099 = -_tmp1097 * _tmp1098 + _tmp754 * (_tmp742 * _tmp888 + _tmp746); - const Scalar _tmp1100 = 2 * _tmp755; - const Scalar _tmp1101 = _tmp1097 * _tmp745; - const Scalar _tmp1102 = _tmp1099 * _tmp1100 - _tmp1101 * _tmp751 * views_1_calibration(0, 0); - const Scalar _tmp1103 = _tmp762 / (_tmp756 * std::sqrt(_tmp756)); - const Scalar _tmp1104 = (Scalar(1) / Scalar(2)) * _tmp755; - const Scalar _tmp1105 = _tmp1103 * _tmp1104; - const Scalar _tmp1106 = - _tmp181 * _tmp757 * _tmp758 * _tmp759 * std::pow(_tmp760, _tmp906) / _tmp761; - const Scalar _tmp1107 = _tmp1104 * _tmp1106; - const Scalar _tmp1108 = _tmp1099 * _tmp763 - _tmp1102 * _tmp1105 + _tmp1102 * _tmp1107; - const Scalar _tmp1109 = _tmp157 * _tmp890 + _tmp161 * _tmp893 + _tmp165 * _tmp895; - const Scalar _tmp1110 = - ((((_tmp169 + _tmp41) > 0) - ((_tmp169 + _tmp41) < 0)) + 1) / std::pow(_tmp170, Scalar(2)); - const Scalar _tmp1111 = _tmp1110 * _tmp175; - const Scalar _tmp1112 = _tmp1111 * _tmp898; - const Scalar _tmp1113 = -_tmp1109 * _tmp1112 + _tmp176 * (_tmp165 * _tmp888 + _tmp168); - const Scalar _tmp1114 = _tmp1110 * _tmp167; - const Scalar _tmp1115 = 2 * _tmp177; + _tmp177 * _tmp753 * _tmp754 * _tmp755 * std::pow(_tmp756, _tmp848) / _tmp757; + const Scalar _tmp1083 = _tmp1078 * _tmp1082; + const Scalar _tmp1084 = _tmp1075 * _tmp759 - _tmp1078 * _tmp1081 + _tmp1079 * _tmp1083; + const Scalar _tmp1085 = _tmp153 * _tmp829 + _tmp157 * _tmp832 + _tmp161 * _tmp834; + const Scalar _tmp1086 = + ((((_tmp165 + _tmp42) > 0) - ((_tmp165 + _tmp42) < 0)) + 1) / std::pow(_tmp166, Scalar(2)); + const Scalar _tmp1087 = _tmp1086 * _tmp171; + const Scalar _tmp1088 = _tmp1087 * _tmp836; + const Scalar _tmp1089 = -_tmp1085 * _tmp1088 + _tmp172 * (_tmp161 * _tmp827 + _tmp164); + const Scalar _tmp1090 = _tmp1086 * _tmp163; + const Scalar _tmp1091 = 2 * _tmp173; + const Scalar _tmp1092 = + -_tmp1085 * _tmp1090 * _tmp169 * views_1_calibration(0, 0) + _tmp1089 * _tmp1091; + const Scalar _tmp1093 = (Scalar(1) / Scalar(2)) * _tmp173; + const Scalar _tmp1094 = + _tmp175 * _tmp176 * _tmp177 * std::pow(_tmp181, _tmp848) * _tmp187 / _tmp186; + const Scalar _tmp1095 = _tmp1093 * _tmp1094; + const Scalar _tmp1096 = _tmp188 / (_tmp174 * std::sqrt(_tmp174)); + const Scalar _tmp1097 = _tmp1093 * _tmp1096; + const Scalar _tmp1098 = _tmp1089 * _tmp189 + _tmp1092 * _tmp1095 - _tmp1092 * _tmp1097; + const Scalar _tmp1099 = _tmp377 * _tmp829 + _tmp378 * _tmp832 + _tmp379 * _tmp834; + const Scalar _tmp1100 = + ((((_tmp383 + _tmp42) > 0) - ((_tmp383 + _tmp42) < 0)) + 1) / std::pow(_tmp384, Scalar(2)); + const Scalar _tmp1101 = _tmp1100 * _tmp389; + const Scalar _tmp1102 = _tmp1101 * _tmp836; + const Scalar _tmp1103 = -_tmp1099 * _tmp1102 + _tmp390 * (_tmp379 * _tmp827 + _tmp382); + const Scalar _tmp1104 = 2 * _tmp391; + const Scalar _tmp1105 = _tmp1100 * _tmp381; + const Scalar _tmp1106 = _tmp1099 * _tmp1105; + const Scalar _tmp1107 = _tmp1103 * _tmp1104 - _tmp1106 * _tmp387 * views_1_calibration(0, 0); + const Scalar _tmp1108 = (Scalar(1) / Scalar(2)) * _tmp391; + const Scalar _tmp1109 = + _tmp177 * _tmp393 * _tmp394 * _tmp395 * std::pow(_tmp396, _tmp848) / _tmp397; + const Scalar _tmp1110 = _tmp1108 * _tmp1109; + const Scalar _tmp1111 = _tmp398 / (_tmp392 * std::sqrt(_tmp392)); + const Scalar _tmp1112 = _tmp1108 * _tmp1111; + const Scalar _tmp1113 = _tmp1103 * _tmp399 + _tmp1107 * _tmp1110 - _tmp1107 * _tmp1112; + const Scalar _tmp1114 = + _tmp905 * priors_1_0_sqrt_info(3, 4) + _tmp906 * priors_1_0_sqrt_info(3, 5); + const Scalar _tmp1115 = _tmp707 * _tmp829 + _tmp708 * _tmp834 + _tmp709 * _tmp832; const Scalar _tmp1116 = - -_tmp1109 * _tmp1114 * _tmp173 * views_1_calibration(0, 0) + _tmp1113 * _tmp1115; - const Scalar _tmp1117 = (Scalar(1) / Scalar(2)) * _tmp177; - const Scalar _tmp1118 = - _tmp179 * _tmp180 * _tmp181 * std::pow(_tmp185, _tmp906) * _tmp191 / _tmp190; - const Scalar _tmp1119 = _tmp1117 * _tmp1118; - const Scalar _tmp1120 = _tmp192 / (_tmp178 * std::sqrt(_tmp178)); - const Scalar _tmp1121 = _tmp1117 * _tmp1120; - const Scalar _tmp1122 = _tmp1113 * _tmp193 + _tmp1116 * _tmp1119 - _tmp1116 * _tmp1121; - const Scalar _tmp1123 = _tmp381 * _tmp890 + _tmp382 * _tmp893 + _tmp383 * _tmp895; - const Scalar _tmp1124 = - ((((_tmp387 + _tmp41) > 0) - ((_tmp387 + _tmp41) < 0)) + 1) / std::pow(_tmp388, Scalar(2)); - const Scalar _tmp1125 = _tmp1124 * _tmp393; - const Scalar _tmp1126 = _tmp1125 * _tmp898; - const Scalar _tmp1127 = -_tmp1123 * _tmp1126 + _tmp394 * (_tmp383 * _tmp888 + _tmp386); - const Scalar _tmp1128 = 2 * _tmp395; - const Scalar _tmp1129 = _tmp1124 * _tmp385; - const Scalar _tmp1130 = _tmp1123 * _tmp1129; - const Scalar _tmp1131 = _tmp1127 * _tmp1128 - _tmp1130 * _tmp391 * views_1_calibration(0, 0); - const Scalar _tmp1132 = (Scalar(1) / Scalar(2)) * _tmp395; - const Scalar _tmp1133 = - _tmp181 * _tmp397 * _tmp398 * _tmp399 * std::pow(_tmp400, _tmp906) / _tmp401; - const Scalar _tmp1134 = _tmp1132 * _tmp1133; - const Scalar _tmp1135 = _tmp402 / (_tmp396 * std::sqrt(_tmp396)); - const Scalar _tmp1136 = _tmp1132 * _tmp1135; - const Scalar _tmp1137 = _tmp1127 * _tmp403 + _tmp1131 * _tmp1134 - _tmp1131 * _tmp1136; - const Scalar _tmp1138 = _tmp131 * priors_1_0_sqrt_info(4, 0); - const Scalar _tmp1139 = _tmp128 * _tmp944; - const Scalar _tmp1140 = _tmp128 * _tmp961; - const Scalar _tmp1141 = _tmp1140 * priors_1_0_sqrt_info(4, 1); - const Scalar _tmp1142 = - -_tmp1138 * _tmp944 + _tmp1138 * _tmp962 - _tmp1139 * priors_1_0_sqrt_info(4, 1) + - _tmp1141 * _tmp938 + _tmp953 * priors_1_0_sqrt_info(4, 1) + - _tmp958 * priors_1_0_sqrt_info(4, 2) + _tmp959 * priors_1_0_sqrt_info(4, 4) + - _tmp960 * priors_1_0_sqrt_info(4, 5) + _tmp965 * priors_1_0_sqrt_info(4, 2) - - _tmp966 * priors_1_0_sqrt_info(4, 2) + _tmp971 * priors_1_0_sqrt_info(4, 0); - const Scalar _tmp1143 = _tmp711 * _tmp890 + _tmp712 * _tmp895 + _tmp713 * _tmp893; + ((((_tmp42 + _tmp713) > 0) - ((_tmp42 + _tmp713) < 0)) + 1) / std::pow(_tmp714, Scalar(2)); + const Scalar _tmp1117 = _tmp1116 * _tmp719; + const Scalar _tmp1118 = _tmp1117 * _tmp836; + const Scalar _tmp1119 = -_tmp1115 * _tmp1118 + _tmp720 * (_tmp708 * _tmp827 + _tmp712); + const Scalar _tmp1120 = 2 * _tmp721; + const Scalar _tmp1121 = _tmp1116 * _tmp711; + const Scalar _tmp1122 = _tmp1115 * _tmp1121; + const Scalar _tmp1123 = _tmp1119 * _tmp1120 - _tmp1122 * _tmp717 * views_1_calibration(0, 0); + const Scalar _tmp1124 = _tmp728 / (_tmp722 * std::sqrt(_tmp722)); + const Scalar _tmp1125 = (Scalar(1) / Scalar(2)) * _tmp721; + const Scalar _tmp1126 = _tmp1124 * _tmp1125; + const Scalar _tmp1127 = + _tmp177 * _tmp723 * _tmp724 * _tmp725 * std::pow(_tmp726, _tmp848) / _tmp727; + const Scalar _tmp1128 = _tmp1125 * _tmp1127; + const Scalar _tmp1129 = _tmp1119 * _tmp729 - _tmp1123 * _tmp1126 + _tmp1123 * _tmp1128; + const Scalar _tmp1130 = _tmp227 * _tmp829 + _tmp228 * _tmp834 + _tmp229 * _tmp832; + const Scalar _tmp1131 = + ((((_tmp233 + _tmp42) > 0) - ((_tmp233 + _tmp42) < 0)) + 1) / std::pow(_tmp234, Scalar(2)); + const Scalar _tmp1132 = _tmp1131 * _tmp231; + const Scalar _tmp1133 = _tmp1132 * _tmp866; + const Scalar _tmp1134 = _tmp1131 * _tmp239; + const Scalar _tmp1135 = _tmp1134 * _tmp836; + const Scalar _tmp1136 = -_tmp1130 * _tmp1135 + _tmp240 * (_tmp228 * _tmp827 + _tmp232); + const Scalar _tmp1137 = 2 * _tmp241; + const Scalar _tmp1138 = + -_tmp1130 * _tmp1132 * _tmp237 * views_1_calibration(0, 0) + _tmp1136 * _tmp1137; + const Scalar _tmp1139 = (Scalar(1) / Scalar(2)) * _tmp237; + const Scalar _tmp1140 = + _tmp177 * _tmp243 * _tmp244 * _tmp245 * std::pow(_tmp246, _tmp848) / _tmp247; + const Scalar _tmp1141 = _tmp1139 * _tmp1140; + const Scalar _tmp1142 = _tmp248 / (_tmp242 * std::sqrt(_tmp242)); + const Scalar _tmp1143 = _tmp1139 * _tmp1142; const Scalar _tmp1144 = - ((((_tmp41 + _tmp717) > 0) - ((_tmp41 + _tmp717) < 0)) + 1) / std::pow(_tmp718, Scalar(2)); - const Scalar _tmp1145 = _tmp1144 * _tmp723; - const Scalar _tmp1146 = _tmp1145 * _tmp898; - const Scalar _tmp1147 = -_tmp1143 * _tmp1146 + _tmp724 * (_tmp712 * _tmp888 + _tmp716); - const Scalar _tmp1148 = 2 * _tmp725; - const Scalar _tmp1149 = _tmp1144 * _tmp715; - const Scalar _tmp1150 = - -_tmp1143 * _tmp1149 * _tmp721 * views_1_calibration(0, 0) + _tmp1147 * _tmp1148; - const Scalar _tmp1151 = _tmp732 / (_tmp726 * std::sqrt(_tmp726)); - const Scalar _tmp1152 = (Scalar(1) / Scalar(2)) * _tmp1151; - const Scalar _tmp1153 = _tmp1152 * _tmp725; - const Scalar _tmp1154 = - _tmp181 * _tmp727 * _tmp728 * _tmp729 * std::pow(_tmp730, _tmp906) / _tmp731; - const Scalar _tmp1155 = (Scalar(1) / Scalar(2)) * _tmp1154; - const Scalar _tmp1156 = _tmp1155 * _tmp725; - const Scalar _tmp1157 = _tmp1147 * _tmp733 - _tmp1150 * _tmp1153 + _tmp1150 * _tmp1156; - const Scalar _tmp1158 = _tmp231 * _tmp890 + _tmp232 * _tmp895 + _tmp233 * _tmp893; - const Scalar _tmp1159 = - ((((_tmp237 + _tmp41) > 0) - ((_tmp237 + _tmp41) < 0)) + 1) / std::pow(_tmp238, Scalar(2)); - const Scalar _tmp1160 = _tmp235 * _tmp924; - const Scalar _tmp1161 = _tmp1159 * _tmp1160; - const Scalar _tmp1162 = _tmp1159 * _tmp235; - const Scalar _tmp1163 = _tmp1159 * _tmp243; - const Scalar _tmp1164 = _tmp1163 * _tmp898; - const Scalar _tmp1165 = -_tmp1158 * _tmp1164 + _tmp244 * (_tmp232 * _tmp888 + _tmp236); - const Scalar _tmp1166 = 2 * _tmp245; - const Scalar _tmp1167 = - -_tmp1158 * _tmp1162 * _tmp241 * views_1_calibration(0, 0) + _tmp1165 * _tmp1166; - const Scalar _tmp1168 = (Scalar(1) / Scalar(2)) * _tmp241; + -_tmp1130 * _tmp1133 * _tmp249 + _tmp1138 * _tmp1141 - _tmp1138 * _tmp1143; + const Scalar _tmp1145 = _tmp122 * priors_1_0_sqrt_info(0, 2); + const Scalar _tmp1146 = _tmp898 * _tmp903; + const Scalar _tmp1147 = _priors_1_0_target_T_src[3] * _tmp873; + const Scalar _tmp1148 = _priors_1_0_target_T_src[0] * _tmp885; + const Scalar _tmp1149 = _priors_1_0_target_T_src[2] * _tmp891; + const Scalar _tmp1150 = _tmp898 * priors_1_0_sqrt_info(0, 0); + const Scalar _tmp1151 = + _tmp1145 * _tmp914 - _tmp1145 * _tmp917 + _tmp1146 * priors_1_0_sqrt_info(0, 2) + + _tmp1150 * (-_priors_1_0_target_T_src[1] * _tmp880 + _tmp1147 - _tmp1148 + _tmp1149) + + _tmp129 * _tmp914 - _tmp129 * _tmp917 + _tmp130 * _tmp914 - _tmp130 * _tmp917 + + _tmp899 * priors_1_0_sqrt_info(0, 1) + _tmp905 * priors_1_0_sqrt_info(0, 4) + + _tmp906 * priors_1_0_sqrt_info(0, 5); + const Scalar _tmp1152 = _tmp122 * priors_1_0_sqrt_info(2, 2); + const Scalar _tmp1153 = _tmp1152 * _tmp913; + const Scalar _tmp1154 = _tmp1146 * priors_1_0_sqrt_info(2, 2) - _tmp1152 * _tmp917 + + _tmp1153 * _tmp911 + _tmp905 * priors_1_0_sqrt_info(2, 4) + + _tmp906 * priors_1_0_sqrt_info(2, 5); + const Scalar _tmp1155 = (Scalar(1) / Scalar(2)) * _tmp717; + const Scalar _tmp1156 = _tmp1127 * _tmp1155; + const Scalar _tmp1157 = _tmp1124 * _tmp1155; + const Scalar _tmp1158 = -_tmp1122 * _tmp729 * _tmp866 + _tmp1123 * _tmp1156 - _tmp1123 * _tmp1157; + const Scalar _tmp1159 = (Scalar(1) / Scalar(2)) * _tmp747; + const Scalar _tmp1160 = _tmp1080 * _tmp1159; + const Scalar _tmp1161 = -_tmp1077 * _tmp759 * _tmp866 - _tmp1078 * _tmp1160 + _tmp1083 * _tmp1159; + const Scalar _tmp1162 = _tmp287 * _tmp829 + _tmp288 * _tmp834 + _tmp289 * _tmp832; + const Scalar _tmp1163 = + ((((_tmp293 + _tmp42) > 0) - ((_tmp293 + _tmp42) < 0)) + 1) / std::pow(_tmp294, Scalar(2)); + const Scalar _tmp1164 = _tmp1163 * _tmp299; + const Scalar _tmp1165 = _tmp1164 * _tmp836; + const Scalar _tmp1166 = -_tmp1162 * _tmp1165 + _tmp300 * (_tmp288 * _tmp827 + _tmp292); + const Scalar _tmp1167 = 2 * _tmp301; + const Scalar _tmp1168 = _tmp1163 * _tmp291; const Scalar _tmp1169 = - _tmp181 * _tmp247 * _tmp248 * _tmp249 * std::pow(_tmp250, _tmp906) / _tmp251; - const Scalar _tmp1170 = _tmp1168 * _tmp1169; - const Scalar _tmp1171 = _tmp252 / (_tmp246 * std::sqrt(_tmp246)); - const Scalar _tmp1172 = _tmp1168 * _tmp1171; + -_tmp1162 * _tmp1168 * _tmp297 * views_1_calibration(0, 0) + _tmp1166 * _tmp1167; + const Scalar _tmp1170 = (Scalar(1) / Scalar(2)) * _tmp301; + const Scalar _tmp1171 = _tmp308 / (_tmp302 * std::sqrt(_tmp302)); + const Scalar _tmp1172 = _tmp1170 * _tmp1171; const Scalar _tmp1173 = - -_tmp1158 * _tmp1161 * _tmp253 + _tmp1167 * _tmp1170 - _tmp1167 * _tmp1172; - const Scalar _tmp1174 = _tmp128 * priors_1_0_sqrt_info(0, 1); - const Scalar _tmp1175 = _tmp131 * priors_1_0_sqrt_info(0, 0); - const Scalar _tmp1176 = - -_tmp1174 * _tmp944 + _tmp1174 * _tmp962 - _tmp1175 * _tmp944 + _tmp1175 * _tmp962 + - _tmp953 * priors_1_0_sqrt_info(0, 1) + _tmp958 * priors_1_0_sqrt_info(0, 2) + - _tmp959 * priors_1_0_sqrt_info(0, 4) + _tmp960 * priors_1_0_sqrt_info(0, 5) + - _tmp965 * priors_1_0_sqrt_info(0, 2) - _tmp966 * priors_1_0_sqrt_info(0, 2) + - _tmp971 * priors_1_0_sqrt_info(0, 0); - const Scalar _tmp1177 = _tmp1155 * _tmp721; - const Scalar _tmp1178 = _tmp1152 * _tmp721; - const Scalar _tmp1179 = _tmp1149 * _tmp924; - const Scalar _tmp1180 = - -_tmp1143 * _tmp1179 * _tmp733 + _tmp1150 * _tmp1177 - _tmp1150 * _tmp1178; - const Scalar _tmp1181 = (Scalar(1) / Scalar(2)) * _tmp751; - const Scalar _tmp1182 = _tmp1106 * _tmp1181; - const Scalar _tmp1183 = _tmp1103 * _tmp1181; - const Scalar _tmp1184 = -_tmp1101 * _tmp763 * _tmp924 + _tmp1102 * _tmp1182 - _tmp1102 * _tmp1183; - const Scalar _tmp1185 = _tmp57 * priors_0_1_sqrt_info(5, 2); - const Scalar _tmp1186 = - _tmp1185 * _tmp856 - _tmp1185 * _tmp870 + _tmp863 * priors_0_1_sqrt_info(5, 0) + - _tmp868 * priors_0_1_sqrt_info(5, 1) - _tmp872 * priors_0_1_sqrt_info(5, 0) + - _tmp873 * priors_0_1_sqrt_info(5, 0) + _tmp875 * priors_0_1_sqrt_info(5, 1) + - _tmp879 * priors_0_1_sqrt_info(5, 2) - _tmp881 * priors_0_1_sqrt_info(5, 1); - const Scalar _tmp1187 = - ((((_tmp297 + _tmp41) > 0) - ((_tmp297 + _tmp41) < 0)) + 1) / std::pow(_tmp298, Scalar(2)); - const Scalar _tmp1188 = _tmp1187 * (_tmp291 * _tmp890 + _tmp292 * _tmp895 + _tmp293 * _tmp893); - const Scalar _tmp1189 = _tmp303 * _tmp898; - const Scalar _tmp1190 = -_tmp1188 * _tmp1189 + _tmp304 * (_tmp292 * _tmp888 + _tmp296); - const Scalar _tmp1191 = 2 * _tmp305; - const Scalar _tmp1192 = - -_tmp1188 * _tmp295 * _tmp301 * views_1_calibration(0, 0) + _tmp1190 * _tmp1191; - const Scalar _tmp1193 = _tmp312 / (_tmp306 * std::sqrt(_tmp306)); - const Scalar _tmp1194 = (Scalar(1) / Scalar(2)) * _tmp305; - const Scalar _tmp1195 = _tmp1193 * _tmp1194; - const Scalar _tmp1196 = - _tmp181 * _tmp307 * _tmp308 * _tmp309 * std::pow(_tmp310, _tmp906) / _tmp311; - const Scalar _tmp1197 = _tmp1194 * _tmp1196; - const Scalar _tmp1198 = _tmp1190 * _tmp313 - _tmp1192 * _tmp1195 + _tmp1192 * _tmp1197; - const Scalar _tmp1199 = _tmp621 * _tmp890 + _tmp622 * _tmp895 + _tmp623 * _tmp893; - const Scalar _tmp1200 = - ((((_tmp41 + _tmp627) > 0) - ((_tmp41 + _tmp627) < 0)) + 1) / std::pow(_tmp628, Scalar(2)); - const Scalar _tmp1201 = _tmp1200 * _tmp633; - const Scalar _tmp1202 = _tmp1201 * _tmp898; - const Scalar _tmp1203 = -_tmp1199 * _tmp1202 + _tmp634 * (_tmp622 * _tmp888 + _tmp626); - const Scalar _tmp1204 = 2 * _tmp635; - const Scalar _tmp1205 = - -_tmp1199 * _tmp1200 * _tmp625 * _tmp631 * views_1_calibration(0, 0) + _tmp1203 * _tmp1204; - const Scalar _tmp1206 = _tmp642 / (_tmp636 * std::sqrt(_tmp636)); - const Scalar _tmp1207 = (Scalar(1) / Scalar(2)) * _tmp631; - const Scalar _tmp1208 = _tmp1206 * _tmp1207; - const Scalar _tmp1209 = _tmp625 * _tmp924; - const Scalar _tmp1210 = _tmp1200 * _tmp1209; - const Scalar _tmp1211 = - _tmp181 * _tmp637 * std::pow(_tmp638, _tmp906) * _tmp640 * _tmp641 / _tmp639; - const Scalar _tmp1212 = _tmp1207 * _tmp1211; - const Scalar _tmp1213 = - -_tmp1199 * _tmp1210 * _tmp643 - _tmp1205 * _tmp1208 + _tmp1205 * _tmp1212; - const Scalar _tmp1214 = (Scalar(1) / Scalar(2)) * _tmp391; - const Scalar _tmp1215 = _tmp1133 * _tmp1214; - const Scalar _tmp1216 = _tmp1135 * _tmp1214; - const Scalar _tmp1217 = -_tmp1130 * _tmp403 * _tmp924 + _tmp1131 * _tmp1215 - _tmp1131 * _tmp1216; - const Scalar _tmp1218 = _tmp411 * _tmp890 + _tmp412 * _tmp893 + _tmp413 * _tmp895; - const Scalar _tmp1219 = - ((((_tmp41 + _tmp417) > 0) - ((_tmp41 + _tmp417) < 0)) + 1) / std::pow(_tmp418, Scalar(2)); - const Scalar _tmp1220 = _tmp1219 * _tmp415; - const Scalar _tmp1221 = _tmp1218 * _tmp1220; - const Scalar _tmp1222 = _tmp1219 * _tmp423; - const Scalar _tmp1223 = _tmp1222 * _tmp898; - const Scalar _tmp1224 = -_tmp1218 * _tmp1223 + _tmp424 * (_tmp413 * _tmp888 + _tmp416); - const Scalar _tmp1225 = 2 * _tmp425; - const Scalar _tmp1226 = -_tmp1221 * _tmp421 * views_1_calibration(0, 0) + _tmp1224 * _tmp1225; - const Scalar _tmp1227 = (Scalar(1) / Scalar(2)) * _tmp425; - const Scalar _tmp1228 = - _tmp181 * _tmp427 * _tmp428 * std::pow(_tmp429, _tmp906) * _tmp431 / _tmp430; - const Scalar _tmp1229 = _tmp1227 * _tmp1228; - const Scalar _tmp1230 = _tmp432 / (_tmp426 * std::sqrt(_tmp426)); - const Scalar _tmp1231 = _tmp1227 * _tmp1230; - const Scalar _tmp1232 = _tmp1224 * _tmp433 + _tmp1226 * _tmp1229 - _tmp1226 * _tmp1231; - const Scalar _tmp1233 = _tmp131 * priors_1_0_sqrt_info(5, 0); - const Scalar _tmp1234 = _tmp128 * priors_1_0_sqrt_info(5, 1); - const Scalar _tmp1235 = - -_tmp1139 * priors_1_0_sqrt_info(5, 1) - _tmp1233 * _tmp944 + _tmp1233 * _tmp962 + - _tmp1234 * _tmp962 + _tmp953 * priors_1_0_sqrt_info(5, 1) + - _tmp958 * priors_1_0_sqrt_info(5, 2) + _tmp959 * priors_1_0_sqrt_info(5, 4) + - _tmp960 * priors_1_0_sqrt_info(5, 5) + _tmp965 * priors_1_0_sqrt_info(5, 2) - - _tmp966 * priors_1_0_sqrt_info(5, 2) + _tmp971 * priors_1_0_sqrt_info(5, 0); - const Scalar _tmp1236 = _tmp1114 * _tmp924; - const Scalar _tmp1237 = (Scalar(1) / Scalar(2)) * _tmp173; - const Scalar _tmp1238 = _tmp1120 * _tmp1237; - const Scalar _tmp1239 = _tmp1118 * _tmp1237; - const Scalar _tmp1240 = - -_tmp1109 * _tmp1236 * _tmp193 - _tmp1116 * _tmp1238 + _tmp1116 * _tmp1239; - const Scalar _tmp1241 = (Scalar(1) / Scalar(2)) * _tmp695; - const Scalar _tmp1242 = _tmp1093 * _tmp1241; - const Scalar _tmp1243 = _tmp1091 * _tmp1241; - const Scalar _tmp1244 = _tmp1085 * _tmp703 - _tmp1089 * _tmp1242 + _tmp1089 * _tmp1243; - const Scalar _tmp1245 = _tmp924 * _tmp975; - const Scalar _tmp1246 = (Scalar(1) / Scalar(2)) * _tmp571; - const Scalar _tmp1247 = _tmp1246 * _tmp982; - const Scalar _tmp1248 = _tmp1246 * _tmp984; - const Scalar _tmp1249 = -_tmp1245 * _tmp583 * _tmp973 - _tmp1247 * _tmp980 + _tmp1248 * _tmp980; - const Scalar _tmp1250 = _tmp595 * _tmp924; - const Scalar _tmp1251 = (Scalar(1) / Scalar(2)) * _tmp601; - const Scalar _tmp1252 = _tmp1251 * _tmp907; - const Scalar _tmp1253 = _tmp1251 * _tmp903; - const Scalar _tmp1254 = -_tmp1250 * _tmp613 * _tmp897 + _tmp1252 * _tmp902 - _tmp1253 * _tmp902; - const Scalar _tmp1255 = (Scalar(1) / Scalar(2)) * _tmp365; - const Scalar _tmp1256 = _tmp1033 * _tmp1255; - const Scalar _tmp1257 = _tmp1025 * _tmp373 + _tmp1028 * _tmp1256 - _tmp1030 * _tmp1255; - const Scalar _tmp1258 = (Scalar(1) / Scalar(2)) * _tmp545; - const Scalar _tmp1259 = _tmp1044 * _tmp1258; - const Scalar _tmp1260 = _tmp1048 * _tmp1258; - const Scalar _tmp1261 = _tmp1041 * _tmp553 - _tmp1043 * _tmp1259 + _tmp1043 * _tmp1260; - const Scalar _tmp1262 = _tmp54 * priors_0_1_sqrt_info(0, 0); - const Scalar _tmp1263 = - _tmp1262 * _tmp856 - _tmp1262 * _tmp870 + _tmp857 * priors_0_1_sqrt_info(0, 2) + - _tmp863 * priors_0_1_sqrt_info(0, 0) + _tmp868 * priors_0_1_sqrt_info(0, 1) - - _tmp871 * priors_0_1_sqrt_info(0, 2) + _tmp875 * priors_0_1_sqrt_info(0, 1) + - _tmp879 * priors_0_1_sqrt_info(0, 2) - _tmp881 * priors_0_1_sqrt_info(0, 1); - const Scalar _tmp1264 = _tmp131 * priors_1_0_sqrt_info(3, 0); - const Scalar _tmp1265 = _tmp1140 * priors_1_0_sqrt_info(3, 1); - const Scalar _tmp1266 = - -_tmp1139 * priors_1_0_sqrt_info(3, 1) - _tmp1264 * _tmp944 + _tmp1264 * _tmp962 + - _tmp1265 * _tmp938 + _tmp138 * _tmp951 * _tmp957 + _tmp953 * priors_1_0_sqrt_info(3, 1) + - _tmp959 * priors_1_0_sqrt_info(3, 4) + _tmp960 * priors_1_0_sqrt_info(3, 5) + - _tmp965 * priors_1_0_sqrt_info(3, 2) - _tmp966 * priors_1_0_sqrt_info(3, 2) + - _tmp971 * priors_1_0_sqrt_info(3, 0); - const Scalar _tmp1267 = (Scalar(1) / Scalar(2)) * _tmp421; - const Scalar _tmp1268 = _tmp1230 * _tmp1267; - const Scalar _tmp1269 = _tmp1228 * _tmp1267; - const Scalar _tmp1270 = -_tmp1221 * _tmp433 * _tmp924 - _tmp1226 * _tmp1268 + _tmp1226 * _tmp1269; - const Scalar _tmp1271 = _tmp1064 * _tmp275; - const Scalar _tmp1272 = _tmp1060 * _tmp275; - const Scalar _tmp1273 = _tmp1056 * _tmp283 - _tmp1058 * _tmp1271 + _tmp1058 * _tmp1272; - const Scalar _tmp1274 = (Scalar(1) / Scalar(2)) * _tmp245; - const Scalar _tmp1275 = _tmp1169 * _tmp1274; - const Scalar _tmp1276 = _tmp1171 * _tmp1274; - const Scalar _tmp1277 = _tmp1165 * _tmp253 + _tmp1167 * _tmp1275 - _tmp1167 * _tmp1276; - const Scalar _tmp1278 = - _tmp857 * priors_0_1_sqrt_info(3, 2) + _tmp863 * priors_0_1_sqrt_info(3, 0) + - _tmp868 * priors_0_1_sqrt_info(3, 1) - _tmp871 * priors_0_1_sqrt_info(3, 2) - - _tmp872 * priors_0_1_sqrt_info(3, 0) + _tmp873 * priors_0_1_sqrt_info(3, 0) + - _tmp875 * priors_0_1_sqrt_info(3, 1) + _tmp879 * priors_0_1_sqrt_info(3, 2) - - _tmp881 * priors_0_1_sqrt_info(3, 1); - const Scalar _tmp1279 = _tmp1075 * _tmp335; - const Scalar _tmp1280 = _tmp1070 * _tmp343 - _tmp1073 * _tmp1279 + _tmp1079 * _tmp335; - const Scalar _tmp1281 = _tmp295 * _tmp924; - const Scalar _tmp1282 = (Scalar(1) / Scalar(2)) * _tmp301; - const Scalar _tmp1283 = _tmp1196 * _tmp1282; - const Scalar _tmp1284 = _tmp1193 * _tmp1282; - const Scalar _tmp1285 = - -_tmp1188 * _tmp1281 * _tmp313 + _tmp1192 * _tmp1283 - _tmp1192 * _tmp1284; - const Scalar _tmp1286 = _tmp441 * _tmp890 + _tmp442 * _tmp893 + _tmp443 * _tmp895; - const Scalar _tmp1287 = - ((((_tmp41 + _tmp447) > 0) - ((_tmp41 + _tmp447) < 0)) + 1) / std::pow(_tmp448, Scalar(2)); - const Scalar _tmp1288 = _tmp1287 * _tmp445; - const Scalar _tmp1289 = _tmp1287 * _tmp453; - const Scalar _tmp1290 = _tmp1289 * _tmp898; - const Scalar _tmp1291 = -_tmp1286 * _tmp1290 + _tmp454 * (_tmp443 * _tmp888 + _tmp446); - const Scalar _tmp1292 = 2 * _tmp455; - const Scalar _tmp1293 = - -_tmp1286 * _tmp1288 * _tmp451 * views_1_calibration(0, 0) + _tmp1291 * _tmp1292; - const Scalar _tmp1294 = (Scalar(1) / Scalar(2)) * _tmp455; - const Scalar _tmp1295 = - _tmp181 * _tmp457 * _tmp458 * _tmp459 * std::pow(_tmp460, _tmp906) / _tmp461; - const Scalar _tmp1296 = _tmp1294 * _tmp1295; - const Scalar _tmp1297 = _tmp462 / (_tmp456 * std::sqrt(_tmp456)); - const Scalar _tmp1298 = _tmp1294 * _tmp1297; - const Scalar _tmp1299 = _tmp1291 * _tmp463 + _tmp1293 * _tmp1296 - _tmp1293 * _tmp1298; - const Scalar _tmp1300 = (Scalar(1) / Scalar(2)) * _tmp635; - const Scalar _tmp1301 = _tmp1211 * _tmp1300; - const Scalar _tmp1302 = _tmp1206 * _tmp1300; - const Scalar _tmp1303 = _tmp1203 * _tmp643 + _tmp1205 * _tmp1301 - _tmp1205 * _tmp1302; - const Scalar _tmp1304 = - ((((_tmp41 + _tmp657) > 0) - ((_tmp41 + _tmp657) < 0)) + 1) / std::pow(_tmp658, Scalar(2)); - const Scalar _tmp1305 = _tmp1304 * (_tmp651 * _tmp890 + _tmp652 * _tmp893 + _tmp653 * _tmp895); - const Scalar _tmp1306 = _tmp663 * _tmp898; - const Scalar _tmp1307 = -_tmp1305 * _tmp1306 + _tmp664 * (_tmp653 * _tmp888 + _tmp656); - const Scalar _tmp1308 = 2 * _tmp665; - const Scalar _tmp1309 = - -_tmp1305 * _tmp655 * _tmp661 * views_1_calibration(0, 0) + _tmp1307 * _tmp1308; - const Scalar _tmp1310 = (Scalar(1) / Scalar(2)) * _tmp665; + _tmp177 * _tmp303 * _tmp304 * _tmp305 * std::pow(_tmp306, _tmp848) / _tmp307; + const Scalar _tmp1174 = _tmp1170 * _tmp1173; + const Scalar _tmp1175 = _tmp1166 * _tmp309 - _tmp1169 * _tmp1172 + _tmp1169 * _tmp1174; + const Scalar _tmp1176 = _tmp617 * _tmp829 + _tmp618 * _tmp834 + _tmp619 * _tmp832; + const Scalar _tmp1177 = + ((((_tmp42 + _tmp623) > 0) - ((_tmp42 + _tmp623) < 0)) + 1) / std::pow(_tmp624, Scalar(2)); + const Scalar _tmp1178 = _tmp1177 * _tmp621; + const Scalar _tmp1179 = _tmp1177 * _tmp629; + const Scalar _tmp1180 = _tmp1179 * _tmp836; + const Scalar _tmp1181 = -_tmp1176 * _tmp1180 + _tmp630 * (_tmp618 * _tmp827 + _tmp622); + const Scalar _tmp1182 = 2 * _tmp631; + const Scalar _tmp1183 = + -_tmp1176 * _tmp1178 * _tmp627 * views_1_calibration(0, 0) + _tmp1181 * _tmp1182; + const Scalar _tmp1184 = _tmp638 / (_tmp632 * std::sqrt(_tmp632)); + const Scalar _tmp1185 = (Scalar(1) / Scalar(2)) * _tmp627; + const Scalar _tmp1186 = _tmp1184 * _tmp1185; + const Scalar _tmp1187 = _tmp1178 * _tmp866; + const Scalar _tmp1188 = + _tmp177 * _tmp633 * std::pow(_tmp634, _tmp848) * _tmp636 * _tmp637 / _tmp635; + const Scalar _tmp1189 = _tmp1185 * _tmp1188; + const Scalar _tmp1190 = + -_tmp1176 * _tmp1187 * _tmp639 - _tmp1183 * _tmp1186 + _tmp1183 * _tmp1189; + const Scalar _tmp1191 = (Scalar(1) / Scalar(2)) * _tmp387; + const Scalar _tmp1192 = _tmp1109 * _tmp1191; + const Scalar _tmp1193 = _tmp1111 * _tmp1191; + const Scalar _tmp1194 = -_tmp1106 * _tmp399 * _tmp866 + _tmp1107 * _tmp1192 - _tmp1107 * _tmp1193; + const Scalar _tmp1195 = + ((((_tmp413 + _tmp42) > 0) - ((_tmp413 + _tmp42) < 0)) + 1) / std::pow(_tmp414, Scalar(2)); + const Scalar _tmp1196 = _tmp1195 * (_tmp407 * _tmp829 + _tmp408 * _tmp832 + _tmp409 * _tmp834); + const Scalar _tmp1197 = _tmp411 * views_1_calibration(0, 0); + const Scalar _tmp1198 = _tmp1196 * _tmp1197; + const Scalar _tmp1199 = -_tmp1196 * _tmp419 * _tmp836 + _tmp420 * (_tmp409 * _tmp827 + _tmp412); + const Scalar _tmp1200 = 2 * _tmp421; + const Scalar _tmp1201 = -_tmp1198 * _tmp417 + _tmp1199 * _tmp1200; + const Scalar _tmp1202 = (Scalar(1) / Scalar(2)) * _tmp421; + const Scalar _tmp1203 = + _tmp177 * _tmp423 * _tmp424 * std::pow(_tmp425, _tmp848) * _tmp427 / _tmp426; + const Scalar _tmp1204 = _tmp1202 * _tmp1203; + const Scalar _tmp1205 = _tmp428 / (_tmp422 * std::sqrt(_tmp422)); + const Scalar _tmp1206 = _tmp1202 * _tmp1205; + const Scalar _tmp1207 = _tmp1199 * _tmp429 + _tmp1201 * _tmp1204 - _tmp1201 * _tmp1206; + const Scalar _tmp1208 = _tmp1090 * _tmp866; + const Scalar _tmp1209 = (Scalar(1) / Scalar(2)) * _tmp169; + const Scalar _tmp1210 = _tmp1096 * _tmp1209; + const Scalar _tmp1211 = _tmp1094 * _tmp1209; + const Scalar _tmp1212 = + -_tmp1085 * _tmp1208 * _tmp189 - _tmp1092 * _tmp1210 + _tmp1092 * _tmp1211; + const Scalar _tmp1213 = (Scalar(1) / Scalar(2)) * _tmp691; + const Scalar _tmp1214 = _tmp1069 * _tmp1213; + const Scalar _tmp1215 = _tmp1067 * _tmp1213; + const Scalar _tmp1216 = _tmp1061 * _tmp699 - _tmp1065 * _tmp1214 + _tmp1065 * _tmp1215; + const Scalar _tmp1217 = std::pow(priors_1_0_sqrt_info(5, 5), Scalar(2)); + const Scalar _tmp1218 = _tmp1217 * _tmp906; + const Scalar _tmp1219 = _tmp866 * _tmp925; + const Scalar _tmp1220 = _tmp567 * _tmp935; + const Scalar _tmp1221 = -_tmp1219 * _tmp579 * _tmp923 + _tmp1220 * _tmp930 - _tmp567 * _tmp933; + const Scalar _tmp1222 = _tmp841 * _tmp866; + const Scalar _tmp1223 = _tmp597 * _tmp850; + const Scalar _tmp1224 = _tmp597 * _tmp845; + const Scalar _tmp1225 = -_tmp1222 * _tmp609 * _tmp835 + _tmp1223 * _tmp843 - _tmp1224 * _tmp843; + const Scalar _tmp1226 = (Scalar(1) / Scalar(2)) * _tmp361; + const Scalar _tmp1227 = _tmp1226 * _tmp978; + const Scalar _tmp1228 = -_tmp1226 * _tmp976 + _tmp1227 * _tmp974 + _tmp369 * _tmp971; + const Scalar _tmp1229 = (Scalar(1) / Scalar(2)) * _tmp541; + const Scalar _tmp1230 = _tmp1229 * _tmp990; + const Scalar _tmp1231 = _tmp1229 * _tmp993; + const Scalar _tmp1232 = -_tmp1230 * _tmp988 + _tmp1231 * _tmp988 + _tmp549 * _tmp986; + const Scalar _tmp1233 = -_priors_0_1_target_T_src[1] * _tmp1000; + const Scalar _tmp1234 = _priors_0_1_target_T_src[3] * _tmp998; + const Scalar _tmp1235 = -_priors_0_1_target_T_src[0] * _tmp885 + _tmp1234; + const Scalar _tmp1236 = _tmp1014 * _tmp55; + const Scalar _tmp1237 = _tmp54 * priors_0_1_sqrt_info(0, 0); + const Scalar _tmp1238 = + _tmp1009 * _tmp1237 + _tmp1010 * priors_0_1_sqrt_info(0, 2) + _tmp1015 * _tmp40 - + _tmp1017 * _tmp1237 - _tmp1018 * priors_0_1_sqrt_info(0, 2) + + _tmp1019 * priors_0_1_sqrt_info(0, 1) + _tmp1024 * priors_0_1_sqrt_info(0, 2) - + _tmp1026 * priors_0_1_sqrt_info(0, 1) + + _tmp1236 * (_priors_0_1_target_T_src[2] * _tmp997 + _tmp1233 + _tmp1235); + const Scalar _tmp1239 = (Scalar(1) / Scalar(2)) * _tmp417; + const Scalar _tmp1240 = _tmp1205 * _tmp1239; + const Scalar _tmp1241 = _tmp1203 * _tmp1239; + const Scalar _tmp1242 = + -Scalar(1) / Scalar(2) * _tmp1198 * _tmp429 - _tmp1201 * _tmp1240 + _tmp1201 * _tmp1241; + const Scalar _tmp1243 = (Scalar(1) / Scalar(2)) * _tmp271; + const Scalar _tmp1244 = _tmp1040 * _tmp1243; + const Scalar _tmp1245 = _tmp1037 * _tmp1243; + const Scalar _tmp1246 = _tmp1033 * _tmp279 - _tmp1035 * _tmp1244 + _tmp1035 * _tmp1245; + const Scalar _tmp1247 = (Scalar(1) / Scalar(2)) * _tmp241; + const Scalar _tmp1248 = _tmp1140 * _tmp1247; + const Scalar _tmp1249 = _tmp1142 * _tmp1247; + const Scalar _tmp1250 = _tmp1136 * _tmp249 + _tmp1138 * _tmp1248 - _tmp1138 * _tmp1249; + const Scalar _tmp1251 = (Scalar(1) / Scalar(2)) * _tmp331; + const Scalar _tmp1252 = _tmp1056 * _tmp1251; + const Scalar _tmp1253 = _tmp1054 * _tmp1251; + const Scalar _tmp1254 = _tmp1047 * _tmp339 + _tmp1052 * _tmp1252 - _tmp1052 * _tmp1253; + const Scalar _tmp1255 = _tmp291 * _tmp866; + const Scalar _tmp1256 = _tmp1163 * _tmp1255; + const Scalar _tmp1257 = (Scalar(1) / Scalar(2)) * _tmp297; + const Scalar _tmp1258 = _tmp1173 * _tmp1257; + const Scalar _tmp1259 = _tmp1171 * _tmp1257; + const Scalar _tmp1260 = + -_tmp1162 * _tmp1256 * _tmp309 + _tmp1169 * _tmp1258 - _tmp1169 * _tmp1259; + const Scalar _tmp1261 = _tmp437 * _tmp829 + _tmp438 * _tmp832 + _tmp439 * _tmp834; + const Scalar _tmp1262 = + ((((_tmp42 + _tmp443) > 0) - ((_tmp42 + _tmp443) < 0)) + 1) / std::pow(_tmp444, Scalar(2)); + const Scalar _tmp1263 = _tmp1262 * _tmp441; + const Scalar _tmp1264 = _tmp1262 * _tmp449; + const Scalar _tmp1265 = _tmp1264 * _tmp836; + const Scalar _tmp1266 = -_tmp1261 * _tmp1265 + _tmp450 * (_tmp439 * _tmp827 + _tmp442); + const Scalar _tmp1267 = 2 * _tmp451; + const Scalar _tmp1268 = + -_tmp1261 * _tmp1263 * _tmp447 * views_1_calibration(0, 0) + _tmp1266 * _tmp1267; + const Scalar _tmp1269 = (Scalar(1) / Scalar(2)) * _tmp451; + const Scalar _tmp1270 = + _tmp177 * _tmp453 * _tmp454 * _tmp455 * std::pow(_tmp456, _tmp848) / _tmp457; + const Scalar _tmp1271 = _tmp1269 * _tmp1270; + const Scalar _tmp1272 = _tmp458 / (_tmp452 * std::sqrt(_tmp452)); + const Scalar _tmp1273 = _tmp1269 * _tmp1272; + const Scalar _tmp1274 = _tmp1266 * _tmp459 + _tmp1268 * _tmp1271 - _tmp1268 * _tmp1273; + const Scalar _tmp1275 = (Scalar(1) / Scalar(2)) * _tmp631; + const Scalar _tmp1276 = _tmp1188 * _tmp1275; + const Scalar _tmp1277 = _tmp1184 * _tmp1275; + const Scalar _tmp1278 = _tmp1181 * _tmp639 + _tmp1183 * _tmp1276 - _tmp1183 * _tmp1277; + const Scalar _tmp1279 = _tmp647 * _tmp829 + _tmp648 * _tmp832 + _tmp649 * _tmp834; + const Scalar _tmp1280 = + ((((_tmp42 + _tmp653) > 0) - ((_tmp42 + _tmp653) < 0)) + 1) / std::pow(_tmp654, Scalar(2)); + const Scalar _tmp1281 = _tmp1280 * _tmp659; + const Scalar _tmp1282 = _tmp1281 * _tmp836; + const Scalar _tmp1283 = -_tmp1279 * _tmp1282 + _tmp660 * (_tmp649 * _tmp827 + _tmp652); + const Scalar _tmp1284 = 2 * _tmp661; + const Scalar _tmp1285 = _tmp1280 * _tmp651; + const Scalar _tmp1286 = + -_tmp1279 * _tmp1285 * _tmp657 * views_1_calibration(0, 0) + _tmp1283 * _tmp1284; + const Scalar _tmp1287 = (Scalar(1) / Scalar(2)) * _tmp661; + const Scalar _tmp1288 = + _tmp177 * _tmp663 * _tmp664 * _tmp665 * std::pow(_tmp666, _tmp848) / _tmp667; + const Scalar _tmp1289 = _tmp1287 * _tmp1288; + const Scalar _tmp1290 = _tmp668 / (_tmp662 * std::sqrt(_tmp662)); + const Scalar _tmp1291 = _tmp1287 * _tmp1290; + const Scalar _tmp1292 = _tmp1283 * _tmp669 + _tmp1286 * _tmp1289 - _tmp1286 * _tmp1291; + const Scalar _tmp1293 = _tmp1263 * _tmp866; + const Scalar _tmp1294 = (Scalar(1) / Scalar(2)) * _tmp447; + const Scalar _tmp1295 = _tmp1270 * _tmp1294; + const Scalar _tmp1296 = _tmp1272 * _tmp1294; + const Scalar _tmp1297 = + -_tmp1261 * _tmp1293 * _tmp459 + _tmp1268 * _tmp1295 - _tmp1268 * _tmp1296; + const Scalar _tmp1298 = (Scalar(1) / Scalar(2)) * _tmp207; + const Scalar _tmp1299 = _tmp1298 * _tmp949; + const Scalar _tmp1300 = _tmp866 * _tmp940; + const Scalar _tmp1301 = _tmp1298 * _tmp946; + const Scalar _tmp1302 = _tmp1299 * _tmp945 - _tmp1300 * _tmp219 * _tmp938 - _tmp1301 * _tmp945; + const Scalar _tmp1303 = _tmp56 * priors_0_1_sqrt_info(2, 2); + const Scalar _tmp1304 = _tmp1008 * _tmp1303; + const Scalar _tmp1305 = _tmp1002 * _tmp1304 - _tmp1017 * _tmp1303 + _tmp1023 * _tmp66; + const Scalar _tmp1306 = _tmp651 * _tmp866; + const Scalar _tmp1307 = _tmp1280 * _tmp1306; + const Scalar _tmp1308 = (Scalar(1) / Scalar(2)) * _tmp657; + const Scalar _tmp1309 = _tmp1288 * _tmp1308; + const Scalar _tmp1310 = _tmp1290 * _tmp1308; const Scalar _tmp1311 = - _tmp181 * _tmp667 * _tmp668 * _tmp669 * std::pow(_tmp670, _tmp906) / _tmp671; - const Scalar _tmp1312 = _tmp1310 * _tmp1311; - const Scalar _tmp1313 = _tmp672 / (_tmp666 * std::sqrt(_tmp666)); - const Scalar _tmp1314 = _tmp1310 * _tmp1313; - const Scalar _tmp1315 = _tmp1307 * _tmp673 + _tmp1309 * _tmp1312 - _tmp1309 * _tmp1314; - const Scalar _tmp1316 = _tmp1288 * _tmp924; - const Scalar _tmp1317 = (Scalar(1) / Scalar(2)) * _tmp451; - const Scalar _tmp1318 = _tmp1295 * _tmp1317; - const Scalar _tmp1319 = _tmp1297 * _tmp1317; - const Scalar _tmp1320 = - -_tmp1286 * _tmp1316 * _tmp463 + _tmp1293 * _tmp1318 - _tmp1293 * _tmp1319; - const Scalar _tmp1321 = (Scalar(1) / Scalar(2)) * _tmp211; - const Scalar _tmp1322 = _tmp1000 * _tmp1321; - const Scalar _tmp1323 = _tmp924 * _tmp991; - const Scalar _tmp1324 = _tmp1321 * _tmp998; - const Scalar _tmp1325 = _tmp1322 * _tmp996 - _tmp1323 * _tmp223 * _tmp989 - _tmp1324 * _tmp996; - const Scalar _tmp1326 = - _tmp64 * _tmp862 + _tmp857 * priors_0_1_sqrt_info(1, 2) + - _tmp868 * priors_0_1_sqrt_info(1, 1) - _tmp871 * priors_0_1_sqrt_info(1, 2) - - _tmp872 * priors_0_1_sqrt_info(1, 0) + _tmp873 * priors_0_1_sqrt_info(1, 0) + - _tmp875 * priors_0_1_sqrt_info(1, 1) + _tmp879 * priors_0_1_sqrt_info(1, 2) - - _tmp881 * priors_0_1_sqrt_info(1, 1); - const Scalar _tmp1327 = _tmp655 * _tmp924; - const Scalar _tmp1328 = (Scalar(1) / Scalar(2)) * _tmp661; - const Scalar _tmp1329 = _tmp1311 * _tmp1328; - const Scalar _tmp1330 = _tmp1313 * _tmp1328; - const Scalar _tmp1331 = - -_tmp1305 * _tmp1327 * _tmp673 + _tmp1309 * _tmp1329 - _tmp1309 * _tmp1330; - const Scalar _tmp1332 = _tmp1005 * _tmp924; - const Scalar _tmp1333 = _tmp1016 * _tmp481; - const Scalar _tmp1334 = _tmp1013 * _tmp481; - const Scalar _tmp1335 = - -_tmp1003 * _tmp1332 * _tmp493 - _tmp1010 * _tmp1333 + _tmp1010 * _tmp1334; - const Scalar _tmp1336 = -_tmp71; - const Scalar _tmp1337 = _tmp1336 + _tmp883 + _tmp886 + _tmp91; - const Scalar _tmp1338 = _tmp1337 * _tmp681 + _tmp684; - const Scalar _tmp1339 = _tmp1083 * _tmp1338; - const Scalar _tmp1340 = _tmp894 + _tmp95; - const Scalar _tmp1341 = _tmp1336 + _tmp74; - const Scalar _tmp1342 = _tmp1341 + _tmp892; - const Scalar _tmp1343 = -_tmp79; - const Scalar _tmp1344 = _tmp1343 + _tmp82; - const Scalar _tmp1345 = _tmp1087 * _tmp924; - const Scalar _tmp1346 = -_tmp1338 * _tmp1345 + - _tmp690 * (_tmp1340 * _tmp682 + _tmp1342 * _tmp683 + _tmp1344 * _tmp681); - const Scalar _tmp1347 = 2 * _tmp691; - const Scalar _tmp1348 = -_tmp1339 * _tmp695 * views_1_calibration(1, 0) + _tmp1346 * _tmp1347; - const Scalar _tmp1349 = _tmp1092 * _tmp1348 - _tmp1094 * _tmp1348 + _tmp1346 * _tmp703; - const Scalar _tmp1350 = _tmp911 * (_tmp1337 * _tmp501 + _tmp504); - const Scalar _tmp1351 = -_tmp1350 * _tmp925 + - _tmp510 * (_tmp1340 * _tmp502 + _tmp1342 * _tmp503 + _tmp1344 * _tmp501); - const Scalar _tmp1352 = 2 * _tmp511; - const Scalar _tmp1353 = _tmp1350 * _tmp513; - const Scalar _tmp1354 = _tmp1351 * _tmp1352 - _tmp1353 * _tmp515 * views_1_calibration(1, 0); - const Scalar _tmp1355 = _tmp1351 * _tmp523 - _tmp1354 * _tmp920 + _tmp1354 * _tmp923; - const Scalar _tmp1356 = _tmp1337 * _tmp411 + _tmp414; - const Scalar _tmp1357 = _tmp1222 * _tmp1356; - const Scalar _tmp1358 = _tmp1220 * _tmp924; - const Scalar _tmp1359 = -_tmp1356 * _tmp1358 + - _tmp420 * (_tmp1340 * _tmp412 + _tmp1342 * _tmp413 + _tmp1344 * _tmp411); - const Scalar _tmp1360 = 2 * _tmp421; - const Scalar _tmp1361 = -_tmp1357 * _tmp425 * views_1_calibration(1, 0) + _tmp1359 * _tmp1360; - const Scalar _tmp1362 = -_tmp1268 * _tmp1361 + _tmp1269 * _tmp1361 + _tmp1359 * _tmp433; - const Scalar _tmp1363 = _tmp1229 * _tmp1361 - _tmp1231 * _tmp1361 - _tmp1357 * _tmp433 * _tmp898; - const Scalar _tmp1364 = -_tmp826 + _tmp827 - _tmp828 + _tmp829; - const Scalar _tmp1365 = -_priors_0_1_target_T_src[0] * _tmp936 - _tmp876; - const Scalar _tmp1366 = -_priors_0_1_target_T_src[2] * _tmp1364 + _tmp1365 - _tmp877; - const Scalar _tmp1367 = _tmp1366 * _tmp855; - const Scalar _tmp1368 = _tmp1367 * _tmp32; - const Scalar _tmp1369 = _priors_0_1_target_T_src[1] * _tmp936; - const Scalar _tmp1370 = _tmp867 * (_priors_0_1_target_T_src[3] * _tmp1364 + _tmp1369 + _tmp848); - const Scalar _tmp1371 = _tmp1366 * _tmp869; - const Scalar _tmp1372 = _tmp1366 * _tmp880; - const Scalar _tmp1373 = -_priors_0_1_target_T_src[2] * _tmp936 + _tmp858; - const Scalar _tmp1374 = _tmp867 * (_priors_0_1_target_T_src[0] * _tmp1364 + _tmp1373 + _tmp859); - const Scalar _tmp1375 = _priors_0_1_target_T_src[3] * _tmp936; - const Scalar _tmp1376 = -_priors_0_1_target_T_src[1] * _tmp1364 + _tmp1375 - _tmp864 + _tmp865; - const Scalar _tmp1377 = _tmp1376 * _tmp867; - const Scalar _tmp1378 = - _tmp1019 * _tmp1367 - _tmp1019 * _tmp1371 + _tmp1367 * _tmp66 + - _tmp1368 * priors_0_1_sqrt_info(2, 1) + _tmp1370 * priors_0_1_sqrt_info(2, 2) - - _tmp1371 * _tmp66 - _tmp1372 * priors_0_1_sqrt_info(2, 1) + - _tmp1374 * priors_0_1_sqrt_info(2, 1) + _tmp1377 * priors_0_1_sqrt_info(2, 0); - const Scalar _tmp1379 = -_tmp1353 * _tmp523 * _tmp898 - _tmp1354 * _tmp928 + _tmp1354 * _tmp929; - const Scalar _tmp1380 = -_tmp954; - const Scalar _tmp1381 = _priors_1_0_target_T_src[3] * _tmp847; - const Scalar _tmp1382 = -_priors_1_0_target_T_src[0] * _tmp836 + _tmp1380 - _tmp1381 + _tmp955; - const Scalar _tmp1383 = _tmp1382 * _tmp961; - const Scalar _tmp1384 = _priors_1_0_target_T_src[1] * _tmp847; - const Scalar _tmp1385 = - _tmp952 * (-_priors_1_0_target_T_src[2] * _tmp836 - _tmp1384 + _tmp967 + _tmp968); - const Scalar _tmp1386 = _tmp123 * _tmp1383; - const Scalar _tmp1387 = -_priors_1_0_target_T_src[0] * _tmp847; - const Scalar _tmp1388 = _priors_1_0_target_T_src[3] * _tmp836 + _tmp1387 + _tmp946 + _tmp948; - const Scalar _tmp1389 = _tmp1388 * _tmp952; - const Scalar _tmp1390 = -_priors_1_0_target_T_src[2] * _tmp847; - const Scalar _tmp1391 = - _tmp951 * (_priors_1_0_target_T_src[1] * _tmp836 + _tmp1390 + _tmp933 + _tmp935); - const Scalar _tmp1392 = _tmp122 * _tmp1391; - const Scalar _tmp1393 = _tmp1382 * _tmp943; - const Scalar _tmp1394 = _tmp131 * _tmp1393; - const Scalar _tmp1395 = _tmp1340 * _views_0_pose[5] - _tmp1340 * _views_1_pose[5] + - _tmp1342 * _views_0_pose[6] - _tmp1342 * _views_1_pose[6] + - _tmp1344 * _views_0_pose[4] - _tmp1344 * _views_1_pose[4]; - const Scalar _tmp1396 = - _tmp1337 * _views_0_pose[4] - _tmp1337 * _views_1_pose[4] - _tmp84 - _tmp88 + _tmp89; - const Scalar _tmp1397 = _tmp123 * _tmp1393; - const Scalar _tmp1398 = - _tmp1383 * _tmp963 + _tmp1383 * _tmp964 + _tmp1385 * priors_1_0_sqrt_info(1, 1) + - _tmp1386 * priors_1_0_sqrt_info(1, 2) + _tmp1389 * priors_1_0_sqrt_info(1, 0) + - _tmp1392 * priors_1_0_sqrt_info(1, 2) - _tmp1393 * _tmp964 - - _tmp1394 * priors_1_0_sqrt_info(1, 0) + _tmp1395 * priors_1_0_sqrt_info(1, 3) + - _tmp1396 * priors_1_0_sqrt_info(1, 5) - _tmp1397 * priors_1_0_sqrt_info(1, 2); - const Scalar _tmp1399 = _tmp1337 * _tmp201 + _tmp204; - const Scalar _tmp1400 = _tmp1399 * _tmp992; - const Scalar _tmp1401 = -_tmp1323 * _tmp1399 + - _tmp210 * (_tmp1340 * _tmp202 + _tmp1342 * _tmp203 + _tmp1344 * _tmp201); - const Scalar _tmp1402 = 2 * _tmp211; - const Scalar _tmp1403 = -_tmp1400 * _tmp215 * views_1_calibration(1, 0) + _tmp1401 * _tmp1402; - const Scalar _tmp1404 = _tmp1001 * _tmp1403 - _tmp1400 * _tmp223 * _tmp898 - _tmp1403 * _tmp999; - const Scalar _tmp1405 = _tmp1337 * _tmp441 + _tmp444; - const Scalar _tmp1406 = -_tmp1316 * _tmp1405 + - _tmp450 * (_tmp1340 * _tmp442 + _tmp1342 * _tmp443 + _tmp1344 * _tmp441); - const Scalar _tmp1407 = 2 * _tmp451; - const Scalar _tmp1408 = - -_tmp1289 * _tmp1405 * _tmp455 * views_1_calibration(1, 0) + _tmp1406 * _tmp1407; - const Scalar _tmp1409 = _tmp1318 * _tmp1408 - _tmp1319 * _tmp1408 + _tmp1406 * _tmp463; - const Scalar _tmp1410 = _tmp1337 * _tmp261 + _tmp264; - const Scalar _tmp1411 = -_tmp1062 * _tmp1410 + - _tmp270 * (_tmp1340 * _tmp262 + _tmp1342 * _tmp263 + _tmp1344 * _tmp261); - const Scalar _tmp1412 = 2 * _tmp271; - const Scalar _tmp1413 = - -_tmp1054 * _tmp1410 * _tmp275 * views_1_calibration(1, 0) + _tmp1411 * _tmp1412; + -_tmp1279 * _tmp1307 * _tmp669 + _tmp1286 * _tmp1309 - _tmp1286 * _tmp1310; + const Scalar _tmp1312 = (Scalar(1) / Scalar(2)) * _tmp477; + const Scalar _tmp1313 = _tmp1312 * _tmp964; + const Scalar _tmp1314 = _tmp1312 * _tmp962; + const Scalar _tmp1315 = -_tmp1313 * _tmp960 + _tmp1314 * _tmp960 - _tmp489 * _tmp866 * _tmp955; + const Scalar _tmp1316 = -_tmp70; + const Scalar _tmp1317 = _tmp1316 + _tmp822 + _tmp825 + _tmp90; + const Scalar _tmp1318 = _tmp1317 * _tmp677 + _tmp680; + const Scalar _tmp1319 = _tmp1059 * _tmp689; + const Scalar _tmp1320 = _tmp1318 * _tmp1319; + const Scalar _tmp1321 = _tmp833 + _tmp94; + const Scalar _tmp1322 = _tmp1316 + _tmp73; + const Scalar _tmp1323 = _tmp1322 + _tmp831; + const Scalar _tmp1324 = -_tmp78; + const Scalar _tmp1325 = _tmp1324 + _tmp80; + const Scalar _tmp1326 = (Scalar(1) / Scalar(2)) * _tmp1059 * _tmp1063; + const Scalar _tmp1327 = -_tmp1318 * _tmp1326 + + _tmp686 * (_tmp1321 * _tmp678 + _tmp1323 * _tmp679 + _tmp1325 * _tmp677); + const Scalar _tmp1328 = 2 * _tmp687; + const Scalar _tmp1329 = -_tmp1320 * _tmp691 * views_1_calibration(1, 0) + _tmp1327 * _tmp1328; + const Scalar _tmp1330 = _tmp1069 * _tmp1329; + const Scalar _tmp1331 = -_tmp1066 * _tmp1330 + _tmp1068 * _tmp1329 + _tmp1327 * _tmp699; + const Scalar _tmp1332 = _tmp1317 * _tmp497 + _tmp500; + const Scalar _tmp1333 = -_tmp1332 * _tmp867 + + _tmp506 * (_tmp1321 * _tmp498 + _tmp1323 * _tmp499 + _tmp1325 * _tmp497); + const Scalar _tmp1334 = 2 * _tmp507; + const Scalar _tmp1335 = _tmp1332 * _tmp856; + const Scalar _tmp1336 = _tmp1333 * _tmp1334 - _tmp1335 * _tmp511 * views_1_calibration(1, 0); + const Scalar _tmp1337 = _tmp1336 * _tmp861; + const Scalar _tmp1338 = _tmp1333 * _tmp519 - _tmp1337 * _tmp862 + _tmp1337 * _tmp864; + const Scalar _tmp1339 = _tmp1317 * _tmp407 + _tmp410; + const Scalar _tmp1340 = _tmp1195 * _tmp419; + const Scalar _tmp1341 = _tmp1339 * _tmp1340; + const Scalar _tmp1342 = _tmp1195 * _tmp1197; + const Scalar _tmp1343 = (Scalar(1) / Scalar(2)) * _tmp1342; + const Scalar _tmp1344 = -_tmp1339 * _tmp1343 + + _tmp416 * (_tmp1321 * _tmp408 + _tmp1323 * _tmp409 + _tmp1325 * _tmp407); + const Scalar _tmp1345 = 2 * _tmp417; + const Scalar _tmp1346 = -_tmp1341 * _tmp421 * views_1_calibration(1, 0) + _tmp1344 * _tmp1345; + const Scalar _tmp1347 = -_tmp1240 * _tmp1346 + _tmp1241 * _tmp1346 + _tmp1344 * _tmp429; + const Scalar _tmp1348 = _tmp1204 * _tmp1346 - _tmp1206 * _tmp1346 - _tmp1341 * _tmp429 * _tmp836; + const Scalar _tmp1349 = -_tmp1335 * _tmp519 * _tmp836 - _tmp1336 * _tmp920 + _tmp1336 * _tmp921; + const Scalar _tmp1350 = + _tmp1317 * _views_0_pose[4] - _tmp1317 * _views_1_pose[4] - _tmp82 - _tmp87 + _tmp88; + const Scalar _tmp1351 = _tmp110 * _tmp1217; + const Scalar _tmp1352 = _tmp1317 * _tmp197 + _tmp200; + const Scalar _tmp1353 = -_tmp1300 * _tmp1352 + + _tmp206 * (_tmp1321 * _tmp198 + _tmp1323 * _tmp199 + _tmp1325 * _tmp197); + const Scalar _tmp1354 = 2 * _tmp207; + const Scalar _tmp1355 = + -_tmp1352 * _tmp211 * _tmp941 * views_1_calibration(1, 0) + _tmp1353 * _tmp1354; + const Scalar _tmp1356 = -_tmp1352 * _tmp219 * _tmp942 - _tmp1355 * _tmp948 + _tmp1355 * _tmp950; + const Scalar _tmp1357 = _tmp1317 * _tmp437 + _tmp440; + const Scalar _tmp1358 = -_tmp1293 * _tmp1357 + + _tmp446 * (_tmp1321 * _tmp438 + _tmp1323 * _tmp439 + _tmp1325 * _tmp437); + const Scalar _tmp1359 = 2 * _tmp447; + const Scalar _tmp1360 = + -_tmp1264 * _tmp1357 * _tmp451 * views_1_calibration(1, 0) + _tmp1358 * _tmp1359; + const Scalar _tmp1361 = _tmp1295 * _tmp1360 - _tmp1296 * _tmp1360 + _tmp1358 * _tmp459; + const Scalar _tmp1362 = _tmp1317 * _tmp257 + _tmp260; + const Scalar _tmp1363 = -_tmp1039 * _tmp1362 + + _tmp266 * (_tmp1321 * _tmp258 + _tmp1323 * _tmp259 + _tmp1325 * _tmp257); + const Scalar _tmp1364 = 2 * _tmp267; + const Scalar _tmp1365 = + -_tmp1031 * _tmp1362 * _tmp271 * views_1_calibration(1, 0) + _tmp1363 * _tmp1364; + const Scalar _tmp1366 = + -_tmp1032 * _tmp1362 * _tmp279 - _tmp1244 * _tmp1365 + _tmp1245 * _tmp1365; + const Scalar _tmp1367 = _tmp1163 * (_tmp1317 * _tmp287 + _tmp290); + const Scalar _tmp1368 = -_tmp1255 * _tmp1367 + + _tmp296 * (_tmp1321 * _tmp289 + _tmp1323 * _tmp288 + _tmp1325 * _tmp287); + const Scalar _tmp1369 = 2 * _tmp297; + const Scalar _tmp1370 = _tmp1367 * _tmp299; + const Scalar _tmp1371 = _tmp1368 * _tmp1369 - _tmp1370 * _tmp301 * views_1_calibration(1, 0); + const Scalar _tmp1372 = _tmp1170 * _tmp1371; + const Scalar _tmp1373 = -_tmp1171 * _tmp1372 + _tmp1173 * _tmp1372 - _tmp1370 * _tmp309 * _tmp836; + const Scalar _tmp1374 = + -_tmp1265 * _tmp1357 * _tmp459 + _tmp1271 * _tmp1360 - _tmp1273 * _tmp1360; + const Scalar _tmp1375 = _tmp1350 * priors_1_0_sqrt_info(4, 5); + const Scalar _tmp1376 = _tmp1280 * (_tmp1317 * _tmp647 + _tmp650); + const Scalar _tmp1377 = _tmp1376 * _tmp659; + const Scalar _tmp1378 = -_tmp1306 * _tmp1376 + + _tmp656 * (_tmp1321 * _tmp648 + _tmp1323 * _tmp649 + _tmp1325 * _tmp647); + const Scalar _tmp1379 = 2 * _tmp657; + const Scalar _tmp1380 = -_tmp1377 * _tmp661 * views_1_calibration(1, 0) + _tmp1378 * _tmp1379; + const Scalar _tmp1381 = _tmp1289 * _tmp1380 - _tmp1291 * _tmp1380 - _tmp1377 * _tmp669 * _tmp836; + const Scalar _tmp1382 = _tmp1317 * _tmp377 + _tmp380; + const Scalar _tmp1383 = _tmp1101 * _tmp1382; + const Scalar _tmp1384 = _tmp1105 * _tmp866; + const Scalar _tmp1385 = -_tmp1382 * _tmp1384 + + _tmp386 * (_tmp1321 * _tmp378 + _tmp1323 * _tmp379 + _tmp1325 * _tmp377); + const Scalar _tmp1386 = 2 * _tmp387; + const Scalar _tmp1387 = -_tmp1383 * _tmp391 * views_1_calibration(1, 0) + _tmp1385 * _tmp1386; + const Scalar _tmp1388 = _tmp1108 * _tmp1387; + const Scalar _tmp1389 = _tmp1109 * _tmp1388 - _tmp1111 * _tmp1388 - _tmp1383 * _tmp399 * _tmp836; + const Scalar _tmp1390 = _tmp1317 * _tmp467 + _tmp470; + const Scalar _tmp1391 = _tmp866 * _tmp954; + const Scalar _tmp1392 = -_tmp1390 * _tmp1391 + + _tmp476 * (_tmp1321 * _tmp469 + _tmp1323 * _tmp468 + _tmp1325 * _tmp467); + const Scalar _tmp1393 = 2 * _tmp477; + const Scalar _tmp1394 = _tmp1390 * _tmp956; + const Scalar _tmp1395 = _tmp1392 * _tmp1393 - _tmp1394 * _tmp481 * views_1_calibration(1, 0); + const Scalar _tmp1396 = -_tmp1313 * _tmp1395 + _tmp1314 * _tmp1395 + _tmp1392 * _tmp489; + const Scalar _tmp1397 = -_tmp900; + const Scalar _tmp1398 = _priors_1_0_target_T_src[3] * _tmp1000; + const Scalar _tmp1399 = -_priors_1_0_target_T_src[0] * _tmp997 + _tmp1397 - _tmp1398 + _tmp901; + const Scalar _tmp1400 = -_priors_1_0_target_T_src[2] * _tmp1000; + const Scalar _tmp1401 = _priors_1_0_target_T_src[1] * _tmp997 + _tmp1400 + _tmp908 + _tmp909; + const Scalar _tmp1402 = _tmp1401 * _tmp898; + const Scalar _tmp1403 = _tmp1399 * _tmp916; + const Scalar _tmp1404 = _tmp1321 * _views_0_pose[5] - _tmp1321 * _views_1_pose[5] + + _tmp1323 * _views_0_pose[6] - _tmp1323 * _views_1_pose[6] + + _tmp1325 * _views_0_pose[4] - _tmp1325 * _views_1_pose[4]; + const Scalar _tmp1405 = + -_tmp1152 * _tmp1403 + _tmp1153 * _tmp1399 + _tmp1350 * priors_1_0_sqrt_info(2, 5) + + _tmp1402 * priors_1_0_sqrt_info(2, 2) + _tmp1404 * priors_1_0_sqrt_info(2, 3); + const Scalar _tmp1406 = _tmp1317 * _tmp153 + _tmp162; + const Scalar _tmp1407 = -_tmp1208 * _tmp1406 + + _tmp168 * (_tmp1321 * _tmp157 + _tmp1323 * _tmp161 + _tmp1325 * _tmp153); + const Scalar _tmp1408 = 2 * _tmp169; + const Scalar _tmp1409 = + -_tmp1087 * _tmp1406 * _tmp173 * views_1_calibration(1, 0) + _tmp1407 * _tmp1408; + const Scalar _tmp1410 = -_tmp1210 * _tmp1409 + _tmp1211 * _tmp1409 + _tmp1407 * _tmp189; + const Scalar _tmp1411 = _tmp1317 * _tmp587 + _tmp590; + const Scalar _tmp1412 = -_tmp1222 * _tmp1411 + + _tmp596 * (_tmp1321 * _tmp588 + _tmp1323 * _tmp589 + _tmp1325 * _tmp587); + const Scalar _tmp1413 = 2 * _tmp597; const Scalar _tmp1414 = - -_tmp1055 * _tmp1410 * _tmp283 - _tmp1271 * _tmp1413 + _tmp1272 * _tmp1413; - const Scalar _tmp1415 = _tmp1187 * (_tmp1337 * _tmp291 + _tmp294); - const Scalar _tmp1416 = -_tmp1281 * _tmp1415 + - _tmp300 * (_tmp1340 * _tmp293 + _tmp1342 * _tmp292 + _tmp1344 * _tmp291); - const Scalar _tmp1417 = 2 * _tmp301; - const Scalar _tmp1418 = _tmp1415 * _tmp303; - const Scalar _tmp1419 = _tmp1416 * _tmp1417 - _tmp1418 * _tmp305 * views_1_calibration(1, 0); - const Scalar _tmp1420 = _tmp1196 * _tmp1419; - const Scalar _tmp1421 = _tmp1194 * _tmp1420 - _tmp1195 * _tmp1419 - _tmp1418 * _tmp313 * _tmp898; - const Scalar _tmp1422 = - -_tmp1290 * _tmp1405 * _tmp463 + _tmp1296 * _tmp1408 - _tmp1298 * _tmp1408; - const Scalar _tmp1423 = _tmp1304 * (_tmp1337 * _tmp651 + _tmp654); - const Scalar _tmp1424 = -_tmp1327 * _tmp1423 + - _tmp660 * (_tmp1340 * _tmp652 + _tmp1342 * _tmp653 + _tmp1344 * _tmp651); - const Scalar _tmp1425 = 2 * _tmp661; - const Scalar _tmp1426 = - -_tmp1423 * _tmp663 * _tmp665 * views_1_calibration(1, 0) + _tmp1424 * _tmp1425; - const Scalar _tmp1427 = - -_tmp1306 * _tmp1423 * _tmp673 + _tmp1312 * _tmp1426 - _tmp1314 * _tmp1426; - const Scalar _tmp1428 = _tmp1337 * _tmp381 + _tmp384; - const Scalar _tmp1429 = _tmp1125 * _tmp1428; - const Scalar _tmp1430 = _tmp1129 * _tmp924; - const Scalar _tmp1431 = -_tmp1428 * _tmp1430 + - _tmp390 * (_tmp1340 * _tmp382 + _tmp1342 * _tmp383 + _tmp1344 * _tmp381); - const Scalar _tmp1432 = 2 * _tmp391; - const Scalar _tmp1433 = -_tmp1429 * _tmp395 * views_1_calibration(1, 0) + _tmp1431 * _tmp1432; - const Scalar _tmp1434 = _tmp1135 * _tmp1433; - const Scalar _tmp1435 = -_tmp1132 * _tmp1434 + _tmp1134 * _tmp1433 - _tmp1429 * _tmp403 * _tmp898; - const Scalar _tmp1436 = _tmp1337 * _tmp471 + _tmp474; - const Scalar _tmp1437 = -_tmp1332 * _tmp1436 + - _tmp480 * (_tmp1340 * _tmp473 + _tmp1342 * _tmp472 + _tmp1344 * _tmp471); - const Scalar _tmp1438 = 2 * _tmp481; - const Scalar _tmp1439 = _tmp1006 * _tmp1436; - const Scalar _tmp1440 = _tmp1437 * _tmp1438 - _tmp1439 * _tmp485 * views_1_calibration(1, 0); - const Scalar _tmp1441 = -_tmp1333 * _tmp1440 + _tmp1334 * _tmp1440 + _tmp1437 * _tmp493; - const Scalar _tmp1442 = _tmp1337 * _tmp157 + _tmp166; - const Scalar _tmp1443 = -_tmp1236 * _tmp1442 + - _tmp172 * (_tmp1340 * _tmp161 + _tmp1342 * _tmp165 + _tmp1344 * _tmp157); - const Scalar _tmp1444 = 2 * _tmp173; - const Scalar _tmp1445 = - -_tmp1111 * _tmp1442 * _tmp177 * views_1_calibration(1, 0) + _tmp1443 * _tmp1444; - const Scalar _tmp1446 = -_tmp1238 * _tmp1445 + _tmp1239 * _tmp1445 + _tmp1443 * _tmp193; - const Scalar _tmp1447 = _tmp1337 * _tmp591 + _tmp594; - const Scalar _tmp1448 = _tmp1447 * _tmp896; - const Scalar _tmp1449 = -_tmp1250 * _tmp1448 + - _tmp600 * (_tmp1340 * _tmp592 + _tmp1342 * _tmp593 + _tmp1344 * _tmp591); - const Scalar _tmp1450 = 2 * _tmp601; - const Scalar _tmp1451 = - -_tmp1448 * _tmp603 * _tmp605 * views_1_calibration(1, 0) + _tmp1449 * _tmp1450; - const Scalar _tmp1452 = _tmp1252 * _tmp1451 - _tmp1253 * _tmp1451 + _tmp1449 * _tmp613; - const Scalar _tmp1453 = _tmp1337 * _tmp321 + _tmp324; - const Scalar _tmp1454 = _tmp1067 * _tmp333; - const Scalar _tmp1455 = _tmp1067 * _tmp325; - const Scalar _tmp1456 = _tmp1455 * _tmp924; - const Scalar _tmp1457 = -_tmp1453 * _tmp1456 + - _tmp330 * (_tmp1340 * _tmp322 + _tmp1342 * _tmp323 + _tmp1344 * _tmp321); - const Scalar _tmp1458 = 2 * _tmp331; - const Scalar _tmp1459 = - -_tmp1453 * _tmp1454 * _tmp335 * views_1_calibration(1, 0) + _tmp1457 * _tmp1458; - const Scalar _tmp1460 = _tmp1078 * _tmp331; - const Scalar _tmp1461 = -_tmp1076 * _tmp1459 + _tmp1457 * _tmp343 + _tmp1459 * _tmp1460; - const Scalar _tmp1462 = _tmp896 * _tmp899; - const Scalar _tmp1463 = -_tmp1447 * _tmp1462 * _tmp613 - _tmp1451 * _tmp905 + _tmp1451 * _tmp908; - const Scalar _tmp1464 = _tmp1337 * _tmp351 + _tmp354; - const Scalar _tmp1465 = -_tmp1032 * _tmp1464 + - _tmp360 * (_tmp1340 * _tmp352 + _tmp1342 * _tmp353 + _tmp1344 * _tmp351); - const Scalar _tmp1466 = 2 * _tmp361; - const Scalar _tmp1467 = - -_tmp1023 * _tmp1464 * _tmp365 * views_1_calibration(1, 0) + _tmp1465 * _tmp1466; - const Scalar _tmp1468 = _tmp1029 * _tmp1255; - const Scalar _tmp1469 = - -_tmp1024 * _tmp1464 * _tmp373 + _tmp1256 * _tmp1467 - _tmp1467 * _tmp1468; - const Scalar _tmp1470 = _tmp1337 * _tmp741 + _tmp744; - const Scalar _tmp1471 = _tmp1096 * _tmp745; - const Scalar _tmp1472 = _tmp1471 * _tmp924; - const Scalar _tmp1473 = -_tmp1470 * _tmp1472 + - _tmp750 * (_tmp1340 * _tmp743 + _tmp1342 * _tmp742 + _tmp1344 * _tmp741); - const Scalar _tmp1474 = 2 * _tmp751; + -_tmp1411 * _tmp601 * _tmp838 * views_1_calibration(1, 0) + _tmp1412 * _tmp1413; + const Scalar _tmp1415 = _tmp1223 * _tmp1414 - _tmp1224 * _tmp1414 + _tmp1412 * _tmp609; + const Scalar _tmp1416 = _tmp1317 * _tmp317 + _tmp320; + const Scalar _tmp1417 = _tmp1045 * _tmp1416; + const Scalar _tmp1418 = (Scalar(1) / Scalar(2)) * _tmp1050; + const Scalar _tmp1419 = -_tmp1416 * _tmp1418 + + _tmp326 * (_tmp1321 * _tmp318 + _tmp1323 * _tmp319 + _tmp1325 * _tmp317); + const Scalar _tmp1420 = 2 * _tmp327; + const Scalar _tmp1421 = -_tmp1417 * _tmp331 * views_1_calibration(1, 0) + _tmp1419 * _tmp1420; + const Scalar _tmp1422 = -_tmp1055 * _tmp1421 + _tmp1057 * _tmp1421 + _tmp1419 * _tmp339; + const Scalar _tmp1423 = + _tmp1350 * priors_1_0_sqrt_info(3, 5) + _tmp1404 * priors_1_0_sqrt_info(3, 3); + const Scalar _tmp1424 = -_tmp1411 * _tmp609 * _tmp839 - _tmp1414 * _tmp846 + _tmp1414 * _tmp851; + const Scalar _tmp1425 = _tmp1317 * _tmp347 + _tmp350; + const Scalar _tmp1426 = _tmp351 * _tmp968; + const Scalar _tmp1427 = _tmp1426 * _tmp866; + const Scalar _tmp1428 = -_tmp1425 * _tmp1427 + + _tmp356 * (_tmp1321 * _tmp348 + _tmp1323 * _tmp349 + _tmp1325 * _tmp347); + const Scalar _tmp1429 = 2 * _tmp357; + const Scalar _tmp1430 = _tmp359 * _tmp968; + const Scalar _tmp1431 = + -_tmp1425 * _tmp1430 * _tmp361 * views_1_calibration(1, 0) + _tmp1428 * _tmp1429; + const Scalar _tmp1432 = _tmp1226 * _tmp975; + const Scalar _tmp1433 = _tmp968 * _tmp970; + const Scalar _tmp1434 = _tmp1227 * _tmp1431 - _tmp1425 * _tmp1433 * _tmp369 - _tmp1431 * _tmp1432; + const Scalar _tmp1435 = _tmp1317 * _tmp737 + _tmp740; + const Scalar _tmp1436 = _tmp1072 * _tmp741; + const Scalar _tmp1437 = _tmp1436 * _tmp866; + const Scalar _tmp1438 = -_tmp1435 * _tmp1437 + + _tmp746 * (_tmp1321 * _tmp739 + _tmp1323 * _tmp738 + _tmp1325 * _tmp737); + const Scalar _tmp1439 = 2 * _tmp747; + const Scalar _tmp1440 = _tmp1072 * _tmp749; + const Scalar _tmp1441 = + -_tmp1435 * _tmp1440 * _tmp751 * views_1_calibration(1, 0) + _tmp1438 * _tmp1439; + const Scalar _tmp1442 = _tmp1082 * _tmp1441; + const Scalar _tmp1443 = _tmp1159 * _tmp1442 - _tmp1160 * _tmp1441 + _tmp1438 * _tmp759; + const Scalar _tmp1444 = _tmp1258 * _tmp1371 - _tmp1259 * _tmp1371 + _tmp1368 * _tmp309; + const Scalar _tmp1445 = _tmp1072 * _tmp1074; + const Scalar _tmp1446 = _tmp1079 * _tmp1442 - _tmp1081 * _tmp1441 - _tmp1435 * _tmp1445 * _tmp759; + const Scalar _tmp1447 = -_tmp881 + _tmp882 - _tmp883 + _tmp884; + const Scalar _tmp1448 = -_priors_0_1_target_T_src[0] * _tmp891 - _tmp1020; + const Scalar _tmp1449 = -_priors_0_1_target_T_src[2] * _tmp1447 - _tmp1021 + _tmp1448; + const Scalar _tmp1450 = _tmp1008 * _tmp1449; + const Scalar _tmp1451 = _tmp41 * priors_0_1_sqrt_info(0, 1); + const Scalar _tmp1452 = _priors_0_1_target_T_src[1] * _tmp891; + const Scalar _tmp1453 = _tmp1014 * (_priors_0_1_target_T_src[3] * _tmp1447 + _tmp1001 + _tmp1452); + const Scalar _tmp1454 = _tmp1453 * _tmp39; + const Scalar _tmp1455 = _tmp1016 * _tmp1449; + const Scalar _tmp1456 = _tmp1455 * _tmp56; + const Scalar _tmp1457 = _tmp1025 * _tmp1449; + const Scalar _tmp1458 = _tmp1450 * _tmp56; + const Scalar _tmp1459 = -_priors_0_1_target_T_src[2] * _tmp891; + const Scalar _tmp1460 = + _tmp1014 * (_priors_0_1_target_T_src[0] * _tmp1447 + _tmp1233 + _tmp1234 + _tmp1459); + const Scalar _tmp1461 = _priors_0_1_target_T_src[3] * _tmp891; + const Scalar _tmp1462 = + _tmp1236 * (-_priors_0_1_target_T_src[1] * _tmp1447 - _tmp1011 + _tmp1012 + _tmp1461) + + _tmp1237 * _tmp1450 - _tmp1237 * _tmp1455 + _tmp1450 * _tmp1451 + + _tmp1454 * priors_0_1_sqrt_info(0, 2) - _tmp1456 * priors_0_1_sqrt_info(0, 2) - + _tmp1457 * priors_0_1_sqrt_info(0, 1) + _tmp1458 * priors_0_1_sqrt_info(0, 2) + + _tmp1460 * _tmp40; + const Scalar _tmp1463 = _tmp1399 * _tmp913; + const Scalar _tmp1464 = _priors_1_0_target_T_src[1] * _tmp1000; + const Scalar _tmp1465 = + _tmp898 * (-_priors_1_0_target_T_src[2] * _tmp997 + _tmp1147 + _tmp1148 - _tmp1464); + const Scalar _tmp1466 = -_tmp132 * _tmp1403 + _tmp132 * _tmp1463 + + _tmp1350 * priors_1_0_sqrt_info(1, 5) + _tmp1401 * _tmp904 - + _tmp1403 * _tmp915 + _tmp1404 * priors_1_0_sqrt_info(1, 3) + + _tmp1463 * _tmp915 + _tmp1465 * priors_1_0_sqrt_info(1, 1); + const Scalar _tmp1467 = _tmp1298 * _tmp1355; + const Scalar _tmp1468 = _tmp1353 * _tmp219 - _tmp1467 * _tmp946 + _tmp1467 * _tmp949; + const Scalar _tmp1469 = -_tmp1213 * _tmp1330 + _tmp1215 * _tmp1329 - _tmp1320 * _tmp699 * _tmp836; + const Scalar _tmp1470 = _tmp1317 * _tmp707 + _tmp710; + const Scalar _tmp1471 = _tmp1121 * _tmp866; + const Scalar _tmp1472 = -_tmp1470 * _tmp1471 + + _tmp716 * (_tmp1321 * _tmp709 + _tmp1323 * _tmp708 + _tmp1325 * _tmp707); + const Scalar _tmp1473 = 2 * _tmp717; + const Scalar _tmp1474 = + -_tmp1117 * _tmp1470 * _tmp721 * views_1_calibration(1, 0) + _tmp1472 * _tmp1473; const Scalar _tmp1475 = - -_tmp1096 * _tmp1470 * _tmp753 * _tmp755 * views_1_calibration(1, 0) + _tmp1473 * _tmp1474; - const Scalar _tmp1476 = _tmp1181 * _tmp1475; - const Scalar _tmp1477 = -_tmp1103 * _tmp1476 + _tmp1106 * _tmp1476 + _tmp1473 * _tmp763; - const Scalar _tmp1478 = _tmp1282 * _tmp1420 - _tmp1284 * _tmp1419 + _tmp1416 * _tmp313; - const Scalar _tmp1479 = _tmp1096 * _tmp1098; - const Scalar _tmp1480 = - -_tmp1105 * _tmp1475 + _tmp1107 * _tmp1475 - _tmp1470 * _tmp1479 * _tmp763; - const Scalar _tmp1481 = - _tmp135 * _tmp1383 - _tmp135 * _tmp1393 + _tmp136 * _tmp1388 * _tmp951 + _tmp1383 * _tmp987 + - _tmp1385 * priors_1_0_sqrt_info(2, 1) + _tmp1386 * priors_1_0_sqrt_info(2, 2) + - _tmp1392 * priors_1_0_sqrt_info(2, 2) - _tmp1394 * priors_1_0_sqrt_info(2, 0) + - _tmp1395 * priors_1_0_sqrt_info(2, 3) + _tmp1396 * priors_1_0_sqrt_info(2, 5) - - _tmp1397 * priors_1_0_sqrt_info(2, 2); - const Scalar _tmp1482 = _tmp1371 * _tmp57; - const Scalar _tmp1483 = _tmp1367 * _tmp57; - const Scalar _tmp1484 = - _tmp1262 * _tmp1367 - _tmp1262 * _tmp1371 + _tmp1368 * priors_0_1_sqrt_info(0, 1) + - _tmp1370 * priors_0_1_sqrt_info(0, 2) - _tmp1372 * priors_0_1_sqrt_info(0, 1) + - _tmp1374 * priors_0_1_sqrt_info(0, 1) + _tmp1377 * priors_0_1_sqrt_info(0, 0) - - _tmp1482 * priors_0_1_sqrt_info(0, 2) + _tmp1483 * priors_0_1_sqrt_info(0, 2); - const Scalar _tmp1485 = _tmp1371 * _tmp54; - const Scalar _tmp1486 = _tmp1367 * _tmp54; - const Scalar _tmp1487 = - _tmp1185 * _tmp1367 - _tmp1185 * _tmp1371 + _tmp1368 * priors_0_1_sqrt_info(5, 1) + - _tmp1370 * priors_0_1_sqrt_info(5, 2) - _tmp1372 * priors_0_1_sqrt_info(5, 1) + - _tmp1374 * priors_0_1_sqrt_info(5, 1) + _tmp1377 * priors_0_1_sqrt_info(5, 0) - - _tmp1485 * priors_0_1_sqrt_info(5, 0) + _tmp1486 * priors_0_1_sqrt_info(5, 0); - const Scalar _tmp1488 = _tmp1322 * _tmp1403 - _tmp1324 * _tmp1403 + _tmp1401 * _tmp223; - const Scalar _tmp1489 = _tmp1241 * _tmp1348; - const Scalar _tmp1490 = _tmp1091 * _tmp1489 - _tmp1093 * _tmp1489 - _tmp1339 * _tmp703 * _tmp898; - const Scalar _tmp1491 = _tmp1337 * _tmp711 + _tmp714; - const Scalar _tmp1492 = -_tmp1179 * _tmp1491 + - _tmp720 * (_tmp1340 * _tmp713 + _tmp1342 * _tmp712 + _tmp1344 * _tmp711); - const Scalar _tmp1493 = 2 * _tmp721; - const Scalar _tmp1494 = - -Scalar(1) / Scalar(2) * _tmp1145 * _tmp1491 * _tmp725 * views_1_calibration(1, 0) + - (Scalar(1) / Scalar(2)) * _tmp1492 * _tmp1493; - const Scalar _tmp1495 = _tmp1494 * _tmp725; - const Scalar _tmp1496 = - -_tmp1146 * _tmp1491 * _tmp733 - _tmp1151 * _tmp1495 + _tmp1154 * _tmp1495; - const Scalar _tmp1497 = _tmp1029 * _tmp1031; - const Scalar _tmp1498 = _tmp1034 * _tmp1467 + _tmp1465 * _tmp373 - _tmp1467 * _tmp1497; - const Scalar _tmp1499 = _tmp1061 * _tmp1413 - _tmp1065 * _tmp1413 + _tmp1411 * _tmp283; - const Scalar _tmp1500 = _tmp1337 * _tmp561 + _tmp564; - const Scalar _tmp1501 = -_tmp1245 * _tmp1500 + - _tmp570 * (_tmp1340 * _tmp562 + _tmp1342 * _tmp563 + _tmp1344 * _tmp561); - const Scalar _tmp1502 = 2 * _tmp571; - const Scalar _tmp1503 = - -_tmp1500 * _tmp575 * _tmp976 * views_1_calibration(1, 0) + _tmp1501 * _tmp1502; - const Scalar _tmp1504 = -_tmp1500 * _tmp583 * _tmp977 - _tmp1503 * _tmp983 + _tmp1503 * _tmp985; + -_tmp1118 * _tmp1470 * _tmp729 - _tmp1126 * _tmp1474 + _tmp1128 * _tmp1474; + const Scalar _tmp1476 = -_tmp1303 * _tmp1455 + _tmp1304 * _tmp1449 + _tmp1453 * _tmp66; + const Scalar _tmp1477 = _tmp975 * _tmp977; + const Scalar _tmp1478 = _tmp1428 * _tmp369 - _tmp1431 * _tmp1477 + _tmp1431 * _tmp979; + const Scalar _tmp1479 = _tmp1038 * _tmp1365 - _tmp1041 * _tmp1365 + _tmp1363 * _tmp279; + const Scalar _tmp1480 = _tmp1317 * _tmp557 + _tmp560; + const Scalar _tmp1481 = _tmp1480 * _tmp926; + const Scalar _tmp1482 = -_tmp1219 * _tmp1480 + + _tmp566 * (_tmp1321 * _tmp558 + _tmp1323 * _tmp559 + _tmp1325 * _tmp557); + const Scalar _tmp1483 = 2 * _tmp567; + const Scalar _tmp1484 = -_tmp1481 * _tmp571 * views_1_calibration(1, 0) + _tmp1482 * _tmp1483; + const Scalar _tmp1485 = _tmp571 * _tmp932; + const Scalar _tmp1486 = -_tmp1481 * _tmp579 * _tmp836 - _tmp1484 * _tmp1485 + _tmp1484 * _tmp936; + const Scalar _tmp1487 = _tmp1317 * _tmp227 + _tmp230; + const Scalar _tmp1488 = -_tmp1133 * _tmp1487 + + _tmp236 * (_tmp1321 * _tmp229 + _tmp1323 * _tmp228 + _tmp1325 * _tmp227); + const Scalar _tmp1489 = 2 * _tmp237; + const Scalar _tmp1490 = + -_tmp1134 * _tmp1487 * _tmp241 * views_1_calibration(1, 0) + _tmp1488 * _tmp1489; + const Scalar _tmp1491 = _tmp1139 * _tmp1490; + const Scalar _tmp1492 = _tmp1140 * _tmp1491 - _tmp1142 * _tmp1491 + _tmp1488 * _tmp249; + const Scalar _tmp1493 = + -_tmp1135 * _tmp1487 * _tmp249 + _tmp1248 * _tmp1490 - _tmp1249 * _tmp1490; + const Scalar _tmp1494 = _tmp1192 * _tmp1387 - _tmp1193 * _tmp1387 + _tmp1385 * _tmp399; + const Scalar _tmp1495 = _tmp1317 * _tmp527 + _tmp530; + const Scalar _tmp1496 = -_tmp1495 * _tmp992 + + _tmp536 * (_tmp1321 * _tmp529 + _tmp1323 * _tmp528 + _tmp1325 * _tmp527); + const Scalar _tmp1497 = 2 * _tmp537; + const Scalar _tmp1498 = + -_tmp1495 * _tmp541 * _tmp984 * views_1_calibration(1, 0) + _tmp1496 * _tmp1497; + const Scalar _tmp1499 = _tmp1498 * _tmp989; + const Scalar _tmp1500 = _tmp1496 * _tmp549 - _tmp1499 * _tmp990 + _tmp1499 * _tmp993; + const Scalar _tmp1501 = _tmp1317 * _tmp617 + _tmp620; + const Scalar _tmp1502 = -_tmp1187 * _tmp1501 + + _tmp626 * (_tmp1321 * _tmp619 + _tmp1323 * _tmp618 + _tmp1325 * _tmp617); + const Scalar _tmp1503 = 2 * _tmp627; + const Scalar _tmp1504 = + -_tmp1179 * _tmp1501 * _tmp631 * views_1_calibration(1, 0) + _tmp1502 * _tmp1503; const Scalar _tmp1505 = - _tmp1233 * _tmp1383 - _tmp1233 * _tmp1393 + _tmp1234 * _tmp1383 - _tmp1234 * _tmp1393 + - _tmp1385 * priors_1_0_sqrt_info(5, 1) + _tmp1386 * priors_1_0_sqrt_info(5, 2) + - _tmp1389 * priors_1_0_sqrt_info(5, 0) + _tmp1392 * priors_1_0_sqrt_info(5, 2) + - _tmp1395 * priors_1_0_sqrt_info(5, 3) + _tmp1396 * priors_1_0_sqrt_info(5, 5) - - _tmp1397 * priors_1_0_sqrt_info(5, 2); - const Scalar _tmp1506 = _tmp123 * priors_1_0_sqrt_info(3, 2); - const Scalar _tmp1507 = _tmp128 * _tmp1393; - const Scalar _tmp1508 = - _tmp1264 * _tmp1383 - _tmp1264 * _tmp1393 + _tmp1265 * _tmp1382 + _tmp138 * _tmp1391 + - _tmp1383 * _tmp1506 + _tmp1385 * priors_1_0_sqrt_info(3, 1) + - _tmp1389 * priors_1_0_sqrt_info(3, 0) + _tmp1395 * priors_1_0_sqrt_info(3, 3) + - _tmp1396 * priors_1_0_sqrt_info(3, 5) - _tmp1397 * priors_1_0_sqrt_info(3, 2) - - _tmp1507 * priors_1_0_sqrt_info(3, 1); - const Scalar _tmp1509 = _tmp1159 * (_tmp1337 * _tmp231 + _tmp234); - const Scalar _tmp1510 = -_tmp1160 * _tmp1509 + - _tmp240 * (_tmp1340 * _tmp233 + _tmp1342 * _tmp232 + _tmp1344 * _tmp231); - const Scalar _tmp1511 = 2 * _tmp241; - const Scalar _tmp1512 = _tmp1509 * _tmp243; - const Scalar _tmp1513 = _tmp1510 * _tmp1511 - _tmp1512 * _tmp245 * views_1_calibration(1, 0); - const Scalar _tmp1514 = _tmp1168 * _tmp1513; - const Scalar _tmp1515 = _tmp1169 * _tmp1514 - _tmp1171 * _tmp1514 + _tmp1510 * _tmp253; - const Scalar _tmp1516 = - _tmp1368 * priors_0_1_sqrt_info(4, 1) + _tmp1370 * priors_0_1_sqrt_info(4, 2) - - _tmp1372 * priors_0_1_sqrt_info(4, 1) + _tmp1374 * priors_0_1_sqrt_info(4, 1) + - _tmp1377 * priors_0_1_sqrt_info(4, 0) - _tmp1482 * priors_0_1_sqrt_info(4, 2) + - _tmp1483 * priors_0_1_sqrt_info(4, 2) - _tmp1485 * priors_0_1_sqrt_info(4, 0) + - _tmp1486 * priors_0_1_sqrt_info(4, 0); - const Scalar _tmp1517 = - _tmp1368 * priors_0_1_sqrt_info(3, 1) + _tmp1370 * priors_0_1_sqrt_info(3, 2) - - _tmp1372 * priors_0_1_sqrt_info(3, 1) + _tmp1374 * priors_0_1_sqrt_info(3, 1) + - _tmp1377 * priors_0_1_sqrt_info(3, 0) - _tmp1482 * priors_0_1_sqrt_info(3, 2) + - _tmp1483 * priors_0_1_sqrt_info(3, 2) - _tmp1485 * priors_0_1_sqrt_info(3, 0) + - _tmp1486 * priors_0_1_sqrt_info(3, 0); - const Scalar _tmp1518 = _tmp1275 * _tmp1513 - _tmp1276 * _tmp1513 - _tmp1512 * _tmp253 * _tmp898; - const Scalar _tmp1519 = -_tmp1214 * _tmp1434 + _tmp1215 * _tmp1433 + _tmp1431 * _tmp403; - const Scalar _tmp1520 = _tmp1337 * _tmp531 + _tmp534; - const Scalar _tmp1521 = -_tmp1047 * _tmp1520 + - _tmp540 * (_tmp1340 * _tmp533 + _tmp1342 * _tmp532 + _tmp1344 * _tmp531); - const Scalar _tmp1522 = 2 * _tmp541; - const Scalar _tmp1523 = - -_tmp1039 * _tmp1520 * _tmp545 * views_1_calibration(1, 0) + _tmp1521 * _tmp1522; - const Scalar _tmp1524 = _tmp1044 * _tmp1523; - const Scalar _tmp1525 = -_tmp1045 * _tmp1524 + _tmp1049 * _tmp1523 + _tmp1521 * _tmp553; - const Scalar _tmp1526 = _tmp64 * _tmp861; - const Scalar _tmp1527 = - _tmp1368 * priors_0_1_sqrt_info(1, 1) + _tmp1370 * priors_0_1_sqrt_info(1, 2) - - _tmp1372 * priors_0_1_sqrt_info(1, 1) + _tmp1374 * priors_0_1_sqrt_info(1, 1) + - _tmp1376 * _tmp1526 - _tmp1482 * priors_0_1_sqrt_info(1, 2) + - _tmp1483 * priors_0_1_sqrt_info(1, 2) - _tmp1485 * priors_0_1_sqrt_info(1, 0) + - _tmp1486 * priors_0_1_sqrt_info(1, 0); - const Scalar _tmp1528 = _tmp1200 * (_tmp1337 * _tmp621 + _tmp624); - const Scalar _tmp1529 = _tmp1528 * _tmp633; - const Scalar _tmp1530 = -_tmp1209 * _tmp1528 + - _tmp630 * (_tmp1340 * _tmp623 + _tmp1342 * _tmp622 + _tmp1344 * _tmp621); - const Scalar _tmp1531 = 2 * _tmp631; - const Scalar _tmp1532 = -_tmp1529 * _tmp635 * views_1_calibration(1, 0) + _tmp1530 * _tmp1531; - const Scalar _tmp1533 = _tmp1301 * _tmp1532 - _tmp1302 * _tmp1532 - _tmp1529 * _tmp643 * _tmp898; - const Scalar _tmp1534 = _tmp1014 * _tmp1440 - _tmp1017 * _tmp1440 - _tmp1439 * _tmp493 * _tmp898; - const Scalar _tmp1535 = _tmp1329 * _tmp1426 - _tmp1330 * _tmp1426 + _tmp1424 * _tmp673; - const Scalar _tmp1536 = -_tmp1247 * _tmp1503 + _tmp1248 * _tmp1503 + _tmp1501 * _tmp583; - const Scalar _tmp1537 = _tmp1067 * _tmp1069; - const Scalar _tmp1538 = _tmp1078 * _tmp335; - const Scalar _tmp1539 = - -_tmp1279 * _tmp1459 - _tmp1453 * _tmp1537 * _tmp343 + _tmp1459 * _tmp1538; - const Scalar _tmp1540 = - -_tmp1112 * _tmp1442 * _tmp193 + _tmp1119 * _tmp1445 - _tmp1121 * _tmp1445; - const Scalar _tmp1541 = - _tmp1174 * _tmp1383 - _tmp1174 * _tmp1393 + _tmp1175 * _tmp1383 - _tmp1175 * _tmp1393 + - _tmp1385 * priors_1_0_sqrt_info(0, 1) + _tmp1386 * priors_1_0_sqrt_info(0, 2) + - _tmp1389 * priors_1_0_sqrt_info(0, 0) + _tmp1392 * priors_1_0_sqrt_info(0, 2) + - _tmp1395 * priors_1_0_sqrt_info(0, 3) + _tmp1396 * priors_1_0_sqrt_info(0, 5) - - _tmp1397 * priors_1_0_sqrt_info(0, 2); - const Scalar _tmp1542 = -_tmp1208 * _tmp1532 + _tmp1212 * _tmp1532 + _tmp1530 * _tmp643; - const Scalar _tmp1543 = _tmp123 * priors_1_0_sqrt_info(4, 2); - const Scalar _tmp1544 = - _tmp1138 * _tmp1383 - _tmp1138 * _tmp1393 + _tmp1141 * _tmp1382 + _tmp1383 * _tmp1543 + - _tmp1385 * priors_1_0_sqrt_info(4, 1) + _tmp1389 * priors_1_0_sqrt_info(4, 0) + - _tmp1392 * priors_1_0_sqrt_info(4, 2) + _tmp1395 * priors_1_0_sqrt_info(4, 3) + - _tmp1396 * priors_1_0_sqrt_info(4, 5) - _tmp1397 * priors_1_0_sqrt_info(4, 2) - - _tmp1507 * priors_1_0_sqrt_info(4, 1); - const Scalar _tmp1545 = - -_tmp1040 * _tmp1520 * _tmp553 - _tmp1258 * _tmp1524 + _tmp1260 * _tmp1523; - const Scalar _tmp1546 = _tmp1494 * _tmp721; - const Scalar _tmp1547 = -_tmp1151 * _tmp1546 + _tmp1154 * _tmp1546 + _tmp1492 * _tmp733; - const Scalar _tmp1548 = _tmp1341 + _tmp887; - const Scalar _tmp1549 = _tmp600 * (_tmp1548 * _tmp592 + _tmp602); - const Scalar _tmp1550 = _tmp71 + _tmp74 + _tmp885 + _tmp891; - const Scalar _tmp1551 = _tmp889 + _tmp99; - const Scalar _tmp1552 = _tmp1343 + _tmp81; - const Scalar _tmp1553 = _tmp604 * (_tmp1550 * _tmp591 + _tmp1551 * _tmp592 + _tmp1552 * _tmp593); - const Scalar _tmp1554 = _tmp1450 * _tmp1549 + _tmp1553 * _tmp901; - const Scalar _tmp1555 = _tmp1553 * _tmp613 - _tmp1554 * _tmp905 + _tmp1554 * _tmp908; - const Scalar _tmp1556 = _tmp424 * (_tmp1550 * _tmp411 + _tmp1551 * _tmp412 + _tmp1552 * _tmp413); - const Scalar _tmp1557 = _tmp420 * (_tmp1548 * _tmp412 + _tmp422); - const Scalar _tmp1558 = _tmp1225 * _tmp1556 + _tmp1360 * _tmp1557; - const Scalar _tmp1559 = _tmp1229 * _tmp1558 - _tmp1231 * _tmp1558 + _tmp1556 * _tmp433; - const Scalar _tmp1560 = _tmp754 * (_tmp1550 * _tmp741 + _tmp1551 * _tmp743 + _tmp1552 * _tmp742); - const Scalar _tmp1561 = _tmp750 * (_tmp1548 * _tmp743 + _tmp752); - const Scalar _tmp1562 = _tmp1100 * _tmp1560 + _tmp1474 * _tmp1561; - const Scalar _tmp1563 = _tmp1182 * _tmp1562 - _tmp1183 * _tmp1562 + _tmp1561 * _tmp763; - const Scalar _tmp1564 = -_priors_1_0_target_T_src[1] * _tmp1364 + _tmp1387 + _tmp947 - _tmp949; - const Scalar _tmp1565 = _tmp1564 * _tmp961; - const Scalar _tmp1566 = - _tmp951 * (-_priors_1_0_target_T_src[0] * _tmp1364 + _tmp1384 + _tmp967 - _tmp969); - const Scalar _tmp1567 = _tmp122 * _tmp1566; - const Scalar _tmp1568 = - _tmp951 * (_priors_1_0_target_T_src[2] * _tmp1364 + _tmp1380 + _tmp1381 + _tmp956); - const Scalar _tmp1569 = _tmp1564 * _tmp943; - const Scalar _tmp1570 = _tmp123 * _tmp1569; - const Scalar _tmp1571 = - -_tmp101 + _tmp102 + _tmp1548 * _views_0_pose[5] - _tmp1548 * _views_1_pose[5] - _tmp98; - const Scalar _tmp1572 = _tmp1550 * _views_0_pose[4] - _tmp1550 * _views_1_pose[4] + - _tmp1551 * _views_0_pose[5] - _tmp1551 * _views_1_pose[5] + - _tmp1552 * _views_0_pose[6] - _tmp1552 * _views_1_pose[6]; - const Scalar _tmp1573 = _tmp123 * _tmp1565; - const Scalar _tmp1574 = - _tmp952 * (_priors_1_0_target_T_src[3] * _tmp1364 + _tmp1390 + _tmp932 + _tmp937); - const Scalar _tmp1575 = _tmp131 * _tmp1569; - const Scalar _tmp1576 = - _tmp135 * _tmp1565 - _tmp135 * _tmp1569 + _tmp136 * _tmp1568 + _tmp1565 * _tmp987 + - _tmp1567 * priors_1_0_sqrt_info(2, 2) - _tmp1570 * priors_1_0_sqrt_info(2, 2) + - _tmp1571 * priors_1_0_sqrt_info(2, 3) + _tmp1572 * priors_1_0_sqrt_info(2, 4) + - _tmp1573 * priors_1_0_sqrt_info(2, 2) + _tmp1574 * priors_1_0_sqrt_info(2, 1) - - _tmp1575 * priors_1_0_sqrt_info(2, 0); - const Scalar _tmp1577 = _tmp720 * (_tmp1548 * _tmp713 + _tmp722); - const Scalar _tmp1578 = _tmp724 * (_tmp1550 * _tmp711 + _tmp1551 * _tmp713 + _tmp1552 * _tmp712); - const Scalar _tmp1579 = _tmp1148 * _tmp1578 + _tmp1493 * _tmp1577; - const Scalar _tmp1580 = -_tmp1153 * _tmp1579 + _tmp1156 * _tmp1579 + _tmp1578 * _tmp733; - const Scalar _tmp1581 = _tmp122 * _tmp1568; - const Scalar _tmp1582 = - _tmp1174 * _tmp1565 - _tmp1174 * _tmp1569 + _tmp1175 * _tmp1565 - _tmp1175 * _tmp1569 + - _tmp1567 * priors_1_0_sqrt_info(0, 2) - _tmp1570 * priors_1_0_sqrt_info(0, 2) + - _tmp1571 * priors_1_0_sqrt_info(0, 3) + _tmp1572 * priors_1_0_sqrt_info(0, 4) + - _tmp1573 * priors_1_0_sqrt_info(0, 2) + _tmp1574 * priors_1_0_sqrt_info(0, 1) + - _tmp1581 * priors_1_0_sqrt_info(0, 0); - const Scalar _tmp1583 = _tmp128 * _tmp1569; - const Scalar _tmp1584 = - _tmp1264 * _tmp1565 - _tmp1264 * _tmp1569 + _tmp1265 * _tmp1564 + _tmp138 * _tmp1566 + - _tmp1506 * _tmp1565 - _tmp1570 * priors_1_0_sqrt_info(3, 2) + - _tmp1571 * priors_1_0_sqrt_info(3, 3) + _tmp1572 * priors_1_0_sqrt_info(3, 4) + - _tmp1574 * priors_1_0_sqrt_info(3, 1) + _tmp1581 * priors_1_0_sqrt_info(3, 0) - - _tmp1583 * priors_1_0_sqrt_info(3, 1); - const Scalar _tmp1585 = _tmp484 * (_tmp1550 * _tmp471 + _tmp1551 * _tmp473 + _tmp1552 * _tmp472); - const Scalar _tmp1586 = _tmp480 * (_tmp1548 * _tmp473 + _tmp482); - const Scalar _tmp1587 = _tmp1009 * _tmp1585 + _tmp1438 * _tmp1586; - const Scalar _tmp1588 = _tmp1014 * _tmp1587 - _tmp1017 * _tmp1587 + _tmp1585 * _tmp493; - const Scalar _tmp1589 = _tmp176 * (_tmp1550 * _tmp157 + _tmp1551 * _tmp161 + _tmp1552 * _tmp165); - const Scalar _tmp1590 = _tmp172 * (_tmp1548 * _tmp161 + _tmp174); - const Scalar _tmp1591 = _tmp1115 * _tmp1589 + _tmp1444 * _tmp1590; - const Scalar _tmp1592 = _tmp1119 * _tmp1591 - _tmp1121 * _tmp1591 + _tmp1589 * _tmp193; - const Scalar _tmp1593 = _tmp664 * (_tmp1550 * _tmp651 + _tmp1551 * _tmp652 + _tmp1552 * _tmp653); - const Scalar _tmp1594 = _tmp660 * (_tmp1548 * _tmp652 + _tmp662); - const Scalar _tmp1595 = _tmp1308 * _tmp1593 + _tmp1425 * _tmp1594; - const Scalar _tmp1596 = _tmp1329 * _tmp1595 - _tmp1330 * _tmp1595 + _tmp1594 * _tmp673; - const Scalar _tmp1597 = -_tmp1238 * _tmp1591 + _tmp1239 * _tmp1591 + _tmp1590 * _tmp193; - const Scalar _tmp1598 = -_priors_0_1_target_T_src[0] * _tmp934 - _tmp1375 + _tmp866; - const Scalar _tmp1599 = _tmp1598 * _tmp855; - const Scalar _tmp1600 = _tmp1599 * _tmp57; - const Scalar _tmp1601 = _tmp867 * (_priors_0_1_target_T_src[1] * _tmp934 + _tmp1373 + _tmp860); - const Scalar _tmp1602 = _tmp1598 * _tmp880; - const Scalar _tmp1603 = _tmp1598 * _tmp874; - const Scalar _tmp1604 = - _tmp867 * (-_priors_0_1_target_T_src[2] * _tmp934 - _tmp1369 + _tmp831 + _tmp842); - const Scalar _tmp1605 = _tmp1598 * _tmp869; - const Scalar _tmp1606 = _tmp1605 * _tmp57; - const Scalar _tmp1607 = _tmp1599 * _tmp54; - const Scalar _tmp1608 = _priors_0_1_target_T_src[3] * _tmp934 + _tmp1365 + _tmp878; - const Scalar _tmp1609 = _tmp1605 * _tmp54; - const Scalar _tmp1610 = - _tmp1526 * _tmp1608 + _tmp1600 * priors_0_1_sqrt_info(1, 2) + - _tmp1601 * priors_0_1_sqrt_info(1, 2) - _tmp1602 * priors_0_1_sqrt_info(1, 1) + - _tmp1603 * priors_0_1_sqrt_info(1, 1) + _tmp1604 * priors_0_1_sqrt_info(1, 1) - - _tmp1606 * priors_0_1_sqrt_info(1, 2) + _tmp1607 * priors_0_1_sqrt_info(1, 0) - - _tmp1609 * priors_0_1_sqrt_info(1, 0); - const Scalar _tmp1611 = _tmp244 * (_tmp1550 * _tmp231 + _tmp1551 * _tmp233 + _tmp1552 * _tmp232); - const Scalar _tmp1612 = _tmp240 * (_tmp1548 * _tmp233 + _tmp242); - const Scalar _tmp1613 = _tmp1166 * _tmp1611 + _tmp1511 * _tmp1612; - const Scalar _tmp1614 = _tmp1275 * _tmp1613 - _tmp1276 * _tmp1613 + _tmp1611 * _tmp253; - const Scalar _tmp1615 = - _tmp1138 * _tmp1565 - _tmp1138 * _tmp1569 + _tmp1141 * _tmp1564 + _tmp1543 * _tmp1565 + - _tmp1567 * priors_1_0_sqrt_info(4, 2) - _tmp1570 * priors_1_0_sqrt_info(4, 2) + - _tmp1571 * priors_1_0_sqrt_info(4, 3) + _tmp1572 * priors_1_0_sqrt_info(4, 4) + - _tmp1574 * priors_1_0_sqrt_info(4, 1) + _tmp1581 * priors_1_0_sqrt_info(4, 0) - - _tmp1583 * priors_1_0_sqrt_info(4, 1); - const Scalar _tmp1616 = _tmp364 * (_tmp1550 * _tmp351 + _tmp1551 * _tmp352 + _tmp1552 * _tmp353); - const Scalar _tmp1617 = _tmp360 * (_tmp1548 * _tmp352 + _tmp362); - const Scalar _tmp1618 = _tmp1026 * _tmp1616 + _tmp1466 * _tmp1617; - const Scalar _tmp1619 = _tmp1034 * _tmp1618 - _tmp1497 * _tmp1618 + _tmp1617 * _tmp373; - const Scalar _tmp1620 = _tmp1608 * _tmp867; - const Scalar _tmp1621 = - _tmp1600 * priors_0_1_sqrt_info(4, 2) + _tmp1601 * priors_0_1_sqrt_info(4, 2) - - _tmp1602 * priors_0_1_sqrt_info(4, 1) + _tmp1603 * priors_0_1_sqrt_info(4, 1) + - _tmp1604 * priors_0_1_sqrt_info(4, 1) - _tmp1606 * priors_0_1_sqrt_info(4, 2) + - _tmp1607 * priors_0_1_sqrt_info(4, 0) - _tmp1609 * priors_0_1_sqrt_info(4, 0) + - _tmp1620 * priors_0_1_sqrt_info(4, 0); - const Scalar _tmp1622 = _tmp690 * (_tmp1548 * _tmp682 + _tmp692); - const Scalar _tmp1623 = _tmp694 * (_tmp1550 * _tmp681 + _tmp1551 * _tmp682 + _tmp1552 * _tmp683); - const Scalar _tmp1624 = _tmp1086 * _tmp1623 + _tmp1347 * _tmp1622; - const Scalar _tmp1625 = -_tmp1242 * _tmp1624 + _tmp1243 * _tmp1624 + _tmp1623 * _tmp703; - const Scalar _tmp1626 = _tmp330 * (_tmp1548 * _tmp322 + _tmp332); - const Scalar _tmp1627 = _tmp334 * (_tmp1550 * _tmp321 + _tmp1551 * _tmp322 + _tmp1552 * _tmp323); - const Scalar _tmp1628 = _tmp1071 * _tmp1627 + _tmp1458 * _tmp1626; - const Scalar _tmp1629 = -_tmp1076 * _tmp1628 + _tmp1460 * _tmp1628 + _tmp1626 * _tmp343; - const Scalar _tmp1630 = _tmp1170 * _tmp1613 - _tmp1172 * _tmp1613 + _tmp1612 * _tmp253; - const Scalar _tmp1631 = - _tmp1262 * _tmp1599 - _tmp1262 * _tmp1605 + _tmp1600 * priors_0_1_sqrt_info(0, 2) + - _tmp1601 * priors_0_1_sqrt_info(0, 2) - _tmp1602 * priors_0_1_sqrt_info(0, 1) + - _tmp1603 * priors_0_1_sqrt_info(0, 1) + _tmp1604 * priors_0_1_sqrt_info(0, 1) - - _tmp1606 * priors_0_1_sqrt_info(0, 2) + _tmp1620 * priors_0_1_sqrt_info(0, 0); - const Scalar _tmp1632 = _tmp630 * (_tmp1548 * _tmp623 + _tmp632); - const Scalar _tmp1633 = _tmp634 * (_tmp1550 * _tmp621 + _tmp1551 * _tmp623 + _tmp1552 * _tmp622); - const Scalar _tmp1634 = _tmp1204 * _tmp1633 + _tmp1531 * _tmp1632; - const Scalar _tmp1635 = _tmp1300 * _tmp1634; - const Scalar _tmp1636 = -_tmp1206 * _tmp1635 + _tmp1211 * _tmp1635 + _tmp1633 * _tmp643; - const Scalar _tmp1637 = -_tmp1268 * _tmp1558 + _tmp1269 * _tmp1558 + _tmp1557 * _tmp433; - const Scalar _tmp1638 = _tmp544 * (_tmp1550 * _tmp531 + _tmp1551 * _tmp533 + _tmp1552 * _tmp532); - const Scalar _tmp1639 = _tmp540 * (_tmp1548 * _tmp533 + _tmp542); - const Scalar _tmp1640 = _tmp1042 * _tmp1638 + _tmp1522 * _tmp1639; - const Scalar _tmp1641 = -_tmp1259 * _tmp1640 + _tmp1260 * _tmp1640 + _tmp1638 * _tmp553; - const Scalar _tmp1642 = _tmp574 * (_tmp1550 * _tmp561 + _tmp1551 * _tmp562 + _tmp1552 * _tmp563); - const Scalar _tmp1643 = _tmp570 * (_tmp1548 * _tmp562 + _tmp572); - const Scalar _tmp1644 = _tmp1502 * _tmp1643 + _tmp1642 * _tmp979; - const Scalar _tmp1645 = _tmp1644 * _tmp981; - const Scalar _tmp1646 = _tmp1642 * _tmp583 - _tmp1645 * _tmp982 + _tmp1645 * _tmp984; - const Scalar _tmp1647 = - _tmp1233 * _tmp1565 - _tmp1233 * _tmp1569 + _tmp1234 * _tmp1565 - _tmp1234 * _tmp1569 + - _tmp1567 * priors_1_0_sqrt_info(5, 2) - _tmp1570 * priors_1_0_sqrt_info(5, 2) + - _tmp1571 * priors_1_0_sqrt_info(5, 3) + _tmp1572 * priors_1_0_sqrt_info(5, 4) + - _tmp1573 * priors_1_0_sqrt_info(5, 2) + _tmp1574 * priors_1_0_sqrt_info(5, 1) + - _tmp1581 * priors_1_0_sqrt_info(5, 0); - const Scalar _tmp1648 = - _tmp1565 * _tmp963 + _tmp1565 * _tmp964 + _tmp1567 * priors_1_0_sqrt_info(1, 2) - - _tmp1569 * _tmp964 - _tmp1570 * priors_1_0_sqrt_info(1, 2) + - _tmp1571 * priors_1_0_sqrt_info(1, 3) + _tmp1572 * priors_1_0_sqrt_info(1, 4) + - _tmp1573 * priors_1_0_sqrt_info(1, 2) + _tmp1574 * priors_1_0_sqrt_info(1, 1) - - _tmp1575 * priors_1_0_sqrt_info(1, 0) + _tmp1581 * priors_1_0_sqrt_info(1, 0); - const Scalar _tmp1649 = _tmp510 * (_tmp1548 * _tmp502 + _tmp512); - const Scalar _tmp1650 = _tmp514 * (_tmp1550 * _tmp501 + _tmp1551 * _tmp502 + _tmp1552 * _tmp503); - const Scalar _tmp1651 = _tmp1352 * _tmp1649 + _tmp1650 * _tmp916; - const Scalar _tmp1652 = _tmp1649 * _tmp523 - _tmp1651 * _tmp920 + _tmp1651 * _tmp923; - const Scalar _tmp1653 = -_tmp1333 * _tmp1587 + _tmp1334 * _tmp1587 + _tmp1586 * _tmp493; - const Scalar _tmp1654 = - _tmp1185 * _tmp1599 + _tmp1601 * priors_0_1_sqrt_info(5, 2) - - _tmp1602 * priors_0_1_sqrt_info(5, 1) + _tmp1603 * priors_0_1_sqrt_info(5, 1) + - _tmp1604 * priors_0_1_sqrt_info(5, 1) - _tmp1606 * priors_0_1_sqrt_info(5, 2) + - _tmp1607 * priors_0_1_sqrt_info(5, 0) - _tmp1609 * priors_0_1_sqrt_info(5, 0) + - _tmp1620 * priors_0_1_sqrt_info(5, 0); - const Scalar _tmp1655 = -_tmp1279 * _tmp1628 + _tmp1538 * _tmp1628 + _tmp1627 * _tmp343; - const Scalar _tmp1656 = _tmp394 * (_tmp1550 * _tmp381 + _tmp1551 * _tmp382 + _tmp1552 * _tmp383); - const Scalar _tmp1657 = _tmp390 * (_tmp1548 * _tmp382 + _tmp392); - const Scalar _tmp1658 = _tmp1128 * _tmp1656 + _tmp1432 * _tmp1657; - const Scalar _tmp1659 = _tmp1134 * _tmp1658 - _tmp1136 * _tmp1658 + _tmp1656 * _tmp403; - const Scalar _tmp1660 = _tmp300 * (_tmp1548 * _tmp293 + _tmp302); - const Scalar _tmp1661 = _tmp304 * (_tmp1550 * _tmp291 + _tmp1551 * _tmp293 + _tmp1552 * _tmp292); - const Scalar _tmp1662 = _tmp1191 * _tmp1661 + _tmp1417 * _tmp1660; - const Scalar _tmp1663 = _tmp1283 * _tmp1662 - _tmp1284 * _tmp1662 + _tmp1660 * _tmp313; - const Scalar _tmp1664 = _tmp1650 * _tmp523 - _tmp1651 * _tmp928 + _tmp1651 * _tmp929; - const Scalar _tmp1665 = _tmp1092 * _tmp1624 - _tmp1094 * _tmp1624 + _tmp1622 * _tmp703; - const Scalar _tmp1666 = _tmp274 * (_tmp1550 * _tmp261 + _tmp1551 * _tmp262 + _tmp1552 * _tmp263); - const Scalar _tmp1667 = _tmp270 * (_tmp1548 * _tmp262 + _tmp272); - const Scalar _tmp1668 = _tmp1057 * _tmp1666 + _tmp1412 * _tmp1667; - const Scalar _tmp1669 = -_tmp1271 * _tmp1668 + _tmp1272 * _tmp1668 + _tmp1666 * _tmp283; - const Scalar _tmp1670 = _tmp1252 * _tmp1554 - _tmp1253 * _tmp1554 + _tmp1549 * _tmp613; - const Scalar _tmp1671 = _tmp210 * (_tmp1548 * _tmp202 + _tmp212); - const Scalar _tmp1672 = _tmp214 * (_tmp1550 * _tmp201 + _tmp1551 * _tmp202 + _tmp1552 * _tmp203); - const Scalar _tmp1673 = _tmp1402 * _tmp1671 + _tmp1672 * _tmp995; - const Scalar _tmp1674 = _tmp1001 * _tmp1673 + _tmp1672 * _tmp223 - _tmp1673 * _tmp999; - const Scalar _tmp1675 = -_tmp1247 * _tmp1644 + _tmp1248 * _tmp1644 + _tmp1643 * _tmp583; - const Scalar _tmp1676 = - _tmp1600 * priors_0_1_sqrt_info(3, 2) + _tmp1601 * priors_0_1_sqrt_info(3, 2) - - _tmp1602 * priors_0_1_sqrt_info(3, 1) + _tmp1603 * priors_0_1_sqrt_info(3, 1) + - _tmp1604 * priors_0_1_sqrt_info(3, 1) - _tmp1606 * priors_0_1_sqrt_info(3, 2) + - _tmp1607 * priors_0_1_sqrt_info(3, 0) - _tmp1609 * priors_0_1_sqrt_info(3, 0) + - _tmp1620 * priors_0_1_sqrt_info(3, 0); - const Scalar _tmp1677 = _tmp1177 * _tmp1579 - _tmp1178 * _tmp1579 + _tmp1577 * _tmp733; - const Scalar _tmp1678 = _tmp1061 * _tmp1668 - _tmp1065 * _tmp1668 + _tmp1667 * _tmp283; - const Scalar _tmp1679 = _tmp1322 * _tmp1673 - _tmp1324 * _tmp1673 + _tmp1671 * _tmp223; - const Scalar _tmp1680 = -_tmp1195 * _tmp1662 + _tmp1197 * _tmp1662 + _tmp1661 * _tmp313; - const Scalar _tmp1681 = -_tmp1046 * _tmp1640 + _tmp1049 * _tmp1640 + _tmp1639 * _tmp553; - const Scalar _tmp1682 = -_tmp1105 * _tmp1562 + _tmp1107 * _tmp1562 + _tmp1560 * _tmp763; - const Scalar _tmp1683 = -_tmp1208 * _tmp1634 + _tmp1212 * _tmp1634 + _tmp1632 * _tmp643; - const Scalar _tmp1684 = _tmp1255 * _tmp1618; - const Scalar _tmp1685 = -_tmp1029 * _tmp1684 + _tmp1033 * _tmp1684 + _tmp1616 * _tmp373; - const Scalar _tmp1686 = _tmp1215 * _tmp1658 - _tmp1216 * _tmp1658 + _tmp1657 * _tmp403; - const Scalar _tmp1687 = - _tmp1019 * _tmp1599 + _tmp1599 * _tmp66 + _tmp1601 * priors_0_1_sqrt_info(2, 2) - - _tmp1602 * priors_0_1_sqrt_info(2, 1) + _tmp1603 * priors_0_1_sqrt_info(2, 1) + - _tmp1604 * priors_0_1_sqrt_info(2, 1) - _tmp1605 * _tmp66 - - _tmp1609 * priors_0_1_sqrt_info(2, 0) + _tmp1620 * priors_0_1_sqrt_info(2, 0); - const Scalar _tmp1688 = _tmp454 * (_tmp1550 * _tmp441 + _tmp1551 * _tmp442 + _tmp1552 * _tmp443); - const Scalar _tmp1689 = _tmp450 * (_tmp1548 * _tmp442 + _tmp452); - const Scalar _tmp1690 = _tmp1292 * _tmp1688 + _tmp1407 * _tmp1689; - const Scalar _tmp1691 = _tmp1294 * _tmp1690; - const Scalar _tmp1692 = _tmp1295 * _tmp1691 - _tmp1297 * _tmp1691 + _tmp1688 * _tmp463; - const Scalar _tmp1693 = _tmp1312 * _tmp1595 - _tmp1314 * _tmp1595 + _tmp1593 * _tmp673; - const Scalar _tmp1694 = _tmp1318 * _tmp1690 - _tmp1319 * _tmp1690 + _tmp1689 * _tmp463; - const Scalar _tmp1695 = _tmp244 * landmarks_2_; - const Scalar _tmp1696 = _tmp1164 * landmarks_2_; - const Scalar _tmp1697 = -_tmp100 * _tmp1695 + _tmp108 * _tmp1696; - const Scalar _tmp1698 = _tmp240 * landmarks_2_; - const Scalar _tmp1699 = _tmp1161 * landmarks_2_; - const Scalar _tmp1700 = _tmp108 * _tmp1699 - _tmp1698 * _tmp77; - const Scalar _tmp1701 = _tmp1166 * _tmp1697 + _tmp1511 * _tmp1700; - const Scalar _tmp1702 = _tmp1171 * _tmp1701; - const Scalar _tmp1703 = -_tmp1274 * _tmp1702 + _tmp1275 * _tmp1701 + _tmp1697 * _tmp253; - const Scalar _tmp1704 = _tmp750 * landmarks_19_; - const Scalar _tmp1705 = _tmp108 * landmarks_19_; - const Scalar _tmp1706 = _tmp1472 * _tmp1705 - _tmp1704 * _tmp77; - const Scalar _tmp1707 = _tmp754 * landmarks_19_; - const Scalar _tmp1708 = -_tmp100 * _tmp1707 + _tmp1479 * _tmp1705; - const Scalar _tmp1709 = _tmp1100 * _tmp1708 + _tmp1474 * _tmp1706; - const Scalar _tmp1710 = _tmp1182 * _tmp1709 - _tmp1183 * _tmp1709 + _tmp1706 * _tmp763; - const Scalar _tmp1711 = _tmp977 * landmarks_13_; - const Scalar _tmp1712 = _tmp574 * landmarks_13_; - const Scalar _tmp1713 = -_tmp100 * _tmp1712 + _tmp108 * _tmp1711; - const Scalar _tmp1714 = _tmp570 * landmarks_13_; - const Scalar _tmp1715 = _tmp1245 * landmarks_13_; - const Scalar _tmp1716 = _tmp108 * _tmp1715 - _tmp1714 * _tmp77; - const Scalar _tmp1717 = _tmp1502 * _tmp1716 + _tmp1713 * _tmp979; - const Scalar _tmp1718 = _tmp1713 * _tmp583 - _tmp1717 * _tmp983 + _tmp1717 * _tmp985; - const Scalar _tmp1719 = _tmp4 * priors_0_1_sqrt_info(5, 4) + _tmp51 * priors_0_1_sqrt_info(5, 5) + - _tmp60 * priors_0_1_sqrt_info(5, 3); - const Scalar _tmp1720 = _tmp720 * landmarks_18_; - const Scalar _tmp1721 = _tmp1179 * landmarks_18_; - const Scalar _tmp1722 = _tmp108 * _tmp1721 - _tmp1720 * _tmp77; - const Scalar _tmp1723 = _tmp724 * landmarks_18_; - const Scalar _tmp1724 = _tmp1146 * landmarks_18_; - const Scalar _tmp1725 = -_tmp100 * _tmp1723 + _tmp108 * _tmp1724; - const Scalar _tmp1726 = _tmp1148 * _tmp1725 + _tmp1493 * _tmp1722; - const Scalar _tmp1727 = _tmp1177 * _tmp1726 - _tmp1178 * _tmp1726 + _tmp1722 * _tmp733; - const Scalar _tmp1728 = -_tmp1105 * _tmp1709 + _tmp1107 * _tmp1709 + _tmp1708 * _tmp763; - const Scalar _tmp1729 = _tmp4 * priors_0_1_sqrt_info(2, 4) + _tmp51 * priors_0_1_sqrt_info(2, 5) + + -_tmp1180 * _tmp1501 * _tmp639 + _tmp1276 * _tmp1504 - _tmp1277 * _tmp1504; + const Scalar _tmp1506 = -_tmp1394 * _tmp489 * _tmp836 + _tmp1395 * _tmp963 - _tmp1395 * _tmp965; + const Scalar _tmp1507 = _tmp1309 * _tmp1380 - _tmp1310 * _tmp1380 + _tmp1378 * _tmp669; + const Scalar _tmp1508 = _tmp567 * _tmp932; + const Scalar _tmp1509 = _tmp1220 * _tmp1484 + _tmp1482 * _tmp579 - _tmp1484 * _tmp1508; + const Scalar _tmp1510 = _tmp41 * priors_0_1_sqrt_info(1, 1); + const Scalar _tmp1511 = _tmp1450 * _tmp1510 + _tmp1454 * priors_0_1_sqrt_info(1, 2) - + _tmp1456 * priors_0_1_sqrt_info(1, 2) - + _tmp1457 * priors_0_1_sqrt_info(1, 1) + + _tmp1458 * priors_0_1_sqrt_info(1, 2) + _tmp1460 * _tmp64; + const Scalar _tmp1512 = _tmp1252 * _tmp1421 - _tmp1253 * _tmp1421 - _tmp1417 * _tmp339 * _tmp836; + const Scalar _tmp1513 = + -_tmp1088 * _tmp1406 * _tmp189 + _tmp1095 * _tmp1409 - _tmp1097 * _tmp1409; + const Scalar _tmp1514 = -_priors_1_0_target_T_src[0] * _tmp1000; + const Scalar _tmp1515 = + -_tmp1145 * _tmp1403 + _tmp1145 * _tmp1463 + + _tmp1150 * (_priors_1_0_target_T_src[3] * _tmp997 + _tmp1514 + _tmp874 + _tmp886) - + _tmp129 * _tmp1403 + _tmp129 * _tmp1463 - _tmp130 * _tmp1403 + _tmp130 * _tmp1463 + + _tmp1350 * priors_1_0_sqrt_info(0, 5) + _tmp1402 * priors_1_0_sqrt_info(0, 2) + + _tmp1404 * priors_1_0_sqrt_info(0, 3) + _tmp1465 * priors_1_0_sqrt_info(0, 1); + const Scalar _tmp1516 = -_tmp1186 * _tmp1504 + _tmp1189 * _tmp1504 + _tmp1502 * _tmp639; + const Scalar _tmp1517 = -_tmp1230 * _tmp1498 + _tmp1231 * _tmp1498 - _tmp1495 * _tmp549 * _tmp985; + const Scalar _tmp1518 = _tmp1156 * _tmp1474 - _tmp1157 * _tmp1474 + _tmp1472 * _tmp729; + const Scalar _tmp1519 = _tmp1322 + _tmp826; + const Scalar _tmp1520 = _tmp596 * (_tmp1519 * _tmp588 + _tmp598); + const Scalar _tmp1521 = _tmp70 + _tmp73 + _tmp824 + _tmp830; + const Scalar _tmp1522 = _tmp828 + _tmp98; + const Scalar _tmp1523 = _tmp1324 + _tmp79; + const Scalar _tmp1524 = _tmp600 * (_tmp1521 * _tmp587 + _tmp1522 * _tmp588 + _tmp1523 * _tmp589); + const Scalar _tmp1525 = _tmp1413 * _tmp1520 + _tmp1524 * _tmp842; + const Scalar _tmp1526 = _tmp1524 * _tmp609 - _tmp1525 * _tmp846 + _tmp1525 * _tmp851; + const Scalar _tmp1527 = _tmp420 * (_tmp1521 * _tmp407 + _tmp1522 * _tmp408 + _tmp1523 * _tmp409); + const Scalar _tmp1528 = _tmp416 * (_tmp1519 * _tmp408 + _tmp418); + const Scalar _tmp1529 = _tmp1200 * _tmp1527 + _tmp1345 * _tmp1528; + const Scalar _tmp1530 = _tmp1204 * _tmp1529 - _tmp1206 * _tmp1529 + _tmp1527 * _tmp429; + const Scalar _tmp1531 = _tmp750 * (_tmp1521 * _tmp737 + _tmp1522 * _tmp739 + _tmp1523 * _tmp738); + const Scalar _tmp1532 = _tmp746 * (_tmp1519 * _tmp739 + _tmp748); + const Scalar _tmp1533 = _tmp1076 * _tmp1531 + _tmp1439 * _tmp1532; + const Scalar _tmp1534 = _tmp1082 * _tmp1533; + const Scalar _tmp1535 = _tmp1159 * _tmp1534 - _tmp1160 * _tmp1533 + _tmp1532 * _tmp759; + const Scalar _tmp1536 = -_priors_0_1_target_T_src[0] * _tmp880 + _tmp1013 - _tmp1461; + const Scalar _tmp1537 = _tmp1014 * (_priors_0_1_target_T_src[1] * _tmp880 + _tmp1235 + _tmp1459); + const Scalar _tmp1538 = _tmp1016 * _tmp1536; + const Scalar _tmp1539 = -_tmp1303 * _tmp1538 + _tmp1304 * _tmp1536 + _tmp1537 * _tmp66; + const Scalar _tmp1540 = _tmp716 * (_tmp1519 * _tmp709 + _tmp718); + const Scalar _tmp1541 = _tmp720 * (_tmp1521 * _tmp707 + _tmp1522 * _tmp709 + _tmp1523 * _tmp708); + const Scalar _tmp1542 = _tmp1120 * _tmp1541 + _tmp1473 * _tmp1540; + const Scalar _tmp1543 = -_tmp1126 * _tmp1542 + _tmp1128 * _tmp1542 + _tmp1541 * _tmp729; + const Scalar _tmp1544 = -_priors_1_0_target_T_src[1] * _tmp1447 + _tmp1514 + _tmp875 - _tmp892; + const Scalar _tmp1545 = _tmp1544 * _tmp913; + const Scalar _tmp1546 = _tmp1544 * _tmp916; + const Scalar _tmp1547 = + _tmp897 * (-_priors_1_0_target_T_src[0] * _tmp1447 + _tmp1147 - _tmp1149 + _tmp1464); + const Scalar _tmp1548 = _tmp121 * _tmp1547; + const Scalar _tmp1549 = + -_tmp100 + _tmp101 + _tmp1519 * _views_0_pose[5] - _tmp1519 * _views_1_pose[5] - _tmp97; + const Scalar _tmp1550 = _tmp1521 * _views_0_pose[4] - _tmp1521 * _views_1_pose[4] + + _tmp1522 * _views_0_pose[5] - _tmp1522 * _views_1_pose[5] + + _tmp1523 * _views_0_pose[6] - _tmp1523 * _views_1_pose[6]; + const Scalar _tmp1551 = + _tmp898 * (_priors_1_0_target_T_src[3] * _tmp1447 + _tmp1400 + _tmp907 + _tmp910); + const Scalar _tmp1552 = + _tmp1145 * _tmp1545 - _tmp1145 * _tmp1546 + + _tmp1150 * (_priors_1_0_target_T_src[2] * _tmp1447 + _tmp1397 + _tmp1398 + _tmp902) + + _tmp129 * _tmp1545 - _tmp129 * _tmp1546 + _tmp130 * _tmp1545 - _tmp130 * _tmp1546 + + _tmp1548 * priors_1_0_sqrt_info(0, 2) + _tmp1549 * priors_1_0_sqrt_info(0, 3) + + _tmp1550 * priors_1_0_sqrt_info(0, 4) + _tmp1551 * priors_1_0_sqrt_info(0, 1); + const Scalar _tmp1553 = _tmp480 * (_tmp1521 * _tmp467 + _tmp1522 * _tmp469 + _tmp1523 * _tmp468); + const Scalar _tmp1554 = _tmp476 * (_tmp1519 * _tmp469 + _tmp478); + const Scalar _tmp1555 = _tmp1393 * _tmp1554 + _tmp1553 * _tmp959; + const Scalar _tmp1556 = _tmp1553 * _tmp489 + _tmp1555 * _tmp963 - _tmp1555 * _tmp965; + const Scalar _tmp1557 = _tmp172 * (_tmp1521 * _tmp153 + _tmp1522 * _tmp157 + _tmp1523 * _tmp161); + const Scalar _tmp1558 = _tmp168 * (_tmp1519 * _tmp157 + _tmp170); + const Scalar _tmp1559 = _tmp1091 * _tmp1557 + _tmp1408 * _tmp1558; + const Scalar _tmp1560 = _tmp1095 * _tmp1559 - _tmp1097 * _tmp1559 + _tmp1557 * _tmp189; + const Scalar _tmp1561 = _tmp660 * (_tmp1521 * _tmp647 + _tmp1522 * _tmp648 + _tmp1523 * _tmp649); + const Scalar _tmp1562 = _tmp656 * (_tmp1519 * _tmp648 + _tmp658); + const Scalar _tmp1563 = _tmp1284 * _tmp1561 + _tmp1379 * _tmp1562; + const Scalar _tmp1564 = _tmp1309 * _tmp1563 - _tmp1310 * _tmp1563 + _tmp1562 * _tmp669; + const Scalar _tmp1565 = _tmp1550 * priors_1_0_sqrt_info(4, 4); + const Scalar _tmp1566 = -_tmp1210 * _tmp1559 + _tmp1211 * _tmp1559 + _tmp1558 * _tmp189; + const Scalar _tmp1567 = _tmp240 * (_tmp1521 * _tmp227 + _tmp1522 * _tmp229 + _tmp1523 * _tmp228); + const Scalar _tmp1568 = _tmp236 * (_tmp1519 * _tmp229 + _tmp238); + const Scalar _tmp1569 = _tmp1137 * _tmp1567 + _tmp1489 * _tmp1568; + const Scalar _tmp1570 = _tmp1248 * _tmp1569 - _tmp1249 * _tmp1569 + _tmp1567 * _tmp249; + const Scalar _tmp1571 = _tmp360 * (_tmp1521 * _tmp347 + _tmp1522 * _tmp348 + _tmp1523 * _tmp349); + const Scalar _tmp1572 = _tmp356 * (_tmp1519 * _tmp348 + _tmp358); + const Scalar _tmp1573 = _tmp1429 * _tmp1572 + _tmp1571 * _tmp972; + const Scalar _tmp1574 = -_tmp1477 * _tmp1573 + _tmp1572 * _tmp369 + _tmp1573 * _tmp979; + const Scalar _tmp1575 = _tmp686 * (_tmp1519 * _tmp678 + _tmp688); + const Scalar _tmp1576 = _tmp690 * (_tmp1521 * _tmp677 + _tmp1522 * _tmp678 + _tmp1523 * _tmp679); + const Scalar _tmp1577 = _tmp1062 * _tmp1576 + _tmp1328 * _tmp1575; + const Scalar _tmp1578 = -_tmp1214 * _tmp1577 + _tmp1215 * _tmp1577 + _tmp1576 * _tmp699; + const Scalar _tmp1579 = _tmp326 * (_tmp1519 * _tmp318 + _tmp328); + const Scalar _tmp1580 = _tmp330 * (_tmp1521 * _tmp317 + _tmp1522 * _tmp318 + _tmp1523 * _tmp319); + const Scalar _tmp1581 = _tmp1048 * _tmp1580 + _tmp1420 * _tmp1579; + const Scalar _tmp1582 = -_tmp1055 * _tmp1581 + _tmp1057 * _tmp1581 + _tmp1579 * _tmp339; + const Scalar _tmp1583 = + _tmp132 * _tmp1545 - _tmp132 * _tmp1546 + _tmp133 * _tmp1547 + _tmp1545 * _tmp915 - + _tmp1546 * _tmp915 + _tmp1549 * priors_1_0_sqrt_info(1, 3) + + _tmp1550 * priors_1_0_sqrt_info(1, 4) + _tmp1551 * priors_1_0_sqrt_info(1, 1); + const Scalar _tmp1584 = _tmp1141 * _tmp1569 - _tmp1143 * _tmp1569 + _tmp1568 * _tmp249; + const Scalar _tmp1585 = _tmp1008 * _tmp1536; + const Scalar _tmp1586 = _tmp1585 * _tmp56; + const Scalar _tmp1587 = _tmp1537 * _tmp39; + const Scalar _tmp1588 = _tmp1025 * _tmp1536; + const Scalar _tmp1589 = + _tmp1014 * (-_priors_0_1_target_T_src[2] * _tmp880 - _tmp1452 + _tmp996 + _tmp999); + const Scalar _tmp1590 = _tmp1538 * _tmp56; + const Scalar _tmp1591 = _tmp1236 * (_priors_0_1_target_T_src[3] * _tmp880 + _tmp1022 + _tmp1448) - + _tmp1237 * _tmp1538 + _tmp1237 * _tmp1585 + _tmp1451 * _tmp1585 + + _tmp1586 * priors_0_1_sqrt_info(0, 2) + + _tmp1587 * priors_0_1_sqrt_info(0, 2) - + _tmp1588 * priors_0_1_sqrt_info(0, 1) + _tmp1589 * _tmp40 - + _tmp1590 * priors_0_1_sqrt_info(0, 2); + const Scalar _tmp1592 = _tmp626 * (_tmp1519 * _tmp619 + _tmp628); + const Scalar _tmp1593 = _tmp630 * (_tmp1521 * _tmp617 + _tmp1522 * _tmp619 + _tmp1523 * _tmp618); + const Scalar _tmp1594 = _tmp1182 * _tmp1593 + _tmp1503 * _tmp1592; + const Scalar _tmp1595 = _tmp1275 * _tmp1594; + const Scalar _tmp1596 = -_tmp1184 * _tmp1595 + _tmp1188 * _tmp1595 + _tmp1593 * _tmp639; + const Scalar _tmp1597 = _tmp1239 * _tmp1529; + const Scalar _tmp1598 = _tmp1203 * _tmp1597 - _tmp1205 * _tmp1597 + _tmp1528 * _tmp429; + const Scalar _tmp1599 = + -_tmp1152 * _tmp1546 + _tmp1153 * _tmp1544 + _tmp1548 * priors_1_0_sqrt_info(2, 2) + + _tmp1549 * priors_1_0_sqrt_info(2, 3) + _tmp1550 * priors_1_0_sqrt_info(2, 4); + const Scalar _tmp1600 = _tmp540 * (_tmp1521 * _tmp527 + _tmp1522 * _tmp529 + _tmp1523 * _tmp528); + const Scalar _tmp1601 = _tmp536 * (_tmp1519 * _tmp529 + _tmp538); + const Scalar _tmp1602 = _tmp1497 * _tmp1601 + _tmp1600 * _tmp987; + const Scalar _tmp1603 = -_tmp1230 * _tmp1602 + _tmp1231 * _tmp1602 + _tmp1600 * _tmp549; + const Scalar _tmp1604 = _tmp570 * (_tmp1521 * _tmp557 + _tmp1522 * _tmp558 + _tmp1523 * _tmp559); + const Scalar _tmp1605 = _tmp566 * (_tmp1519 * _tmp558 + _tmp568); + const Scalar _tmp1606 = + (Scalar(1) / Scalar(2)) * _tmp1483 * _tmp1605 + (Scalar(1) / Scalar(2)) * _tmp1604 * _tmp929; + const Scalar _tmp1607 = _tmp1606 * _tmp571; + const Scalar _tmp1608 = _tmp177 * _tmp934; + const Scalar _tmp1609 = _tmp1604 * _tmp579 + _tmp1607 * _tmp1608 - _tmp1607 * _tmp931; + const Scalar _tmp1610 = _tmp506 * (_tmp1519 * _tmp498 + _tmp508); + const Scalar _tmp1611 = _tmp510 * (_tmp1521 * _tmp497 + _tmp1522 * _tmp498 + _tmp1523 * _tmp499); + const Scalar _tmp1612 = _tmp1334 * _tmp1610 + _tmp1611 * _tmp859; + const Scalar _tmp1613 = _tmp1610 * _tmp519 - _tmp1612 * _tmp863 + _tmp1612 * _tmp865; + const Scalar _tmp1614 = _tmp1312 * _tmp1555; + const Scalar _tmp1615 = _tmp1554 * _tmp489 + _tmp1614 * _tmp962 - _tmp1614 * _tmp964; + const Scalar _tmp1616 = _tmp1252 * _tmp1581 - _tmp1253 * _tmp1581 + _tmp1580 * _tmp339; + const Scalar _tmp1617 = _tmp390 * (_tmp1521 * _tmp377 + _tmp1522 * _tmp378 + _tmp1523 * _tmp379); + const Scalar _tmp1618 = _tmp386 * (_tmp1519 * _tmp378 + _tmp388); + const Scalar _tmp1619 = _tmp1104 * _tmp1617 + _tmp1386 * _tmp1618; + const Scalar _tmp1620 = _tmp1109 * _tmp1619; + const Scalar _tmp1621 = _tmp1108 * _tmp1620 - _tmp1112 * _tmp1619 + _tmp1617 * _tmp399; + const Scalar _tmp1622 = _tmp296 * (_tmp1519 * _tmp289 + _tmp298); + const Scalar _tmp1623 = _tmp300 * (_tmp1521 * _tmp287 + _tmp1522 * _tmp289 + _tmp1523 * _tmp288); + const Scalar _tmp1624 = _tmp1167 * _tmp1623 + _tmp1369 * _tmp1622; + const Scalar _tmp1625 = _tmp1258 * _tmp1624 - _tmp1259 * _tmp1624 + _tmp1622 * _tmp309; + const Scalar _tmp1626 = _tmp1611 * _tmp519 - _tmp1612 * _tmp920 + _tmp1612 * _tmp921; + const Scalar _tmp1627 = _tmp1068 * _tmp1577 - _tmp1070 * _tmp1577 + _tmp1575 * _tmp699; + const Scalar _tmp1628 = _tmp270 * (_tmp1521 * _tmp257 + _tmp1522 * _tmp258 + _tmp1523 * _tmp259); + const Scalar _tmp1629 = _tmp266 * (_tmp1519 * _tmp258 + _tmp268); + const Scalar _tmp1630 = _tmp1034 * _tmp1628 + _tmp1364 * _tmp1629; + const Scalar _tmp1631 = -_tmp1244 * _tmp1630 + _tmp1245 * _tmp1630 + _tmp1628 * _tmp279; + const Scalar _tmp1632 = _tmp1223 * _tmp1525 - _tmp1224 * _tmp1525 + _tmp1520 * _tmp609; + const Scalar _tmp1633 = _tmp206 * (_tmp1519 * _tmp198 + _tmp208); + const Scalar _tmp1634 = _tmp210 * (_tmp1521 * _tmp197 + _tmp1522 * _tmp198 + _tmp1523 * _tmp199); + const Scalar _tmp1635 = _tmp1354 * _tmp1633 + _tmp1634 * _tmp944; + const Scalar _tmp1636 = _tmp1634 * _tmp219 - _tmp1635 * _tmp948 + _tmp1635 * _tmp950; + const Scalar _tmp1637 = _tmp1606 * _tmp567; + const Scalar _tmp1638 = _tmp1605 * _tmp579 + _tmp1608 * _tmp1637 - _tmp1637 * _tmp931; + const Scalar _tmp1639 = _tmp1156 * _tmp1542 - _tmp1157 * _tmp1542 + _tmp1540 * _tmp729; + const Scalar _tmp1640 = _tmp1038 * _tmp1630 - _tmp1041 * _tmp1630 + _tmp1629 * _tmp279; + const Scalar _tmp1641 = _tmp1299 * _tmp1635 - _tmp1301 * _tmp1635 + _tmp1633 * _tmp219; + const Scalar _tmp1642 = + _tmp1549 * priors_1_0_sqrt_info(3, 3) + _tmp1550 * priors_1_0_sqrt_info(3, 4); + const Scalar _tmp1643 = -_tmp1172 * _tmp1624 + _tmp1174 * _tmp1624 + _tmp1623 * _tmp309; + const Scalar _tmp1644 = _tmp1601 * _tmp549 - _tmp1602 * _tmp991 + _tmp1602 * _tmp994; + const Scalar _tmp1645 = _tmp1079 * _tmp1534 - _tmp1081 * _tmp1533 + _tmp1531 * _tmp759; + const Scalar _tmp1646 = -_tmp1186 * _tmp1594 + _tmp1189 * _tmp1594 + _tmp1592 * _tmp639; + const Scalar _tmp1647 = _tmp1226 * _tmp1573; + const Scalar _tmp1648 = _tmp1571 * _tmp369 - _tmp1647 * _tmp975 + _tmp1647 * _tmp978; + const Scalar _tmp1649 = _tmp1191 * _tmp1620 - _tmp1193 * _tmp1619 + _tmp1618 * _tmp399; + const Scalar _tmp1650 = _tmp450 * (_tmp1521 * _tmp437 + _tmp1522 * _tmp438 + _tmp1523 * _tmp439); + const Scalar _tmp1651 = _tmp446 * (_tmp1519 * _tmp438 + _tmp448); + const Scalar _tmp1652 = _tmp1267 * _tmp1650 + _tmp1359 * _tmp1651; + const Scalar _tmp1653 = _tmp1269 * _tmp1652; + const Scalar _tmp1654 = _tmp1270 * _tmp1653 - _tmp1272 * _tmp1653 + _tmp1650 * _tmp459; + const Scalar _tmp1655 = _tmp1289 * _tmp1563 - _tmp1291 * _tmp1563 + _tmp1561 * _tmp669; + const Scalar _tmp1656 = _tmp1295 * _tmp1652 - _tmp1296 * _tmp1652 + _tmp1651 * _tmp459; + const Scalar _tmp1657 = _tmp1510 * _tmp1585 + _tmp1586 * priors_0_1_sqrt_info(1, 2) + + _tmp1587 * priors_0_1_sqrt_info(1, 2) - + _tmp1588 * priors_0_1_sqrt_info(1, 1) + _tmp1589 * _tmp64 - + _tmp1590 * priors_0_1_sqrt_info(1, 2); + const Scalar _tmp1658 = _tmp240 * landmarks_2_; + const Scalar _tmp1659 = _tmp1135 * landmarks_2_; + const Scalar _tmp1660 = _tmp107 * _tmp1659 - _tmp1658 * _tmp99; + const Scalar _tmp1661 = _tmp236 * landmarks_2_; + const Scalar _tmp1662 = _tmp1133 * landmarks_2_; + const Scalar _tmp1663 = _tmp107 * _tmp1662 - _tmp1661 * _tmp76; + const Scalar _tmp1664 = _tmp1137 * _tmp1660 + _tmp1489 * _tmp1663; + const Scalar _tmp1665 = _tmp1142 * _tmp1664; + const Scalar _tmp1666 = -_tmp1247 * _tmp1665 + _tmp1248 * _tmp1664 + _tmp1660 * _tmp249; + const Scalar _tmp1667 = _tmp746 * landmarks_19_; + const Scalar _tmp1668 = _tmp107 * landmarks_19_; + const Scalar _tmp1669 = _tmp1437 * _tmp1668 - _tmp1667 * _tmp76; + const Scalar _tmp1670 = _tmp750 * landmarks_19_; + const Scalar _tmp1671 = _tmp1445 * _tmp1668 - _tmp1670 * _tmp99; + const Scalar _tmp1672 = _tmp1076 * _tmp1671 + _tmp1439 * _tmp1669; + const Scalar _tmp1673 = _tmp1082 * _tmp1672; + const Scalar _tmp1674 = _tmp1159 * _tmp1673 - _tmp1160 * _tmp1672 + _tmp1669 * _tmp759; + const Scalar _tmp1675 = _tmp927 * landmarks_13_; + const Scalar _tmp1676 = _tmp570 * landmarks_13_; + const Scalar _tmp1677 = _tmp107 * _tmp1675 - _tmp1676 * _tmp99; + const Scalar _tmp1678 = _tmp566 * landmarks_13_; + const Scalar _tmp1679 = _tmp1219 * landmarks_13_; + const Scalar _tmp1680 = _tmp107 * _tmp1679 - _tmp1678 * _tmp76; + const Scalar _tmp1681 = _tmp1483 * _tmp1680 + _tmp1677 * _tmp929; + const Scalar _tmp1682 = -_tmp1485 * _tmp1681 + _tmp1677 * _tmp579 + _tmp1681 * _tmp936; + const Scalar _tmp1683 = _tmp716 * landmarks_18_; + const Scalar _tmp1684 = _tmp1471 * landmarks_18_; + const Scalar _tmp1685 = _tmp107 * _tmp1684 - _tmp1683 * _tmp76; + const Scalar _tmp1686 = _tmp720 * landmarks_18_; + const Scalar _tmp1687 = _tmp1118 * landmarks_18_; + const Scalar _tmp1688 = _tmp107 * _tmp1687 - _tmp1686 * _tmp99; + const Scalar _tmp1689 = _tmp1120 * _tmp1688 + _tmp1473 * _tmp1685; + const Scalar _tmp1690 = _tmp1156 * _tmp1689 - _tmp1157 * _tmp1689 + _tmp1685 * _tmp729; + const Scalar _tmp1691 = _tmp1079 * _tmp1673 - _tmp1081 * _tmp1672 + _tmp1671 * _tmp759; + const Scalar _tmp1692 = _tmp992 * landmarks_12_; + const Scalar _tmp1693 = _tmp536 * landmarks_12_; + const Scalar _tmp1694 = _tmp107 * _tmp1692 - _tmp1693 * _tmp76; + const Scalar _tmp1695 = _tmp540 * landmarks_12_; + const Scalar _tmp1696 = _tmp985 * landmarks_12_; + const Scalar _tmp1697 = _tmp107 * _tmp1696 - _tmp1695 * _tmp99; + const Scalar _tmp1698 = _tmp1497 * _tmp1694 + _tmp1697 * _tmp987; + const Scalar _tmp1699 = -_tmp1230 * _tmp1698 + _tmp1231 * _tmp1698 + _tmp1697 * _tmp549; + const Scalar _tmp1700 = _tmp506 * landmarks_11_; + const Scalar _tmp1701 = _tmp107 * landmarks_11_; + const Scalar _tmp1702 = -_tmp1700 * _tmp76 + _tmp1701 * _tmp867; + const Scalar _tmp1703 = _tmp510 * landmarks_11_; + const Scalar _tmp1704 = _tmp1701 * _tmp857 - _tmp1703 * _tmp99; + const Scalar _tmp1705 = _tmp1334 * _tmp1702 + _tmp1704 * _tmp859; + const Scalar _tmp1706 = _tmp1704 * _tmp519 - _tmp1705 * _tmp920 + _tmp1705 * _tmp921; + const Scalar _tmp1707 = _tmp1694 * _tmp549 - _tmp1698 * _tmp991 + _tmp1698 * _tmp994; + const Scalar _tmp1708 = _tmp1433 * landmarks_6_; + const Scalar _tmp1709 = _tmp360 * landmarks_6_; + const Scalar _tmp1710 = _tmp107 * _tmp1708 - _tmp1709 * _tmp99; + const Scalar _tmp1711 = _tmp356 * landmarks_6_; + const Scalar _tmp1712 = _tmp1427 * landmarks_6_; + const Scalar _tmp1713 = _tmp107 * _tmp1712 - _tmp1711 * _tmp76; + const Scalar _tmp1714 = _tmp1429 * _tmp1713 + _tmp1710 * _tmp972; + const Scalar _tmp1715 = _tmp1227 * _tmp1714 - _tmp1432 * _tmp1714 + _tmp1710 * _tmp369; + const Scalar _tmp1716 = -_tmp1477 * _tmp1714 + _tmp1713 * _tmp369 + _tmp1714 * _tmp979; + const Scalar _tmp1717 = _tmp1418 * landmarks_5_; + const Scalar _tmp1718 = _tmp326 * landmarks_5_; + const Scalar _tmp1719 = _tmp107 * _tmp1717 - _tmp1718 * _tmp76; + const Scalar _tmp1720 = _tmp330 * landmarks_5_; + const Scalar _tmp1721 = _tmp1046 * landmarks_5_; + const Scalar _tmp1722 = _tmp107 * _tmp1721 - _tmp1720 * _tmp99; + const Scalar _tmp1723 = _tmp1048 * _tmp1722 + _tmp1420 * _tmp1719; + const Scalar _tmp1724 = _tmp1053 * _tmp1723; + const Scalar _tmp1725 = -_tmp1054 * _tmp1724 + _tmp1056 * _tmp1724 + _tmp1719 * _tmp339; + const Scalar _tmp1726 = _tmp1208 * landmarks_0_; + const Scalar _tmp1727 = _tmp168 * landmarks_0_; + const Scalar _tmp1728 = _tmp107 * _tmp1726 - _tmp1727 * _tmp76; + const Scalar _tmp1729 = _tmp1088 * landmarks_0_; + const Scalar _tmp1730 = _tmp172 * landmarks_0_; + const Scalar _tmp1731 = _tmp107 * _tmp1729 - _tmp1730 * _tmp99; + const Scalar _tmp1732 = _tmp1091 * _tmp1731 + _tmp1408 * _tmp1728; + const Scalar _tmp1733 = _tmp1096 * _tmp1732; + const Scalar _tmp1734 = -_tmp1209 * _tmp1733 + _tmp1211 * _tmp1732 + _tmp1728 * _tmp189; + const Scalar _tmp1735 = _tmp296 * landmarks_4_; + const Scalar _tmp1736 = _tmp1256 * landmarks_4_; + const Scalar _tmp1737 = _tmp107 * _tmp1736 - _tmp1735 * _tmp76; + const Scalar _tmp1738 = _tmp1165 * landmarks_4_; + const Scalar _tmp1739 = _tmp300 * landmarks_4_; + const Scalar _tmp1740 = _tmp107 * _tmp1738 - _tmp1739 * _tmp99; + const Scalar _tmp1741 = _tmp1167 * _tmp1740 + _tmp1369 * _tmp1737; + const Scalar _tmp1742 = -_tmp1172 * _tmp1741 + _tmp1174 * _tmp1741 + _tmp1740 * _tmp309; + const Scalar _tmp1743 = _tmp107 * landmarks_15_; + const Scalar _tmp1744 = _tmp630 * landmarks_15_; + const Scalar _tmp1745 = _tmp1180 * _tmp1743 - _tmp1744 * _tmp99; + const Scalar _tmp1746 = _tmp626 * landmarks_15_; + const Scalar _tmp1747 = _tmp1187 * _tmp1743 - _tmp1746 * _tmp76; + const Scalar _tmp1748 = _tmp1182 * _tmp1745 + _tmp1503 * _tmp1747; + const Scalar _tmp1749 = _tmp1276 * _tmp1748 - _tmp1277 * _tmp1748 + _tmp1745 * _tmp639; + const Scalar _tmp1750 = _tmp3 * priors_0_1_sqrt_info(2, 4) + _tmp51 * priors_0_1_sqrt_info(2, 5) + _tmp60 * priors_0_1_sqrt_info(2, 3); - const Scalar _tmp1730 = _tmp1047 * landmarks_12_; - const Scalar _tmp1731 = _tmp540 * landmarks_12_; - const Scalar _tmp1732 = _tmp108 * _tmp1730 - _tmp1731 * _tmp77; - const Scalar _tmp1733 = _tmp544 * landmarks_12_; - const Scalar _tmp1734 = _tmp1040 * landmarks_12_; - const Scalar _tmp1735 = -_tmp100 * _tmp1733 + _tmp108 * _tmp1734; - const Scalar _tmp1736 = _tmp1042 * _tmp1735 + _tmp1522 * _tmp1732; - const Scalar _tmp1737 = -_tmp1259 * _tmp1736 + _tmp1260 * _tmp1736 + _tmp1735 * _tmp553; - const Scalar _tmp1738 = _tmp510 * landmarks_11_; - const Scalar _tmp1739 = _tmp926 * landmarks_11_; - const Scalar _tmp1740 = _tmp108 * _tmp1739 - _tmp1738 * _tmp77; - const Scalar _tmp1741 = _tmp514 * landmarks_11_; - const Scalar _tmp1742 = _tmp914 * landmarks_11_; - const Scalar _tmp1743 = -_tmp100 * _tmp1741 + _tmp108 * _tmp1742; - const Scalar _tmp1744 = - (Scalar(1) / Scalar(2)) * _tmp1352 * _tmp1740 + (Scalar(1) / Scalar(2)) * _tmp1743 * _tmp916; - const Scalar _tmp1745 = _tmp1744 * _tmp515; - const Scalar _tmp1746 = _tmp1743 * _tmp523 - _tmp1745 * _tmp918 + _tmp1745 * _tmp921; - const Scalar _tmp1747 = -_tmp1046 * _tmp1736 + _tmp1049 * _tmp1736 + _tmp1732 * _tmp553; - const Scalar _tmp1748 = _tmp4 * priors_0_1_sqrt_info(4, 4) + _tmp51 * priors_0_1_sqrt_info(4, 5) + - _tmp60 * priors_0_1_sqrt_info(4, 3); - const Scalar _tmp1749 = _tmp1024 * landmarks_6_; - const Scalar _tmp1750 = _tmp364 * landmarks_6_; - const Scalar _tmp1751 = -_tmp100 * _tmp1750 + _tmp108 * _tmp1749; - const Scalar _tmp1752 = _tmp360 * landmarks_6_; - const Scalar _tmp1753 = _tmp1032 * landmarks_6_; - const Scalar _tmp1754 = _tmp108 * _tmp1753 - _tmp1752 * _tmp77; - const Scalar _tmp1755 = _tmp1026 * _tmp1751 + _tmp1466 * _tmp1754; - const Scalar _tmp1756 = _tmp1256 * _tmp1755 - _tmp1468 * _tmp1755 + _tmp1751 * _tmp373; - const Scalar _tmp1757 = _tmp1034 * _tmp1755 - _tmp1497 * _tmp1755 + _tmp1754 * _tmp373; - const Scalar _tmp1758 = _tmp1456 * landmarks_5_; - const Scalar _tmp1759 = _tmp330 * landmarks_5_; - const Scalar _tmp1760 = _tmp108 * _tmp1758 - _tmp1759 * _tmp77; - const Scalar _tmp1761 = _tmp334 * landmarks_5_; - const Scalar _tmp1762 = _tmp1537 * landmarks_5_; - const Scalar _tmp1763 = -_tmp100 * _tmp1761 + _tmp108 * _tmp1762; - const Scalar _tmp1764 = _tmp1071 * _tmp1763 + _tmp1458 * _tmp1760; - const Scalar _tmp1765 = -_tmp1076 * _tmp1764 + _tmp1460 * _tmp1764 + _tmp1760 * _tmp343; - const Scalar _tmp1766 = _tmp1236 * landmarks_0_; - const Scalar _tmp1767 = _tmp172 * landmarks_0_; - const Scalar _tmp1768 = _tmp108 * _tmp1766 - _tmp1767 * _tmp77; - const Scalar _tmp1769 = _tmp1112 * landmarks_0_; - const Scalar _tmp1770 = _tmp176 * landmarks_0_; - const Scalar _tmp1771 = -_tmp100 * _tmp1770 + _tmp108 * _tmp1769; - const Scalar _tmp1772 = _tmp1115 * _tmp1771 + _tmp1444 * _tmp1768; - const Scalar _tmp1773 = -_tmp1238 * _tmp1772 + _tmp1239 * _tmp1772 + _tmp1768 * _tmp193; - const Scalar _tmp1774 = _tmp300 * landmarks_4_; - const Scalar _tmp1775 = _tmp1187 * landmarks_4_; - const Scalar _tmp1776 = _tmp1281 * _tmp1775; - const Scalar _tmp1777 = _tmp108 * _tmp1776 - _tmp1774 * _tmp77; - const Scalar _tmp1778 = _tmp1189 * _tmp1775; - const Scalar _tmp1779 = _tmp304 * landmarks_4_; - const Scalar _tmp1780 = -_tmp100 * _tmp1779 + _tmp108 * _tmp1778; - const Scalar _tmp1781 = _tmp1191 * _tmp1780 + _tmp1417 * _tmp1777; - const Scalar _tmp1782 = -_tmp1195 * _tmp1781 + _tmp1197 * _tmp1781 + _tmp1780 * _tmp313; - const Scalar _tmp1783 = _tmp108 * landmarks_15_; - const Scalar _tmp1784 = _tmp634 * landmarks_15_; - const Scalar _tmp1785 = -_tmp100 * _tmp1784 + _tmp1202 * _tmp1783; - const Scalar _tmp1786 = _tmp630 * landmarks_15_; - const Scalar _tmp1787 = _tmp1210 * _tmp1783 - _tmp1786 * _tmp77; - const Scalar _tmp1788 = _tmp1204 * _tmp1785 + _tmp1531 * _tmp1787; - const Scalar _tmp1789 = _tmp1301 * _tmp1788 - _tmp1302 * _tmp1788 + _tmp1785 * _tmp643; - const Scalar _tmp1790 = _tmp72 + _tmp75 - 1; - const Scalar _tmp1791 = _tmp1344 * priors_1_0_sqrt_info(1, 5) + - _tmp1790 * priors_1_0_sqrt_info(1, 3) + - _tmp890 * priors_1_0_sqrt_info(1, 4); - const Scalar _tmp1792 = _tmp4 * priors_0_1_sqrt_info(1, 4) + _tmp51 * priors_0_1_sqrt_info(1, 5) + + const Scalar _tmp1751 = _tmp386 * landmarks_7_; + const Scalar _tmp1752 = _tmp1384 * landmarks_7_; + const Scalar _tmp1753 = _tmp107 * _tmp1752 - _tmp1751 * _tmp76; + const Scalar _tmp1754 = _tmp1102 * landmarks_7_; + const Scalar _tmp1755 = _tmp390 * landmarks_7_; + const Scalar _tmp1756 = _tmp107 * _tmp1754 - _tmp1755 * _tmp99; + const Scalar _tmp1757 = _tmp1104 * _tmp1756 + _tmp1386 * _tmp1753; + const Scalar _tmp1758 = _tmp1192 * _tmp1757 - _tmp1193 * _tmp1757 + _tmp1753 * _tmp399; + const Scalar _tmp1759 = _tmp71 + _tmp74 - 1; + const Scalar _tmp1760 = _tmp1325 * priors_1_0_sqrt_info(3, 5) + + _tmp1759 * priors_1_0_sqrt_info(3, 3) + + _tmp829 * priors_1_0_sqrt_info(3, 4); + const Scalar _tmp1761 = _tmp1252 * _tmp1723 - _tmp1253 * _tmp1723 + _tmp1722 * _tmp339; + const Scalar _tmp1762 = _tmp1325 * priors_1_0_sqrt_info(0, 5) + + _tmp1759 * priors_1_0_sqrt_info(0, 3) + + _tmp829 * priors_1_0_sqrt_info(0, 4); + const Scalar _tmp1763 = -_tmp1139 * _tmp1665 + _tmp1141 * _tmp1664 + _tmp1663 * _tmp249; + const Scalar _tmp1764 = -_tmp1093 * _tmp1733 + _tmp1095 * _tmp1732 + _tmp1731 * _tmp189; + const Scalar _tmp1765 = _tmp1340 * landmarks_8_; + const Scalar _tmp1766 = _tmp107 * _tmp836; + const Scalar _tmp1767 = _tmp420 * landmarks_8_; + const Scalar _tmp1768 = _tmp1765 * _tmp1766 - _tmp1767 * _tmp99; + const Scalar _tmp1769 = _tmp1343 * landmarks_8_; + const Scalar _tmp1770 = _tmp416 * landmarks_8_; + const Scalar _tmp1771 = _tmp107 * _tmp1769 - _tmp1770 * _tmp76; + const Scalar _tmp1772 = _tmp1200 * _tmp1768 + _tmp1345 * _tmp1771; + const Scalar _tmp1773 = -_tmp1240 * _tmp1772 + _tmp1241 * _tmp1772 + _tmp1771 * _tmp429; + const Scalar _tmp1774 = _tmp3 * priors_0_1_sqrt_info(1, 4) + _tmp51 * priors_0_1_sqrt_info(1, 5) + _tmp60 * priors_0_1_sqrt_info(1, 3); - const Scalar _tmp1793 = _tmp390 * landmarks_7_; - const Scalar _tmp1794 = _tmp1430 * landmarks_7_; - const Scalar _tmp1795 = _tmp108 * _tmp1794 - _tmp1793 * _tmp77; - const Scalar _tmp1796 = _tmp1126 * landmarks_7_; - const Scalar _tmp1797 = _tmp394 * landmarks_7_; - const Scalar _tmp1798 = -_tmp100 * _tmp1797 + _tmp108 * _tmp1796; - const Scalar _tmp1799 = _tmp1128 * _tmp1798 + _tmp1432 * _tmp1795; - const Scalar _tmp1800 = _tmp1215 * _tmp1799 - _tmp1216 * _tmp1799 + _tmp1795 * _tmp403; - const Scalar _tmp1801 = -_tmp1279 * _tmp1764 + _tmp1538 * _tmp1764 + _tmp1763 * _tmp343; - const Scalar _tmp1802 = _tmp1344 * priors_1_0_sqrt_info(0, 5) + - _tmp1790 * priors_1_0_sqrt_info(0, 3) + - _tmp890 * priors_1_0_sqrt_info(0, 4); - const Scalar _tmp1803 = -_tmp1168 * _tmp1702 + _tmp1170 * _tmp1701 + _tmp1700 * _tmp253; - const Scalar _tmp1804 = _tmp1117 * _tmp1772; - const Scalar _tmp1805 = _tmp1118 * _tmp1804 - _tmp1120 * _tmp1804 + _tmp1771 * _tmp193; - const Scalar _tmp1806 = _tmp1223 * landmarks_8_; - const Scalar _tmp1807 = _tmp424 * landmarks_8_; - const Scalar _tmp1808 = -_tmp100 * _tmp1807 + _tmp108 * _tmp1806; - const Scalar _tmp1809 = _tmp1358 * landmarks_8_; - const Scalar _tmp1810 = _tmp420 * landmarks_8_; - const Scalar _tmp1811 = _tmp108 * _tmp1809 - _tmp1810 * _tmp77; - const Scalar _tmp1812 = _tmp1225 * _tmp1808 + _tmp1360 * _tmp1811; - const Scalar _tmp1813 = _tmp1267 * _tmp1812; - const Scalar _tmp1814 = _tmp1228 * _tmp1813 - _tmp1230 * _tmp1813 + _tmp1811 * _tmp433; - const Scalar _tmp1815 = _tmp1344 * priors_1_0_sqrt_info(4, 5) + - _tmp1790 * priors_1_0_sqrt_info(4, 3) + - _tmp890 * priors_1_0_sqrt_info(4, 4); - const Scalar _tmp1816 = _tmp1250 * _tmp896; - const Scalar _tmp1817 = _tmp108 * landmarks_14_; - const Scalar _tmp1818 = _tmp600 * landmarks_14_; - const Scalar _tmp1819 = _tmp1816 * _tmp1817 - _tmp1818 * _tmp77; - const Scalar _tmp1820 = _tmp604 * landmarks_14_; - const Scalar _tmp1821 = -_tmp100 * _tmp1820 + _tmp1462 * _tmp1817; - const Scalar _tmp1822 = _tmp1450 * _tmp1819 + _tmp1821 * _tmp901; - const Scalar _tmp1823 = _tmp1251 * _tmp1822; - const Scalar _tmp1824 = _tmp1819 * _tmp613 - _tmp1823 * _tmp903 + _tmp1823 * _tmp907; - const Scalar _tmp1825 = _tmp214 * landmarks_1_; - const Scalar _tmp1826 = _tmp108 * landmarks_1_; - const Scalar _tmp1827 = -_tmp100 * _tmp1825 + _tmp1826 * _tmp993; - const Scalar _tmp1828 = _tmp210 * landmarks_1_; - const Scalar _tmp1829 = _tmp1323 * _tmp1826 - _tmp1828 * _tmp77; - const Scalar _tmp1830 = _tmp1402 * _tmp1829 + _tmp1827 * _tmp995; - const Scalar _tmp1831 = _tmp1001 * _tmp1830 + _tmp1827 * _tmp223 - _tmp1830 * _tmp999; - const Scalar _tmp1832 = _tmp1229 * _tmp1812 - _tmp1231 * _tmp1812 + _tmp1808 * _tmp433; - const Scalar _tmp1833 = _tmp1344 * priors_1_0_sqrt_info(3, 5) + - _tmp1790 * priors_1_0_sqrt_info(3, 3) + - _tmp890 * priors_1_0_sqrt_info(3, 4); - const Scalar _tmp1834 = _tmp1744 * _tmp511; - const Scalar _tmp1835 = _tmp1740 * _tmp523 - _tmp1834 * _tmp918 + _tmp1834 * _tmp921; - const Scalar _tmp1836 = _tmp4 * priors_0_1_sqrt_info(0, 4) + _tmp51 * priors_0_1_sqrt_info(0, 5) + + const Scalar _tmp1775 = _tmp1222 * landmarks_14_; + const Scalar _tmp1776 = _tmp596 * landmarks_14_; + const Scalar _tmp1777 = _tmp107 * _tmp1775 - _tmp1776 * _tmp76; + const Scalar _tmp1778 = _tmp839 * landmarks_14_; + const Scalar _tmp1779 = _tmp600 * landmarks_14_; + const Scalar _tmp1780 = _tmp107 * _tmp1778 - _tmp1779 * _tmp99; + const Scalar _tmp1781 = + (Scalar(1) / Scalar(2)) * _tmp1413 * _tmp1777 + (Scalar(1) / Scalar(2)) * _tmp1780 * _tmp842; + const Scalar _tmp1782 = _tmp1781 * _tmp597; + const Scalar _tmp1783 = _tmp177 * _tmp849; + const Scalar _tmp1784 = _tmp1777 * _tmp609 + _tmp1782 * _tmp1783 - _tmp1782 * _tmp844; + const Scalar _tmp1785 = _tmp210 * landmarks_1_; + const Scalar _tmp1786 = _tmp942 * landmarks_1_; + const Scalar _tmp1787 = _tmp107 * _tmp1786 - _tmp1785 * _tmp99; + const Scalar _tmp1788 = _tmp1300 * landmarks_1_; + const Scalar _tmp1789 = _tmp206 * landmarks_1_; + const Scalar _tmp1790 = _tmp107 * _tmp1788 - _tmp1789 * _tmp76; + const Scalar _tmp1791 = _tmp1354 * _tmp1790 + _tmp1787 * _tmp944; + const Scalar _tmp1792 = _tmp1787 * _tmp219 - _tmp1791 * _tmp948 + _tmp1791 * _tmp950; + const Scalar _tmp1793 = _tmp1204 * _tmp1772 - _tmp1206 * _tmp1772 + _tmp1768 * _tmp429; + const Scalar _tmp1794 = _tmp1325 * priors_1_0_sqrt_info(1, 5) + + _tmp1759 * priors_1_0_sqrt_info(1, 3) + + _tmp829 * priors_1_0_sqrt_info(1, 4); + const Scalar _tmp1795 = _tmp1702 * _tmp519 - _tmp1705 * _tmp863 + _tmp1705 * _tmp865; + const Scalar _tmp1796 = _tmp3 * priors_0_1_sqrt_info(0, 4) + _tmp51 * priors_0_1_sqrt_info(0, 5) + _tmp60 * priors_0_1_sqrt_info(0, 3); - const Scalar _tmp1837 = _tmp1322 * _tmp1830 - _tmp1324 * _tmp1830 + _tmp1829 * _tmp223; - const Scalar _tmp1838 = _tmp1283 * _tmp1781 - _tmp1284 * _tmp1781 + _tmp1777 * _tmp313; - const Scalar _tmp1839 = _tmp1134 * _tmp1799 - _tmp1136 * _tmp1799 + _tmp1798 * _tmp403; - const Scalar _tmp1840 = _tmp480 * landmarks_10_; - const Scalar _tmp1841 = _tmp1332 * landmarks_10_; - const Scalar _tmp1842 = _tmp108 * _tmp1841 - _tmp1840 * _tmp77; - const Scalar _tmp1843 = _tmp1007 * landmarks_10_; - const Scalar _tmp1844 = _tmp484 * landmarks_10_; - const Scalar _tmp1845 = -_tmp100 * _tmp1844 + _tmp108 * _tmp1843; - const Scalar _tmp1846 = - (Scalar(1) / Scalar(2)) * _tmp1009 * _tmp1845 + (Scalar(1) / Scalar(2)) * _tmp1438 * _tmp1842; - const Scalar _tmp1847 = _tmp1846 * _tmp481; - const Scalar _tmp1848 = _tmp1012 * _tmp181; - const Scalar _tmp1849 = -_tmp1015 * _tmp1847 + _tmp1842 * _tmp493 + _tmp1847 * _tmp1848; - const Scalar _tmp1850 = _tmp1304 * _tmp1306; - const Scalar _tmp1851 = _tmp1850 * landmarks_16_; - const Scalar _tmp1852 = _tmp664 * landmarks_16_; - const Scalar _tmp1853 = -_tmp100 * _tmp1852 + _tmp108 * _tmp1851; - const Scalar _tmp1854 = _tmp660 * landmarks_16_; - const Scalar _tmp1855 = _tmp1304 * _tmp1327 * landmarks_16_; - const Scalar _tmp1856 = _tmp108 * _tmp1855 - _tmp1854 * _tmp77; - const Scalar _tmp1857 = _tmp1308 * _tmp1853 + _tmp1425 * _tmp1856; - const Scalar _tmp1858 = _tmp1312 * _tmp1857 - _tmp1314 * _tmp1857 + _tmp1853 * _tmp673; - const Scalar _tmp1859 = _tmp1846 * _tmp485; - const Scalar _tmp1860 = -_tmp1015 * _tmp1859 + _tmp1845 * _tmp493 + _tmp1848 * _tmp1859; - const Scalar _tmp1861 = _tmp694 * landmarks_17_; - const Scalar _tmp1862 = _tmp1084 * landmarks_17_; - const Scalar _tmp1863 = -_tmp100 * _tmp1861 + _tmp108 * _tmp1862; - const Scalar _tmp1864 = _tmp1345 * landmarks_17_; - const Scalar _tmp1865 = _tmp690 * landmarks_17_; - const Scalar _tmp1866 = _tmp108 * _tmp1864 - _tmp1865 * _tmp77; - const Scalar _tmp1867 = _tmp1086 * _tmp1863 + _tmp1347 * _tmp1866; - const Scalar _tmp1868 = _tmp1093 * _tmp1867; - const Scalar _tmp1869 = -_tmp1241 * _tmp1868 + _tmp1243 * _tmp1867 + _tmp1863 * _tmp703; - const Scalar _tmp1870 = -_tmp1153 * _tmp1726 + _tmp1156 * _tmp1726 + _tmp1725 * _tmp733; - const Scalar _tmp1871 = _tmp270 * landmarks_3_; - const Scalar _tmp1872 = _tmp108 * landmarks_3_; - const Scalar _tmp1873 = _tmp1062 * _tmp1872 - _tmp1871 * _tmp77; - const Scalar _tmp1874 = _tmp274 * landmarks_3_; - const Scalar _tmp1875 = -_tmp100 * _tmp1874 + _tmp1055 * _tmp1872; - const Scalar _tmp1876 = _tmp1057 * _tmp1875 + _tmp1412 * _tmp1873; - const Scalar _tmp1877 = _tmp1061 * _tmp1876 - _tmp1065 * _tmp1876 + _tmp1873 * _tmp283; - const Scalar _tmp1878 = _tmp1821 * _tmp613 - _tmp1822 * _tmp905 + _tmp1822 * _tmp908; - const Scalar _tmp1879 = -_tmp1208 * _tmp1788 + _tmp1212 * _tmp1788 + _tmp1787 * _tmp643; - const Scalar _tmp1880 = _tmp454 * landmarks_9_; - const Scalar _tmp1881 = _tmp1290 * landmarks_9_; - const Scalar _tmp1882 = -_tmp100 * _tmp1880 + _tmp108 * _tmp1881; - const Scalar _tmp1883 = _tmp1316 * landmarks_9_; - const Scalar _tmp1884 = _tmp450 * landmarks_9_; - const Scalar _tmp1885 = _tmp108 * _tmp1883 - _tmp1884 * _tmp77; - const Scalar _tmp1886 = _tmp1292 * _tmp1882 + _tmp1407 * _tmp1885; - const Scalar _tmp1887 = _tmp1296 * _tmp1886 - _tmp1298 * _tmp1886 + _tmp1882 * _tmp463; - const Scalar _tmp1888 = _tmp4 * priors_0_1_sqrt_info(3, 4) + _tmp51 * priors_0_1_sqrt_info(3, 5) + + const Scalar _tmp1797 = _tmp3 * priors_0_1_sqrt_info(4, 4) + _tmp51 * priors_0_1_sqrt_info(4, 5); + const Scalar _tmp1798 = _tmp1299 * _tmp1791 - _tmp1301 * _tmp1791 + _tmp1790 * _tmp219; + const Scalar _tmp1799 = _tmp1258 * _tmp1741 - _tmp1259 * _tmp1741 + _tmp1737 * _tmp309; + const Scalar _tmp1800 = _tmp1110 * _tmp1757 - _tmp1112 * _tmp1757 + _tmp1756 * _tmp399; + const Scalar _tmp1801 = _tmp476 * landmarks_10_; + const Scalar _tmp1802 = _tmp107 * landmarks_10_; + const Scalar _tmp1803 = _tmp1391 * _tmp1802 - _tmp1801 * _tmp76; + const Scalar _tmp1804 = _tmp480 * landmarks_10_; + const Scalar _tmp1805 = _tmp1802 * _tmp957 - _tmp1804 * _tmp99; + const Scalar _tmp1806 = _tmp1393 * _tmp1803 + _tmp1805 * _tmp959; + const Scalar _tmp1807 = _tmp1806 * _tmp962; + const Scalar _tmp1808 = _tmp1312 * _tmp1807 - _tmp1313 * _tmp1806 + _tmp1803 * _tmp489; + const Scalar _tmp1809 = _tmp3 * priors_0_1_sqrt_info(3, 4) + _tmp51 * priors_0_1_sqrt_info(3, 5) + _tmp60 * priors_0_1_sqrt_info(3, 3); - const Scalar _tmp1889 = _tmp1318 * _tmp1886 - _tmp1319 * _tmp1886 + _tmp1885 * _tmp463; - const Scalar _tmp1890 = _tmp1344 * priors_1_0_sqrt_info(2, 5) + - _tmp1790 * priors_1_0_sqrt_info(2, 3) + - _tmp890 * priors_1_0_sqrt_info(2, 4); - const Scalar _tmp1891 = -_tmp1247 * _tmp1717 + _tmp1248 * _tmp1717 + _tmp1716 * _tmp583; - const Scalar _tmp1892 = _tmp1329 * _tmp1857 - _tmp1330 * _tmp1857 + _tmp1856 * _tmp673; - const Scalar _tmp1893 = -_tmp1090 * _tmp1868 + _tmp1092 * _tmp1867 + _tmp1866 * _tmp703; - const Scalar _tmp1894 = -_tmp1271 * _tmp1876 + _tmp1272 * _tmp1876 + _tmp1875 * _tmp283; - const Scalar _tmp1895 = _tmp1344 * priors_1_0_sqrt_info(5, 5) + - _tmp1790 * priors_1_0_sqrt_info(5, 3) + - _tmp890 * priors_1_0_sqrt_info(5, 4); - const Scalar _tmp1896 = _tmp106 * _tmp1809 - _tmp1810 * _tmp87; - const Scalar _tmp1897 = _tmp106 * _tmp1806 - _tmp1807 * _tmp94; - const Scalar _tmp1898 = _tmp1225 * _tmp1897 + _tmp1360 * _tmp1896; - const Scalar _tmp1899 = -_tmp1268 * _tmp1898 + _tmp1269 * _tmp1898 + _tmp1896 * _tmp433; - const Scalar _tmp1900 = _tmp106 * _tmp1739 - _tmp1738 * _tmp87; - const Scalar _tmp1901 = _tmp106 * _tmp1742 - _tmp1741 * _tmp94; - const Scalar _tmp1902 = _tmp1352 * _tmp1900 + _tmp1901 * _tmp916; - const Scalar _tmp1903 = _tmp1900 * _tmp523 - _tmp1902 * _tmp920 + _tmp1902 * _tmp923; - const Scalar _tmp1904 = _tmp106 * _tmp1862 - _tmp1861 * _tmp94; - const Scalar _tmp1905 = _tmp106 * _tmp1864 - _tmp1865 * _tmp87; - const Scalar _tmp1906 = _tmp1086 * _tmp1904 + _tmp1347 * _tmp1905; - const Scalar _tmp1907 = -_tmp1242 * _tmp1906 + _tmp1243 * _tmp1906 + _tmp1904 * _tmp703; - const Scalar _tmp1908 = _tmp106 * _tmp1711 - _tmp1712 * _tmp94; - const Scalar _tmp1909 = _tmp106 * _tmp1715 - _tmp1714 * _tmp87; - const Scalar _tmp1910 = _tmp1502 * _tmp1909 + _tmp1908 * _tmp979; - const Scalar _tmp1911 = _tmp1908 * _tmp583 - _tmp1910 * _tmp983 + _tmp1910 * _tmp985; - const Scalar _tmp1912 = -_tmp1247 * _tmp1910 + _tmp1248 * _tmp1910 + _tmp1909 * _tmp583; - const Scalar _tmp1913 = _tmp106 * _tmp1841 - _tmp1840 * _tmp87; - const Scalar _tmp1914 = _tmp106 * _tmp1843 - _tmp1844 * _tmp94; - const Scalar _tmp1915 = _tmp1009 * _tmp1914 + _tmp1438 * _tmp1913; - const Scalar _tmp1916 = _tmp1014 * _tmp1915 - _tmp1017 * _tmp1915 + _tmp1914 * _tmp493; - const Scalar _tmp1917 = _tmp106 * landmarks_3_; - const Scalar _tmp1918 = _tmp1062 * _tmp1917 - _tmp1871 * _tmp87; - const Scalar _tmp1919 = _tmp1055 * _tmp1917 - _tmp1874 * _tmp94; - const Scalar _tmp1920 = _tmp1057 * _tmp1919 + _tmp1412 * _tmp1918; - const Scalar _tmp1921 = -_tmp1271 * _tmp1920 + _tmp1272 * _tmp1920 + _tmp1919 * _tmp283; - const Scalar _tmp1922 = _tmp52 * priors_0_1_sqrt_info(5, 5) + - _tmp59 * priors_0_1_sqrt_info(5, 3) + _tmp7 * priors_0_1_sqrt_info(5, 4); - const Scalar _tmp1923 = _tmp52 * priors_0_1_sqrt_info(4, 5) + - _tmp59 * priors_0_1_sqrt_info(4, 3) + _tmp7 * priors_0_1_sqrt_info(4, 4); - const Scalar _tmp1924 = _tmp106 * _tmp1758 - _tmp1759 * _tmp87; - const Scalar _tmp1925 = _tmp106 * _tmp1762 - _tmp1761 * _tmp94; - const Scalar _tmp1926 = - (Scalar(1) / Scalar(2)) * _tmp1071 * _tmp1925 + (Scalar(1) / Scalar(2)) * _tmp1458 * _tmp1924; - const Scalar _tmp1927 = _tmp1926 * _tmp331; - const Scalar _tmp1928 = _tmp1077 * _tmp181; - const Scalar _tmp1929 = -_tmp1074 * _tmp1927 + _tmp1924 * _tmp343 + _tmp1927 * _tmp1928; - const Scalar _tmp1930 = _tmp106 * landmarks_14_; - const Scalar _tmp1931 = _tmp1816 * _tmp1930 - _tmp1818 * _tmp87; - const Scalar _tmp1932 = _tmp1462 * _tmp1930 - _tmp1820 * _tmp94; - const Scalar _tmp1933 = _tmp1450 * _tmp1931 + _tmp1932 * _tmp901; - const Scalar _tmp1934 = _tmp1933 * _tmp903; - const Scalar _tmp1935 = _tmp1932 * _tmp613 + _tmp1933 * _tmp908 - _tmp1934 * _tmp904; - const Scalar _tmp1936 = _tmp106 * _tmp1724 - _tmp1723 * _tmp94; - const Scalar _tmp1937 = _tmp106 * _tmp1721 - _tmp1720 * _tmp87; - const Scalar _tmp1938 = _tmp1148 * _tmp1936 + _tmp1493 * _tmp1937; - const Scalar _tmp1939 = -_tmp1153 * _tmp1938 + _tmp1156 * _tmp1938 + _tmp1936 * _tmp733; - const Scalar _tmp1940 = _tmp106 * _tmp1749 - _tmp1750 * _tmp94; - const Scalar _tmp1941 = _tmp106 * _tmp1753 - _tmp1752 * _tmp87; - const Scalar _tmp1942 = _tmp1026 * _tmp1940 + _tmp1466 * _tmp1941; - const Scalar _tmp1943 = _tmp1256 * _tmp1942 - _tmp1468 * _tmp1942 + _tmp1940 * _tmp373; - const Scalar _tmp1944 = _tmp106 * _tmp1730 - _tmp1731 * _tmp87; - const Scalar _tmp1945 = _tmp106 * _tmp1734 - _tmp1733 * _tmp94; - const Scalar _tmp1946 = _tmp1042 * _tmp1945 + _tmp1522 * _tmp1944; - const Scalar _tmp1947 = -_tmp1046 * _tmp1946 + _tmp1049 * _tmp1946 + _tmp1944 * _tmp553; - const Scalar _tmp1948 = _tmp106 * _tmp1696 - _tmp1695 * _tmp94; - const Scalar _tmp1949 = _tmp106 * _tmp1699 - _tmp1698 * _tmp87; - const Scalar _tmp1950 = _tmp1166 * _tmp1948 + _tmp1511 * _tmp1949; - const Scalar _tmp1951 = _tmp1275 * _tmp1950 - _tmp1276 * _tmp1950 + _tmp1948 * _tmp253; - const Scalar _tmp1952 = _tmp52 * priors_0_1_sqrt_info(2, 5) + - _tmp59 * priors_0_1_sqrt_info(2, 3) + _tmp7 * priors_0_1_sqrt_info(2, 4); - const Scalar _tmp1953 = _tmp92 - 1; - const Scalar _tmp1954 = _tmp1953 + _tmp72; - const Scalar _tmp1955 = _tmp1340 * priors_1_0_sqrt_info(1, 5) + - _tmp1551 * priors_1_0_sqrt_info(1, 3) + - _tmp1954 * priors_1_0_sqrt_info(1, 4); - const Scalar _tmp1956 = _tmp106 * landmarks_1_; - const Scalar _tmp1957 = _tmp1323 * _tmp1956 - _tmp1828 * _tmp87; - const Scalar _tmp1958 = -_tmp1825 * _tmp94 + _tmp1956 * _tmp993; - const Scalar _tmp1959 = _tmp1402 * _tmp1957 + _tmp1958 * _tmp995; - const Scalar _tmp1960 = _tmp1000 * _tmp1959; - const Scalar _tmp1961 = _tmp1321 * _tmp1960 - _tmp1324 * _tmp1959 + _tmp1957 * _tmp223; - const Scalar _tmp1962 = _tmp1034 * _tmp1942 - _tmp1497 * _tmp1942 + _tmp1941 * _tmp373; - const Scalar _tmp1963 = _tmp1901 * _tmp523 - _tmp1902 * _tmp928 + _tmp1902 * _tmp929; - const Scalar _tmp1964 = _tmp1092 * _tmp1906 - _tmp1094 * _tmp1906 + _tmp1905 * _tmp703; - const Scalar _tmp1965 = _tmp1177 * _tmp1938 - _tmp1178 * _tmp1938 + _tmp1937 * _tmp733; - const Scalar _tmp1966 = _tmp1340 * priors_1_0_sqrt_info(4, 5) + - _tmp1551 * priors_1_0_sqrt_info(4, 3) + - _tmp1954 * priors_1_0_sqrt_info(4, 4); - const Scalar _tmp1967 = _tmp106 * landmarks_15_; - const Scalar _tmp1968 = _tmp1202 * _tmp1967 - _tmp1784 * _tmp94; - const Scalar _tmp1969 = _tmp1210 * _tmp1967 - _tmp1786 * _tmp87; - const Scalar _tmp1970 = _tmp1204 * _tmp1968 + _tmp1531 * _tmp1969; - const Scalar _tmp1971 = _tmp1301 * _tmp1970 - _tmp1302 * _tmp1970 + _tmp1968 * _tmp643; - const Scalar _tmp1972 = _tmp1340 * priors_1_0_sqrt_info(0, 5) + - _tmp1551 * priors_1_0_sqrt_info(0, 3) + - _tmp1954 * priors_1_0_sqrt_info(0, 4); - const Scalar _tmp1973 = _tmp1229 * _tmp1898 - _tmp1231 * _tmp1898 + _tmp1897 * _tmp433; - const Scalar _tmp1974 = _tmp106 * _tmp1796 - _tmp1797 * _tmp94; - const Scalar _tmp1975 = _tmp106 * _tmp1794 - _tmp1793 * _tmp87; - const Scalar _tmp1976 = _tmp1128 * _tmp1974 + _tmp1432 * _tmp1975; - const Scalar _tmp1977 = _tmp1132 * _tmp1976; - const Scalar _tmp1978 = _tmp1133 * _tmp1977 - _tmp1135 * _tmp1977 + _tmp1974 * _tmp403; - const Scalar _tmp1979 = _tmp106 * _tmp1883 - _tmp1884 * _tmp87; - const Scalar _tmp1980 = _tmp106 * _tmp1881 - _tmp1880 * _tmp94; - const Scalar _tmp1981 = _tmp1292 * _tmp1980 + _tmp1407 * _tmp1979; - const Scalar _tmp1982 = _tmp1318 * _tmp1981 - _tmp1319 * _tmp1981 + _tmp1979 * _tmp463; - const Scalar _tmp1983 = -_tmp1208 * _tmp1970 + _tmp1212 * _tmp1970 + _tmp1969 * _tmp643; - const Scalar _tmp1984 = -_tmp1333 * _tmp1915 + _tmp1334 * _tmp1915 + _tmp1913 * _tmp493; - const Scalar _tmp1985 = _tmp106 * _tmp1769 - _tmp1770 * _tmp94; - const Scalar _tmp1986 = _tmp106 * _tmp1766 - _tmp1767 * _tmp87; - const Scalar _tmp1987 = _tmp1115 * _tmp1985 + _tmp1444 * _tmp1986; - const Scalar _tmp1988 = _tmp1119 * _tmp1987 - _tmp1121 * _tmp1987 + _tmp193 * _tmp1985; - const Scalar _tmp1989 = _tmp52 * priors_0_1_sqrt_info(0, 5) + - _tmp59 * priors_0_1_sqrt_info(0, 3) + _tmp7 * priors_0_1_sqrt_info(0, 4); - const Scalar _tmp1990 = _tmp52 * priors_0_1_sqrt_info(1, 5) + - _tmp59 * priors_0_1_sqrt_info(1, 3) + _tmp7 * priors_0_1_sqrt_info(1, 4); - const Scalar _tmp1991 = _tmp106 * _tmp1776 - _tmp1774 * _tmp87; - const Scalar _tmp1992 = _tmp106 * _tmp1778 - _tmp1779 * _tmp94; - const Scalar _tmp1993 = _tmp1191 * _tmp1992 + _tmp1417 * _tmp1991; - const Scalar _tmp1994 = _tmp1283 * _tmp1993 - _tmp1284 * _tmp1993 + _tmp1991 * _tmp313; - const Scalar _tmp1995 = _tmp52 * priors_0_1_sqrt_info(3, 5) + - _tmp59 * priors_0_1_sqrt_info(3, 3) + _tmp7 * priors_0_1_sqrt_info(3, 4); - const Scalar _tmp1996 = -_tmp1195 * _tmp1993 + _tmp1197 * _tmp1993 + _tmp1992 * _tmp313; - const Scalar _tmp1997 = _tmp1340 * priors_1_0_sqrt_info(3, 5) + - _tmp1551 * priors_1_0_sqrt_info(3, 3) + - _tmp1954 * priors_1_0_sqrt_info(3, 4); - const Scalar _tmp1998 = _tmp106 * _tmp1855 - _tmp1854 * _tmp87; - const Scalar _tmp1999 = _tmp106 * _tmp1851 - _tmp1852 * _tmp94; - const Scalar _tmp2000 = _tmp1308 * _tmp1999 + _tmp1425 * _tmp1998; - const Scalar _tmp2001 = _tmp1328 * _tmp2000; - const Scalar _tmp2002 = _tmp1311 * _tmp2001 - _tmp1313 * _tmp2001 + _tmp1998 * _tmp673; - const Scalar _tmp2003 = _tmp1958 * _tmp223 - _tmp1959 * _tmp999 + _tmp1960 * _tmp997; - const Scalar _tmp2004 = -_tmp1238 * _tmp1987 + _tmp1239 * _tmp1987 + _tmp193 * _tmp1986; - const Scalar _tmp2005 = _tmp1312 * _tmp2000 - _tmp1314 * _tmp2000 + _tmp1999 * _tmp673; - const Scalar _tmp2006 = _tmp1170 * _tmp1950 - _tmp1172 * _tmp1950 + _tmp1949 * _tmp253; - const Scalar _tmp2007 = _tmp1061 * _tmp1920 - _tmp1065 * _tmp1920 + _tmp1918 * _tmp283; - const Scalar _tmp2008 = _tmp1479 * landmarks_19_; - const Scalar _tmp2009 = _tmp106 * _tmp2008 - _tmp1707 * _tmp94; - const Scalar _tmp2010 = _tmp1472 * landmarks_19_; - const Scalar _tmp2011 = _tmp106 * _tmp2010 - _tmp1704 * _tmp87; - const Scalar _tmp2012 = _tmp1100 * _tmp2009 + _tmp1474 * _tmp2011; - const Scalar _tmp2013 = _tmp1103 * _tmp2012; - const Scalar _tmp2014 = -_tmp1181 * _tmp2013 + _tmp1182 * _tmp2012 + _tmp2011 * _tmp763; - const Scalar _tmp2015 = _tmp1296 * _tmp1981 - _tmp1298 * _tmp1981 + _tmp1980 * _tmp463; - const Scalar _tmp2016 = -_tmp1104 * _tmp2013 + _tmp1107 * _tmp2012 + _tmp2009 * _tmp763; - const Scalar _tmp2017 = _tmp1340 * priors_1_0_sqrt_info(2, 5) + - _tmp1551 * priors_1_0_sqrt_info(2, 3) + - _tmp1954 * priors_1_0_sqrt_info(2, 4); - const Scalar _tmp2018 = -_tmp1251 * _tmp1934 + _tmp1252 * _tmp1933 + _tmp1931 * _tmp613; - const Scalar _tmp2019 = -_tmp1259 * _tmp1946 + _tmp1260 * _tmp1946 + _tmp1945 * _tmp553; - const Scalar _tmp2020 = _tmp1926 * _tmp335; - const Scalar _tmp2021 = -_tmp1074 * _tmp2020 + _tmp1925 * _tmp343 + _tmp1928 * _tmp2020; - const Scalar _tmp2022 = _tmp1340 * priors_1_0_sqrt_info(5, 5) + - _tmp1551 * priors_1_0_sqrt_info(5, 3) + - _tmp1954 * priors_1_0_sqrt_info(5, 4); - const Scalar _tmp2023 = _tmp1215 * _tmp1976 - _tmp1216 * _tmp1976 + _tmp1975 * _tmp403; - const Scalar _tmp2024 = _tmp104 * _tmp1724 - _tmp1723 * _tmp97; - const Scalar _tmp2025 = _tmp104 * _tmp1721 - _tmp1720 * _tmp83; - const Scalar _tmp2026 = _tmp1148 * _tmp2024 + _tmp1493 * _tmp2025; - const Scalar _tmp2027 = -_tmp1153 * _tmp2026 + _tmp1156 * _tmp2026 + _tmp2024 * _tmp733; - const Scalar _tmp2028 = _tmp104 * _tmp1794 - _tmp1793 * _tmp83; - const Scalar _tmp2029 = _tmp104 * _tmp1796 - _tmp1797 * _tmp97; - const Scalar _tmp2030 = _tmp1128 * _tmp2029 + _tmp1432 * _tmp2028; - const Scalar _tmp2031 = _tmp1215 * _tmp2030 - _tmp1216 * _tmp2030 + _tmp2028 * _tmp403; - const Scalar _tmp2032 = _tmp104 * _tmp1862 - _tmp1861 * _tmp97; - const Scalar _tmp2033 = _tmp104 * _tmp1864 - _tmp1865 * _tmp83; - const Scalar _tmp2034 = _tmp1086 * _tmp2032 + _tmp1347 * _tmp2033; - const Scalar _tmp2035 = -_tmp1242 * _tmp2034 + _tmp1243 * _tmp2034 + _tmp2032 * _tmp703; - const Scalar _tmp2036 = _tmp1134 * _tmp2030 - _tmp1136 * _tmp2030 + _tmp2029 * _tmp403; - const Scalar _tmp2037 = _tmp104 * _tmp1696 - _tmp1695 * _tmp97; - const Scalar _tmp2038 = _tmp104 * _tmp1699 - _tmp1698 * _tmp83; - const Scalar _tmp2039 = _tmp1166 * _tmp2037 + _tmp1511 * _tmp2038; - const Scalar _tmp2040 = _tmp1275 * _tmp2039 - _tmp1276 * _tmp2039 + _tmp2037 * _tmp253; - const Scalar _tmp2041 = _tmp104 * _tmp1739 - _tmp1738 * _tmp83; - const Scalar _tmp2042 = _tmp104 * _tmp1742 - _tmp1741 * _tmp97; - const Scalar _tmp2043 = _tmp1352 * _tmp2041 + _tmp2042 * _tmp916; - const Scalar _tmp2044 = _tmp2041 * _tmp523 - _tmp2043 * _tmp920 + _tmp2043 * _tmp923; - const Scalar _tmp2045 = _tmp104 * _tmp1715 - _tmp1714 * _tmp83; - const Scalar _tmp2046 = _tmp104 * _tmp1711 - _tmp1712 * _tmp97; - const Scalar _tmp2047 = _tmp1502 * _tmp2045 + _tmp2046 * _tmp979; - const Scalar _tmp2048 = -_tmp1247 * _tmp2047 + _tmp1248 * _tmp2047 + _tmp2045 * _tmp583; - const Scalar _tmp2049 = _tmp104 * _tmp1841 - _tmp1840 * _tmp83; - const Scalar _tmp2050 = _tmp104 * _tmp1843 - _tmp1844 * _tmp97; - const Scalar _tmp2051 = _tmp1009 * _tmp2050 + _tmp1438 * _tmp2049; - const Scalar _tmp2052 = -_tmp1333 * _tmp2051 + _tmp1334 * _tmp2051 + _tmp2049 * _tmp493; - const Scalar _tmp2053 = _tmp10 * priors_0_1_sqrt_info(2, 4) + + const Scalar _tmp1810 = _tmp1282 * landmarks_16_; + const Scalar _tmp1811 = _tmp660 * landmarks_16_; + const Scalar _tmp1812 = _tmp107 * _tmp1810 - _tmp1811 * _tmp99; + const Scalar _tmp1813 = _tmp656 * landmarks_16_; + const Scalar _tmp1814 = _tmp1307 * landmarks_16_; + const Scalar _tmp1815 = _tmp107 * _tmp1814 - _tmp1813 * _tmp76; + const Scalar _tmp1816 = _tmp1284 * _tmp1812 + _tmp1379 * _tmp1815; + const Scalar _tmp1817 = _tmp1289 * _tmp1816 - _tmp1291 * _tmp1816 + _tmp1812 * _tmp669; + const Scalar _tmp1818 = _tmp1805 * _tmp489 - _tmp1806 * _tmp965 + _tmp1807 * _tmp961; + const Scalar _tmp1819 = _tmp690 * landmarks_17_; + const Scalar _tmp1820 = _tmp1319 * landmarks_17_; + const Scalar _tmp1821 = _tmp1766 * _tmp1820 - _tmp1819 * _tmp99; + const Scalar _tmp1822 = _tmp1326 * landmarks_17_; + const Scalar _tmp1823 = _tmp686 * landmarks_17_; + const Scalar _tmp1824 = _tmp107 * _tmp1822 - _tmp1823 * _tmp76; + const Scalar _tmp1825 = _tmp1062 * _tmp1821 + _tmp1328 * _tmp1824; + const Scalar _tmp1826 = -_tmp1214 * _tmp1825 + _tmp1215 * _tmp1825 + _tmp1821 * _tmp699; + const Scalar _tmp1827 = std::pow(priors_0_1_sqrt_info(5, 5), Scalar(2)); + const Scalar _tmp1828 = _tmp1827 * _tmp53; + const Scalar _tmp1829 = -_tmp1126 * _tmp1689 + _tmp1128 * _tmp1689 + _tmp1688 * _tmp729; + const Scalar _tmp1830 = _tmp266 * landmarks_3_; + const Scalar _tmp1831 = _tmp1039 * landmarks_3_; + const Scalar _tmp1832 = _tmp107 * _tmp1831 - _tmp1830 * _tmp76; + const Scalar _tmp1833 = _tmp1032 * landmarks_3_; + const Scalar _tmp1834 = _tmp270 * landmarks_3_; + const Scalar _tmp1835 = _tmp107 * _tmp1833 - _tmp1834 * _tmp99; + const Scalar _tmp1836 = _tmp1034 * _tmp1835 + _tmp1364 * _tmp1832; + const Scalar _tmp1837 = _tmp1038 * _tmp1836 - _tmp1041 * _tmp1836 + _tmp1832 * _tmp279; + const Scalar _tmp1838 = _tmp1781 * _tmp601; + const Scalar _tmp1839 = _tmp1780 * _tmp609 + _tmp1783 * _tmp1838 - _tmp1838 * _tmp844; + const Scalar _tmp1840 = + _tmp1325 * priors_1_0_sqrt_info(4, 5) + _tmp829 * priors_1_0_sqrt_info(4, 4); + const Scalar _tmp1841 = -_tmp1186 * _tmp1748 + _tmp1189 * _tmp1748 + _tmp1747 * _tmp639; + const Scalar _tmp1842 = _tmp450 * landmarks_9_; + const Scalar _tmp1843 = _tmp1265 * landmarks_9_; + const Scalar _tmp1844 = _tmp107 * _tmp1843 - _tmp1842 * _tmp99; + const Scalar _tmp1845 = _tmp1293 * landmarks_9_; + const Scalar _tmp1846 = _tmp446 * landmarks_9_; + const Scalar _tmp1847 = _tmp107 * _tmp1845 - _tmp1846 * _tmp76; + const Scalar _tmp1848 = _tmp1267 * _tmp1844 + _tmp1359 * _tmp1847; + const Scalar _tmp1849 = _tmp1271 * _tmp1848 - _tmp1273 * _tmp1848 + _tmp1844 * _tmp459; + const Scalar _tmp1850 = _tmp1295 * _tmp1848 - _tmp1296 * _tmp1848 + _tmp1847 * _tmp459; + const Scalar _tmp1851 = _tmp1220 * _tmp1681 - _tmp1508 * _tmp1681 + _tmp1680 * _tmp579; + const Scalar _tmp1852 = _tmp1308 * _tmp1816; + const Scalar _tmp1853 = _tmp1288 * _tmp1852 - _tmp1290 * _tmp1852 + _tmp1815 * _tmp669; + const Scalar _tmp1854 = _tmp1068 * _tmp1825 - _tmp1070 * _tmp1825 + _tmp1824 * _tmp699; + const Scalar _tmp1855 = -_tmp1244 * _tmp1836 + _tmp1245 * _tmp1836 + _tmp1835 * _tmp279; + const Scalar _tmp1856 = _tmp1325 * priors_1_0_sqrt_info(2, 5) + + _tmp1759 * priors_1_0_sqrt_info(2, 3) + + _tmp829 * priors_1_0_sqrt_info(2, 4); + const Scalar _tmp1857 = _tmp105 * _tmp1769 - _tmp1770 * _tmp86; + const Scalar _tmp1858 = _tmp105 * _tmp836; + const Scalar _tmp1859 = _tmp1765 * _tmp1858 - _tmp1767 * _tmp93; + const Scalar _tmp1860 = _tmp1200 * _tmp1859 + _tmp1345 * _tmp1857; + const Scalar _tmp1861 = -_tmp1240 * _tmp1860 + _tmp1241 * _tmp1860 + _tmp1857 * _tmp429; + const Scalar _tmp1862 = _tmp105 * landmarks_11_; + const Scalar _tmp1863 = -_tmp1700 * _tmp86 + _tmp1862 * _tmp867; + const Scalar _tmp1864 = -_tmp1703 * _tmp93 + _tmp1862 * _tmp857; + const Scalar _tmp1865 = _tmp1334 * _tmp1863 + _tmp1864 * _tmp859; + const Scalar _tmp1866 = _tmp1865 * _tmp862; + const Scalar _tmp1867 = _tmp1863 * _tmp519 + _tmp1865 * _tmp865 - _tmp1866 * _tmp861; + const Scalar _tmp1868 = -_tmp1819 * _tmp93 + _tmp1820 * _tmp1858; + const Scalar _tmp1869 = _tmp105 * _tmp1822 - _tmp1823 * _tmp86; + const Scalar _tmp1870 = _tmp1062 * _tmp1868 + _tmp1328 * _tmp1869; + const Scalar _tmp1871 = -_tmp1214 * _tmp1870 + _tmp1215 * _tmp1870 + _tmp1868 * _tmp699; + const Scalar _tmp1872 = _tmp105 * _tmp1675 - _tmp1676 * _tmp93; + const Scalar _tmp1873 = _tmp105 * _tmp1679 - _tmp1678 * _tmp86; + const Scalar _tmp1874 = _tmp1483 * _tmp1873 + _tmp1872 * _tmp929; + const Scalar _tmp1875 = -_tmp1485 * _tmp1874 + _tmp1872 * _tmp579 + _tmp1874 * _tmp936; + const Scalar _tmp1876 = _tmp1220 * _tmp1874 - _tmp1508 * _tmp1874 + _tmp1873 * _tmp579; + const Scalar _tmp1877 = _tmp105 * landmarks_10_; + const Scalar _tmp1878 = _tmp1391 * _tmp1877 - _tmp1801 * _tmp86; + const Scalar _tmp1879 = -_tmp1804 * _tmp93 + _tmp1877 * _tmp957; + const Scalar _tmp1880 = _tmp1393 * _tmp1878 + _tmp1879 * _tmp959; + const Scalar _tmp1881 = _tmp1880 * _tmp962; + const Scalar _tmp1882 = _tmp1879 * _tmp489 - _tmp1880 * _tmp965 + _tmp1881 * _tmp961; + const Scalar _tmp1883 = _tmp105 * _tmp1831 - _tmp1830 * _tmp86; + const Scalar _tmp1884 = _tmp105 * _tmp1833 - _tmp1834 * _tmp93; + const Scalar _tmp1885 = _tmp1034 * _tmp1884 + _tmp1364 * _tmp1883; + const Scalar _tmp1886 = -_tmp1244 * _tmp1885 + _tmp1245 * _tmp1885 + _tmp1884 * _tmp279; + const Scalar _tmp1887 = _tmp105 * _tmp1717 - _tmp1718 * _tmp86; + const Scalar _tmp1888 = _tmp105 * _tmp1721 - _tmp1720 * _tmp93; + const Scalar _tmp1889 = _tmp1048 * _tmp1888 + _tmp1420 * _tmp1887; + const Scalar _tmp1890 = _tmp1054 * _tmp1889; + const Scalar _tmp1891 = -_tmp1053 * _tmp1890 + _tmp1057 * _tmp1889 + _tmp1887 * _tmp339; + const Scalar _tmp1892 = _tmp105 * _tmp1775 - _tmp1776 * _tmp86; + const Scalar _tmp1893 = _tmp105 * _tmp1778 - _tmp1779 * _tmp93; + const Scalar _tmp1894 = _tmp1413 * _tmp1892 + _tmp1893 * _tmp842; + const Scalar _tmp1895 = _tmp1893 * _tmp609 - _tmp1894 * _tmp846 + _tmp1894 * _tmp851; + const Scalar _tmp1896 = _tmp105 * _tmp1687 - _tmp1686 * _tmp93; + const Scalar _tmp1897 = _tmp105 * _tmp1684 - _tmp1683 * _tmp86; + const Scalar _tmp1898 = _tmp1120 * _tmp1896 + _tmp1473 * _tmp1897; + const Scalar _tmp1899 = -_tmp1126 * _tmp1898 + _tmp1128 * _tmp1898 + _tmp1896 * _tmp729; + const Scalar _tmp1900 = _tmp91 - 1; + const Scalar _tmp1901 = _tmp1900 + _tmp71; + const Scalar _tmp1902 = + _tmp1321 * priors_1_0_sqrt_info(4, 5) + _tmp1901 * priors_1_0_sqrt_info(4, 4); + const Scalar _tmp1903 = _tmp105 * _tmp1708 - _tmp1709 * _tmp93; + const Scalar _tmp1904 = _tmp105 * _tmp1712 - _tmp1711 * _tmp86; + const Scalar _tmp1905 = _tmp1429 * _tmp1904 + _tmp1903 * _tmp972; + const Scalar _tmp1906 = _tmp1227 * _tmp1905 - _tmp1432 * _tmp1905 + _tmp1903 * _tmp369; + const Scalar _tmp1907 = _tmp105 * _tmp1692 - _tmp1693 * _tmp86; + const Scalar _tmp1908 = _tmp105 * _tmp1696 - _tmp1695 * _tmp93; + const Scalar _tmp1909 = _tmp1497 * _tmp1907 + _tmp1908 * _tmp987; + const Scalar _tmp1910 = _tmp1907 * _tmp549 - _tmp1909 * _tmp991 + _tmp1909 * _tmp994; + const Scalar _tmp1911 = _tmp105 * _tmp1659 - _tmp1658 * _tmp93; + const Scalar _tmp1912 = _tmp105 * _tmp1662 - _tmp1661 * _tmp86; + const Scalar _tmp1913 = _tmp1137 * _tmp1911 + _tmp1489 * _tmp1912; + const Scalar _tmp1914 = _tmp1248 * _tmp1913 - _tmp1249 * _tmp1913 + _tmp1911 * _tmp249; + const Scalar _tmp1915 = _tmp52 * priors_0_1_sqrt_info(1, 5) + + _tmp59 * priors_0_1_sqrt_info(1, 3) + _tmp6 * priors_0_1_sqrt_info(1, 4); + const Scalar _tmp1916 = _tmp105 * _tmp1788 - _tmp1789 * _tmp86; + const Scalar _tmp1917 = _tmp105 * _tmp1786 - _tmp1785 * _tmp93; + const Scalar _tmp1918 = _tmp1354 * _tmp1916 + _tmp1917 * _tmp944; + const Scalar _tmp1919 = _tmp1299 * _tmp1918 - _tmp1301 * _tmp1918 + _tmp1916 * _tmp219; + const Scalar _tmp1920 = -_tmp1477 * _tmp1905 + _tmp1904 * _tmp369 + _tmp1905 * _tmp979; + const Scalar _tmp1921 = _tmp1864 * _tmp519 + _tmp1865 * _tmp921 - _tmp1866 * _tmp919; + const Scalar _tmp1922 = _tmp1068 * _tmp1870 - _tmp1070 * _tmp1870 + _tmp1869 * _tmp699; + const Scalar _tmp1923 = _tmp52 * priors_0_1_sqrt_info(3, 5) + + _tmp59 * priors_0_1_sqrt_info(3, 3) + _tmp6 * priors_0_1_sqrt_info(3, 4); + const Scalar _tmp1924 = _tmp1156 * _tmp1898 - _tmp1157 * _tmp1898 + _tmp1897 * _tmp729; + const Scalar _tmp1925 = _tmp1217 * _tmp1321; + const Scalar _tmp1926 = _tmp105 * landmarks_15_; + const Scalar _tmp1927 = _tmp1180 * _tmp1926 - _tmp1744 * _tmp93; + const Scalar _tmp1928 = _tmp1187 * _tmp1926 - _tmp1746 * _tmp86; + const Scalar _tmp1929 = _tmp1182 * _tmp1927 + _tmp1503 * _tmp1928; + const Scalar _tmp1930 = _tmp1276 * _tmp1929 - _tmp1277 * _tmp1929 + _tmp1927 * _tmp639; + const Scalar _tmp1931 = _tmp1321 * priors_1_0_sqrt_info(0, 5) + + _tmp1522 * priors_1_0_sqrt_info(0, 3) + + _tmp1901 * priors_1_0_sqrt_info(0, 4); + const Scalar _tmp1932 = _tmp1321 * priors_1_0_sqrt_info(3, 5) + + _tmp1522 * priors_1_0_sqrt_info(3, 3) + + _tmp1901 * priors_1_0_sqrt_info(3, 4); + const Scalar _tmp1933 = _tmp1204 * _tmp1860 - _tmp1206 * _tmp1860 + _tmp1859 * _tmp429; + const Scalar _tmp1934 = _tmp105 * _tmp1754 - _tmp1755 * _tmp93; + const Scalar _tmp1935 = _tmp105 * _tmp1752 - _tmp1751 * _tmp86; + const Scalar _tmp1936 = _tmp1104 * _tmp1934 + _tmp1386 * _tmp1935; + const Scalar _tmp1937 = _tmp1110 * _tmp1936 - _tmp1112 * _tmp1936 + _tmp1934 * _tmp399; + const Scalar _tmp1938 = _tmp105 * _tmp1845 - _tmp1846 * _tmp86; + const Scalar _tmp1939 = _tmp105 * _tmp1843 - _tmp1842 * _tmp93; + const Scalar _tmp1940 = _tmp1267 * _tmp1939 + _tmp1359 * _tmp1938; + const Scalar _tmp1941 = _tmp1295 * _tmp1940 - _tmp1296 * _tmp1940 + _tmp1938 * _tmp459; + const Scalar _tmp1942 = -_tmp1186 * _tmp1929 + _tmp1189 * _tmp1929 + _tmp1928 * _tmp639; + const Scalar _tmp1943 = _tmp1321 * priors_1_0_sqrt_info(1, 5) + + _tmp1522 * priors_1_0_sqrt_info(1, 3) + + _tmp1901 * priors_1_0_sqrt_info(1, 4); + const Scalar _tmp1944 = _tmp1312 * _tmp1881 - _tmp1313 * _tmp1880 + _tmp1878 * _tmp489; + const Scalar _tmp1945 = _tmp105 * _tmp1729 - _tmp1730 * _tmp93; + const Scalar _tmp1946 = _tmp105 * _tmp1726 - _tmp1727 * _tmp86; + const Scalar _tmp1947 = _tmp1091 * _tmp1945 + _tmp1408 * _tmp1946; + const Scalar _tmp1948 = _tmp1094 * _tmp1947; + const Scalar _tmp1949 = _tmp1093 * _tmp1948 - _tmp1097 * _tmp1947 + _tmp189 * _tmp1945; + const Scalar _tmp1950 = _tmp52 * priors_0_1_sqrt_info(0, 5) + + _tmp59 * priors_0_1_sqrt_info(0, 3) + _tmp6 * priors_0_1_sqrt_info(0, 4); + const Scalar _tmp1951 = _tmp105 * _tmp1736 - _tmp1735 * _tmp86; + const Scalar _tmp1952 = _tmp105 * _tmp1738 - _tmp1739 * _tmp93; + const Scalar _tmp1953 = _tmp1167 * _tmp1952 + _tmp1369 * _tmp1951; + const Scalar _tmp1954 = _tmp1258 * _tmp1953 - _tmp1259 * _tmp1953 + _tmp1951 * _tmp309; + const Scalar _tmp1955 = -_tmp1172 * _tmp1953 + _tmp1174 * _tmp1953 + _tmp1952 * _tmp309; + const Scalar _tmp1956 = _tmp52 * priors_0_1_sqrt_info(4, 5) + _tmp6 * priors_0_1_sqrt_info(4, 4); + const Scalar _tmp1957 = _tmp105 * _tmp1814 - _tmp1813 * _tmp86; + const Scalar _tmp1958 = _tmp105 * _tmp1810 - _tmp1811 * _tmp93; + const Scalar _tmp1959 = _tmp1284 * _tmp1958 + _tmp1379 * _tmp1957; + const Scalar _tmp1960 = _tmp1290 * _tmp1959; + const Scalar _tmp1961 = -_tmp1308 * _tmp1960 + _tmp1309 * _tmp1959 + _tmp1957 * _tmp669; + const Scalar _tmp1962 = _tmp1917 * _tmp219 - _tmp1918 * _tmp948 + _tmp1918 * _tmp950; + const Scalar _tmp1963 = _tmp1209 * _tmp1948 - _tmp1210 * _tmp1947 + _tmp189 * _tmp1946; + const Scalar _tmp1964 = -_tmp1287 * _tmp1960 + _tmp1289 * _tmp1959 + _tmp1958 * _tmp669; + const Scalar _tmp1965 = _tmp52 * priors_0_1_sqrt_info(2, 5) + + _tmp59 * priors_0_1_sqrt_info(2, 3) + _tmp6 * priors_0_1_sqrt_info(2, 4); + const Scalar _tmp1966 = _tmp1141 * _tmp1913 - _tmp1143 * _tmp1913 + _tmp1912 * _tmp249; + const Scalar _tmp1967 = _tmp1038 * _tmp1885 - _tmp1041 * _tmp1885 + _tmp1883 * _tmp279; + const Scalar _tmp1968 = _tmp105 * landmarks_19_; + const Scalar _tmp1969 = _tmp1445 * _tmp1968 - _tmp1670 * _tmp93; + const Scalar _tmp1970 = _tmp1437 * _tmp1968 - _tmp1667 * _tmp86; + const Scalar _tmp1971 = _tmp1076 * _tmp1969 + _tmp1439 * _tmp1970; + const Scalar _tmp1972 = _tmp1082 * _tmp1971; + const Scalar _tmp1973 = _tmp1159 * _tmp1972 - _tmp1160 * _tmp1971 + _tmp1970 * _tmp759; + const Scalar _tmp1974 = _tmp1271 * _tmp1940 - _tmp1273 * _tmp1940 + _tmp1939 * _tmp459; + const Scalar _tmp1975 = _tmp1079 * _tmp1972 - _tmp1081 * _tmp1971 + _tmp1969 * _tmp759; + const Scalar _tmp1976 = _tmp1223 * _tmp1894 - _tmp1224 * _tmp1894 + _tmp1892 * _tmp609; + const Scalar _tmp1977 = -_tmp1230 * _tmp1909 + _tmp1231 * _tmp1909 + _tmp1908 * _tmp549; + const Scalar _tmp1978 = -_tmp1251 * _tmp1890 + _tmp1252 * _tmp1889 + _tmp1888 * _tmp339; + const Scalar _tmp1979 = _tmp1321 * priors_1_0_sqrt_info(2, 5) + + _tmp1522 * priors_1_0_sqrt_info(2, 3) + + _tmp1901 * priors_1_0_sqrt_info(2, 4); + const Scalar _tmp1980 = _tmp1192 * _tmp1936 - _tmp1193 * _tmp1936 + _tmp1935 * _tmp399; + const Scalar _tmp1981 = _tmp103 * _tmp1687 - _tmp1686 * _tmp96; + const Scalar _tmp1982 = _tmp103 * _tmp1684 - _tmp1683 * _tmp81; + const Scalar _tmp1983 = _tmp1120 * _tmp1981 + _tmp1473 * _tmp1982; + const Scalar _tmp1984 = -_tmp1126 * _tmp1983 + _tmp1128 * _tmp1983 + _tmp1981 * _tmp729; + const Scalar _tmp1985 = _tmp103 * _tmp1752 - _tmp1751 * _tmp81; + const Scalar _tmp1986 = _tmp103 * _tmp1754 - _tmp1755 * _tmp96; + const Scalar _tmp1987 = _tmp1104 * _tmp1986 + _tmp1386 * _tmp1985; + const Scalar _tmp1988 = _tmp1192 * _tmp1987 - _tmp1193 * _tmp1987 + _tmp1985 * _tmp399; + const Scalar _tmp1989 = _tmp103 * _tmp836; + const Scalar _tmp1990 = -_tmp1819 * _tmp96 + _tmp1820 * _tmp1989; + const Scalar _tmp1991 = _tmp103 * _tmp1822 - _tmp1823 * _tmp81; + const Scalar _tmp1992 = _tmp1062 * _tmp1990 + _tmp1328 * _tmp1991; + const Scalar _tmp1993 = -_tmp1214 * _tmp1992 + _tmp1215 * _tmp1992 + _tmp1990 * _tmp699; + const Scalar _tmp1994 = _tmp1110 * _tmp1987 - _tmp1112 * _tmp1987 + _tmp1986 * _tmp399; + const Scalar _tmp1995 = _tmp103 * _tmp1659 - _tmp1658 * _tmp96; + const Scalar _tmp1996 = _tmp103 * _tmp1662 - _tmp1661 * _tmp81; + const Scalar _tmp1997 = _tmp1137 * _tmp1995 + _tmp1489 * _tmp1996; + const Scalar _tmp1998 = _tmp1248 * _tmp1997 - _tmp1249 * _tmp1997 + _tmp1995 * _tmp249; + const Scalar _tmp1999 = _tmp103 * landmarks_11_; + const Scalar _tmp2000 = -_tmp1700 * _tmp81 + _tmp1999 * _tmp867; + const Scalar _tmp2001 = -_tmp1703 * _tmp96 + _tmp1999 * _tmp857; + const Scalar _tmp2002 = _tmp1334 * _tmp2000 + _tmp2001 * _tmp859; + const Scalar _tmp2003 = _tmp2000 * _tmp519 - _tmp2002 * _tmp863 + _tmp2002 * _tmp865; + const Scalar _tmp2004 = _tmp103 * _tmp1679 - _tmp1678 * _tmp81; + const Scalar _tmp2005 = _tmp103 * _tmp1675 - _tmp1676 * _tmp96; + const Scalar _tmp2006 = _tmp1483 * _tmp2004 + _tmp2005 * _tmp929; + const Scalar _tmp2007 = _tmp1220 * _tmp2006 - _tmp1508 * _tmp2006 + _tmp2004 * _tmp579; + const Scalar _tmp2008 = _tmp103 * landmarks_10_; + const Scalar _tmp2009 = _tmp1391 * _tmp2008 - _tmp1801 * _tmp81; + const Scalar _tmp2010 = -_tmp1804 * _tmp96 + _tmp2008 * _tmp957; + const Scalar _tmp2011 = _tmp1393 * _tmp2009 + _tmp2010 * _tmp959; + const Scalar _tmp2012 = -_tmp1313 * _tmp2011 + _tmp1314 * _tmp2011 + _tmp2009 * _tmp489; + const Scalar _tmp2013 = _tmp1155 * _tmp1983; + const Scalar _tmp2014 = -_tmp1124 * _tmp2013 + _tmp1127 * _tmp2013 + _tmp1982 * _tmp729; + const Scalar _tmp2015 = _tmp103 * landmarks_4_; + const Scalar _tmp2016 = _tmp1256 * _tmp2015 - _tmp1735 * _tmp81; + const Scalar _tmp2017 = _tmp1165 * _tmp2015 - _tmp1739 * _tmp96; + const Scalar _tmp2018 = _tmp1167 * _tmp2017 + _tmp1369 * _tmp2016; + const Scalar _tmp2019 = _tmp1171 * _tmp2018; + const Scalar _tmp2020 = -_tmp1257 * _tmp2019 + _tmp1258 * _tmp2018 + _tmp2016 * _tmp309; + const Scalar _tmp2021 = _tmp103 * _tmp1726 - _tmp1727 * _tmp81; + const Scalar _tmp2022 = _tmp103 * _tmp1729 - _tmp1730 * _tmp96; + const Scalar _tmp2023 = _tmp1091 * _tmp2022 + _tmp1408 * _tmp2021; + const Scalar _tmp2024 = -_tmp1210 * _tmp2023 + _tmp1211 * _tmp2023 + _tmp189 * _tmp2021; + const Scalar _tmp2025 = _tmp10 * priors_0_1_sqrt_info(2, 4) + _tmp48 * priors_0_1_sqrt_info(2, 5) + _tmp61 * priors_0_1_sqrt_info(2, 3); - const Scalar _tmp2054 = _tmp1177 * _tmp2026 - _tmp1178 * _tmp2026 + _tmp2025 * _tmp733; - const Scalar _tmp2055 = _tmp104 * _tmp1775; - const Scalar _tmp2056 = _tmp1281 * _tmp2055 - _tmp1774 * _tmp83; - const Scalar _tmp2057 = _tmp1189 * _tmp2055 - _tmp1779 * _tmp97; - const Scalar _tmp2058 = _tmp1191 * _tmp2057 + _tmp1417 * _tmp2056; - const Scalar _tmp2059 = _tmp1282 * _tmp2058; - const Scalar _tmp2060 = -_tmp1193 * _tmp2059 + _tmp1196 * _tmp2059 + _tmp2056 * _tmp313; - const Scalar _tmp2061 = _tmp104 * _tmp1766 - _tmp1767 * _tmp83; - const Scalar _tmp2062 = _tmp104 * _tmp1769 - _tmp1770 * _tmp97; - const Scalar _tmp2063 = _tmp1115 * _tmp2062 + _tmp1444 * _tmp2061; - const Scalar _tmp2064 = -_tmp1238 * _tmp2063 + _tmp1239 * _tmp2063 + _tmp193 * _tmp2061; - const Scalar _tmp2065 = _tmp10 * priors_0_1_sqrt_info(4, 4) + - _tmp48 * priors_0_1_sqrt_info(4, 5) + _tmp61 * priors_0_1_sqrt_info(4, 3); - const Scalar _tmp2066 = _tmp10 * priors_0_1_sqrt_info(5, 4) + - _tmp48 * priors_0_1_sqrt_info(5, 5) + _tmp61 * priors_0_1_sqrt_info(5, 3); - const Scalar _tmp2067 = _tmp104 * _tmp2008 - _tmp1707 * _tmp97; - const Scalar _tmp2068 = _tmp104 * _tmp2010 - _tmp1704 * _tmp83; - const Scalar _tmp2069 = _tmp1100 * _tmp2067 + _tmp1474 * _tmp2068; - const Scalar _tmp2070 = -_tmp1105 * _tmp2069 + _tmp1107 * _tmp2069 + _tmp2067 * _tmp763; - const Scalar _tmp2071 = _tmp104 * _tmp1883 - _tmp1884 * _tmp83; - const Scalar _tmp2072 = _tmp104 * _tmp1881 - _tmp1880 * _tmp97; - const Scalar _tmp2073 = _tmp1292 * _tmp2072 + _tmp1407 * _tmp2071; - const Scalar _tmp2074 = _tmp1297 * _tmp2073; - const Scalar _tmp2075 = -_tmp1317 * _tmp2074 + _tmp1318 * _tmp2073 + _tmp2071 * _tmp463; - const Scalar _tmp2076 = _tmp104 * _tmp1753 - _tmp1752 * _tmp83; - const Scalar _tmp2077 = _tmp104 * _tmp1749 - _tmp1750 * _tmp97; - const Scalar _tmp2078 = _tmp1026 * _tmp2077 + _tmp1466 * _tmp2076; - const Scalar _tmp2079 = _tmp1034 * _tmp2078 - _tmp1497 * _tmp2078 + _tmp2076 * _tmp373; - const Scalar _tmp2080 = _tmp1256 * _tmp2078 - _tmp1468 * _tmp2078 + _tmp2077 * _tmp373; - const Scalar _tmp2081 = _tmp104 * _tmp1851 - _tmp1852 * _tmp97; - const Scalar _tmp2082 = _tmp104 * _tmp1855 - _tmp1854 * _tmp83; - const Scalar _tmp2083 = _tmp1308 * _tmp2081 + _tmp1425 * _tmp2082; - const Scalar _tmp2084 = _tmp1312 * _tmp2083 - _tmp1314 * _tmp2083 + _tmp2081 * _tmp673; - const Scalar _tmp2085 = _tmp2046 * _tmp583 - _tmp2047 * _tmp983 + _tmp2047 * _tmp985; - const Scalar _tmp2086 = _tmp104 * landmarks_1_; - const Scalar _tmp2087 = -_tmp1825 * _tmp97 + _tmp2086 * _tmp993; - const Scalar _tmp2088 = _tmp1323 * _tmp2086 - _tmp1828 * _tmp83; - const Scalar _tmp2089 = _tmp1402 * _tmp2088 + _tmp2087 * _tmp995; - const Scalar _tmp2090 = _tmp1322 * _tmp2089 - _tmp1324 * _tmp2089 + _tmp2088 * _tmp223; - const Scalar _tmp2091 = _tmp104 * _tmp1758 - _tmp1759 * _tmp83; - const Scalar _tmp2092 = _tmp104 * _tmp1762 - _tmp1761 * _tmp97; - const Scalar _tmp2093 = _tmp1071 * _tmp2092 + _tmp1458 * _tmp2091; - const Scalar _tmp2094 = -_tmp1279 * _tmp2093 + _tmp1538 * _tmp2093 + _tmp2092 * _tmp343; - const Scalar _tmp2095 = -_tmp1294 * _tmp2074 + _tmp1296 * _tmp2073 + _tmp2072 * _tmp463; - const Scalar _tmp2096 = _tmp104 * landmarks_14_; - const Scalar _tmp2097 = _tmp1816 * _tmp2096 - _tmp1818 * _tmp83; - const Scalar _tmp2098 = _tmp1462 * _tmp2096 - _tmp1820 * _tmp97; - const Scalar _tmp2099 = _tmp1450 * _tmp2097 + _tmp2098 * _tmp901; - const Scalar _tmp2100 = _tmp1252 * _tmp2099 - _tmp1253 * _tmp2099 + _tmp2097 * _tmp613; - const Scalar _tmp2101 = _tmp1182 * _tmp2069 - _tmp1183 * _tmp2069 + _tmp2068 * _tmp763; - const Scalar _tmp2102 = _tmp10 * priors_0_1_sqrt_info(1, 4) + - _tmp48 * priors_0_1_sqrt_info(1, 5) + _tmp61 * priors_0_1_sqrt_info(1, 3); - const Scalar _tmp2103 = _tmp104 * _tmp1734 - _tmp1733 * _tmp97; - const Scalar _tmp2104 = _tmp104 * _tmp1730 - _tmp1731 * _tmp83; - const Scalar _tmp2105 = _tmp1042 * _tmp2103 + _tmp1522 * _tmp2104; - const Scalar _tmp2106 = -_tmp1046 * _tmp2105 + _tmp1049 * _tmp2105 + _tmp2104 * _tmp553; - const Scalar _tmp2107 = _tmp1953 + _tmp75; - const Scalar _tmp2108 = _tmp1552 * priors_1_0_sqrt_info(1, 3) + - _tmp2107 * priors_1_0_sqrt_info(1, 5) + - _tmp895 * priors_1_0_sqrt_info(1, 4); - const Scalar _tmp2109 = _tmp1258 * _tmp2105; - const Scalar _tmp2110 = -_tmp1044 * _tmp2109 + _tmp1048 * _tmp2109 + _tmp2103 * _tmp553; - const Scalar _tmp2111 = _tmp1552 * priors_1_0_sqrt_info(4, 3) + - _tmp2107 * priors_1_0_sqrt_info(4, 5) + - _tmp895 * priors_1_0_sqrt_info(4, 4); - const Scalar _tmp2112 = _tmp1552 * priors_1_0_sqrt_info(0, 3) + - _tmp2107 * priors_1_0_sqrt_info(0, 5) + - _tmp895 * priors_1_0_sqrt_info(0, 4); - const Scalar _tmp2113 = _tmp1329 * _tmp2083 - _tmp1330 * _tmp2083 + _tmp2082 * _tmp673; - const Scalar _tmp2114 = _tmp104 * landmarks_3_; - const Scalar _tmp2115 = _tmp1055 * _tmp2114 - _tmp1874 * _tmp97; - const Scalar _tmp2116 = _tmp1062 * _tmp2114 - _tmp1871 * _tmp83; - const Scalar _tmp2117 = _tmp1057 * _tmp2115 + _tmp1412 * _tmp2116; - const Scalar _tmp2118 = -_tmp1271 * _tmp2117 + _tmp1272 * _tmp2117 + _tmp2115 * _tmp283; - const Scalar _tmp2119 = _tmp104 * landmarks_15_; - const Scalar _tmp2120 = _tmp1202 * _tmp2119 - _tmp1784 * _tmp97; - const Scalar _tmp2121 = _tmp1210 * _tmp2119 - _tmp1786 * _tmp83; - const Scalar _tmp2122 = _tmp1204 * _tmp2120 + _tmp1531 * _tmp2121; - const Scalar _tmp2123 = _tmp1301 * _tmp2122 - _tmp1302 * _tmp2122 + _tmp2120 * _tmp643; - const Scalar _tmp2124 = _tmp2042 * _tmp523 - _tmp2043 * _tmp928 + _tmp2043 * _tmp929; - const Scalar _tmp2125 = -_tmp1076 * _tmp2093 + _tmp1460 * _tmp2093 + _tmp2091 * _tmp343; - const Scalar _tmp2126 = _tmp104 * _tmp1809 - _tmp1810 * _tmp83; - const Scalar _tmp2127 = _tmp104 * _tmp1806 - _tmp1807 * _tmp97; - const Scalar _tmp2128 = _tmp1225 * _tmp2127 + _tmp1360 * _tmp2126; - const Scalar _tmp2129 = -_tmp1268 * _tmp2128 + _tmp1269 * _tmp2128 + _tmp2126 * _tmp433; - const Scalar _tmp2130 = _tmp10 * priors_0_1_sqrt_info(0, 4) + + const Scalar _tmp2026 = _tmp103 * landmarks_19_; + const Scalar _tmp2027 = _tmp1445 * _tmp2026 - _tmp1670 * _tmp96; + const Scalar _tmp2028 = _tmp1437 * _tmp2026 - _tmp1667 * _tmp81; + const Scalar _tmp2029 = _tmp1076 * _tmp2027 + _tmp1439 * _tmp2028; + const Scalar _tmp2030 = _tmp1082 * _tmp2029; + const Scalar _tmp2031 = _tmp1079 * _tmp2030 - _tmp1081 * _tmp2029 + _tmp2027 * _tmp759; + const Scalar _tmp2032 = _tmp103 * _tmp1845 - _tmp1846 * _tmp81; + const Scalar _tmp2033 = _tmp103 * _tmp1843 - _tmp1842 * _tmp96; + const Scalar _tmp2034 = _tmp1267 * _tmp2033 + _tmp1359 * _tmp2032; + const Scalar _tmp2035 = _tmp1295 * _tmp2034 - _tmp1296 * _tmp2034 + _tmp2032 * _tmp459; + const Scalar _tmp2036 = _tmp103 * _tmp1712 - _tmp1711 * _tmp81; + const Scalar _tmp2037 = _tmp103 * _tmp1708 - _tmp1709 * _tmp96; + const Scalar _tmp2038 = _tmp1429 * _tmp2036 + _tmp2037 * _tmp972; + const Scalar _tmp2039 = -_tmp1477 * _tmp2038 + _tmp2036 * _tmp369 + _tmp2038 * _tmp979; + const Scalar _tmp2040 = _tmp1227 * _tmp2038 - _tmp1432 * _tmp2038 + _tmp2037 * _tmp369; + const Scalar _tmp2041 = _tmp103 * _tmp1810 - _tmp1811 * _tmp96; + const Scalar _tmp2042 = _tmp103 * _tmp1814 - _tmp1813 * _tmp81; + const Scalar _tmp2043 = _tmp1284 * _tmp2041 + _tmp1379 * _tmp2042; + const Scalar _tmp2044 = _tmp1289 * _tmp2043 - _tmp1291 * _tmp2043 + _tmp2041 * _tmp669; + const Scalar _tmp2045 = -_tmp1485 * _tmp2006 + _tmp2005 * _tmp579 + _tmp2006 * _tmp936; + const Scalar _tmp2046 = _tmp103 * _tmp1786 - _tmp1785 * _tmp96; + const Scalar _tmp2047 = _tmp103 * _tmp1788 - _tmp1789 * _tmp81; + const Scalar _tmp2048 = _tmp1354 * _tmp2047 + _tmp2046 * _tmp944; + const Scalar _tmp2049 = _tmp2048 * _tmp946; + const Scalar _tmp2050 = -_tmp1298 * _tmp2049 + _tmp1299 * _tmp2048 + _tmp2047 * _tmp219; + const Scalar _tmp2051 = _tmp103 * _tmp1717 - _tmp1718 * _tmp81; + const Scalar _tmp2052 = _tmp103 * _tmp1721 - _tmp1720 * _tmp96; + const Scalar _tmp2053 = _tmp1048 * _tmp2052 + _tmp1420 * _tmp2051; + const Scalar _tmp2054 = _tmp1252 * _tmp2053 - _tmp1253 * _tmp2053 + _tmp2052 * _tmp339; + const Scalar _tmp2055 = _tmp1271 * _tmp2034 - _tmp1273 * _tmp2034 + _tmp2033 * _tmp459; + const Scalar _tmp2056 = _tmp103 * landmarks_14_; + const Scalar _tmp2057 = _tmp1222 * _tmp2056 - _tmp1776 * _tmp81; + const Scalar _tmp2058 = -_tmp1779 * _tmp96 + _tmp2056 * _tmp839; + const Scalar _tmp2059 = _tmp1413 * _tmp2057 + _tmp2058 * _tmp842; + const Scalar _tmp2060 = _tmp1223 * _tmp2059 - _tmp1224 * _tmp2059 + _tmp2057 * _tmp609; + const Scalar _tmp2061 = _tmp1159 * _tmp2030 - _tmp1160 * _tmp2029 + _tmp2028 * _tmp759; + const Scalar _tmp2062 = _tmp103 * _tmp1696 - _tmp1695 * _tmp96; + const Scalar _tmp2063 = _tmp103 * _tmp1692 - _tmp1693 * _tmp81; + const Scalar _tmp2064 = _tmp1497 * _tmp2063 + _tmp2062 * _tmp987; + const Scalar _tmp2065 = _tmp2063 * _tmp549 - _tmp2064 * _tmp991 + _tmp2064 * _tmp994; + const Scalar _tmp2066 = -_tmp1230 * _tmp2064 + _tmp1231 * _tmp2064 + _tmp2062 * _tmp549; + const Scalar _tmp2067 = _tmp1900 + _tmp74; + const Scalar _tmp2068 = _tmp1523 * priors_1_0_sqrt_info(0, 3) + + _tmp2067 * priors_1_0_sqrt_info(0, 5) + + _tmp834 * priors_1_0_sqrt_info(0, 4); + const Scalar _tmp2069 = _tmp1309 * _tmp2043 - _tmp1310 * _tmp2043 + _tmp2042 * _tmp669; + const Scalar _tmp2070 = _tmp103 * _tmp1833 - _tmp1834 * _tmp96; + const Scalar _tmp2071 = _tmp103 * _tmp1831 - _tmp1830 * _tmp81; + const Scalar _tmp2072 = _tmp1034 * _tmp2070 + _tmp1364 * _tmp2071; + const Scalar _tmp2073 = -_tmp1244 * _tmp2072 + _tmp1245 * _tmp2072 + _tmp2070 * _tmp279; + const Scalar _tmp2074 = _tmp103 * landmarks_15_; + const Scalar _tmp2075 = _tmp1180 * _tmp2074 - _tmp1744 * _tmp96; + const Scalar _tmp2076 = _tmp1187 * _tmp2074 - _tmp1746 * _tmp81; + const Scalar _tmp2077 = _tmp1182 * _tmp2075 + _tmp1503 * _tmp2076; + const Scalar _tmp2078 = _tmp1276 * _tmp2077 - _tmp1277 * _tmp2077 + _tmp2075 * _tmp639; + const Scalar _tmp2079 = _tmp1523 * priors_1_0_sqrt_info(3, 3) + + _tmp2067 * priors_1_0_sqrt_info(3, 5) + + _tmp834 * priors_1_0_sqrt_info(3, 4); + const Scalar _tmp2080 = + _tmp2067 * priors_1_0_sqrt_info(4, 5) + _tmp834 * priors_1_0_sqrt_info(4, 4); + const Scalar _tmp2081 = _tmp2001 * _tmp519 - _tmp2002 * _tmp920 + _tmp2002 * _tmp921; + const Scalar _tmp2082 = -_tmp1055 * _tmp2053 + _tmp1057 * _tmp2053 + _tmp2051 * _tmp339; + const Scalar _tmp2083 = _tmp103 * _tmp1769 - _tmp1770 * _tmp81; + const Scalar _tmp2084 = _tmp1765 * _tmp1989 - _tmp1767 * _tmp96; + const Scalar _tmp2085 = _tmp1200 * _tmp2084 + _tmp1345 * _tmp2083; + const Scalar _tmp2086 = -_tmp1240 * _tmp2085 + _tmp1241 * _tmp2085 + _tmp2083 * _tmp429; + const Scalar _tmp2087 = _tmp10 * priors_0_1_sqrt_info(0, 4) + _tmp48 * priors_0_1_sqrt_info(0, 5) + _tmp61 * priors_0_1_sqrt_info(0, 3); - const Scalar _tmp2131 = _tmp1229 * _tmp2128 - _tmp1231 * _tmp2128 + _tmp2127 * _tmp433; - const Scalar _tmp2132 = _tmp1001 * _tmp2089 + _tmp2087 * _tmp223 - _tmp2089 * _tmp999; - const Scalar _tmp2133 = _tmp10 * priors_0_1_sqrt_info(3, 4) + + const Scalar _tmp2088 = _tmp1204 * _tmp2085 - _tmp1206 * _tmp2085 + _tmp2084 * _tmp429; + const Scalar _tmp2089 = _tmp1523 * priors_1_0_sqrt_info(1, 3) + + _tmp2067 * priors_1_0_sqrt_info(1, 5) + + _tmp834 * priors_1_0_sqrt_info(1, 4); + const Scalar _tmp2090 = _tmp2046 * _tmp219 + _tmp2048 * _tmp950 - _tmp2049 * _tmp947; + const Scalar _tmp2091 = _tmp2010 * _tmp489 + _tmp2011 * _tmp963 - _tmp2011 * _tmp965; + const Scalar _tmp2092 = -_tmp1186 * _tmp2077 + _tmp1189 * _tmp2077 + _tmp2076 * _tmp639; + const Scalar _tmp2093 = _tmp1095 * _tmp2023 - _tmp1097 * _tmp2023 + _tmp189 * _tmp2022; + const Scalar _tmp2094 = -_tmp1170 * _tmp2019 + _tmp1174 * _tmp2018 + _tmp2017 * _tmp309; + const Scalar _tmp2095 = _tmp1038 * _tmp2072 - _tmp1041 * _tmp2072 + _tmp2071 * _tmp279; + const Scalar _tmp2096 = _tmp2058 * _tmp609 - _tmp2059 * _tmp846 + _tmp2059 * _tmp851; + const Scalar _tmp2097 = _tmp10 * priors_0_1_sqrt_info(4, 4) + _tmp48 * priors_0_1_sqrt_info(4, 5); + const Scalar _tmp2098 = _tmp1141 * _tmp1997 - _tmp1143 * _tmp1997 + _tmp1996 * _tmp249; + const Scalar _tmp2099 = _tmp10 * priors_0_1_sqrt_info(3, 4) + _tmp48 * priors_0_1_sqrt_info(3, 5) + _tmp61 * priors_0_1_sqrt_info(3, 3); - const Scalar _tmp2134 = _tmp1014 * _tmp2051 - _tmp1017 * _tmp2051 + _tmp2050 * _tmp493; - const Scalar _tmp2135 = -_tmp1208 * _tmp2122 + _tmp1212 * _tmp2122 + _tmp2121 * _tmp643; - const Scalar _tmp2136 = _tmp1119 * _tmp2063 - _tmp1121 * _tmp2063 + _tmp193 * _tmp2062; - const Scalar _tmp2137 = _tmp1552 * priors_1_0_sqrt_info(3, 3) + - _tmp2107 * priors_1_0_sqrt_info(3, 5) + - _tmp895 * priors_1_0_sqrt_info(3, 4); - const Scalar _tmp2138 = -_tmp1195 * _tmp2058 + _tmp1197 * _tmp2058 + _tmp2057 * _tmp313; - const Scalar _tmp2139 = _tmp1061 * _tmp2117 - _tmp1065 * _tmp2117 + _tmp2116 * _tmp283; - const Scalar _tmp2140 = _tmp2098 * _tmp613 - _tmp2099 * _tmp905 + _tmp2099 * _tmp908; - const Scalar _tmp2141 = _tmp1170 * _tmp2039 - _tmp1172 * _tmp2039 + _tmp2038 * _tmp253; - const Scalar _tmp2142 = _tmp1092 * _tmp2034 - _tmp1094 * _tmp2034 + _tmp2033 * _tmp703; - const Scalar _tmp2143 = _tmp1552 * priors_1_0_sqrt_info(2, 3) + - _tmp2107 * priors_1_0_sqrt_info(2, 5) + - _tmp895 * priors_1_0_sqrt_info(2, 4); - const Scalar _tmp2144 = _tmp1552 * priors_1_0_sqrt_info(5, 3) + - _tmp2107 * priors_1_0_sqrt_info(5, 5) + - _tmp895 * priors_1_0_sqrt_info(5, 4); - const Scalar _tmp2145 = _tmp100 * _tmp153 + _tmp160 * _tmp94 + _tmp164 * _tmp97; - const Scalar _tmp2146 = _tmp2145 * views_1_calibration(1, 0); - const Scalar _tmp2147 = _tmp104 * _tmp164 + _tmp106 * _tmp160 + _tmp108 * _tmp153; - const Scalar _tmp2148 = _tmp2147 * _tmp898; - const Scalar _tmp2149 = -_tmp1111 * _tmp2148 + _tmp171 * _tmp2146; - const Scalar _tmp2150 = _tmp153 * _tmp77 + _tmp160 * _tmp87 + _tmp164 * _tmp83; - const Scalar _tmp2151 = _tmp2150 * views_1_calibration(0, 0); - const Scalar _tmp2152 = _tmp2147 * _tmp924; - const Scalar _tmp2153 = -_tmp1114 * _tmp2152 + _tmp171 * _tmp2151; - const Scalar _tmp2154 = _tmp1115 * _tmp2149 + _tmp1444 * _tmp2153; - const Scalar _tmp2155 = -_tmp1238 * _tmp2154 + _tmp1239 * _tmp2154 + _tmp193 * _tmp2153; - const Scalar _tmp2156 = std::pow(matches_0_0_weight, Scalar(2)) / std::pow(_tmp767, Scalar(2)); - const Scalar _tmp2157 = _tmp1119 * _tmp2154 - _tmp1121 * _tmp2154 + _tmp193 * _tmp2149; - const Scalar _tmp2158 = std::pow(matches_0_1_weight, Scalar(2)) / std::pow(_tmp770, Scalar(2)); - const Scalar _tmp2159 = _tmp209 * _tmp2151 - _tmp2152 * _tmp991; - const Scalar _tmp2160 = _tmp209 * _tmp2146 - _tmp2148 * _tmp992; - const Scalar _tmp2161 = _tmp1402 * _tmp2159 + _tmp2160 * _tmp995; - const Scalar _tmp2162 = _tmp1322 * _tmp2161 - _tmp1324 * _tmp2161 + _tmp2159 * _tmp223; - const Scalar _tmp2163 = _tmp2161 * _tmp997; - const Scalar _tmp2164 = _tmp1000 * _tmp2163 + _tmp2160 * _tmp223 - _tmp2163 * _tmp998; - const Scalar _tmp2165 = std::pow(matches_0_2_weight, Scalar(2)) / std::pow(_tmp773, Scalar(2)); - const Scalar _tmp2166 = -_tmp1162 * _tmp2152 + _tmp2151 * _tmp239; - const Scalar _tmp2167 = -_tmp1163 * _tmp2148 + _tmp2146 * _tmp239; - const Scalar _tmp2168 = _tmp1166 * _tmp2167 + _tmp1511 * _tmp2166; - const Scalar _tmp2169 = _tmp1170 * _tmp2168 - _tmp1172 * _tmp2168 + _tmp2166 * _tmp253; - const Scalar _tmp2170 = _tmp1275 * _tmp2168 - _tmp1276 * _tmp2168 + _tmp2167 * _tmp253; - const Scalar _tmp2171 = -_tmp1053 * _tmp2152 + _tmp2151 * _tmp269; - const Scalar _tmp2172 = -_tmp1054 * _tmp2148 + _tmp2146 * _tmp269; - const Scalar _tmp2173 = - (Scalar(1) / Scalar(2)) * _tmp1057 * _tmp2172 + (Scalar(1) / Scalar(2)) * _tmp1412 * _tmp2171; - const Scalar _tmp2174 = _tmp2173 * _tmp271; - const Scalar _tmp2175 = _tmp1059 * _tmp2174 - _tmp1063 * _tmp2174 + _tmp2171 * _tmp283; - const Scalar _tmp2176 = std::pow(matches_0_3_weight, Scalar(2)) / std::pow(_tmp776, Scalar(2)); - const Scalar _tmp2177 = _tmp2173 * _tmp275; - const Scalar _tmp2178 = _tmp1059 * _tmp2177 - _tmp1063 * _tmp2177 + _tmp2172 * _tmp283; - const Scalar _tmp2179 = std::pow(matches_0_4_weight, Scalar(2)) / std::pow(_tmp779, Scalar(2)); - const Scalar _tmp2180 = -_tmp1187 * _tmp2148 * _tmp303 + _tmp2146 * _tmp299; - const Scalar _tmp2181 = -_tmp1187 * _tmp2152 * _tmp295 + _tmp2151 * _tmp299; - const Scalar _tmp2182 = _tmp1191 * _tmp2180 + _tmp1417 * _tmp2181; - const Scalar _tmp2183 = _tmp1196 * _tmp2182; - const Scalar _tmp2184 = _tmp1194 * _tmp2183 - _tmp1195 * _tmp2182 + _tmp2180 * _tmp313; - const Scalar _tmp2185 = _tmp1282 * _tmp2183 - _tmp1284 * _tmp2182 + _tmp2181 * _tmp313; - const Scalar _tmp2186 = -_tmp1455 * _tmp2152 + _tmp2151 * _tmp329; - const Scalar _tmp2187 = -_tmp1454 * _tmp2148 + _tmp2146 * _tmp329; - const Scalar _tmp2188 = _tmp1071 * _tmp2187 + _tmp1458 * _tmp2186; - const Scalar _tmp2189 = -_tmp1076 * _tmp2188 + _tmp1460 * _tmp2188 + _tmp2186 * _tmp343; - const Scalar _tmp2190 = -_tmp1279 * _tmp2188 + _tmp1538 * _tmp2188 + _tmp2187 * _tmp343; - const Scalar _tmp2191 = std::pow(matches_0_5_weight, Scalar(2)) / std::pow(_tmp782, Scalar(2)); - const Scalar _tmp2192 = -_tmp1023 * _tmp2148 + _tmp2146 * _tmp359; - const Scalar _tmp2193 = -_tmp1027 * _tmp2152 + _tmp2151 * _tmp359; - const Scalar _tmp2194 = _tmp1026 * _tmp2192 + _tmp1466 * _tmp2193; - const Scalar _tmp2195 = _tmp1034 * _tmp2194 - _tmp1497 * _tmp2194 + _tmp2193 * _tmp373; - const Scalar _tmp2196 = _tmp1256 * _tmp2194 - _tmp1468 * _tmp2194 + _tmp2192 * _tmp373; - const Scalar _tmp2197 = std::pow(matches_0_6_weight, Scalar(2)) / std::pow(_tmp785, Scalar(2)); - const Scalar _tmp2198 = std::pow(matches_0_7_weight, Scalar(2)) / std::pow(_tmp788, Scalar(2)); - const Scalar _tmp2199 = -_tmp1129 * _tmp2152 + _tmp2151 * _tmp389; - const Scalar _tmp2200 = -_tmp1125 * _tmp2148 + _tmp2146 * _tmp389; - const Scalar _tmp2201 = _tmp1128 * _tmp2200 + _tmp1432 * _tmp2199; - const Scalar _tmp2202 = _tmp1215 * _tmp2201 - _tmp1216 * _tmp2201 + _tmp2199 * _tmp403; - const Scalar _tmp2203 = _tmp1134 * _tmp2201 - _tmp1136 * _tmp2201 + _tmp2200 * _tmp403; - const Scalar _tmp2204 = -_tmp1220 * _tmp2152 + _tmp2151 * _tmp419; - const Scalar _tmp2205 = -_tmp1222 * _tmp2148 + _tmp2146 * _tmp419; - const Scalar _tmp2206 = _tmp1225 * _tmp2205 + _tmp1360 * _tmp2204; - const Scalar _tmp2207 = -_tmp1268 * _tmp2206 + _tmp1269 * _tmp2206 + _tmp2204 * _tmp433; - const Scalar _tmp2208 = std::pow(matches_0_8_weight, Scalar(2)) / std::pow(_tmp791, Scalar(2)); - const Scalar _tmp2209 = _tmp1229 * _tmp2206 - _tmp1231 * _tmp2206 + _tmp2205 * _tmp433; - const Scalar _tmp2210 = -_tmp1290 * _tmp2147 + _tmp2146 * _tmp449; - const Scalar _tmp2211 = -_tmp1288 * _tmp2152 + _tmp2151 * _tmp449; - const Scalar _tmp2212 = _tmp1292 * _tmp2210 + _tmp1407 * _tmp2211; - const Scalar _tmp2213 = _tmp1296 * _tmp2212 - _tmp1298 * _tmp2212 + _tmp2210 * _tmp463; - const Scalar _tmp2214 = std::pow(matches_0_9_weight, Scalar(2)) / std::pow(_tmp794, Scalar(2)); - const Scalar _tmp2215 = _tmp1318 * _tmp2212 - _tmp1319 * _tmp2212 + _tmp2211 * _tmp463; - const Scalar _tmp2216 = -_tmp1005 * _tmp2152 + _tmp2151 * _tmp479; - const Scalar _tmp2217 = -_tmp1006 * _tmp2148 + _tmp2146 * _tmp479; - const Scalar _tmp2218 = _tmp1009 * _tmp2217 + _tmp1438 * _tmp2216; - const Scalar _tmp2219 = -_tmp1333 * _tmp2218 + _tmp1334 * _tmp2218 + _tmp2216 * _tmp493; - const Scalar _tmp2220 = _tmp1014 * _tmp2218 - _tmp1017 * _tmp2218 + _tmp2217 * _tmp493; - const Scalar _tmp2221 = std::pow(matches_0_10_weight, Scalar(2)) / std::pow(_tmp797, Scalar(2)); - const Scalar _tmp2222 = std::pow(matches_0_11_weight, Scalar(2)) / std::pow(_tmp800, Scalar(2)); - const Scalar _tmp2223 = _tmp2146 * _tmp509 - _tmp2148 * _tmp913; - const Scalar _tmp2224 = _tmp2151 * _tmp509 - _tmp2152 * _tmp912; - const Scalar _tmp2225 = _tmp1352 * _tmp2224 + _tmp2223 * _tmp916; - const Scalar _tmp2226 = _tmp2223 * _tmp523 - _tmp2225 * _tmp928 + _tmp2225 * _tmp929; - const Scalar _tmp2227 = _tmp2224 * _tmp523 - _tmp2225 * _tmp920 + _tmp2225 * _tmp923; - const Scalar _tmp2228 = -_tmp1038 * _tmp2152 + _tmp2151 * _tmp539; - const Scalar _tmp2229 = -_tmp1039 * _tmp2148 + _tmp2146 * _tmp539; - const Scalar _tmp2230 = _tmp1042 * _tmp2229 + _tmp1522 * _tmp2228; - const Scalar _tmp2231 = -_tmp1259 * _tmp2230 + _tmp1260 * _tmp2230 + _tmp2229 * _tmp553; - const Scalar _tmp2232 = std::pow(matches_0_12_weight, Scalar(2)) / std::pow(_tmp803, Scalar(2)); - const Scalar _tmp2233 = -_tmp1046 * _tmp2230 + _tmp1049 * _tmp2230 + _tmp2228 * _tmp553; - const Scalar _tmp2234 = std::pow(matches_0_13_weight, Scalar(2)) / std::pow(_tmp806, Scalar(2)); - const Scalar _tmp2235 = _tmp2146 * _tmp569 - _tmp2148 * _tmp976; - const Scalar _tmp2236 = _tmp2151 * _tmp569 - _tmp2152 * _tmp975; - const Scalar _tmp2237 = _tmp1502 * _tmp2236 + _tmp2235 * _tmp979; - const Scalar _tmp2238 = -_tmp1247 * _tmp2237 + _tmp1248 * _tmp2237 + _tmp2236 * _tmp583; - const Scalar _tmp2239 = _tmp2235 * _tmp583 - _tmp2237 * _tmp983 + _tmp2237 * _tmp985; - const Scalar _tmp2240 = _tmp2151 * _tmp599 - _tmp2152 * _tmp595 * _tmp896; - const Scalar _tmp2241 = -_tmp1462 * _tmp2147 + _tmp2146 * _tmp599; - const Scalar _tmp2242 = _tmp1450 * _tmp2240 + _tmp2241 * _tmp901; - const Scalar _tmp2243 = _tmp1252 * _tmp2242 - _tmp1253 * _tmp2242 + _tmp2240 * _tmp613; - const Scalar _tmp2244 = _tmp2241 * _tmp613 - _tmp2242 * _tmp905 + _tmp2242 * _tmp908; - const Scalar _tmp2245 = std::pow(matches_0_14_weight, Scalar(2)) / std::pow(_tmp809, Scalar(2)); - const Scalar _tmp2246 = std::pow(matches_0_15_weight, Scalar(2)) / std::pow(_tmp812, Scalar(2)); - const Scalar _tmp2247 = -_tmp1201 * _tmp2148 + _tmp2146 * _tmp629; - const Scalar _tmp2248 = -_tmp1210 * _tmp2147 + _tmp2150 * _tmp630; - const Scalar _tmp2249 = _tmp1204 * _tmp2247 + _tmp1531 * _tmp2248; - const Scalar _tmp2250 = -_tmp1208 * _tmp2249 + _tmp1212 * _tmp2249 + _tmp2248 * _tmp643; - const Scalar _tmp2251 = _tmp1301 * _tmp2249 - _tmp1302 * _tmp2249 + _tmp2247 * _tmp643; - const Scalar _tmp2252 = std::pow(matches_0_16_weight, Scalar(2)) / std::pow(_tmp815, Scalar(2)); - const Scalar _tmp2253 = -_tmp1304 * _tmp2152 * _tmp655 + _tmp2151 * _tmp659; - const Scalar _tmp2254 = -_tmp1850 * _tmp2147 + _tmp2146 * _tmp659; - const Scalar _tmp2255 = _tmp1308 * _tmp2254 + _tmp1425 * _tmp2253; - const Scalar _tmp2256 = _tmp1312 * _tmp2255 - _tmp1314 * _tmp2255 + _tmp2254 * _tmp673; - const Scalar _tmp2257 = _tmp1329 * _tmp2255 - _tmp1330 * _tmp2255 + _tmp2253 * _tmp673; - const Scalar _tmp2258 = std::pow(matches_0_17_weight, Scalar(2)) / std::pow(_tmp818, Scalar(2)); - const Scalar _tmp2259 = -_tmp1087 * _tmp2152 + _tmp2151 * _tmp689; - const Scalar _tmp2260 = -_tmp1083 * _tmp2148 + _tmp2146 * _tmp689; - const Scalar _tmp2261 = _tmp1086 * _tmp2260 + _tmp1347 * _tmp2259; - const Scalar _tmp2262 = _tmp1093 * _tmp2261; - const Scalar _tmp2263 = -_tmp1090 * _tmp2262 + _tmp1092 * _tmp2261 + _tmp2259 * _tmp703; - const Scalar _tmp2264 = -_tmp1241 * _tmp2262 + _tmp1243 * _tmp2261 + _tmp2260 * _tmp703; - const Scalar _tmp2265 = -_tmp1145 * _tmp2148 + _tmp2146 * _tmp719; - const Scalar _tmp2266 = -_tmp1149 * _tmp2152 + _tmp2151 * _tmp719; - const Scalar _tmp2267 = _tmp1148 * _tmp2265 + _tmp1493 * _tmp2266; - const Scalar _tmp2268 = _tmp1177 * _tmp2267 - _tmp1178 * _tmp2267 + _tmp2266 * _tmp733; - const Scalar _tmp2269 = std::pow(matches_0_18_weight, Scalar(2)) / std::pow(_tmp821, Scalar(2)); - const Scalar _tmp2270 = -_tmp1153 * _tmp2267 + _tmp1156 * _tmp2267 + _tmp2265 * _tmp733; - const Scalar _tmp2271 = std::pow(matches_0_19_weight, Scalar(2)) / std::pow(_tmp824, Scalar(2)); - const Scalar _tmp2272 = -_tmp1479 * _tmp2147 + _tmp2145 * _tmp754; - const Scalar _tmp2273 = -_tmp1471 * _tmp2152 + _tmp2151 * _tmp749; - const Scalar _tmp2274 = _tmp1100 * _tmp2272 + _tmp1474 * _tmp2273; - const Scalar _tmp2275 = _tmp1103 * _tmp2274; - const Scalar _tmp2276 = -_tmp1181 * _tmp2275 + _tmp1182 * _tmp2274 + _tmp2273 * _tmp763; - const Scalar _tmp2277 = -_tmp1104 * _tmp2275 + _tmp1107 * _tmp2274 + _tmp2272 * _tmp763; + const Scalar _tmp2100 = _tmp1068 * _tmp1992 - _tmp1070 * _tmp1992 + _tmp1991 * _tmp699; + const Scalar _tmp2101 = _tmp10 * priors_0_1_sqrt_info(1, 4) + + _tmp48 * priors_0_1_sqrt_info(1, 5) + _tmp61 * priors_0_1_sqrt_info(1, 3); + const Scalar _tmp2102 = _tmp1523 * priors_1_0_sqrt_info(2, 3) + + _tmp2067 * priors_1_0_sqrt_info(2, 5) + + _tmp834 * priors_1_0_sqrt_info(2, 4); + const Scalar _tmp2103 = _tmp149 * _tmp99 + _tmp156 * _tmp93 + _tmp160 * _tmp96; + const Scalar _tmp2104 = _tmp2103 * views_1_calibration(1, 0); + const Scalar _tmp2105 = (Scalar(1) / Scalar(2)) * _tmp103 * _tmp160 + + (Scalar(1) / Scalar(2)) * _tmp105 * _tmp156 + + (Scalar(1) / Scalar(2)) * _tmp107 * _tmp149; + const Scalar _tmp2106 = _tmp2105 * views_1_calibration(1, 0); + const Scalar _tmp2107 = -_tmp1087 * _tmp2106 + _tmp167 * _tmp2104; + const Scalar _tmp2108 = _tmp149 * _tmp76 + _tmp156 * _tmp86 + _tmp160 * _tmp81; + const Scalar _tmp2109 = _tmp2108 * views_1_calibration(0, 0); + const Scalar _tmp2110 = _tmp2105 * views_1_calibration(0, 0); + const Scalar _tmp2111 = -_tmp1090 * _tmp2110 + _tmp167 * _tmp2109; + const Scalar _tmp2112 = _tmp1091 * _tmp2107 + _tmp1408 * _tmp2111; + const Scalar _tmp2113 = _tmp1209 * _tmp2112; + const Scalar _tmp2114 = _tmp1094 * _tmp2113 - _tmp1096 * _tmp2113 + _tmp189 * _tmp2111; + const Scalar _tmp2115 = std::pow(matches_0_0_weight, Scalar(2)) / std::pow(_tmp763, Scalar(2)); + const Scalar _tmp2116 = _tmp1095 * _tmp2112 - _tmp1097 * _tmp2112 + _tmp189 * _tmp2107; + const Scalar _tmp2117 = std::pow(matches_0_1_weight, Scalar(2)) / std::pow(_tmp766, Scalar(2)); + const Scalar _tmp2118 = _tmp205 * _tmp2109 - _tmp2110 * _tmp940; + const Scalar _tmp2119 = _tmp205 * _tmp2104 - _tmp2106 * _tmp941; + const Scalar _tmp2120 = _tmp1354 * _tmp2118 + _tmp2119 * _tmp944; + const Scalar _tmp2121 = _tmp1299 * _tmp2120 - _tmp1301 * _tmp2120 + _tmp2118 * _tmp219; + const Scalar _tmp2122 = _tmp2119 * _tmp219 - _tmp2120 * _tmp948 + _tmp2120 * _tmp950; + const Scalar _tmp2123 = std::pow(matches_0_2_weight, Scalar(2)) / std::pow(_tmp769, Scalar(2)); + const Scalar _tmp2124 = -_tmp1132 * _tmp2110 + _tmp2109 * _tmp235; + const Scalar _tmp2125 = -_tmp1134 * _tmp2106 + _tmp2104 * _tmp235; + const Scalar _tmp2126 = _tmp1137 * _tmp2125 + _tmp1489 * _tmp2124; + const Scalar _tmp2127 = _tmp1141 * _tmp2126 - _tmp1143 * _tmp2126 + _tmp2124 * _tmp249; + const Scalar _tmp2128 = _tmp1248 * _tmp2126 - _tmp1249 * _tmp2126 + _tmp2125 * _tmp249; + const Scalar _tmp2129 = -_tmp1030 * _tmp2110 + _tmp2109 * _tmp265; + const Scalar _tmp2130 = -_tmp1031 * _tmp2106 + _tmp2104 * _tmp265; + const Scalar _tmp2131 = _tmp1034 * _tmp2130 + _tmp1364 * _tmp2129; + const Scalar _tmp2132 = _tmp1038 * _tmp2131 - _tmp1041 * _tmp2131 + _tmp2129 * _tmp279; + const Scalar _tmp2133 = std::pow(matches_0_3_weight, Scalar(2)) / std::pow(_tmp772, Scalar(2)); + const Scalar _tmp2134 = _tmp1243 * _tmp2131; + const Scalar _tmp2135 = _tmp1037 * _tmp2134 - _tmp1040 * _tmp2134 + _tmp2130 * _tmp279; + const Scalar _tmp2136 = std::pow(matches_0_4_weight, Scalar(2)) / std::pow(_tmp775, Scalar(2)); + const Scalar _tmp2137 = -_tmp1164 * _tmp2106 + _tmp2104 * _tmp295; + const Scalar _tmp2138 = -_tmp1168 * _tmp2110 + _tmp2109 * _tmp295; + const Scalar _tmp2139 = _tmp1167 * _tmp2137 + _tmp1369 * _tmp2138; + const Scalar _tmp2140 = -_tmp1172 * _tmp2139 + _tmp1174 * _tmp2139 + _tmp2137 * _tmp309; + const Scalar _tmp2141 = _tmp1258 * _tmp2139 - _tmp1259 * _tmp2139 + _tmp2138 * _tmp309; + const Scalar _tmp2142 = -_tmp1049 * _tmp2110 + _tmp2109 * _tmp325; + const Scalar _tmp2143 = -_tmp1045 * _tmp2106 + _tmp2104 * _tmp325; + const Scalar _tmp2144 = _tmp1048 * _tmp2143 + _tmp1420 * _tmp2142; + const Scalar _tmp2145 = -_tmp1055 * _tmp2144 + _tmp1057 * _tmp2144 + _tmp2142 * _tmp339; + const Scalar _tmp2146 = _tmp1252 * _tmp2144 - _tmp1253 * _tmp2144 + _tmp2143 * _tmp339; + const Scalar _tmp2147 = std::pow(matches_0_5_weight, Scalar(2)) / std::pow(_tmp778, Scalar(2)); + const Scalar _tmp2148 = -_tmp1430 * _tmp2106 + _tmp2104 * _tmp355; + const Scalar _tmp2149 = -_tmp1426 * _tmp2110 + _tmp2109 * _tmp355; + const Scalar _tmp2150 = _tmp1429 * _tmp2149 + _tmp2148 * _tmp972; + const Scalar _tmp2151 = _tmp2150 * _tmp975; + const Scalar _tmp2152 = _tmp2149 * _tmp369 + _tmp2150 * _tmp979 - _tmp2151 * _tmp977; + const Scalar _tmp2153 = -_tmp1226 * _tmp2151 + _tmp1227 * _tmp2150 + _tmp2148 * _tmp369; + const Scalar _tmp2154 = std::pow(matches_0_6_weight, Scalar(2)) / std::pow(_tmp781, Scalar(2)); + const Scalar _tmp2155 = std::pow(matches_0_7_weight, Scalar(2)) / std::pow(_tmp784, Scalar(2)); + const Scalar _tmp2156 = -_tmp1105 * _tmp2110 + _tmp2109 * _tmp385; + const Scalar _tmp2157 = -_tmp1101 * _tmp2106 + _tmp2104 * _tmp385; + const Scalar _tmp2158 = _tmp1104 * _tmp2157 + _tmp1386 * _tmp2156; + const Scalar _tmp2159 = _tmp1192 * _tmp2158 - _tmp1193 * _tmp2158 + _tmp2156 * _tmp399; + const Scalar _tmp2160 = _tmp1110 * _tmp2158 - _tmp1112 * _tmp2158 + _tmp2157 * _tmp399; + const Scalar _tmp2161 = -_tmp1342 * _tmp2105 + _tmp2109 * _tmp415; + const Scalar _tmp2162 = -_tmp1340 * _tmp2106 + _tmp2104 * _tmp415; + const Scalar _tmp2163 = _tmp1200 * _tmp2162 + _tmp1345 * _tmp2161; + const Scalar _tmp2164 = -_tmp1240 * _tmp2163 + _tmp1241 * _tmp2163 + _tmp2161 * _tmp429; + const Scalar _tmp2165 = std::pow(matches_0_8_weight, Scalar(2)) / std::pow(_tmp787, Scalar(2)); + const Scalar _tmp2166 = _tmp1204 * _tmp2163 - _tmp1206 * _tmp2163 + _tmp2162 * _tmp429; + const Scalar _tmp2167 = -_tmp1264 * _tmp2106 + _tmp2104 * _tmp445; + const Scalar _tmp2168 = -_tmp1263 * _tmp2110 + _tmp2109 * _tmp445; + const Scalar _tmp2169 = _tmp1267 * _tmp2167 + _tmp1359 * _tmp2168; + const Scalar _tmp2170 = _tmp1270 * _tmp2169; + const Scalar _tmp2171 = _tmp1269 * _tmp2170 - _tmp1273 * _tmp2169 + _tmp2167 * _tmp459; + const Scalar _tmp2172 = std::pow(matches_0_9_weight, Scalar(2)) / std::pow(_tmp790, Scalar(2)); + const Scalar _tmp2173 = _tmp1294 * _tmp2170 - _tmp1296 * _tmp2169 + _tmp2168 * _tmp459; + const Scalar _tmp2174 = _tmp2109 * _tmp475 - _tmp2110 * _tmp954; + const Scalar _tmp2175 = _tmp2103 * _tmp480 - _tmp2106 * _tmp956; + const Scalar _tmp2176 = _tmp1393 * _tmp2174 + _tmp2175 * _tmp959; + const Scalar _tmp2177 = _tmp2176 * _tmp962; + const Scalar _tmp2178 = _tmp1312 * _tmp2177 - _tmp1313 * _tmp2176 + _tmp2174 * _tmp489; + const Scalar _tmp2179 = _tmp2175 * _tmp489 - _tmp2176 * _tmp965 + _tmp2177 * _tmp961; + const Scalar _tmp2180 = std::pow(matches_0_10_weight, Scalar(2)) / std::pow(_tmp793, Scalar(2)); + const Scalar _tmp2181 = std::pow(matches_0_11_weight, Scalar(2)) / std::pow(_tmp796, Scalar(2)); + const Scalar _tmp2182 = _tmp2104 * _tmp505 - _tmp2106 * _tmp856; + const Scalar _tmp2183 = _tmp2109 * _tmp505 - _tmp2110 * _tmp855; + const Scalar _tmp2184 = _tmp1334 * _tmp2183 + _tmp2182 * _tmp859; + const Scalar _tmp2185 = _tmp2182 * _tmp519 - _tmp2184 * _tmp920 + _tmp2184 * _tmp921; + const Scalar _tmp2186 = _tmp2183 * _tmp519 - _tmp2184 * _tmp863 + _tmp2184 * _tmp865; + const Scalar _tmp2187 = _tmp2109 * _tmp535 - _tmp2110 * _tmp983; + const Scalar _tmp2188 = _tmp2104 * _tmp535 - _tmp2106 * _tmp984; + const Scalar _tmp2189 = _tmp1497 * _tmp2187 + _tmp2188 * _tmp987; + const Scalar _tmp2190 = -_tmp1230 * _tmp2189 + _tmp1231 * _tmp2189 + _tmp2188 * _tmp549; + const Scalar _tmp2191 = std::pow(matches_0_12_weight, Scalar(2)) / std::pow(_tmp799, Scalar(2)); + const Scalar _tmp2192 = _tmp2187 * _tmp549 - _tmp2189 * _tmp991 + _tmp2189 * _tmp994; + const Scalar _tmp2193 = std::pow(matches_0_13_weight, Scalar(2)) / std::pow(_tmp802, Scalar(2)); + const Scalar _tmp2194 = _tmp2104 * _tmp565 - _tmp2106 * _tmp926; + const Scalar _tmp2195 = _tmp2109 * _tmp565 - _tmp2110 * _tmp925; + const Scalar _tmp2196 = _tmp1483 * _tmp2195 + _tmp2194 * _tmp929; + const Scalar _tmp2197 = _tmp1220 * _tmp2196 - _tmp1508 * _tmp2196 + _tmp2195 * _tmp579; + const Scalar _tmp2198 = -_tmp1485 * _tmp2196 + _tmp2194 * _tmp579 + _tmp2196 * _tmp936; + const Scalar _tmp2199 = _tmp2109 * _tmp595 - _tmp2110 * _tmp841; + const Scalar _tmp2200 = _tmp2104 * _tmp595 - _tmp2106 * _tmp838; + const Scalar _tmp2201 = _tmp1413 * _tmp2199 + _tmp2200 * _tmp842; + const Scalar _tmp2202 = _tmp1223 * _tmp2201 - _tmp1224 * _tmp2201 + _tmp2199 * _tmp609; + const Scalar _tmp2203 = _tmp2200 * _tmp609 - _tmp2201 * _tmp846 + _tmp2201 * _tmp851; + const Scalar _tmp2204 = std::pow(matches_0_14_weight, Scalar(2)) / std::pow(_tmp805, Scalar(2)); + const Scalar _tmp2205 = std::pow(matches_0_15_weight, Scalar(2)) / std::pow(_tmp808, Scalar(2)); + const Scalar _tmp2206 = -_tmp1179 * _tmp2106 + _tmp2103 * _tmp630; + const Scalar _tmp2207 = -_tmp1178 * _tmp2110 + _tmp2108 * _tmp626; + const Scalar _tmp2208 = _tmp1182 * _tmp2206 + _tmp1503 * _tmp2207; + const Scalar _tmp2209 = -_tmp1186 * _tmp2208 + _tmp1189 * _tmp2208 + _tmp2207 * _tmp639; + const Scalar _tmp2210 = _tmp1276 * _tmp2208 - _tmp1277 * _tmp2208 + _tmp2206 * _tmp639; + const Scalar _tmp2211 = std::pow(matches_0_16_weight, Scalar(2)) / std::pow(_tmp811, Scalar(2)); + const Scalar _tmp2212 = -_tmp1285 * _tmp2110 + _tmp2109 * _tmp655; + const Scalar _tmp2213 = -_tmp1281 * _tmp2106 + _tmp2104 * _tmp655; + const Scalar _tmp2214 = _tmp1284 * _tmp2213 + _tmp1379 * _tmp2212; + const Scalar _tmp2215 = _tmp1289 * _tmp2214 - _tmp1291 * _tmp2214 + _tmp2213 * _tmp669; + const Scalar _tmp2216 = _tmp1309 * _tmp2214 - _tmp1310 * _tmp2214 + _tmp2212 * _tmp669; + const Scalar _tmp2217 = std::pow(matches_0_17_weight, Scalar(2)) / std::pow(_tmp814, Scalar(2)); + const Scalar _tmp2218 = -_tmp1059 * _tmp2110 * _tmp681 + _tmp2109 * _tmp685; + const Scalar _tmp2219 = -_tmp1319 * _tmp2106 + _tmp2104 * _tmp685; + const Scalar _tmp2220 = _tmp1062 * _tmp2219 + _tmp1328 * _tmp2218; + const Scalar _tmp2221 = _tmp1066 * _tmp2220; + const Scalar _tmp2222 = _tmp1067 * _tmp2221 - _tmp1069 * _tmp2221 + _tmp2218 * _tmp699; + const Scalar _tmp2223 = -_tmp1214 * _tmp2220 + _tmp1215 * _tmp2220 + _tmp2219 * _tmp699; + const Scalar _tmp2224 = -_tmp1117 * _tmp2106 + _tmp2104 * _tmp715; + const Scalar _tmp2225 = -_tmp1121 * _tmp2110 + _tmp2109 * _tmp715; + const Scalar _tmp2226 = _tmp1120 * _tmp2224 + _tmp1473 * _tmp2225; + const Scalar _tmp2227 = _tmp1156 * _tmp2226 - _tmp1157 * _tmp2226 + _tmp2225 * _tmp729; + const Scalar _tmp2228 = std::pow(matches_0_18_weight, Scalar(2)) / std::pow(_tmp817, Scalar(2)); + const Scalar _tmp2229 = -_tmp1126 * _tmp2226 + _tmp1128 * _tmp2226 + _tmp2224 * _tmp729; + const Scalar _tmp2230 = std::pow(matches_0_19_weight, Scalar(2)) / std::pow(_tmp820, Scalar(2)); + const Scalar _tmp2231 = -_tmp1440 * _tmp2106 + _tmp2103 * _tmp750; + const Scalar _tmp2232 = -_tmp1436 * _tmp2110 + _tmp2109 * _tmp745; + const Scalar _tmp2233 = _tmp1076 * _tmp2231 + _tmp1439 * _tmp2232; + const Scalar _tmp2234 = _tmp1082 * _tmp1159 * _tmp2233 - _tmp1160 * _tmp2233 + _tmp2232 * _tmp759; + const Scalar _tmp2235 = _tmp1079 * _tmp2233; + const Scalar _tmp2236 = -_tmp1080 * _tmp2235 + _tmp1082 * _tmp2235 + _tmp2231 * _tmp759; + const Scalar _tmp2237 = std::pow(_tmp1350, Scalar(2)); + const Scalar _tmp2238 = _tmp1217 * _tmp1350; + const Scalar _tmp2239 = _tmp1827 * _tmp48; // Output terms (4) if (residual != nullptr) { @@ -3000,585 +2808,575 @@ void Linearization( _residual(2, 0) = _tmp67; _residual(3, 0) = _tmp68; _residual(4, 0) = _tmp69; - _residual(5, 0) = _tmp70; - _residual(6, 0) = _tmp133; + _residual(5, 0) = _tmp53 * priors_0_1_sqrt_info(5, 5); + _residual(6, 0) = _tmp131; _residual(7, 0) = _tmp134; - _residual(8, 0) = _tmp137; - _residual(9, 0) = _tmp139; - _residual(10, 0) = _tmp140; - _residual(11, 0) = _tmp141; - _residual(12, 0) = _tmp194; - _residual(13, 0) = _tmp195; - _residual(14, 0) = _tmp224; - _residual(15, 0) = _tmp225; - _residual(16, 0) = _tmp254; - _residual(17, 0) = _tmp255; - _residual(18, 0) = _tmp284; - _residual(19, 0) = _tmp285; - _residual(20, 0) = _tmp314; - _residual(21, 0) = _tmp315; - _residual(22, 0) = _tmp344; - _residual(23, 0) = _tmp345; - _residual(24, 0) = _tmp374; - _residual(25, 0) = _tmp375; - _residual(26, 0) = _tmp404; - _residual(27, 0) = _tmp405; - _residual(28, 0) = _tmp434; - _residual(29, 0) = _tmp435; - _residual(30, 0) = _tmp464; - _residual(31, 0) = _tmp465; - _residual(32, 0) = _tmp494; - _residual(33, 0) = _tmp495; - _residual(34, 0) = _tmp524; - _residual(35, 0) = _tmp525; - _residual(36, 0) = _tmp554; - _residual(37, 0) = _tmp555; - _residual(38, 0) = _tmp584; - _residual(39, 0) = _tmp585; - _residual(40, 0) = _tmp614; - _residual(41, 0) = _tmp615; - _residual(42, 0) = _tmp644; - _residual(43, 0) = _tmp645; - _residual(44, 0) = _tmp674; - _residual(45, 0) = _tmp675; - _residual(46, 0) = _tmp704; - _residual(47, 0) = _tmp705; - _residual(48, 0) = _tmp734; - _residual(49, 0) = _tmp735; - _residual(50, 0) = _tmp764; - _residual(51, 0) = _tmp765; - _residual(52, 0) = _tmp766 * _tmp768; - _residual(53, 0) = _tmp769 * _tmp771; - _residual(54, 0) = _tmp772 * _tmp774; - _residual(55, 0) = _tmp775 * _tmp777; - _residual(56, 0) = _tmp778 * _tmp780; - _residual(57, 0) = _tmp781 * _tmp783; - _residual(58, 0) = _tmp784 * _tmp786; - _residual(59, 0) = _tmp787 * _tmp789; - _residual(60, 0) = _tmp790 * _tmp792; - _residual(61, 0) = _tmp793 * _tmp795; - _residual(62, 0) = _tmp796 * _tmp798; - _residual(63, 0) = _tmp799 * _tmp801; - _residual(64, 0) = _tmp802 * _tmp804; - _residual(65, 0) = _tmp805 * _tmp807; - _residual(66, 0) = _tmp808 * _tmp810; - _residual(67, 0) = _tmp811 * _tmp813; - _residual(68, 0) = _tmp814 * _tmp816; - _residual(69, 0) = _tmp817 * _tmp819; - _residual(70, 0) = _tmp820 * _tmp822; - _residual(71, 0) = _tmp823 * _tmp825; + _residual(8, 0) = _tmp135; + _residual(9, 0) = _tmp136; + _residual(10, 0) = _tmp137; + _residual(11, 0) = _tmp110 * priors_1_0_sqrt_info(5, 5); + _residual(12, 0) = _tmp190; + _residual(13, 0) = _tmp191; + _residual(14, 0) = _tmp220; + _residual(15, 0) = _tmp221; + _residual(16, 0) = _tmp250; + _residual(17, 0) = _tmp251; + _residual(18, 0) = _tmp280; + _residual(19, 0) = _tmp281; + _residual(20, 0) = _tmp310; + _residual(21, 0) = _tmp311; + _residual(22, 0) = _tmp340; + _residual(23, 0) = _tmp341; + _residual(24, 0) = _tmp370; + _residual(25, 0) = _tmp371; + _residual(26, 0) = _tmp400; + _residual(27, 0) = _tmp401; + _residual(28, 0) = _tmp430; + _residual(29, 0) = _tmp431; + _residual(30, 0) = _tmp460; + _residual(31, 0) = _tmp461; + _residual(32, 0) = _tmp490; + _residual(33, 0) = _tmp491; + _residual(34, 0) = _tmp520; + _residual(35, 0) = _tmp521; + _residual(36, 0) = _tmp550; + _residual(37, 0) = _tmp551; + _residual(38, 0) = _tmp580; + _residual(39, 0) = _tmp581; + _residual(40, 0) = _tmp610; + _residual(41, 0) = _tmp611; + _residual(42, 0) = _tmp640; + _residual(43, 0) = _tmp641; + _residual(44, 0) = _tmp670; + _residual(45, 0) = _tmp671; + _residual(46, 0) = _tmp700; + _residual(47, 0) = _tmp701; + _residual(48, 0) = _tmp730; + _residual(49, 0) = _tmp731; + _residual(50, 0) = _tmp760; + _residual(51, 0) = _tmp761; + _residual(52, 0) = _tmp762 * _tmp764; + _residual(53, 0) = _tmp765 * _tmp767; + _residual(54, 0) = _tmp768 * _tmp770; + _residual(55, 0) = _tmp771 * _tmp773; + _residual(56, 0) = _tmp774 * _tmp776; + _residual(57, 0) = _tmp777 * _tmp779; + _residual(58, 0) = _tmp780 * _tmp782; + _residual(59, 0) = _tmp783 * _tmp785; + _residual(60, 0) = _tmp786 * _tmp788; + _residual(61, 0) = _tmp789 * _tmp791; + _residual(62, 0) = _tmp792 * _tmp794; + _residual(63, 0) = _tmp795 * _tmp797; + _residual(64, 0) = _tmp798 * _tmp800; + _residual(65, 0) = _tmp801 * _tmp803; + _residual(66, 0) = _tmp804 * _tmp806; + _residual(67, 0) = _tmp807 * _tmp809; + _residual(68, 0) = _tmp810 * _tmp812; + _residual(69, 0) = _tmp813 * _tmp815; + _residual(70, 0) = _tmp816 * _tmp818; + _residual(71, 0) = _tmp819 * _tmp821; } if (rhs != nullptr) { Eigen::Matrix& _rhs = (*rhs); - _rhs(0, 0) = _tmp1002 * _tmp225 + _tmp1018 * _tmp495 + _tmp1020 * _tmp67 + _tmp1035 * _tmp374 + - _tmp1050 * _tmp554 + _tmp1066 * _tmp284 + _tmp1080 * _tmp344 + _tmp1095 * _tmp704 + - _tmp1108 * _tmp765 + _tmp1122 * _tmp195 + _tmp1137 * _tmp405 + _tmp1142 * _tmp140 + - _tmp1157 * _tmp735 + _tmp1173 * _tmp254 + _tmp1176 * _tmp133 + _tmp1180 * _tmp734 + - _tmp1184 * _tmp764 + _tmp1186 * _tmp70 + _tmp1198 * _tmp315 + _tmp1213 * _tmp644 + - _tmp1217 * _tmp404 + _tmp1232 * _tmp435 + _tmp1235 * _tmp141 + _tmp1240 * _tmp194 + - _tmp1244 * _tmp705 + _tmp1249 * _tmp584 + _tmp1254 * _tmp614 + _tmp1257 * _tmp375 + - _tmp1261 * _tmp555 + _tmp1263 * _tmp63 + _tmp1266 * _tmp139 + _tmp1270 * _tmp434 + - _tmp1273 * _tmp285 + _tmp1277 * _tmp255 + _tmp1278 * _tmp68 + _tmp1280 * _tmp345 + - _tmp1285 * _tmp314 + _tmp1299 * _tmp465 + _tmp1303 * _tmp645 + _tmp1315 * _tmp675 + - _tmp1320 * _tmp464 + _tmp1325 * _tmp224 + _tmp1326 * _tmp65 + _tmp1331 * _tmp674 + - _tmp1335 * _tmp494 + _tmp134 * _tmp972 + _tmp137 * _tmp988 + _tmp524 * _tmp927 + - _tmp525 * _tmp930 + _tmp585 * _tmp986 + _tmp615 * _tmp909 + _tmp69 * _tmp882; - _rhs(1, 0) = _tmp133 * _tmp1541 + _tmp134 * _tmp1398 + _tmp1349 * _tmp704 + _tmp1355 * _tmp524 + - _tmp1362 * _tmp434 + _tmp1363 * _tmp435 + _tmp137 * _tmp1481 + _tmp1378 * _tmp67 + - _tmp1379 * _tmp525 + _tmp139 * _tmp1508 + _tmp140 * _tmp1544 + _tmp1404 * _tmp225 + - _tmp1409 * _tmp464 + _tmp141 * _tmp1505 + _tmp1414 * _tmp285 + _tmp1421 * _tmp315 + - _tmp1422 * _tmp465 + _tmp1427 * _tmp675 + _tmp1435 * _tmp405 + _tmp1441 * _tmp494 + - _tmp1446 * _tmp194 + _tmp1452 * _tmp614 + _tmp1461 * _tmp344 + _tmp1463 * _tmp615 + - _tmp1469 * _tmp375 + _tmp1477 * _tmp764 + _tmp1478 * _tmp314 + _tmp1480 * _tmp765 + - _tmp1484 * _tmp63 + _tmp1487 * _tmp70 + _tmp1488 * _tmp224 + _tmp1490 * _tmp705 + - _tmp1496 * _tmp735 + _tmp1498 * _tmp374 + _tmp1499 * _tmp284 + _tmp1504 * _tmp585 + - _tmp1515 * _tmp254 + _tmp1516 * _tmp69 + _tmp1517 * _tmp68 + _tmp1518 * _tmp255 + - _tmp1519 * _tmp404 + _tmp1525 * _tmp554 + _tmp1527 * _tmp65 + _tmp1533 * _tmp645 + - _tmp1534 * _tmp495 + _tmp1535 * _tmp674 + _tmp1536 * _tmp584 + _tmp1539 * _tmp345 + - _tmp1540 * _tmp195 + _tmp1542 * _tmp644 + _tmp1545 * _tmp555 + _tmp1547 * _tmp734; - _rhs(2, 0) = _tmp133 * _tmp1582 + _tmp134 * _tmp1648 + _tmp137 * _tmp1576 + _tmp139 * _tmp1584 + - _tmp140 * _tmp1615 + _tmp141 * _tmp1647 + _tmp1555 * _tmp615 + _tmp1559 * _tmp435 + - _tmp1563 * _tmp764 + _tmp1580 * _tmp735 + _tmp1588 * _tmp495 + _tmp1592 * _tmp195 + - _tmp1596 * _tmp674 + _tmp1597 * _tmp194 + _tmp1610 * _tmp65 + _tmp1614 * _tmp255 + - _tmp1619 * _tmp374 + _tmp1621 * _tmp69 + _tmp1625 * _tmp705 + _tmp1629 * _tmp344 + - _tmp1630 * _tmp254 + _tmp1631 * _tmp63 + _tmp1636 * _tmp645 + _tmp1637 * _tmp434 + - _tmp1641 * _tmp555 + _tmp1646 * _tmp585 + _tmp1652 * _tmp524 + _tmp1653 * _tmp494 + - _tmp1654 * _tmp70 + _tmp1655 * _tmp345 + _tmp1659 * _tmp405 + _tmp1663 * _tmp314 + - _tmp1664 * _tmp525 + _tmp1665 * _tmp704 + _tmp1669 * _tmp285 + _tmp1670 * _tmp614 + - _tmp1674 * _tmp225 + _tmp1675 * _tmp584 + _tmp1676 * _tmp68 + _tmp1677 * _tmp734 + - _tmp1678 * _tmp284 + _tmp1679 * _tmp224 + _tmp1680 * _tmp315 + _tmp1681 * _tmp554 + - _tmp1682 * _tmp765 + _tmp1683 * _tmp644 + _tmp1685 * _tmp375 + _tmp1686 * _tmp404 + - _tmp1687 * _tmp67 + _tmp1692 * _tmp465 + _tmp1693 * _tmp675 + _tmp1694 * _tmp464; - _rhs(3, 0) = _tmp133 * _tmp1802 + _tmp134 * _tmp1791 + _tmp137 * _tmp1890 + _tmp139 * _tmp1833 + - _tmp140 * _tmp1815 + _tmp141 * _tmp1895 + _tmp1703 * _tmp255 + _tmp1710 * _tmp764 + - _tmp1718 * _tmp585 + _tmp1719 * _tmp70 + _tmp1727 * _tmp734 + _tmp1728 * _tmp765 + - _tmp1729 * _tmp67 + _tmp1737 * _tmp555 + _tmp1746 * _tmp525 + _tmp1747 * _tmp554 + - _tmp1748 * _tmp69 + _tmp1756 * _tmp375 + _tmp1757 * _tmp374 + _tmp1765 * _tmp344 + - _tmp1773 * _tmp194 + _tmp1782 * _tmp315 + _tmp1789 * _tmp645 + _tmp1792 * _tmp65 + - _tmp1800 * _tmp404 + _tmp1801 * _tmp345 + _tmp1803 * _tmp254 + _tmp1805 * _tmp195 + - _tmp1814 * _tmp434 + _tmp1824 * _tmp614 + _tmp1831 * _tmp225 + _tmp1832 * _tmp435 + - _tmp1835 * _tmp524 + _tmp1836 * _tmp63 + _tmp1837 * _tmp224 + _tmp1838 * _tmp314 + - _tmp1839 * _tmp405 + _tmp1849 * _tmp494 + _tmp1858 * _tmp675 + _tmp1860 * _tmp495 + - _tmp1869 * _tmp705 + _tmp1870 * _tmp735 + _tmp1877 * _tmp284 + _tmp1878 * _tmp615 + - _tmp1879 * _tmp644 + _tmp1887 * _tmp465 + _tmp1888 * _tmp68 + _tmp1889 * _tmp464 + - _tmp1891 * _tmp584 + _tmp1892 * _tmp674 + _tmp1893 * _tmp704 + _tmp1894 * _tmp285; - _rhs(4, 0) = _tmp133 * _tmp1972 + _tmp134 * _tmp1955 + _tmp137 * _tmp2017 + _tmp139 * _tmp1997 + - _tmp140 * _tmp1966 + _tmp141 * _tmp2022 + _tmp1899 * _tmp434 + _tmp1903 * _tmp524 + - _tmp1907 * _tmp705 + _tmp1911 * _tmp585 + _tmp1912 * _tmp584 + _tmp1916 * _tmp495 + - _tmp1921 * _tmp285 + _tmp1922 * _tmp70 + _tmp1923 * _tmp69 + _tmp1929 * _tmp344 + - _tmp1935 * _tmp615 + _tmp1939 * _tmp735 + _tmp194 * _tmp2004 + _tmp1943 * _tmp375 + - _tmp1947 * _tmp554 + _tmp195 * _tmp1988 + _tmp1951 * _tmp255 + _tmp1952 * _tmp67 + - _tmp1961 * _tmp224 + _tmp1962 * _tmp374 + _tmp1963 * _tmp525 + _tmp1964 * _tmp704 + - _tmp1965 * _tmp734 + _tmp1971 * _tmp645 + _tmp1973 * _tmp435 + _tmp1978 * _tmp405 + - _tmp1982 * _tmp464 + _tmp1983 * _tmp644 + _tmp1984 * _tmp494 + _tmp1989 * _tmp63 + - _tmp1990 * _tmp65 + _tmp1994 * _tmp314 + _tmp1995 * _tmp68 + _tmp1996 * _tmp315 + - _tmp2002 * _tmp674 + _tmp2003 * _tmp225 + _tmp2005 * _tmp675 + _tmp2006 * _tmp254 + - _tmp2007 * _tmp284 + _tmp2014 * _tmp764 + _tmp2015 * _tmp465 + _tmp2016 * _tmp765 + - _tmp2018 * _tmp614 + _tmp2019 * _tmp555 + _tmp2021 * _tmp345 + _tmp2023 * _tmp404; - _rhs(5, 0) = _tmp133 * _tmp2112 + _tmp134 * _tmp2108 + _tmp137 * _tmp2143 + _tmp139 * _tmp2137 + - _tmp140 * _tmp2111 + _tmp141 * _tmp2144 + _tmp194 * _tmp2064 + _tmp195 * _tmp2136 + - _tmp2027 * _tmp735 + _tmp2031 * _tmp404 + _tmp2035 * _tmp705 + _tmp2036 * _tmp405 + - _tmp2040 * _tmp255 + _tmp2044 * _tmp524 + _tmp2048 * _tmp584 + _tmp2052 * _tmp494 + - _tmp2053 * _tmp67 + _tmp2054 * _tmp734 + _tmp2060 * _tmp314 + _tmp2065 * _tmp69 + - _tmp2066 * _tmp70 + _tmp2070 * _tmp765 + _tmp2075 * _tmp464 + _tmp2079 * _tmp374 + - _tmp2080 * _tmp375 + _tmp2084 * _tmp675 + _tmp2085 * _tmp585 + _tmp2090 * _tmp224 + - _tmp2094 * _tmp345 + _tmp2095 * _tmp465 + _tmp2100 * _tmp614 + _tmp2101 * _tmp764 + - _tmp2102 * _tmp65 + _tmp2106 * _tmp554 + _tmp2110 * _tmp555 + _tmp2113 * _tmp674 + - _tmp2118 * _tmp285 + _tmp2123 * _tmp645 + _tmp2124 * _tmp525 + _tmp2125 * _tmp344 + - _tmp2129 * _tmp434 + _tmp2130 * _tmp63 + _tmp2131 * _tmp435 + _tmp2132 * _tmp225 + - _tmp2133 * _tmp68 + _tmp2134 * _tmp495 + _tmp2135 * _tmp644 + _tmp2138 * _tmp315 + - _tmp2139 * _tmp284 + _tmp2140 * _tmp615 + _tmp2141 * _tmp254 + _tmp2142 * _tmp704; - _rhs(6, 0) = _tmp194 * _tmp2155 + _tmp195 * _tmp2157 + _tmp2156 * _tmp766; - _rhs(7, 0) = _tmp2158 * _tmp769 + _tmp2162 * _tmp224 + _tmp2164 * _tmp225; - _rhs(8, 0) = _tmp2165 * _tmp772 + _tmp2169 * _tmp254 + _tmp2170 * _tmp255; - _rhs(9, 0) = _tmp2175 * _tmp284 + _tmp2176 * _tmp775 + _tmp2178 * _tmp285; - _rhs(10, 0) = _tmp2179 * _tmp778 + _tmp2184 * _tmp315 + _tmp2185 * _tmp314; - _rhs(11, 0) = _tmp2189 * _tmp344 + _tmp2190 * _tmp345 + _tmp2191 * _tmp781; - _rhs(12, 0) = _tmp2195 * _tmp374 + _tmp2196 * _tmp375 + _tmp2197 * _tmp784; - _rhs(13, 0) = _tmp2198 * _tmp787 + _tmp2202 * _tmp404 + _tmp2203 * _tmp405; - _rhs(14, 0) = _tmp2207 * _tmp434 + _tmp2208 * _tmp790 + _tmp2209 * _tmp435; - _rhs(15, 0) = _tmp2213 * _tmp465 + _tmp2214 * _tmp793 + _tmp2215 * _tmp464; - _rhs(16, 0) = _tmp2219 * _tmp494 + _tmp2220 * _tmp495 + _tmp2221 * _tmp796; - _rhs(17, 0) = _tmp2222 * _tmp799 + _tmp2226 * _tmp525 + _tmp2227 * _tmp524; - _rhs(18, 0) = _tmp2231 * _tmp555 + _tmp2232 * _tmp802 + _tmp2233 * _tmp554; - _rhs(19, 0) = _tmp2234 * _tmp805 + _tmp2238 * _tmp584 + _tmp2239 * _tmp585; - _rhs(20, 0) = _tmp2243 * _tmp614 + _tmp2244 * _tmp615 + _tmp2245 * _tmp808; - _rhs(21, 0) = _tmp2246 * _tmp811 + _tmp2250 * _tmp644 + _tmp2251 * _tmp645; - _rhs(22, 0) = _tmp2252 * _tmp814 + _tmp2256 * _tmp675 + _tmp2257 * _tmp674; - _rhs(23, 0) = _tmp2258 * _tmp817 + _tmp2263 * _tmp704 + _tmp2264 * _tmp705; - _rhs(24, 0) = _tmp2268 * _tmp734 + _tmp2269 * _tmp820 + _tmp2270 * _tmp735; - _rhs(25, 0) = _tmp2271 * _tmp823 + _tmp2276 * _tmp764 + _tmp2277 * _tmp765; + _rhs(0, 0) = _tmp1027 * _tmp65 + _tmp1042 * _tmp280 + _tmp1058 * _tmp340 + _tmp1071 * _tmp700 + + _tmp1084 * _tmp761 + _tmp1098 * _tmp191 + _tmp110 * _tmp1218 + _tmp1113 * _tmp401 + + _tmp1114 * _tmp136 + _tmp1129 * _tmp731 + _tmp1144 * _tmp250 + _tmp1151 * _tmp131 + + _tmp1154 * _tmp135 + _tmp1158 * _tmp730 + _tmp1161 * _tmp760 + _tmp1175 * _tmp311 + + _tmp1190 * _tmp640 + _tmp1194 * _tmp400 + _tmp1207 * _tmp431 + _tmp1212 * _tmp190 + + _tmp1216 * _tmp701 + _tmp1221 * _tmp580 + _tmp1225 * _tmp610 + _tmp1228 * _tmp371 + + _tmp1232 * _tmp551 + _tmp1238 * _tmp63 + _tmp1242 * _tmp430 + _tmp1246 * _tmp281 + + _tmp1250 * _tmp251 + _tmp1254 * _tmp341 + _tmp1260 * _tmp310 + _tmp1274 * _tmp461 + + _tmp1278 * _tmp641 + _tmp1292 * _tmp671 + _tmp1297 * _tmp460 + _tmp1302 * _tmp220 + + _tmp1305 * _tmp67 + _tmp1311 * _tmp670 + _tmp1315 * _tmp490 + _tmp134 * _tmp918 + + _tmp137 * _tmp967 + _tmp221 * _tmp951 + _tmp370 * _tmp980 + _tmp491 * _tmp966 + + _tmp520 * _tmp868 + _tmp521 * _tmp922 + _tmp550 * _tmp995 + _tmp581 * _tmp937 + + _tmp611 * _tmp852; + _rhs(1, 0) = _tmp131 * _tmp1515 + _tmp1331 * _tmp700 + _tmp1338 * _tmp520 + _tmp134 * _tmp1466 + + _tmp1347 * _tmp430 + _tmp1348 * _tmp431 + _tmp1349 * _tmp521 + _tmp135 * _tmp1405 + + _tmp1350 * _tmp1351 + _tmp1356 * _tmp221 + _tmp136 * _tmp1423 + + _tmp1361 * _tmp460 + _tmp1366 * _tmp281 + _tmp137 * _tmp1375 + _tmp1373 * _tmp311 + + _tmp1374 * _tmp461 + _tmp1381 * _tmp671 + _tmp1389 * _tmp401 + _tmp1396 * _tmp490 + + _tmp1410 * _tmp190 + _tmp1415 * _tmp610 + _tmp1422 * _tmp340 + _tmp1424 * _tmp611 + + _tmp1434 * _tmp371 + _tmp1443 * _tmp760 + _tmp1444 * _tmp310 + _tmp1446 * _tmp761 + + _tmp1462 * _tmp63 + _tmp1468 * _tmp220 + _tmp1469 * _tmp701 + _tmp1475 * _tmp731 + + _tmp1476 * _tmp67 + _tmp1478 * _tmp370 + _tmp1479 * _tmp280 + _tmp1486 * _tmp581 + + _tmp1492 * _tmp250 + _tmp1493 * _tmp251 + _tmp1494 * _tmp400 + _tmp1500 * _tmp550 + + _tmp1505 * _tmp641 + _tmp1506 * _tmp491 + _tmp1507 * _tmp670 + _tmp1509 * _tmp580 + + _tmp1511 * _tmp65 + _tmp1512 * _tmp341 + _tmp1513 * _tmp191 + _tmp1516 * _tmp640 + + _tmp1517 * _tmp551 + _tmp1518 * _tmp730; + _rhs(2, 0) = _tmp131 * _tmp1552 + _tmp134 * _tmp1583 + _tmp135 * _tmp1599 + _tmp136 * _tmp1642 + + _tmp137 * _tmp1565 + _tmp1526 * _tmp611 + _tmp1530 * _tmp431 + _tmp1535 * _tmp760 + + _tmp1539 * _tmp67 + _tmp1543 * _tmp731 + _tmp1556 * _tmp491 + _tmp1560 * _tmp191 + + _tmp1564 * _tmp670 + _tmp1566 * _tmp190 + _tmp1570 * _tmp251 + _tmp1574 * _tmp370 + + _tmp1578 * _tmp701 + _tmp1582 * _tmp340 + _tmp1584 * _tmp250 + _tmp1591 * _tmp63 + + _tmp1596 * _tmp641 + _tmp1598 * _tmp430 + _tmp1603 * _tmp551 + _tmp1609 * _tmp581 + + _tmp1613 * _tmp520 + _tmp1615 * _tmp490 + _tmp1616 * _tmp341 + _tmp1621 * _tmp401 + + _tmp1625 * _tmp310 + _tmp1626 * _tmp521 + _tmp1627 * _tmp700 + _tmp1631 * _tmp281 + + _tmp1632 * _tmp610 + _tmp1636 * _tmp221 + _tmp1638 * _tmp580 + _tmp1639 * _tmp730 + + _tmp1640 * _tmp280 + _tmp1641 * _tmp220 + _tmp1643 * _tmp311 + _tmp1644 * _tmp550 + + _tmp1645 * _tmp761 + _tmp1646 * _tmp640 + _tmp1648 * _tmp371 + _tmp1649 * _tmp400 + + _tmp1654 * _tmp461 + _tmp1655 * _tmp671 + _tmp1656 * _tmp460 + _tmp1657 * _tmp65; + _rhs(3, 0) = _tmp131 * _tmp1762 + _tmp1325 * _tmp1351 + _tmp134 * _tmp1794 + + _tmp135 * _tmp1856 + _tmp136 * _tmp1760 + _tmp137 * _tmp1840 + _tmp1666 * _tmp251 + + _tmp1674 * _tmp760 + _tmp1682 * _tmp581 + _tmp1690 * _tmp730 + _tmp1691 * _tmp761 + + _tmp1699 * _tmp551 + _tmp1706 * _tmp521 + _tmp1707 * _tmp550 + _tmp1715 * _tmp371 + + _tmp1716 * _tmp370 + _tmp1725 * _tmp340 + _tmp1734 * _tmp190 + _tmp1742 * _tmp311 + + _tmp1749 * _tmp641 + _tmp1750 * _tmp67 + _tmp1758 * _tmp400 + _tmp1761 * _tmp341 + + _tmp1763 * _tmp250 + _tmp1764 * _tmp191 + _tmp1773 * _tmp430 + _tmp1774 * _tmp65 + + _tmp1784 * _tmp610 + _tmp1792 * _tmp221 + _tmp1793 * _tmp431 + _tmp1795 * _tmp520 + + _tmp1796 * _tmp63 + _tmp1797 * _tmp69 + _tmp1798 * _tmp220 + _tmp1799 * _tmp310 + + _tmp1800 * _tmp401 + _tmp1808 * _tmp490 + _tmp1809 * _tmp68 + _tmp1817 * _tmp671 + + _tmp1818 * _tmp491 + _tmp1826 * _tmp701 + _tmp1828 * _tmp51 + _tmp1829 * _tmp731 + + _tmp1837 * _tmp280 + _tmp1839 * _tmp611 + _tmp1841 * _tmp640 + _tmp1849 * _tmp461 + + _tmp1850 * _tmp460 + _tmp1851 * _tmp580 + _tmp1853 * _tmp670 + _tmp1854 * _tmp700 + + _tmp1855 * _tmp281; + _rhs(4, 0) = _tmp110 * _tmp1925 + _tmp131 * _tmp1931 + _tmp134 * _tmp1943 + _tmp135 * _tmp1979 + + _tmp136 * _tmp1932 + _tmp137 * _tmp1902 + _tmp1828 * _tmp52 + _tmp1861 * _tmp430 + + _tmp1867 * _tmp520 + _tmp1871 * _tmp701 + _tmp1875 * _tmp581 + _tmp1876 * _tmp580 + + _tmp1882 * _tmp491 + _tmp1886 * _tmp281 + _tmp1891 * _tmp340 + _tmp1895 * _tmp611 + + _tmp1899 * _tmp731 + _tmp190 * _tmp1963 + _tmp1906 * _tmp371 + _tmp191 * _tmp1949 + + _tmp1910 * _tmp550 + _tmp1914 * _tmp251 + _tmp1915 * _tmp65 + _tmp1919 * _tmp220 + + _tmp1920 * _tmp370 + _tmp1921 * _tmp521 + _tmp1922 * _tmp700 + _tmp1923 * _tmp68 + + _tmp1924 * _tmp730 + _tmp1930 * _tmp641 + _tmp1933 * _tmp431 + _tmp1937 * _tmp401 + + _tmp1941 * _tmp460 + _tmp1942 * _tmp640 + _tmp1944 * _tmp490 + _tmp1950 * _tmp63 + + _tmp1954 * _tmp310 + _tmp1955 * _tmp311 + _tmp1956 * _tmp69 + _tmp1961 * _tmp670 + + _tmp1962 * _tmp221 + _tmp1964 * _tmp671 + _tmp1965 * _tmp67 + _tmp1966 * _tmp250 + + _tmp1967 * _tmp280 + _tmp1973 * _tmp760 + _tmp1974 * _tmp461 + _tmp1975 * _tmp761 + + _tmp1976 * _tmp610 + _tmp1977 * _tmp551 + _tmp1978 * _tmp341 + _tmp1980 * _tmp400; + _rhs(5, 0) = _tmp131 * _tmp2068 + _tmp134 * _tmp2089 + _tmp135 * _tmp2102 + + _tmp1351 * _tmp2067 + _tmp136 * _tmp2079 + _tmp137 * _tmp2080 + _tmp1828 * _tmp48 + + _tmp190 * _tmp2024 + _tmp191 * _tmp2093 + _tmp1984 * _tmp731 + _tmp1988 * _tmp400 + + _tmp1993 * _tmp701 + _tmp1994 * _tmp401 + _tmp1998 * _tmp251 + _tmp2003 * _tmp520 + + _tmp2007 * _tmp580 + _tmp2012 * _tmp490 + _tmp2014 * _tmp730 + _tmp2020 * _tmp310 + + _tmp2025 * _tmp67 + _tmp2031 * _tmp761 + _tmp2035 * _tmp460 + _tmp2039 * _tmp370 + + _tmp2040 * _tmp371 + _tmp2044 * _tmp671 + _tmp2045 * _tmp581 + _tmp2050 * _tmp220 + + _tmp2054 * _tmp341 + _tmp2055 * _tmp461 + _tmp2060 * _tmp610 + _tmp2061 * _tmp760 + + _tmp2065 * _tmp550 + _tmp2066 * _tmp551 + _tmp2069 * _tmp670 + _tmp2073 * _tmp281 + + _tmp2078 * _tmp641 + _tmp2081 * _tmp521 + _tmp2082 * _tmp340 + _tmp2086 * _tmp430 + + _tmp2087 * _tmp63 + _tmp2088 * _tmp431 + _tmp2090 * _tmp221 + _tmp2091 * _tmp491 + + _tmp2092 * _tmp640 + _tmp2094 * _tmp311 + _tmp2095 * _tmp280 + _tmp2096 * _tmp611 + + _tmp2097 * _tmp69 + _tmp2098 * _tmp250 + _tmp2099 * _tmp68 + _tmp2100 * _tmp700 + + _tmp2101 * _tmp65; + _rhs(6, 0) = _tmp190 * _tmp2114 + _tmp191 * _tmp2116 + _tmp2115 * _tmp762; + _rhs(7, 0) = _tmp2117 * _tmp765 + _tmp2121 * _tmp220 + _tmp2122 * _tmp221; + _rhs(8, 0) = _tmp2123 * _tmp768 + _tmp2127 * _tmp250 + _tmp2128 * _tmp251; + _rhs(9, 0) = _tmp2132 * _tmp280 + _tmp2133 * _tmp771 + _tmp2135 * _tmp281; + _rhs(10, 0) = _tmp2136 * _tmp774 + _tmp2140 * _tmp311 + _tmp2141 * _tmp310; + _rhs(11, 0) = _tmp2145 * _tmp340 + _tmp2146 * _tmp341 + _tmp2147 * _tmp777; + _rhs(12, 0) = _tmp2152 * _tmp370 + _tmp2153 * _tmp371 + _tmp2154 * _tmp780; + _rhs(13, 0) = _tmp2155 * _tmp783 + _tmp2159 * _tmp400 + _tmp2160 * _tmp401; + _rhs(14, 0) = _tmp2164 * _tmp430 + _tmp2165 * _tmp786 + _tmp2166 * _tmp431; + _rhs(15, 0) = _tmp2171 * _tmp461 + _tmp2172 * _tmp789 + _tmp2173 * _tmp460; + _rhs(16, 0) = _tmp2178 * _tmp490 + _tmp2179 * _tmp491 + _tmp2180 * _tmp792; + _rhs(17, 0) = _tmp2181 * _tmp795 + _tmp2185 * _tmp521 + _tmp2186 * _tmp520; + _rhs(18, 0) = _tmp2190 * _tmp551 + _tmp2191 * _tmp798 + _tmp2192 * _tmp550; + _rhs(19, 0) = _tmp2193 * _tmp801 + _tmp2197 * _tmp580 + _tmp2198 * _tmp581; + _rhs(20, 0) = _tmp2202 * _tmp610 + _tmp2203 * _tmp611 + _tmp2204 * _tmp804; + _rhs(21, 0) = _tmp2205 * _tmp807 + _tmp2209 * _tmp640 + _tmp2210 * _tmp641; + _rhs(22, 0) = _tmp2211 * _tmp810 + _tmp2215 * _tmp671 + _tmp2216 * _tmp670; + _rhs(23, 0) = _tmp2217 * _tmp813 + _tmp2222 * _tmp700 + _tmp2223 * _tmp701; + _rhs(24, 0) = _tmp2227 * _tmp730 + _tmp2228 * _tmp816 + _tmp2229 * _tmp731; + _rhs(25, 0) = _tmp2230 * _tmp819 + _tmp2234 * _tmp760 + _tmp2236 * _tmp761; } if (jacobian != nullptr) { static constexpr int kRows_jacobian = 72; static constexpr int kCols_jacobian = 26; - static constexpr int kNumNonZero_jacobian = 372; - static constexpr int kColPtrs_jacobian[] = {0, 52, 104, 156, 208, 260, 312, 315, 318, - 321, 324, 327, 330, 333, 336, 339, 342, 345, - 348, 351, 354, 357, 360, 363, 366, 369, 372}; + static constexpr int kNumNonZero_jacobian = 362; + static constexpr int kColPtrs_jacobian[] = {0, 49, 98, 146, 198, 250, 302, 305, 308, + 311, 314, 317, 320, 323, 326, 329, 332, 335, + 338, 341, 344, 347, 350, 353, 356, 359, 362}; static constexpr int kRowIndices_jacobian[] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, - 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, - 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, - 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, - 50, 51, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, - 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, - 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, - 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, - 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, - 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, - 48, 49, 50, 51, 12, 13, 52, 14, 15, 53, 16, 17, 54, 18, 19, 55, 20, 21, 56, 22, 23, 57, - 24, 25, 58, 26, 27, 59, 28, 29, 60, 30, 31, 61, 32, 33, 62, 34, 35, 63, 36, 37, 64, 38, - 39, 65, 40, 41, 66, 42, 43, 67, 44, 45, 68, 46, 47, 69, 48, 49, 70, 50, 51, 71}; - if (jacobian->nonZeros() != 372 || jacobian->outerSize() != 26 || jacobian->innerSize() != 72 || + 0, 1, 2, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, + 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, + 49, 50, 51, 0, 1, 2, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, + 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, + 46, 47, 48, 49, 50, 51, 0, 1, 2, 6, 7, 8, 9, 10, 12, 13, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, + 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, + 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, 3, 4, 5, 6, 7, 8, + 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, + 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, + 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, + 49, 50, 51, 12, 13, 52, 14, 15, 53, 16, 17, 54, 18, 19, 55, 20, 21, 56, 22, 23, 57, 24, 25, + 58, 26, 27, 59, 28, 29, 60, 30, 31, 61, 32, 33, 62, 34, 35, 63, 36, 37, 64, 38, 39, 65, 40, + 41, 66, 42, 43, 67, 44, 45, 68, 46, 47, 69, 48, 49, 70, 50, 51, 71}; + if (jacobian->nonZeros() != 362 || jacobian->outerSize() != 26 || jacobian->innerSize() != 72 || !jacobian->isCompressed()) { // Matrix does not have the expected layout, create a correctly initialized sparse matrix - Scalar jacobian_empty_value_ptr[372]; + Scalar jacobian_empty_value_ptr[362]; *jacobian = Eigen::Map>( kRows_jacobian, kCols_jacobian, kNumNonZero_jacobian, kColPtrs_jacobian, kRowIndices_jacobian, jacobian_empty_value_ptr); } Scalar* jacobian_value_ptr = jacobian->valuePtr(); - jacobian_value_ptr[0] = _tmp1263; - jacobian_value_ptr[1] = _tmp1326; - jacobian_value_ptr[2] = _tmp1020; - jacobian_value_ptr[3] = _tmp1278; - jacobian_value_ptr[4] = _tmp882; - jacobian_value_ptr[5] = _tmp1186; - jacobian_value_ptr[6] = _tmp1176; - jacobian_value_ptr[7] = _tmp972; - jacobian_value_ptr[8] = _tmp988; - jacobian_value_ptr[9] = _tmp1266; - jacobian_value_ptr[10] = _tmp1142; - jacobian_value_ptr[11] = _tmp1235; - jacobian_value_ptr[12] = _tmp1240; - jacobian_value_ptr[13] = _tmp1122; - jacobian_value_ptr[14] = _tmp1325; - jacobian_value_ptr[15] = _tmp1002; - jacobian_value_ptr[16] = _tmp1173; - jacobian_value_ptr[17] = _tmp1277; - jacobian_value_ptr[18] = _tmp1066; - jacobian_value_ptr[19] = _tmp1273; - jacobian_value_ptr[20] = _tmp1285; - jacobian_value_ptr[21] = _tmp1198; - jacobian_value_ptr[22] = _tmp1080; - jacobian_value_ptr[23] = _tmp1280; - jacobian_value_ptr[24] = _tmp1035; - jacobian_value_ptr[25] = _tmp1257; - jacobian_value_ptr[26] = _tmp1217; - jacobian_value_ptr[27] = _tmp1137; - jacobian_value_ptr[28] = _tmp1270; - jacobian_value_ptr[29] = _tmp1232; - jacobian_value_ptr[30] = _tmp1320; - jacobian_value_ptr[31] = _tmp1299; - jacobian_value_ptr[32] = _tmp1335; - jacobian_value_ptr[33] = _tmp1018; - jacobian_value_ptr[34] = _tmp927; - jacobian_value_ptr[35] = _tmp930; - jacobian_value_ptr[36] = _tmp1050; - jacobian_value_ptr[37] = _tmp1261; - jacobian_value_ptr[38] = _tmp1249; - jacobian_value_ptr[39] = _tmp986; - jacobian_value_ptr[40] = _tmp1254; - jacobian_value_ptr[41] = _tmp909; - jacobian_value_ptr[42] = _tmp1213; - jacobian_value_ptr[43] = _tmp1303; - jacobian_value_ptr[44] = _tmp1331; - jacobian_value_ptr[45] = _tmp1315; - jacobian_value_ptr[46] = _tmp1095; - jacobian_value_ptr[47] = _tmp1244; - jacobian_value_ptr[48] = _tmp1180; - jacobian_value_ptr[49] = _tmp1157; - jacobian_value_ptr[50] = _tmp1184; - jacobian_value_ptr[51] = _tmp1108; - jacobian_value_ptr[52] = _tmp1484; - jacobian_value_ptr[53] = _tmp1527; - jacobian_value_ptr[54] = _tmp1378; - jacobian_value_ptr[55] = _tmp1517; - jacobian_value_ptr[56] = _tmp1516; - jacobian_value_ptr[57] = _tmp1487; - jacobian_value_ptr[58] = _tmp1541; - jacobian_value_ptr[59] = _tmp1398; - jacobian_value_ptr[60] = _tmp1481; - jacobian_value_ptr[61] = _tmp1508; - jacobian_value_ptr[62] = _tmp1544; - jacobian_value_ptr[63] = _tmp1505; - jacobian_value_ptr[64] = _tmp1446; - jacobian_value_ptr[65] = _tmp1540; - jacobian_value_ptr[66] = _tmp1488; - jacobian_value_ptr[67] = _tmp1404; - jacobian_value_ptr[68] = _tmp1515; - jacobian_value_ptr[69] = _tmp1518; - jacobian_value_ptr[70] = _tmp1499; - jacobian_value_ptr[71] = _tmp1414; - jacobian_value_ptr[72] = _tmp1478; - jacobian_value_ptr[73] = _tmp1421; - jacobian_value_ptr[74] = _tmp1461; - jacobian_value_ptr[75] = _tmp1539; - jacobian_value_ptr[76] = _tmp1498; - jacobian_value_ptr[77] = _tmp1469; - jacobian_value_ptr[78] = _tmp1519; - jacobian_value_ptr[79] = _tmp1435; - jacobian_value_ptr[80] = _tmp1362; - jacobian_value_ptr[81] = _tmp1363; - jacobian_value_ptr[82] = _tmp1409; - jacobian_value_ptr[83] = _tmp1422; - jacobian_value_ptr[84] = _tmp1441; - jacobian_value_ptr[85] = _tmp1534; - jacobian_value_ptr[86] = _tmp1355; - jacobian_value_ptr[87] = _tmp1379; - jacobian_value_ptr[88] = _tmp1525; - jacobian_value_ptr[89] = _tmp1545; - jacobian_value_ptr[90] = _tmp1536; - jacobian_value_ptr[91] = _tmp1504; - jacobian_value_ptr[92] = _tmp1452; - jacobian_value_ptr[93] = _tmp1463; - jacobian_value_ptr[94] = _tmp1542; - jacobian_value_ptr[95] = _tmp1533; - jacobian_value_ptr[96] = _tmp1535; - jacobian_value_ptr[97] = _tmp1427; - jacobian_value_ptr[98] = _tmp1349; - jacobian_value_ptr[99] = _tmp1490; - jacobian_value_ptr[100] = _tmp1547; - jacobian_value_ptr[101] = _tmp1496; - jacobian_value_ptr[102] = _tmp1477; - jacobian_value_ptr[103] = _tmp1480; - jacobian_value_ptr[104] = _tmp1631; - jacobian_value_ptr[105] = _tmp1610; - jacobian_value_ptr[106] = _tmp1687; - jacobian_value_ptr[107] = _tmp1676; - jacobian_value_ptr[108] = _tmp1621; - jacobian_value_ptr[109] = _tmp1654; - jacobian_value_ptr[110] = _tmp1582; - jacobian_value_ptr[111] = _tmp1648; - jacobian_value_ptr[112] = _tmp1576; - jacobian_value_ptr[113] = _tmp1584; - jacobian_value_ptr[114] = _tmp1615; - jacobian_value_ptr[115] = _tmp1647; - jacobian_value_ptr[116] = _tmp1597; - jacobian_value_ptr[117] = _tmp1592; - jacobian_value_ptr[118] = _tmp1679; - jacobian_value_ptr[119] = _tmp1674; - jacobian_value_ptr[120] = _tmp1630; - jacobian_value_ptr[121] = _tmp1614; - jacobian_value_ptr[122] = _tmp1678; - jacobian_value_ptr[123] = _tmp1669; - jacobian_value_ptr[124] = _tmp1663; - jacobian_value_ptr[125] = _tmp1680; - jacobian_value_ptr[126] = _tmp1629; - jacobian_value_ptr[127] = _tmp1655; - jacobian_value_ptr[128] = _tmp1619; - jacobian_value_ptr[129] = _tmp1685; - jacobian_value_ptr[130] = _tmp1686; - jacobian_value_ptr[131] = _tmp1659; - jacobian_value_ptr[132] = _tmp1637; - jacobian_value_ptr[133] = _tmp1559; - jacobian_value_ptr[134] = _tmp1694; - jacobian_value_ptr[135] = _tmp1692; - jacobian_value_ptr[136] = _tmp1653; - jacobian_value_ptr[137] = _tmp1588; - jacobian_value_ptr[138] = _tmp1652; - jacobian_value_ptr[139] = _tmp1664; - jacobian_value_ptr[140] = _tmp1681; - jacobian_value_ptr[141] = _tmp1641; - jacobian_value_ptr[142] = _tmp1675; - jacobian_value_ptr[143] = _tmp1646; - jacobian_value_ptr[144] = _tmp1670; - jacobian_value_ptr[145] = _tmp1555; - jacobian_value_ptr[146] = _tmp1683; - jacobian_value_ptr[147] = _tmp1636; - jacobian_value_ptr[148] = _tmp1596; - jacobian_value_ptr[149] = _tmp1693; - jacobian_value_ptr[150] = _tmp1665; - jacobian_value_ptr[151] = _tmp1625; - jacobian_value_ptr[152] = _tmp1677; - jacobian_value_ptr[153] = _tmp1580; - jacobian_value_ptr[154] = _tmp1563; - jacobian_value_ptr[155] = _tmp1682; - jacobian_value_ptr[156] = _tmp1836; - jacobian_value_ptr[157] = _tmp1792; - jacobian_value_ptr[158] = _tmp1729; - jacobian_value_ptr[159] = _tmp1888; - jacobian_value_ptr[160] = _tmp1748; - jacobian_value_ptr[161] = _tmp1719; - jacobian_value_ptr[162] = _tmp1802; - jacobian_value_ptr[163] = _tmp1791; - jacobian_value_ptr[164] = _tmp1890; - jacobian_value_ptr[165] = _tmp1833; - jacobian_value_ptr[166] = _tmp1815; - jacobian_value_ptr[167] = _tmp1895; - jacobian_value_ptr[168] = _tmp1773; - jacobian_value_ptr[169] = _tmp1805; - jacobian_value_ptr[170] = _tmp1837; - jacobian_value_ptr[171] = _tmp1831; - jacobian_value_ptr[172] = _tmp1803; - jacobian_value_ptr[173] = _tmp1703; - jacobian_value_ptr[174] = _tmp1877; - jacobian_value_ptr[175] = _tmp1894; - jacobian_value_ptr[176] = _tmp1838; - jacobian_value_ptr[177] = _tmp1782; - jacobian_value_ptr[178] = _tmp1765; - jacobian_value_ptr[179] = _tmp1801; - jacobian_value_ptr[180] = _tmp1757; - jacobian_value_ptr[181] = _tmp1756; - jacobian_value_ptr[182] = _tmp1800; - jacobian_value_ptr[183] = _tmp1839; - jacobian_value_ptr[184] = _tmp1814; - jacobian_value_ptr[185] = _tmp1832; - jacobian_value_ptr[186] = _tmp1889; - jacobian_value_ptr[187] = _tmp1887; - jacobian_value_ptr[188] = _tmp1849; - jacobian_value_ptr[189] = _tmp1860; - jacobian_value_ptr[190] = _tmp1835; - jacobian_value_ptr[191] = _tmp1746; - jacobian_value_ptr[192] = _tmp1747; - jacobian_value_ptr[193] = _tmp1737; - jacobian_value_ptr[194] = _tmp1891; - jacobian_value_ptr[195] = _tmp1718; - jacobian_value_ptr[196] = _tmp1824; - jacobian_value_ptr[197] = _tmp1878; - jacobian_value_ptr[198] = _tmp1879; - jacobian_value_ptr[199] = _tmp1789; - jacobian_value_ptr[200] = _tmp1892; - jacobian_value_ptr[201] = _tmp1858; - jacobian_value_ptr[202] = _tmp1893; - jacobian_value_ptr[203] = _tmp1869; - jacobian_value_ptr[204] = _tmp1727; - jacobian_value_ptr[205] = _tmp1870; - jacobian_value_ptr[206] = _tmp1710; - jacobian_value_ptr[207] = _tmp1728; - jacobian_value_ptr[208] = _tmp1989; - jacobian_value_ptr[209] = _tmp1990; - jacobian_value_ptr[210] = _tmp1952; - jacobian_value_ptr[211] = _tmp1995; - jacobian_value_ptr[212] = _tmp1923; - jacobian_value_ptr[213] = _tmp1922; - jacobian_value_ptr[214] = _tmp1972; - jacobian_value_ptr[215] = _tmp1955; - jacobian_value_ptr[216] = _tmp2017; - jacobian_value_ptr[217] = _tmp1997; - jacobian_value_ptr[218] = _tmp1966; - jacobian_value_ptr[219] = _tmp2022; - jacobian_value_ptr[220] = _tmp2004; - jacobian_value_ptr[221] = _tmp1988; - jacobian_value_ptr[222] = _tmp1961; - jacobian_value_ptr[223] = _tmp2003; - jacobian_value_ptr[224] = _tmp2006; - jacobian_value_ptr[225] = _tmp1951; - jacobian_value_ptr[226] = _tmp2007; - jacobian_value_ptr[227] = _tmp1921; - jacobian_value_ptr[228] = _tmp1994; - jacobian_value_ptr[229] = _tmp1996; - jacobian_value_ptr[230] = _tmp1929; - jacobian_value_ptr[231] = _tmp2021; - jacobian_value_ptr[232] = _tmp1962; - jacobian_value_ptr[233] = _tmp1943; - jacobian_value_ptr[234] = _tmp2023; - jacobian_value_ptr[235] = _tmp1978; - jacobian_value_ptr[236] = _tmp1899; - jacobian_value_ptr[237] = _tmp1973; - jacobian_value_ptr[238] = _tmp1982; - jacobian_value_ptr[239] = _tmp2015; - jacobian_value_ptr[240] = _tmp1984; - jacobian_value_ptr[241] = _tmp1916; - jacobian_value_ptr[242] = _tmp1903; - jacobian_value_ptr[243] = _tmp1963; - jacobian_value_ptr[244] = _tmp1947; - jacobian_value_ptr[245] = _tmp2019; - jacobian_value_ptr[246] = _tmp1912; - jacobian_value_ptr[247] = _tmp1911; - jacobian_value_ptr[248] = _tmp2018; - jacobian_value_ptr[249] = _tmp1935; - jacobian_value_ptr[250] = _tmp1983; - jacobian_value_ptr[251] = _tmp1971; - jacobian_value_ptr[252] = _tmp2002; - jacobian_value_ptr[253] = _tmp2005; - jacobian_value_ptr[254] = _tmp1964; - jacobian_value_ptr[255] = _tmp1907; - jacobian_value_ptr[256] = _tmp1965; - jacobian_value_ptr[257] = _tmp1939; - jacobian_value_ptr[258] = _tmp2014; - jacobian_value_ptr[259] = _tmp2016; - jacobian_value_ptr[260] = _tmp2130; - jacobian_value_ptr[261] = _tmp2102; - jacobian_value_ptr[262] = _tmp2053; - jacobian_value_ptr[263] = _tmp2133; - jacobian_value_ptr[264] = _tmp2065; - jacobian_value_ptr[265] = _tmp2066; - jacobian_value_ptr[266] = _tmp2112; - jacobian_value_ptr[267] = _tmp2108; - jacobian_value_ptr[268] = _tmp2143; - jacobian_value_ptr[269] = _tmp2137; - jacobian_value_ptr[270] = _tmp2111; - jacobian_value_ptr[271] = _tmp2144; - jacobian_value_ptr[272] = _tmp2064; - jacobian_value_ptr[273] = _tmp2136; - jacobian_value_ptr[274] = _tmp2090; - jacobian_value_ptr[275] = _tmp2132; - jacobian_value_ptr[276] = _tmp2141; - jacobian_value_ptr[277] = _tmp2040; - jacobian_value_ptr[278] = _tmp2139; - jacobian_value_ptr[279] = _tmp2118; - jacobian_value_ptr[280] = _tmp2060; - jacobian_value_ptr[281] = _tmp2138; - jacobian_value_ptr[282] = _tmp2125; - jacobian_value_ptr[283] = _tmp2094; - jacobian_value_ptr[284] = _tmp2079; - jacobian_value_ptr[285] = _tmp2080; - jacobian_value_ptr[286] = _tmp2031; - jacobian_value_ptr[287] = _tmp2036; - jacobian_value_ptr[288] = _tmp2129; - jacobian_value_ptr[289] = _tmp2131; - jacobian_value_ptr[290] = _tmp2075; - jacobian_value_ptr[291] = _tmp2095; - jacobian_value_ptr[292] = _tmp2052; - jacobian_value_ptr[293] = _tmp2134; - jacobian_value_ptr[294] = _tmp2044; - jacobian_value_ptr[295] = _tmp2124; - jacobian_value_ptr[296] = _tmp2106; - jacobian_value_ptr[297] = _tmp2110; - jacobian_value_ptr[298] = _tmp2048; - jacobian_value_ptr[299] = _tmp2085; - jacobian_value_ptr[300] = _tmp2100; - jacobian_value_ptr[301] = _tmp2140; - jacobian_value_ptr[302] = _tmp2135; - jacobian_value_ptr[303] = _tmp2123; - jacobian_value_ptr[304] = _tmp2113; - jacobian_value_ptr[305] = _tmp2084; - jacobian_value_ptr[306] = _tmp2142; - jacobian_value_ptr[307] = _tmp2035; - jacobian_value_ptr[308] = _tmp2054; - jacobian_value_ptr[309] = _tmp2027; - jacobian_value_ptr[310] = _tmp2101; - jacobian_value_ptr[311] = _tmp2070; - jacobian_value_ptr[312] = _tmp2155; - jacobian_value_ptr[313] = _tmp2157; - jacobian_value_ptr[314] = _tmp768; - jacobian_value_ptr[315] = _tmp2162; - jacobian_value_ptr[316] = _tmp2164; - jacobian_value_ptr[317] = _tmp771; - jacobian_value_ptr[318] = _tmp2169; - jacobian_value_ptr[319] = _tmp2170; - jacobian_value_ptr[320] = _tmp774; - jacobian_value_ptr[321] = _tmp2175; - jacobian_value_ptr[322] = _tmp2178; - jacobian_value_ptr[323] = _tmp777; - jacobian_value_ptr[324] = _tmp2185; - jacobian_value_ptr[325] = _tmp2184; - jacobian_value_ptr[326] = _tmp780; - jacobian_value_ptr[327] = _tmp2189; - jacobian_value_ptr[328] = _tmp2190; - jacobian_value_ptr[329] = _tmp783; - jacobian_value_ptr[330] = _tmp2195; - jacobian_value_ptr[331] = _tmp2196; - jacobian_value_ptr[332] = _tmp786; - jacobian_value_ptr[333] = _tmp2202; - jacobian_value_ptr[334] = _tmp2203; - jacobian_value_ptr[335] = _tmp789; - jacobian_value_ptr[336] = _tmp2207; - jacobian_value_ptr[337] = _tmp2209; - jacobian_value_ptr[338] = _tmp792; - jacobian_value_ptr[339] = _tmp2215; - jacobian_value_ptr[340] = _tmp2213; - jacobian_value_ptr[341] = _tmp795; - jacobian_value_ptr[342] = _tmp2219; - jacobian_value_ptr[343] = _tmp2220; - jacobian_value_ptr[344] = _tmp798; - jacobian_value_ptr[345] = _tmp2227; - jacobian_value_ptr[346] = _tmp2226; - jacobian_value_ptr[347] = _tmp801; - jacobian_value_ptr[348] = _tmp2233; - jacobian_value_ptr[349] = _tmp2231; - jacobian_value_ptr[350] = _tmp804; - jacobian_value_ptr[351] = _tmp2238; - jacobian_value_ptr[352] = _tmp2239; - jacobian_value_ptr[353] = _tmp807; - jacobian_value_ptr[354] = _tmp2243; - jacobian_value_ptr[355] = _tmp2244; - jacobian_value_ptr[356] = _tmp810; - jacobian_value_ptr[357] = _tmp2250; - jacobian_value_ptr[358] = _tmp2251; - jacobian_value_ptr[359] = _tmp813; - jacobian_value_ptr[360] = _tmp2257; - jacobian_value_ptr[361] = _tmp2256; - jacobian_value_ptr[362] = _tmp816; - jacobian_value_ptr[363] = _tmp2263; - jacobian_value_ptr[364] = _tmp2264; - jacobian_value_ptr[365] = _tmp819; - jacobian_value_ptr[366] = _tmp2268; - jacobian_value_ptr[367] = _tmp2270; - jacobian_value_ptr[368] = _tmp822; - jacobian_value_ptr[369] = _tmp2276; - jacobian_value_ptr[370] = _tmp2277; - jacobian_value_ptr[371] = _tmp825; + jacobian_value_ptr[0] = _tmp1238; + jacobian_value_ptr[1] = _tmp1027; + jacobian_value_ptr[2] = _tmp1305; + jacobian_value_ptr[3] = _tmp1151; + jacobian_value_ptr[4] = _tmp918; + jacobian_value_ptr[5] = _tmp1154; + jacobian_value_ptr[6] = _tmp1114; + jacobian_value_ptr[7] = _tmp967; + jacobian_value_ptr[8] = _tmp906 * priors_1_0_sqrt_info(5, 5); + jacobian_value_ptr[9] = _tmp1212; + jacobian_value_ptr[10] = _tmp1098; + jacobian_value_ptr[11] = _tmp1302; + jacobian_value_ptr[12] = _tmp951; + jacobian_value_ptr[13] = _tmp1144; + jacobian_value_ptr[14] = _tmp1250; + jacobian_value_ptr[15] = _tmp1042; + jacobian_value_ptr[16] = _tmp1246; + jacobian_value_ptr[17] = _tmp1260; + jacobian_value_ptr[18] = _tmp1175; + jacobian_value_ptr[19] = _tmp1058; + jacobian_value_ptr[20] = _tmp1254; + jacobian_value_ptr[21] = _tmp980; + jacobian_value_ptr[22] = _tmp1228; + jacobian_value_ptr[23] = _tmp1194; + jacobian_value_ptr[24] = _tmp1113; + jacobian_value_ptr[25] = _tmp1242; + jacobian_value_ptr[26] = _tmp1207; + jacobian_value_ptr[27] = _tmp1297; + jacobian_value_ptr[28] = _tmp1274; + jacobian_value_ptr[29] = _tmp1315; + jacobian_value_ptr[30] = _tmp966; + jacobian_value_ptr[31] = _tmp868; + jacobian_value_ptr[32] = _tmp922; + jacobian_value_ptr[33] = _tmp995; + jacobian_value_ptr[34] = _tmp1232; + jacobian_value_ptr[35] = _tmp1221; + jacobian_value_ptr[36] = _tmp937; + jacobian_value_ptr[37] = _tmp1225; + jacobian_value_ptr[38] = _tmp852; + jacobian_value_ptr[39] = _tmp1190; + jacobian_value_ptr[40] = _tmp1278; + jacobian_value_ptr[41] = _tmp1311; + jacobian_value_ptr[42] = _tmp1292; + jacobian_value_ptr[43] = _tmp1071; + jacobian_value_ptr[44] = _tmp1216; + jacobian_value_ptr[45] = _tmp1158; + jacobian_value_ptr[46] = _tmp1129; + jacobian_value_ptr[47] = _tmp1161; + jacobian_value_ptr[48] = _tmp1084; + jacobian_value_ptr[49] = _tmp1462; + jacobian_value_ptr[50] = _tmp1511; + jacobian_value_ptr[51] = _tmp1476; + jacobian_value_ptr[52] = _tmp1515; + jacobian_value_ptr[53] = _tmp1466; + jacobian_value_ptr[54] = _tmp1405; + jacobian_value_ptr[55] = _tmp1423; + jacobian_value_ptr[56] = _tmp1375; + jacobian_value_ptr[57] = _tmp1350 * priors_1_0_sqrt_info(5, 5); + jacobian_value_ptr[58] = _tmp1410; + jacobian_value_ptr[59] = _tmp1513; + jacobian_value_ptr[60] = _tmp1468; + jacobian_value_ptr[61] = _tmp1356; + jacobian_value_ptr[62] = _tmp1492; + jacobian_value_ptr[63] = _tmp1493; + jacobian_value_ptr[64] = _tmp1479; + jacobian_value_ptr[65] = _tmp1366; + jacobian_value_ptr[66] = _tmp1444; + jacobian_value_ptr[67] = _tmp1373; + jacobian_value_ptr[68] = _tmp1422; + jacobian_value_ptr[69] = _tmp1512; + jacobian_value_ptr[70] = _tmp1478; + jacobian_value_ptr[71] = _tmp1434; + jacobian_value_ptr[72] = _tmp1494; + jacobian_value_ptr[73] = _tmp1389; + jacobian_value_ptr[74] = _tmp1347; + jacobian_value_ptr[75] = _tmp1348; + jacobian_value_ptr[76] = _tmp1361; + jacobian_value_ptr[77] = _tmp1374; + jacobian_value_ptr[78] = _tmp1396; + jacobian_value_ptr[79] = _tmp1506; + jacobian_value_ptr[80] = _tmp1338; + jacobian_value_ptr[81] = _tmp1349; + jacobian_value_ptr[82] = _tmp1500; + jacobian_value_ptr[83] = _tmp1517; + jacobian_value_ptr[84] = _tmp1509; + jacobian_value_ptr[85] = _tmp1486; + jacobian_value_ptr[86] = _tmp1415; + jacobian_value_ptr[87] = _tmp1424; + jacobian_value_ptr[88] = _tmp1516; + jacobian_value_ptr[89] = _tmp1505; + jacobian_value_ptr[90] = _tmp1507; + jacobian_value_ptr[91] = _tmp1381; + jacobian_value_ptr[92] = _tmp1331; + jacobian_value_ptr[93] = _tmp1469; + jacobian_value_ptr[94] = _tmp1518; + jacobian_value_ptr[95] = _tmp1475; + jacobian_value_ptr[96] = _tmp1443; + jacobian_value_ptr[97] = _tmp1446; + jacobian_value_ptr[98] = _tmp1591; + jacobian_value_ptr[99] = _tmp1657; + jacobian_value_ptr[100] = _tmp1539; + jacobian_value_ptr[101] = _tmp1552; + jacobian_value_ptr[102] = _tmp1583; + jacobian_value_ptr[103] = _tmp1599; + jacobian_value_ptr[104] = _tmp1642; + jacobian_value_ptr[105] = _tmp1565; + jacobian_value_ptr[106] = _tmp1566; + jacobian_value_ptr[107] = _tmp1560; + jacobian_value_ptr[108] = _tmp1641; + jacobian_value_ptr[109] = _tmp1636; + jacobian_value_ptr[110] = _tmp1584; + jacobian_value_ptr[111] = _tmp1570; + jacobian_value_ptr[112] = _tmp1640; + jacobian_value_ptr[113] = _tmp1631; + jacobian_value_ptr[114] = _tmp1625; + jacobian_value_ptr[115] = _tmp1643; + jacobian_value_ptr[116] = _tmp1582; + jacobian_value_ptr[117] = _tmp1616; + jacobian_value_ptr[118] = _tmp1574; + jacobian_value_ptr[119] = _tmp1648; + jacobian_value_ptr[120] = _tmp1649; + jacobian_value_ptr[121] = _tmp1621; + jacobian_value_ptr[122] = _tmp1598; + jacobian_value_ptr[123] = _tmp1530; + jacobian_value_ptr[124] = _tmp1656; + jacobian_value_ptr[125] = _tmp1654; + jacobian_value_ptr[126] = _tmp1615; + jacobian_value_ptr[127] = _tmp1556; + jacobian_value_ptr[128] = _tmp1613; + jacobian_value_ptr[129] = _tmp1626; + jacobian_value_ptr[130] = _tmp1644; + jacobian_value_ptr[131] = _tmp1603; + jacobian_value_ptr[132] = _tmp1638; + jacobian_value_ptr[133] = _tmp1609; + jacobian_value_ptr[134] = _tmp1632; + jacobian_value_ptr[135] = _tmp1526; + jacobian_value_ptr[136] = _tmp1646; + jacobian_value_ptr[137] = _tmp1596; + jacobian_value_ptr[138] = _tmp1564; + jacobian_value_ptr[139] = _tmp1655; + jacobian_value_ptr[140] = _tmp1627; + jacobian_value_ptr[141] = _tmp1578; + jacobian_value_ptr[142] = _tmp1639; + jacobian_value_ptr[143] = _tmp1543; + jacobian_value_ptr[144] = _tmp1535; + jacobian_value_ptr[145] = _tmp1645; + jacobian_value_ptr[146] = _tmp1796; + jacobian_value_ptr[147] = _tmp1774; + jacobian_value_ptr[148] = _tmp1750; + jacobian_value_ptr[149] = _tmp1809; + jacobian_value_ptr[150] = _tmp1797; + jacobian_value_ptr[151] = _tmp51 * priors_0_1_sqrt_info(5, 5); + jacobian_value_ptr[152] = _tmp1762; + jacobian_value_ptr[153] = _tmp1794; + jacobian_value_ptr[154] = _tmp1856; + jacobian_value_ptr[155] = _tmp1760; + jacobian_value_ptr[156] = _tmp1840; + jacobian_value_ptr[157] = _tmp1325 * priors_1_0_sqrt_info(5, 5); + jacobian_value_ptr[158] = _tmp1734; + jacobian_value_ptr[159] = _tmp1764; + jacobian_value_ptr[160] = _tmp1798; + jacobian_value_ptr[161] = _tmp1792; + jacobian_value_ptr[162] = _tmp1763; + jacobian_value_ptr[163] = _tmp1666; + jacobian_value_ptr[164] = _tmp1837; + jacobian_value_ptr[165] = _tmp1855; + jacobian_value_ptr[166] = _tmp1799; + jacobian_value_ptr[167] = _tmp1742; + jacobian_value_ptr[168] = _tmp1725; + jacobian_value_ptr[169] = _tmp1761; + jacobian_value_ptr[170] = _tmp1716; + jacobian_value_ptr[171] = _tmp1715; + jacobian_value_ptr[172] = _tmp1758; + jacobian_value_ptr[173] = _tmp1800; + jacobian_value_ptr[174] = _tmp1773; + jacobian_value_ptr[175] = _tmp1793; + jacobian_value_ptr[176] = _tmp1850; + jacobian_value_ptr[177] = _tmp1849; + jacobian_value_ptr[178] = _tmp1808; + jacobian_value_ptr[179] = _tmp1818; + jacobian_value_ptr[180] = _tmp1795; + jacobian_value_ptr[181] = _tmp1706; + jacobian_value_ptr[182] = _tmp1707; + jacobian_value_ptr[183] = _tmp1699; + jacobian_value_ptr[184] = _tmp1851; + jacobian_value_ptr[185] = _tmp1682; + jacobian_value_ptr[186] = _tmp1784; + jacobian_value_ptr[187] = _tmp1839; + jacobian_value_ptr[188] = _tmp1841; + jacobian_value_ptr[189] = _tmp1749; + jacobian_value_ptr[190] = _tmp1853; + jacobian_value_ptr[191] = _tmp1817; + jacobian_value_ptr[192] = _tmp1854; + jacobian_value_ptr[193] = _tmp1826; + jacobian_value_ptr[194] = _tmp1690; + jacobian_value_ptr[195] = _tmp1829; + jacobian_value_ptr[196] = _tmp1674; + jacobian_value_ptr[197] = _tmp1691; + jacobian_value_ptr[198] = _tmp1950; + jacobian_value_ptr[199] = _tmp1915; + jacobian_value_ptr[200] = _tmp1965; + jacobian_value_ptr[201] = _tmp1923; + jacobian_value_ptr[202] = _tmp1956; + jacobian_value_ptr[203] = _tmp52 * priors_0_1_sqrt_info(5, 5); + jacobian_value_ptr[204] = _tmp1931; + jacobian_value_ptr[205] = _tmp1943; + jacobian_value_ptr[206] = _tmp1979; + jacobian_value_ptr[207] = _tmp1932; + jacobian_value_ptr[208] = _tmp1902; + jacobian_value_ptr[209] = _tmp1321 * priors_1_0_sqrt_info(5, 5); + jacobian_value_ptr[210] = _tmp1963; + jacobian_value_ptr[211] = _tmp1949; + jacobian_value_ptr[212] = _tmp1919; + jacobian_value_ptr[213] = _tmp1962; + jacobian_value_ptr[214] = _tmp1966; + jacobian_value_ptr[215] = _tmp1914; + jacobian_value_ptr[216] = _tmp1967; + jacobian_value_ptr[217] = _tmp1886; + jacobian_value_ptr[218] = _tmp1954; + jacobian_value_ptr[219] = _tmp1955; + jacobian_value_ptr[220] = _tmp1891; + jacobian_value_ptr[221] = _tmp1978; + jacobian_value_ptr[222] = _tmp1920; + jacobian_value_ptr[223] = _tmp1906; + jacobian_value_ptr[224] = _tmp1980; + jacobian_value_ptr[225] = _tmp1937; + jacobian_value_ptr[226] = _tmp1861; + jacobian_value_ptr[227] = _tmp1933; + jacobian_value_ptr[228] = _tmp1941; + jacobian_value_ptr[229] = _tmp1974; + jacobian_value_ptr[230] = _tmp1944; + jacobian_value_ptr[231] = _tmp1882; + jacobian_value_ptr[232] = _tmp1867; + jacobian_value_ptr[233] = _tmp1921; + jacobian_value_ptr[234] = _tmp1910; + jacobian_value_ptr[235] = _tmp1977; + jacobian_value_ptr[236] = _tmp1876; + jacobian_value_ptr[237] = _tmp1875; + jacobian_value_ptr[238] = _tmp1976; + jacobian_value_ptr[239] = _tmp1895; + jacobian_value_ptr[240] = _tmp1942; + jacobian_value_ptr[241] = _tmp1930; + jacobian_value_ptr[242] = _tmp1961; + jacobian_value_ptr[243] = _tmp1964; + jacobian_value_ptr[244] = _tmp1922; + jacobian_value_ptr[245] = _tmp1871; + jacobian_value_ptr[246] = _tmp1924; + jacobian_value_ptr[247] = _tmp1899; + jacobian_value_ptr[248] = _tmp1973; + jacobian_value_ptr[249] = _tmp1975; + jacobian_value_ptr[250] = _tmp2087; + jacobian_value_ptr[251] = _tmp2101; + jacobian_value_ptr[252] = _tmp2025; + jacobian_value_ptr[253] = _tmp2099; + jacobian_value_ptr[254] = _tmp2097; + jacobian_value_ptr[255] = _tmp48 * priors_0_1_sqrt_info(5, 5); + jacobian_value_ptr[256] = _tmp2068; + jacobian_value_ptr[257] = _tmp2089; + jacobian_value_ptr[258] = _tmp2102; + jacobian_value_ptr[259] = _tmp2079; + jacobian_value_ptr[260] = _tmp2080; + jacobian_value_ptr[261] = _tmp2067 * priors_1_0_sqrt_info(5, 5); + jacobian_value_ptr[262] = _tmp2024; + jacobian_value_ptr[263] = _tmp2093; + jacobian_value_ptr[264] = _tmp2050; + jacobian_value_ptr[265] = _tmp2090; + jacobian_value_ptr[266] = _tmp2098; + jacobian_value_ptr[267] = _tmp1998; + jacobian_value_ptr[268] = _tmp2095; + jacobian_value_ptr[269] = _tmp2073; + jacobian_value_ptr[270] = _tmp2020; + jacobian_value_ptr[271] = _tmp2094; + jacobian_value_ptr[272] = _tmp2082; + jacobian_value_ptr[273] = _tmp2054; + jacobian_value_ptr[274] = _tmp2039; + jacobian_value_ptr[275] = _tmp2040; + jacobian_value_ptr[276] = _tmp1988; + jacobian_value_ptr[277] = _tmp1994; + jacobian_value_ptr[278] = _tmp2086; + jacobian_value_ptr[279] = _tmp2088; + jacobian_value_ptr[280] = _tmp2035; + jacobian_value_ptr[281] = _tmp2055; + jacobian_value_ptr[282] = _tmp2012; + jacobian_value_ptr[283] = _tmp2091; + jacobian_value_ptr[284] = _tmp2003; + jacobian_value_ptr[285] = _tmp2081; + jacobian_value_ptr[286] = _tmp2065; + jacobian_value_ptr[287] = _tmp2066; + jacobian_value_ptr[288] = _tmp2007; + jacobian_value_ptr[289] = _tmp2045; + jacobian_value_ptr[290] = _tmp2060; + jacobian_value_ptr[291] = _tmp2096; + jacobian_value_ptr[292] = _tmp2092; + jacobian_value_ptr[293] = _tmp2078; + jacobian_value_ptr[294] = _tmp2069; + jacobian_value_ptr[295] = _tmp2044; + jacobian_value_ptr[296] = _tmp2100; + jacobian_value_ptr[297] = _tmp1993; + jacobian_value_ptr[298] = _tmp2014; + jacobian_value_ptr[299] = _tmp1984; + jacobian_value_ptr[300] = _tmp2061; + jacobian_value_ptr[301] = _tmp2031; + jacobian_value_ptr[302] = _tmp2114; + jacobian_value_ptr[303] = _tmp2116; + jacobian_value_ptr[304] = _tmp764; + jacobian_value_ptr[305] = _tmp2121; + jacobian_value_ptr[306] = _tmp2122; + jacobian_value_ptr[307] = _tmp767; + jacobian_value_ptr[308] = _tmp2127; + jacobian_value_ptr[309] = _tmp2128; + jacobian_value_ptr[310] = _tmp770; + jacobian_value_ptr[311] = _tmp2132; + jacobian_value_ptr[312] = _tmp2135; + jacobian_value_ptr[313] = _tmp773; + jacobian_value_ptr[314] = _tmp2141; + jacobian_value_ptr[315] = _tmp2140; + jacobian_value_ptr[316] = _tmp776; + jacobian_value_ptr[317] = _tmp2145; + jacobian_value_ptr[318] = _tmp2146; + jacobian_value_ptr[319] = _tmp779; + jacobian_value_ptr[320] = _tmp2152; + jacobian_value_ptr[321] = _tmp2153; + jacobian_value_ptr[322] = _tmp782; + jacobian_value_ptr[323] = _tmp2159; + jacobian_value_ptr[324] = _tmp2160; + jacobian_value_ptr[325] = _tmp785; + jacobian_value_ptr[326] = _tmp2164; + jacobian_value_ptr[327] = _tmp2166; + jacobian_value_ptr[328] = _tmp788; + jacobian_value_ptr[329] = _tmp2173; + jacobian_value_ptr[330] = _tmp2171; + jacobian_value_ptr[331] = _tmp791; + jacobian_value_ptr[332] = _tmp2178; + jacobian_value_ptr[333] = _tmp2179; + jacobian_value_ptr[334] = _tmp794; + jacobian_value_ptr[335] = _tmp2186; + jacobian_value_ptr[336] = _tmp2185; + jacobian_value_ptr[337] = _tmp797; + jacobian_value_ptr[338] = _tmp2192; + jacobian_value_ptr[339] = _tmp2190; + jacobian_value_ptr[340] = _tmp800; + jacobian_value_ptr[341] = _tmp2197; + jacobian_value_ptr[342] = _tmp2198; + jacobian_value_ptr[343] = _tmp803; + jacobian_value_ptr[344] = _tmp2202; + jacobian_value_ptr[345] = _tmp2203; + jacobian_value_ptr[346] = _tmp806; + jacobian_value_ptr[347] = _tmp2209; + jacobian_value_ptr[348] = _tmp2210; + jacobian_value_ptr[349] = _tmp809; + jacobian_value_ptr[350] = _tmp2216; + jacobian_value_ptr[351] = _tmp2215; + jacobian_value_ptr[352] = _tmp812; + jacobian_value_ptr[353] = _tmp2222; + jacobian_value_ptr[354] = _tmp2223; + jacobian_value_ptr[355] = _tmp815; + jacobian_value_ptr[356] = _tmp2227; + jacobian_value_ptr[357] = _tmp2229; + jacobian_value_ptr[358] = _tmp818; + jacobian_value_ptr[359] = _tmp2234; + jacobian_value_ptr[360] = _tmp2236; + jacobian_value_ptr[361] = _tmp821; } if (hessian != nullptr) { @@ -3606,532 +3404,529 @@ void Linearization( } Scalar* hessian_value_ptr = hessian->valuePtr(); - hessian_value_ptr[0] = std::pow(_tmp1002, Scalar(2)) + std::pow(_tmp1018, Scalar(2)) + - std::pow(_tmp1020, Scalar(2)) + std::pow(_tmp1035, Scalar(2)) + - std::pow(_tmp1050, Scalar(2)) + std::pow(_tmp1066, Scalar(2)) + - std::pow(_tmp1080, Scalar(2)) + std::pow(_tmp1095, Scalar(2)) + - std::pow(_tmp1108, Scalar(2)) + std::pow(_tmp1122, Scalar(2)) + - std::pow(_tmp1137, Scalar(2)) + std::pow(_tmp1142, Scalar(2)) + - std::pow(_tmp1157, Scalar(2)) + std::pow(_tmp1173, Scalar(2)) + - std::pow(_tmp1176, Scalar(2)) + std::pow(_tmp1180, Scalar(2)) + - std::pow(_tmp1184, Scalar(2)) + std::pow(_tmp1186, Scalar(2)) + - std::pow(_tmp1198, Scalar(2)) + std::pow(_tmp1213, Scalar(2)) + - std::pow(_tmp1217, Scalar(2)) + std::pow(_tmp1232, Scalar(2)) + - std::pow(_tmp1235, Scalar(2)) + std::pow(_tmp1240, Scalar(2)) + - std::pow(_tmp1244, Scalar(2)) + std::pow(_tmp1249, Scalar(2)) + - std::pow(_tmp1254, Scalar(2)) + std::pow(_tmp1257, Scalar(2)) + - std::pow(_tmp1261, Scalar(2)) + std::pow(_tmp1263, Scalar(2)) + - std::pow(_tmp1266, Scalar(2)) + std::pow(_tmp1270, Scalar(2)) + - std::pow(_tmp1273, Scalar(2)) + std::pow(_tmp1277, Scalar(2)) + - std::pow(_tmp1278, Scalar(2)) + std::pow(_tmp1280, Scalar(2)) + - std::pow(_tmp1285, Scalar(2)) + std::pow(_tmp1299, Scalar(2)) + - std::pow(_tmp1303, Scalar(2)) + std::pow(_tmp1315, Scalar(2)) + - std::pow(_tmp1320, Scalar(2)) + std::pow(_tmp1325, Scalar(2)) + - std::pow(_tmp1326, Scalar(2)) + std::pow(_tmp1331, Scalar(2)) + - std::pow(_tmp1335, Scalar(2)) + std::pow(_tmp882, Scalar(2)) + - std::pow(_tmp909, Scalar(2)) + std::pow(_tmp927, Scalar(2)) + - std::pow(_tmp930, Scalar(2)) + std::pow(_tmp972, Scalar(2)) + - std::pow(_tmp986, Scalar(2)) + std::pow(_tmp988, Scalar(2)); + hessian_value_ptr[0] = + std::pow(_tmp1027, Scalar(2)) + std::pow(_tmp1042, Scalar(2)) + + std::pow(_tmp1058, Scalar(2)) + std::pow(_tmp1071, Scalar(2)) + + std::pow(_tmp1084, Scalar(2)) + std::pow(_tmp1098, Scalar(2)) + + std::pow(_tmp1113, Scalar(2)) + std::pow(_tmp1114, Scalar(2)) + + std::pow(_tmp1129, Scalar(2)) + std::pow(_tmp1144, Scalar(2)) + + std::pow(_tmp1151, Scalar(2)) + std::pow(_tmp1154, Scalar(2)) + + std::pow(_tmp1158, Scalar(2)) + std::pow(_tmp1161, Scalar(2)) + + std::pow(_tmp1175, Scalar(2)) + std::pow(_tmp1190, Scalar(2)) + + std::pow(_tmp1194, Scalar(2)) + std::pow(_tmp1207, Scalar(2)) + + std::pow(_tmp1212, Scalar(2)) + std::pow(_tmp1216, Scalar(2)) + + _tmp1217 * std::pow(_tmp906, Scalar(2)) + std::pow(_tmp1221, Scalar(2)) + + std::pow(_tmp1225, Scalar(2)) + std::pow(_tmp1228, Scalar(2)) + + std::pow(_tmp1232, Scalar(2)) + std::pow(_tmp1238, Scalar(2)) + + std::pow(_tmp1242, Scalar(2)) + std::pow(_tmp1246, Scalar(2)) + + std::pow(_tmp1250, Scalar(2)) + std::pow(_tmp1254, Scalar(2)) + + std::pow(_tmp1260, Scalar(2)) + std::pow(_tmp1274, Scalar(2)) + + std::pow(_tmp1278, Scalar(2)) + std::pow(_tmp1292, Scalar(2)) + + std::pow(_tmp1297, Scalar(2)) + std::pow(_tmp1302, Scalar(2)) + + std::pow(_tmp1305, Scalar(2)) + std::pow(_tmp1311, Scalar(2)) + + std::pow(_tmp1315, Scalar(2)) + std::pow(_tmp852, Scalar(2)) + + std::pow(_tmp868, Scalar(2)) + std::pow(_tmp918, Scalar(2)) + std::pow(_tmp922, Scalar(2)) + + std::pow(_tmp937, Scalar(2)) + std::pow(_tmp951, Scalar(2)) + std::pow(_tmp966, Scalar(2)) + + std::pow(_tmp967, Scalar(2)) + std::pow(_tmp980, Scalar(2)) + std::pow(_tmp995, Scalar(2)); hessian_value_ptr[1] = - _tmp1002 * _tmp1404 + _tmp1018 * _tmp1534 + _tmp1020 * _tmp1378 + _tmp1035 * _tmp1498 + - _tmp1050 * _tmp1525 + _tmp1066 * _tmp1499 + _tmp1080 * _tmp1461 + _tmp1095 * _tmp1349 + - _tmp1108 * _tmp1480 + _tmp1122 * _tmp1540 + _tmp1137 * _tmp1435 + _tmp1142 * _tmp1544 + - _tmp1157 * _tmp1496 + _tmp1173 * _tmp1515 + _tmp1176 * _tmp1541 + _tmp1180 * _tmp1547 + - _tmp1184 * _tmp1477 + _tmp1186 * _tmp1487 + _tmp1198 * _tmp1421 + _tmp1213 * _tmp1542 + - _tmp1217 * _tmp1519 + _tmp1232 * _tmp1363 + _tmp1235 * _tmp1505 + _tmp1240 * _tmp1446 + - _tmp1244 * _tmp1490 + _tmp1249 * _tmp1536 + _tmp1254 * _tmp1452 + _tmp1257 * _tmp1469 + - _tmp1261 * _tmp1545 + _tmp1263 * _tmp1484 + _tmp1266 * _tmp1508 + _tmp1270 * _tmp1362 + - _tmp1273 * _tmp1414 + _tmp1277 * _tmp1518 + _tmp1278 * _tmp1517 + _tmp1280 * _tmp1539 + - _tmp1285 * _tmp1478 + _tmp1299 * _tmp1422 + _tmp1303 * _tmp1533 + _tmp1315 * _tmp1427 + - _tmp1320 * _tmp1409 + _tmp1325 * _tmp1488 + _tmp1326 * _tmp1527 + _tmp1331 * _tmp1535 + - _tmp1335 * _tmp1441 + _tmp1355 * _tmp927 + _tmp1379 * _tmp930 + _tmp1398 * _tmp972 + - _tmp1463 * _tmp909 + _tmp1481 * _tmp988 + _tmp1504 * _tmp986 + _tmp1516 * _tmp882; + _tmp1027 * _tmp1511 + _tmp1042 * _tmp1479 + _tmp1058 * _tmp1422 + _tmp1071 * _tmp1331 + + _tmp1084 * _tmp1446 + _tmp1098 * _tmp1513 + _tmp1113 * _tmp1389 + _tmp1114 * _tmp1423 + + _tmp1129 * _tmp1475 + _tmp1144 * _tmp1492 + _tmp1151 * _tmp1515 + _tmp1154 * _tmp1405 + + _tmp1158 * _tmp1518 + _tmp1161 * _tmp1443 + _tmp1175 * _tmp1373 + _tmp1190 * _tmp1516 + + _tmp1194 * _tmp1494 + _tmp1207 * _tmp1348 + _tmp1212 * _tmp1410 + _tmp1216 * _tmp1469 + + _tmp1218 * _tmp1350 + _tmp1221 * _tmp1509 + _tmp1225 * _tmp1415 + _tmp1228 * _tmp1434 + + _tmp1232 * _tmp1517 + _tmp1238 * _tmp1462 + _tmp1242 * _tmp1347 + _tmp1246 * _tmp1366 + + _tmp1250 * _tmp1493 + _tmp1254 * _tmp1512 + _tmp1260 * _tmp1444 + _tmp1274 * _tmp1374 + + _tmp1278 * _tmp1505 + _tmp1292 * _tmp1381 + _tmp1297 * _tmp1361 + _tmp1302 * _tmp1468 + + _tmp1305 * _tmp1476 + _tmp1311 * _tmp1507 + _tmp1315 * _tmp1396 + _tmp1338 * _tmp868 + + _tmp1349 * _tmp922 + _tmp1356 * _tmp951 + _tmp1375 * _tmp967 + _tmp1424 * _tmp852 + + _tmp1466 * _tmp918 + _tmp1478 * _tmp980 + _tmp1486 * _tmp937 + _tmp1500 * _tmp995 + + _tmp1506 * _tmp966; hessian_value_ptr[2] = - _tmp1002 * _tmp1674 + _tmp1018 * _tmp1588 + _tmp1020 * _tmp1687 + _tmp1035 * _tmp1619 + - _tmp1050 * _tmp1681 + _tmp1066 * _tmp1678 + _tmp1080 * _tmp1629 + _tmp1095 * _tmp1665 + - _tmp1108 * _tmp1682 + _tmp1122 * _tmp1592 + _tmp1137 * _tmp1659 + _tmp1142 * _tmp1615 + - _tmp1157 * _tmp1580 + _tmp1173 * _tmp1630 + _tmp1176 * _tmp1582 + _tmp1180 * _tmp1677 + - _tmp1184 * _tmp1563 + _tmp1186 * _tmp1654 + _tmp1198 * _tmp1680 + _tmp1213 * _tmp1683 + - _tmp1217 * _tmp1686 + _tmp1232 * _tmp1559 + _tmp1235 * _tmp1647 + _tmp1240 * _tmp1597 + - _tmp1244 * _tmp1625 + _tmp1249 * _tmp1675 + _tmp1254 * _tmp1670 + _tmp1257 * _tmp1685 + - _tmp1261 * _tmp1641 + _tmp1263 * _tmp1631 + _tmp1266 * _tmp1584 + _tmp1270 * _tmp1637 + - _tmp1273 * _tmp1669 + _tmp1277 * _tmp1614 + _tmp1278 * _tmp1676 + _tmp1280 * _tmp1655 + - _tmp1285 * _tmp1663 + _tmp1299 * _tmp1692 + _tmp1303 * _tmp1636 + _tmp1315 * _tmp1693 + - _tmp1320 * _tmp1694 + _tmp1325 * _tmp1679 + _tmp1326 * _tmp1610 + _tmp1331 * _tmp1596 + - _tmp1335 * _tmp1653 + _tmp1555 * _tmp909 + _tmp1576 * _tmp988 + _tmp1621 * _tmp882 + - _tmp1646 * _tmp986 + _tmp1648 * _tmp972 + _tmp1652 * _tmp927 + _tmp1664 * _tmp930; + _tmp1027 * _tmp1657 + _tmp1042 * _tmp1640 + _tmp1058 * _tmp1582 + _tmp1071 * _tmp1627 + + _tmp1084 * _tmp1645 + _tmp1098 * _tmp1560 + _tmp1113 * _tmp1621 + _tmp1114 * _tmp1642 + + _tmp1129 * _tmp1543 + _tmp1144 * _tmp1584 + _tmp1151 * _tmp1552 + _tmp1154 * _tmp1599 + + _tmp1158 * _tmp1639 + _tmp1161 * _tmp1535 + _tmp1175 * _tmp1643 + _tmp1190 * _tmp1646 + + _tmp1194 * _tmp1649 + _tmp1207 * _tmp1530 + _tmp1212 * _tmp1566 + _tmp1216 * _tmp1578 + + _tmp1221 * _tmp1638 + _tmp1225 * _tmp1632 + _tmp1228 * _tmp1648 + _tmp1232 * _tmp1603 + + _tmp1238 * _tmp1591 + _tmp1242 * _tmp1598 + _tmp1246 * _tmp1631 + _tmp1250 * _tmp1570 + + _tmp1254 * _tmp1616 + _tmp1260 * _tmp1625 + _tmp1274 * _tmp1654 + _tmp1278 * _tmp1596 + + _tmp1292 * _tmp1655 + _tmp1297 * _tmp1656 + _tmp1302 * _tmp1641 + _tmp1305 * _tmp1539 + + _tmp1311 * _tmp1564 + _tmp1315 * _tmp1615 + _tmp1526 * _tmp852 + _tmp1556 * _tmp966 + + _tmp1565 * _tmp967 + _tmp1574 * _tmp980 + _tmp1583 * _tmp918 + _tmp1609 * _tmp937 + + _tmp1613 * _tmp868 + _tmp1626 * _tmp922 + _tmp1636 * _tmp951 + _tmp1644 * _tmp995; hessian_value_ptr[3] = - _tmp1002 * _tmp1831 + _tmp1018 * _tmp1860 + _tmp1020 * _tmp1729 + _tmp1035 * _tmp1757 + - _tmp1050 * _tmp1747 + _tmp1066 * _tmp1877 + _tmp1080 * _tmp1765 + _tmp1095 * _tmp1893 + - _tmp1108 * _tmp1728 + _tmp1122 * _tmp1805 + _tmp1137 * _tmp1839 + _tmp1142 * _tmp1815 + - _tmp1157 * _tmp1870 + _tmp1173 * _tmp1803 + _tmp1176 * _tmp1802 + _tmp1180 * _tmp1727 + - _tmp1184 * _tmp1710 + _tmp1186 * _tmp1719 + _tmp1198 * _tmp1782 + _tmp1213 * _tmp1879 + - _tmp1217 * _tmp1800 + _tmp1232 * _tmp1832 + _tmp1235 * _tmp1895 + _tmp1240 * _tmp1773 + - _tmp1244 * _tmp1869 + _tmp1249 * _tmp1891 + _tmp1254 * _tmp1824 + _tmp1257 * _tmp1756 + - _tmp1261 * _tmp1737 + _tmp1263 * _tmp1836 + _tmp1266 * _tmp1833 + _tmp1270 * _tmp1814 + - _tmp1273 * _tmp1894 + _tmp1277 * _tmp1703 + _tmp1278 * _tmp1888 + _tmp1280 * _tmp1801 + - _tmp1285 * _tmp1838 + _tmp1299 * _tmp1887 + _tmp1303 * _tmp1789 + _tmp1315 * _tmp1858 + - _tmp1320 * _tmp1889 + _tmp1325 * _tmp1837 + _tmp1326 * _tmp1792 + _tmp1331 * _tmp1892 + - _tmp1335 * _tmp1849 + _tmp1718 * _tmp986 + _tmp1746 * _tmp930 + _tmp1748 * _tmp882 + - _tmp1791 * _tmp972 + _tmp1835 * _tmp927 + _tmp1878 * _tmp909 + _tmp1890 * _tmp988; + _tmp1027 * _tmp1774 + _tmp1042 * _tmp1837 + _tmp1058 * _tmp1725 + _tmp1071 * _tmp1854 + + _tmp1084 * _tmp1691 + _tmp1098 * _tmp1764 + _tmp1113 * _tmp1800 + _tmp1114 * _tmp1760 + + _tmp1129 * _tmp1829 + _tmp1144 * _tmp1763 + _tmp1151 * _tmp1762 + _tmp1154 * _tmp1856 + + _tmp1158 * _tmp1690 + _tmp1161 * _tmp1674 + _tmp1175 * _tmp1742 + _tmp1190 * _tmp1841 + + _tmp1194 * _tmp1758 + _tmp1207 * _tmp1793 + _tmp1212 * _tmp1734 + _tmp1216 * _tmp1826 + + _tmp1218 * _tmp1325 + _tmp1221 * _tmp1851 + _tmp1225 * _tmp1784 + _tmp1228 * _tmp1715 + + _tmp1232 * _tmp1699 + _tmp1238 * _tmp1796 + _tmp1242 * _tmp1773 + _tmp1246 * _tmp1855 + + _tmp1250 * _tmp1666 + _tmp1254 * _tmp1761 + _tmp1260 * _tmp1799 + _tmp1274 * _tmp1849 + + _tmp1278 * _tmp1749 + _tmp1292 * _tmp1817 + _tmp1297 * _tmp1850 + _tmp1302 * _tmp1798 + + _tmp1305 * _tmp1750 + _tmp1311 * _tmp1853 + _tmp1315 * _tmp1808 + _tmp1682 * _tmp937 + + _tmp1706 * _tmp922 + _tmp1707 * _tmp995 + _tmp1716 * _tmp980 + _tmp1792 * _tmp951 + + _tmp1794 * _tmp918 + _tmp1795 * _tmp868 + _tmp1818 * _tmp966 + _tmp1839 * _tmp852 + + _tmp1840 * _tmp967; hessian_value_ptr[4] = - _tmp1002 * _tmp2003 + _tmp1018 * _tmp1916 + _tmp1020 * _tmp1952 + _tmp1035 * _tmp1962 + - _tmp1050 * _tmp1947 + _tmp1066 * _tmp2007 + _tmp1080 * _tmp1929 + _tmp1095 * _tmp1964 + - _tmp1108 * _tmp2016 + _tmp1122 * _tmp1988 + _tmp1137 * _tmp1978 + _tmp1142 * _tmp1966 + - _tmp1157 * _tmp1939 + _tmp1173 * _tmp2006 + _tmp1176 * _tmp1972 + _tmp1180 * _tmp1965 + - _tmp1184 * _tmp2014 + _tmp1186 * _tmp1922 + _tmp1198 * _tmp1996 + _tmp1213 * _tmp1983 + - _tmp1217 * _tmp2023 + _tmp1232 * _tmp1973 + _tmp1235 * _tmp2022 + _tmp1240 * _tmp2004 + - _tmp1244 * _tmp1907 + _tmp1249 * _tmp1912 + _tmp1254 * _tmp2018 + _tmp1257 * _tmp1943 + - _tmp1261 * _tmp2019 + _tmp1263 * _tmp1989 + _tmp1266 * _tmp1997 + _tmp1270 * _tmp1899 + - _tmp1273 * _tmp1921 + _tmp1277 * _tmp1951 + _tmp1278 * _tmp1995 + _tmp1280 * _tmp2021 + - _tmp1285 * _tmp1994 + _tmp1299 * _tmp2015 + _tmp1303 * _tmp1971 + _tmp1315 * _tmp2005 + - _tmp1320 * _tmp1982 + _tmp1325 * _tmp1961 + _tmp1326 * _tmp1990 + _tmp1331 * _tmp2002 + - _tmp1335 * _tmp1984 + _tmp1903 * _tmp927 + _tmp1911 * _tmp986 + _tmp1923 * _tmp882 + - _tmp1935 * _tmp909 + _tmp1955 * _tmp972 + _tmp1963 * _tmp930 + _tmp2017 * _tmp988; + _tmp1027 * _tmp1915 + _tmp1042 * _tmp1967 + _tmp1058 * _tmp1891 + _tmp1071 * _tmp1922 + + _tmp1084 * _tmp1975 + _tmp1098 * _tmp1949 + _tmp1113 * _tmp1937 + _tmp1114 * _tmp1932 + + _tmp1129 * _tmp1899 + _tmp1144 * _tmp1966 + _tmp1151 * _tmp1931 + _tmp1154 * _tmp1979 + + _tmp1158 * _tmp1924 + _tmp1161 * _tmp1973 + _tmp1175 * _tmp1955 + _tmp1190 * _tmp1942 + + _tmp1194 * _tmp1980 + _tmp1207 * _tmp1933 + _tmp1212 * _tmp1963 + _tmp1216 * _tmp1871 + + _tmp1221 * _tmp1876 + _tmp1225 * _tmp1976 + _tmp1228 * _tmp1906 + _tmp1232 * _tmp1977 + + _tmp1238 * _tmp1950 + _tmp1242 * _tmp1861 + _tmp1246 * _tmp1886 + _tmp1250 * _tmp1914 + + _tmp1254 * _tmp1978 + _tmp1260 * _tmp1954 + _tmp1274 * _tmp1974 + _tmp1278 * _tmp1930 + + _tmp1292 * _tmp1964 + _tmp1297 * _tmp1941 + _tmp1302 * _tmp1919 + _tmp1305 * _tmp1965 + + _tmp1311 * _tmp1961 + _tmp1315 * _tmp1944 + _tmp1867 * _tmp868 + _tmp1875 * _tmp937 + + _tmp1882 * _tmp966 + _tmp1895 * _tmp852 + _tmp1902 * _tmp967 + _tmp1910 * _tmp995 + + _tmp1920 * _tmp980 + _tmp1921 * _tmp922 + _tmp1925 * _tmp906 + _tmp1943 * _tmp918 + + _tmp1962 * _tmp951; hessian_value_ptr[5] = - _tmp1002 * _tmp2132 + _tmp1018 * _tmp2134 + _tmp1020 * _tmp2053 + _tmp1035 * _tmp2079 + - _tmp1050 * _tmp2106 + _tmp1066 * _tmp2139 + _tmp1080 * _tmp2125 + _tmp1095 * _tmp2142 + - _tmp1108 * _tmp2070 + _tmp1122 * _tmp2136 + _tmp1137 * _tmp2036 + _tmp1142 * _tmp2111 + - _tmp1157 * _tmp2027 + _tmp1173 * _tmp2141 + _tmp1176 * _tmp2112 + _tmp1180 * _tmp2054 + - _tmp1184 * _tmp2101 + _tmp1186 * _tmp2066 + _tmp1198 * _tmp2138 + _tmp1213 * _tmp2135 + - _tmp1217 * _tmp2031 + _tmp1232 * _tmp2131 + _tmp1235 * _tmp2144 + _tmp1240 * _tmp2064 + - _tmp1244 * _tmp2035 + _tmp1249 * _tmp2048 + _tmp1254 * _tmp2100 + _tmp1257 * _tmp2080 + - _tmp1261 * _tmp2110 + _tmp1263 * _tmp2130 + _tmp1266 * _tmp2137 + _tmp1270 * _tmp2129 + - _tmp1273 * _tmp2118 + _tmp1277 * _tmp2040 + _tmp1278 * _tmp2133 + _tmp1280 * _tmp2094 + - _tmp1285 * _tmp2060 + _tmp1299 * _tmp2095 + _tmp1303 * _tmp2123 + _tmp1315 * _tmp2084 + - _tmp1320 * _tmp2075 + _tmp1325 * _tmp2090 + _tmp1326 * _tmp2102 + _tmp1331 * _tmp2113 + - _tmp1335 * _tmp2052 + _tmp2044 * _tmp927 + _tmp2065 * _tmp882 + _tmp2085 * _tmp986 + - _tmp2108 * _tmp972 + _tmp2124 * _tmp930 + _tmp2140 * _tmp909 + _tmp2143 * _tmp988; - hessian_value_ptr[6] = _tmp1122 * _tmp2157 + _tmp1240 * _tmp2155; - hessian_value_ptr[7] = _tmp1002 * _tmp2164 + _tmp1325 * _tmp2162; - hessian_value_ptr[8] = _tmp1173 * _tmp2169 + _tmp1277 * _tmp2170; - hessian_value_ptr[9] = _tmp1066 * _tmp2175 + _tmp1273 * _tmp2178; - hessian_value_ptr[10] = _tmp1198 * _tmp2184 + _tmp1285 * _tmp2185; - hessian_value_ptr[11] = _tmp1080 * _tmp2189 + _tmp1280 * _tmp2190; - hessian_value_ptr[12] = _tmp1035 * _tmp2195 + _tmp1257 * _tmp2196; - hessian_value_ptr[13] = _tmp1137 * _tmp2203 + _tmp1217 * _tmp2202; - hessian_value_ptr[14] = _tmp1232 * _tmp2209 + _tmp1270 * _tmp2207; - hessian_value_ptr[15] = _tmp1299 * _tmp2213 + _tmp1320 * _tmp2215; - hessian_value_ptr[16] = _tmp1018 * _tmp2220 + _tmp1335 * _tmp2219; - hessian_value_ptr[17] = _tmp2226 * _tmp930 + _tmp2227 * _tmp927; - hessian_value_ptr[18] = _tmp1050 * _tmp2233 + _tmp1261 * _tmp2231; - hessian_value_ptr[19] = _tmp1249 * _tmp2238 + _tmp2239 * _tmp986; - hessian_value_ptr[20] = _tmp1254 * _tmp2243 + _tmp2244 * _tmp909; - hessian_value_ptr[21] = _tmp1213 * _tmp2250 + _tmp1303 * _tmp2251; - hessian_value_ptr[22] = _tmp1315 * _tmp2256 + _tmp1331 * _tmp2257; - hessian_value_ptr[23] = _tmp1095 * _tmp2263 + _tmp1244 * _tmp2264; - hessian_value_ptr[24] = _tmp1157 * _tmp2270 + _tmp1180 * _tmp2268; - hessian_value_ptr[25] = _tmp1108 * _tmp2277 + _tmp1184 * _tmp2276; - hessian_value_ptr[26] = std::pow(_tmp1349, Scalar(2)) + std::pow(_tmp1355, Scalar(2)) + - std::pow(_tmp1362, Scalar(2)) + std::pow(_tmp1363, Scalar(2)) + - std::pow(_tmp1378, Scalar(2)) + std::pow(_tmp1379, Scalar(2)) + - std::pow(_tmp1398, Scalar(2)) + std::pow(_tmp1404, Scalar(2)) + - std::pow(_tmp1409, Scalar(2)) + std::pow(_tmp1414, Scalar(2)) + - std::pow(_tmp1421, Scalar(2)) + std::pow(_tmp1422, Scalar(2)) + - std::pow(_tmp1427, Scalar(2)) + std::pow(_tmp1435, Scalar(2)) + - std::pow(_tmp1441, Scalar(2)) + std::pow(_tmp1446, Scalar(2)) + - std::pow(_tmp1452, Scalar(2)) + std::pow(_tmp1461, Scalar(2)) + - std::pow(_tmp1463, Scalar(2)) + std::pow(_tmp1469, Scalar(2)) + - std::pow(_tmp1477, Scalar(2)) + std::pow(_tmp1478, Scalar(2)) + - std::pow(_tmp1480, Scalar(2)) + std::pow(_tmp1481, Scalar(2)) + - std::pow(_tmp1484, Scalar(2)) + std::pow(_tmp1487, Scalar(2)) + - std::pow(_tmp1488, Scalar(2)) + std::pow(_tmp1490, Scalar(2)) + - std::pow(_tmp1496, Scalar(2)) + std::pow(_tmp1498, Scalar(2)) + - std::pow(_tmp1499, Scalar(2)) + std::pow(_tmp1504, Scalar(2)) + - std::pow(_tmp1505, Scalar(2)) + std::pow(_tmp1508, Scalar(2)) + + _tmp1027 * _tmp2101 + _tmp1042 * _tmp2095 + _tmp1058 * _tmp2082 + _tmp1071 * _tmp2100 + + _tmp1084 * _tmp2031 + _tmp1098 * _tmp2093 + _tmp1113 * _tmp1994 + _tmp1114 * _tmp2079 + + _tmp1129 * _tmp1984 + _tmp1144 * _tmp2098 + _tmp1151 * _tmp2068 + _tmp1154 * _tmp2102 + + _tmp1158 * _tmp2014 + _tmp1161 * _tmp2061 + _tmp1175 * _tmp2094 + _tmp1190 * _tmp2092 + + _tmp1194 * _tmp1988 + _tmp1207 * _tmp2088 + _tmp1212 * _tmp2024 + _tmp1216 * _tmp1993 + + _tmp1218 * _tmp2067 + _tmp1221 * _tmp2007 + _tmp1225 * _tmp2060 + _tmp1228 * _tmp2040 + + _tmp1232 * _tmp2066 + _tmp1238 * _tmp2087 + _tmp1242 * _tmp2086 + _tmp1246 * _tmp2073 + + _tmp1250 * _tmp1998 + _tmp1254 * _tmp2054 + _tmp1260 * _tmp2020 + _tmp1274 * _tmp2055 + + _tmp1278 * _tmp2078 + _tmp1292 * _tmp2044 + _tmp1297 * _tmp2035 + _tmp1302 * _tmp2050 + + _tmp1305 * _tmp2025 + _tmp1311 * _tmp2069 + _tmp1315 * _tmp2012 + _tmp2003 * _tmp868 + + _tmp2039 * _tmp980 + _tmp2045 * _tmp937 + _tmp2065 * _tmp995 + _tmp2080 * _tmp967 + + _tmp2081 * _tmp922 + _tmp2089 * _tmp918 + _tmp2090 * _tmp951 + _tmp2091 * _tmp966 + + _tmp2096 * _tmp852; + hessian_value_ptr[6] = _tmp1098 * _tmp2116 + _tmp1212 * _tmp2114; + hessian_value_ptr[7] = _tmp1302 * _tmp2121 + _tmp2122 * _tmp951; + hessian_value_ptr[8] = _tmp1144 * _tmp2127 + _tmp1250 * _tmp2128; + hessian_value_ptr[9] = _tmp1042 * _tmp2132 + _tmp1246 * _tmp2135; + hessian_value_ptr[10] = _tmp1175 * _tmp2140 + _tmp1260 * _tmp2141; + hessian_value_ptr[11] = _tmp1058 * _tmp2145 + _tmp1254 * _tmp2146; + hessian_value_ptr[12] = _tmp1228 * _tmp2153 + _tmp2152 * _tmp980; + hessian_value_ptr[13] = _tmp1113 * _tmp2160 + _tmp1194 * _tmp2159; + hessian_value_ptr[14] = _tmp1207 * _tmp2166 + _tmp1242 * _tmp2164; + hessian_value_ptr[15] = _tmp1274 * _tmp2171 + _tmp1297 * _tmp2173; + hessian_value_ptr[16] = _tmp1315 * _tmp2178 + _tmp2179 * _tmp966; + hessian_value_ptr[17] = _tmp2185 * _tmp922 + _tmp2186 * _tmp868; + hessian_value_ptr[18] = _tmp1232 * _tmp2190 + _tmp2192 * _tmp995; + hessian_value_ptr[19] = _tmp1221 * _tmp2197 + _tmp2198 * _tmp937; + hessian_value_ptr[20] = _tmp1225 * _tmp2202 + _tmp2203 * _tmp852; + hessian_value_ptr[21] = _tmp1190 * _tmp2209 + _tmp1278 * _tmp2210; + hessian_value_ptr[22] = _tmp1292 * _tmp2215 + _tmp1311 * _tmp2216; + hessian_value_ptr[23] = _tmp1071 * _tmp2222 + _tmp1216 * _tmp2223; + hessian_value_ptr[24] = _tmp1129 * _tmp2229 + _tmp1158 * _tmp2227; + hessian_value_ptr[25] = _tmp1084 * _tmp2236 + _tmp1161 * _tmp2234; + hessian_value_ptr[26] = _tmp1217 * _tmp2237 + std::pow(_tmp1331, Scalar(2)) + + std::pow(_tmp1338, Scalar(2)) + std::pow(_tmp1347, Scalar(2)) + + std::pow(_tmp1348, Scalar(2)) + std::pow(_tmp1349, Scalar(2)) + + std::pow(_tmp1356, Scalar(2)) + std::pow(_tmp1361, Scalar(2)) + + std::pow(_tmp1366, Scalar(2)) + std::pow(_tmp1373, Scalar(2)) + + std::pow(_tmp1374, Scalar(2)) + std::pow(_tmp1381, Scalar(2)) + + std::pow(_tmp1389, Scalar(2)) + std::pow(_tmp1396, Scalar(2)) + + std::pow(_tmp1405, Scalar(2)) + std::pow(_tmp1410, Scalar(2)) + + std::pow(_tmp1415, Scalar(2)) + std::pow(_tmp1422, Scalar(2)) + + std::pow(_tmp1423, Scalar(2)) + std::pow(_tmp1424, Scalar(2)) + + std::pow(_tmp1434, Scalar(2)) + std::pow(_tmp1443, Scalar(2)) + + std::pow(_tmp1444, Scalar(2)) + std::pow(_tmp1446, Scalar(2)) + + std::pow(_tmp1462, Scalar(2)) + std::pow(_tmp1466, Scalar(2)) + + std::pow(_tmp1468, Scalar(2)) + std::pow(_tmp1469, Scalar(2)) + + std::pow(_tmp1475, Scalar(2)) + std::pow(_tmp1476, Scalar(2)) + + std::pow(_tmp1478, Scalar(2)) + std::pow(_tmp1479, Scalar(2)) + + std::pow(_tmp1486, Scalar(2)) + std::pow(_tmp1492, Scalar(2)) + + std::pow(_tmp1493, Scalar(2)) + std::pow(_tmp1494, Scalar(2)) + + std::pow(_tmp1500, Scalar(2)) + std::pow(_tmp1505, Scalar(2)) + + std::pow(_tmp1506, Scalar(2)) + std::pow(_tmp1507, Scalar(2)) + + std::pow(_tmp1509, Scalar(2)) + std::pow(_tmp1511, Scalar(2)) + + std::pow(_tmp1512, Scalar(2)) + std::pow(_tmp1513, Scalar(2)) + std::pow(_tmp1515, Scalar(2)) + std::pow(_tmp1516, Scalar(2)) + std::pow(_tmp1517, Scalar(2)) + std::pow(_tmp1518, Scalar(2)) + - std::pow(_tmp1519, Scalar(2)) + std::pow(_tmp1525, Scalar(2)) + - std::pow(_tmp1527, Scalar(2)) + std::pow(_tmp1533, Scalar(2)) + - std::pow(_tmp1534, Scalar(2)) + std::pow(_tmp1535, Scalar(2)) + - std::pow(_tmp1536, Scalar(2)) + std::pow(_tmp1539, Scalar(2)) + - std::pow(_tmp1540, Scalar(2)) + std::pow(_tmp1541, Scalar(2)) + - std::pow(_tmp1542, Scalar(2)) + std::pow(_tmp1544, Scalar(2)) + - std::pow(_tmp1545, Scalar(2)) + std::pow(_tmp1547, Scalar(2)); + _tmp2237 * std::pow(priors_1_0_sqrt_info(4, 5), Scalar(2)); hessian_value_ptr[27] = - _tmp1349 * _tmp1665 + _tmp1355 * _tmp1652 + _tmp1362 * _tmp1637 + _tmp1363 * _tmp1559 + - _tmp1378 * _tmp1687 + _tmp1379 * _tmp1664 + _tmp1398 * _tmp1648 + _tmp1404 * _tmp1674 + - _tmp1409 * _tmp1694 + _tmp1414 * _tmp1669 + _tmp1421 * _tmp1680 + _tmp1422 * _tmp1692 + - _tmp1427 * _tmp1693 + _tmp1435 * _tmp1659 + _tmp1441 * _tmp1653 + _tmp1446 * _tmp1597 + - _tmp1452 * _tmp1670 + _tmp1461 * _tmp1629 + _tmp1463 * _tmp1555 + _tmp1469 * _tmp1685 + - _tmp1477 * _tmp1563 + _tmp1478 * _tmp1663 + _tmp1480 * _tmp1682 + _tmp1481 * _tmp1576 + - _tmp1484 * _tmp1631 + _tmp1487 * _tmp1654 + _tmp1488 * _tmp1679 + _tmp1490 * _tmp1625 + - _tmp1496 * _tmp1580 + _tmp1498 * _tmp1619 + _tmp1499 * _tmp1678 + _tmp1504 * _tmp1646 + - _tmp1505 * _tmp1647 + _tmp1508 * _tmp1584 + _tmp1515 * _tmp1630 + _tmp1516 * _tmp1621 + - _tmp1517 * _tmp1676 + _tmp1518 * _tmp1614 + _tmp1519 * _tmp1686 + _tmp1525 * _tmp1681 + - _tmp1527 * _tmp1610 + _tmp1533 * _tmp1636 + _tmp1534 * _tmp1588 + _tmp1535 * _tmp1596 + - _tmp1536 * _tmp1675 + _tmp1539 * _tmp1655 + _tmp1540 * _tmp1592 + _tmp1541 * _tmp1582 + - _tmp1542 * _tmp1683 + _tmp1544 * _tmp1615 + _tmp1545 * _tmp1641 + _tmp1547 * _tmp1677; + _tmp1331 * _tmp1627 + _tmp1338 * _tmp1613 + _tmp1347 * _tmp1598 + _tmp1348 * _tmp1530 + + _tmp1349 * _tmp1626 + _tmp1356 * _tmp1636 + _tmp1361 * _tmp1656 + _tmp1366 * _tmp1631 + + _tmp1373 * _tmp1643 + _tmp1374 * _tmp1654 + _tmp1375 * _tmp1565 + _tmp1381 * _tmp1655 + + _tmp1389 * _tmp1621 + _tmp1396 * _tmp1615 + _tmp1405 * _tmp1599 + _tmp1410 * _tmp1566 + + _tmp1415 * _tmp1632 + _tmp1422 * _tmp1582 + _tmp1423 * _tmp1642 + _tmp1424 * _tmp1526 + + _tmp1434 * _tmp1648 + _tmp1443 * _tmp1535 + _tmp1444 * _tmp1625 + _tmp1446 * _tmp1645 + + _tmp1462 * _tmp1591 + _tmp1466 * _tmp1583 + _tmp1468 * _tmp1641 + _tmp1469 * _tmp1578 + + _tmp1475 * _tmp1543 + _tmp1476 * _tmp1539 + _tmp1478 * _tmp1574 + _tmp1479 * _tmp1640 + + _tmp1486 * _tmp1609 + _tmp1492 * _tmp1584 + _tmp1493 * _tmp1570 + _tmp1494 * _tmp1649 + + _tmp1500 * _tmp1644 + _tmp1505 * _tmp1596 + _tmp1506 * _tmp1556 + _tmp1507 * _tmp1564 + + _tmp1509 * _tmp1638 + _tmp1511 * _tmp1657 + _tmp1512 * _tmp1616 + _tmp1513 * _tmp1560 + + _tmp1515 * _tmp1552 + _tmp1516 * _tmp1646 + _tmp1517 * _tmp1603 + _tmp1518 * _tmp1639; hessian_value_ptr[28] = - _tmp1349 * _tmp1893 + _tmp1355 * _tmp1835 + _tmp1362 * _tmp1814 + _tmp1363 * _tmp1832 + - _tmp1378 * _tmp1729 + _tmp1379 * _tmp1746 + _tmp1398 * _tmp1791 + _tmp1404 * _tmp1831 + - _tmp1409 * _tmp1889 + _tmp1414 * _tmp1894 + _tmp1421 * _tmp1782 + _tmp1422 * _tmp1887 + - _tmp1427 * _tmp1858 + _tmp1435 * _tmp1839 + _tmp1441 * _tmp1849 + _tmp1446 * _tmp1773 + - _tmp1452 * _tmp1824 + _tmp1461 * _tmp1765 + _tmp1463 * _tmp1878 + _tmp1469 * _tmp1756 + - _tmp1477 * _tmp1710 + _tmp1478 * _tmp1838 + _tmp1480 * _tmp1728 + _tmp1481 * _tmp1890 + - _tmp1484 * _tmp1836 + _tmp1487 * _tmp1719 + _tmp1488 * _tmp1837 + _tmp1490 * _tmp1869 + - _tmp1496 * _tmp1870 + _tmp1498 * _tmp1757 + _tmp1499 * _tmp1877 + _tmp1504 * _tmp1718 + - _tmp1505 * _tmp1895 + _tmp1508 * _tmp1833 + _tmp1515 * _tmp1803 + _tmp1516 * _tmp1748 + - _tmp1517 * _tmp1888 + _tmp1518 * _tmp1703 + _tmp1519 * _tmp1800 + _tmp1525 * _tmp1747 + - _tmp1527 * _tmp1792 + _tmp1533 * _tmp1789 + _tmp1534 * _tmp1860 + _tmp1535 * _tmp1892 + - _tmp1536 * _tmp1891 + _tmp1539 * _tmp1801 + _tmp1540 * _tmp1805 + _tmp1541 * _tmp1802 + - _tmp1542 * _tmp1879 + _tmp1544 * _tmp1815 + _tmp1545 * _tmp1737 + _tmp1547 * _tmp1727; + _tmp1325 * _tmp2238 + _tmp1331 * _tmp1854 + _tmp1338 * _tmp1795 + _tmp1347 * _tmp1773 + + _tmp1348 * _tmp1793 + _tmp1349 * _tmp1706 + _tmp1356 * _tmp1792 + _tmp1361 * _tmp1850 + + _tmp1366 * _tmp1855 + _tmp1373 * _tmp1742 + _tmp1374 * _tmp1849 + _tmp1375 * _tmp1840 + + _tmp1381 * _tmp1817 + _tmp1389 * _tmp1800 + _tmp1396 * _tmp1808 + _tmp1405 * _tmp1856 + + _tmp1410 * _tmp1734 + _tmp1415 * _tmp1784 + _tmp1422 * _tmp1725 + _tmp1423 * _tmp1760 + + _tmp1424 * _tmp1839 + _tmp1434 * _tmp1715 + _tmp1443 * _tmp1674 + _tmp1444 * _tmp1799 + + _tmp1446 * _tmp1691 + _tmp1462 * _tmp1796 + _tmp1466 * _tmp1794 + _tmp1468 * _tmp1798 + + _tmp1469 * _tmp1826 + _tmp1475 * _tmp1829 + _tmp1476 * _tmp1750 + _tmp1478 * _tmp1716 + + _tmp1479 * _tmp1837 + _tmp1486 * _tmp1682 + _tmp1492 * _tmp1763 + _tmp1493 * _tmp1666 + + _tmp1494 * _tmp1758 + _tmp1500 * _tmp1707 + _tmp1505 * _tmp1749 + _tmp1506 * _tmp1818 + + _tmp1507 * _tmp1853 + _tmp1509 * _tmp1851 + _tmp1511 * _tmp1774 + _tmp1512 * _tmp1761 + + _tmp1513 * _tmp1764 + _tmp1515 * _tmp1762 + _tmp1516 * _tmp1841 + _tmp1517 * _tmp1699 + + _tmp1518 * _tmp1690; hessian_value_ptr[29] = - _tmp1349 * _tmp1964 + _tmp1355 * _tmp1903 + _tmp1362 * _tmp1899 + _tmp1363 * _tmp1973 + - _tmp1378 * _tmp1952 + _tmp1379 * _tmp1963 + _tmp1398 * _tmp1955 + _tmp1404 * _tmp2003 + - _tmp1409 * _tmp1982 + _tmp1414 * _tmp1921 + _tmp1421 * _tmp1996 + _tmp1422 * _tmp2015 + - _tmp1427 * _tmp2005 + _tmp1435 * _tmp1978 + _tmp1441 * _tmp1984 + _tmp1446 * _tmp2004 + - _tmp1452 * _tmp2018 + _tmp1461 * _tmp1929 + _tmp1463 * _tmp1935 + _tmp1469 * _tmp1943 + - _tmp1477 * _tmp2014 + _tmp1478 * _tmp1994 + _tmp1480 * _tmp2016 + _tmp1481 * _tmp2017 + - _tmp1484 * _tmp1989 + _tmp1487 * _tmp1922 + _tmp1488 * _tmp1961 + _tmp1490 * _tmp1907 + - _tmp1496 * _tmp1939 + _tmp1498 * _tmp1962 + _tmp1499 * _tmp2007 + _tmp1504 * _tmp1911 + - _tmp1505 * _tmp2022 + _tmp1508 * _tmp1997 + _tmp1515 * _tmp2006 + _tmp1516 * _tmp1923 + - _tmp1517 * _tmp1995 + _tmp1518 * _tmp1951 + _tmp1519 * _tmp2023 + _tmp1525 * _tmp1947 + - _tmp1527 * _tmp1990 + _tmp1533 * _tmp1971 + _tmp1534 * _tmp1916 + _tmp1535 * _tmp2002 + - _tmp1536 * _tmp1912 + _tmp1539 * _tmp2021 + _tmp1540 * _tmp1988 + _tmp1541 * _tmp1972 + - _tmp1542 * _tmp1983 + _tmp1544 * _tmp1966 + _tmp1545 * _tmp2019 + _tmp1547 * _tmp1965; + _tmp1331 * _tmp1922 + _tmp1338 * _tmp1867 + _tmp1347 * _tmp1861 + _tmp1348 * _tmp1933 + + _tmp1349 * _tmp1921 + _tmp1350 * _tmp1925 + _tmp1356 * _tmp1962 + _tmp1361 * _tmp1941 + + _tmp1366 * _tmp1886 + _tmp1373 * _tmp1955 + _tmp1374 * _tmp1974 + _tmp1375 * _tmp1902 + + _tmp1381 * _tmp1964 + _tmp1389 * _tmp1937 + _tmp1396 * _tmp1944 + _tmp1405 * _tmp1979 + + _tmp1410 * _tmp1963 + _tmp1415 * _tmp1976 + _tmp1422 * _tmp1891 + _tmp1423 * _tmp1932 + + _tmp1424 * _tmp1895 + _tmp1434 * _tmp1906 + _tmp1443 * _tmp1973 + _tmp1444 * _tmp1954 + + _tmp1446 * _tmp1975 + _tmp1462 * _tmp1950 + _tmp1466 * _tmp1943 + _tmp1468 * _tmp1919 + + _tmp1469 * _tmp1871 + _tmp1475 * _tmp1899 + _tmp1476 * _tmp1965 + _tmp1478 * _tmp1920 + + _tmp1479 * _tmp1967 + _tmp1486 * _tmp1875 + _tmp1492 * _tmp1966 + _tmp1493 * _tmp1914 + + _tmp1494 * _tmp1980 + _tmp1500 * _tmp1910 + _tmp1505 * _tmp1930 + _tmp1506 * _tmp1882 + + _tmp1507 * _tmp1961 + _tmp1509 * _tmp1876 + _tmp1511 * _tmp1915 + _tmp1512 * _tmp1978 + + _tmp1513 * _tmp1949 + _tmp1515 * _tmp1931 + _tmp1516 * _tmp1942 + _tmp1517 * _tmp1977 + + _tmp1518 * _tmp1924; hessian_value_ptr[30] = - _tmp1349 * _tmp2142 + _tmp1355 * _tmp2044 + _tmp1362 * _tmp2129 + _tmp1363 * _tmp2131 + - _tmp1378 * _tmp2053 + _tmp1379 * _tmp2124 + _tmp1398 * _tmp2108 + _tmp1404 * _tmp2132 + - _tmp1409 * _tmp2075 + _tmp1414 * _tmp2118 + _tmp1421 * _tmp2138 + _tmp1422 * _tmp2095 + - _tmp1427 * _tmp2084 + _tmp1435 * _tmp2036 + _tmp1441 * _tmp2052 + _tmp1446 * _tmp2064 + - _tmp1452 * _tmp2100 + _tmp1461 * _tmp2125 + _tmp1463 * _tmp2140 + _tmp1469 * _tmp2080 + - _tmp1477 * _tmp2101 + _tmp1478 * _tmp2060 + _tmp1480 * _tmp2070 + _tmp1481 * _tmp2143 + - _tmp1484 * _tmp2130 + _tmp1487 * _tmp2066 + _tmp1488 * _tmp2090 + _tmp1490 * _tmp2035 + - _tmp1496 * _tmp2027 + _tmp1498 * _tmp2079 + _tmp1499 * _tmp2139 + _tmp1504 * _tmp2085 + - _tmp1505 * _tmp2144 + _tmp1508 * _tmp2137 + _tmp1515 * _tmp2141 + _tmp1516 * _tmp2065 + - _tmp1517 * _tmp2133 + _tmp1518 * _tmp2040 + _tmp1519 * _tmp2031 + _tmp1525 * _tmp2106 + - _tmp1527 * _tmp2102 + _tmp1533 * _tmp2123 + _tmp1534 * _tmp2134 + _tmp1535 * _tmp2113 + - _tmp1536 * _tmp2048 + _tmp1539 * _tmp2094 + _tmp1540 * _tmp2136 + _tmp1541 * _tmp2112 + - _tmp1542 * _tmp2135 + _tmp1544 * _tmp2111 + _tmp1545 * _tmp2110 + _tmp1547 * _tmp2054; - hessian_value_ptr[31] = _tmp1446 * _tmp2155 + _tmp1540 * _tmp2157; - hessian_value_ptr[32] = _tmp1404 * _tmp2164 + _tmp1488 * _tmp2162; - hessian_value_ptr[33] = _tmp1515 * _tmp2169 + _tmp1518 * _tmp2170; - hessian_value_ptr[34] = _tmp1414 * _tmp2178 + _tmp1499 * _tmp2175; - hessian_value_ptr[35] = _tmp1421 * _tmp2184 + _tmp1478 * _tmp2185; - hessian_value_ptr[36] = _tmp1461 * _tmp2189 + _tmp1539 * _tmp2190; - hessian_value_ptr[37] = _tmp1469 * _tmp2196 + _tmp1498 * _tmp2195; - hessian_value_ptr[38] = _tmp1435 * _tmp2203 + _tmp1519 * _tmp2202; - hessian_value_ptr[39] = _tmp1362 * _tmp2207 + _tmp1363 * _tmp2209; - hessian_value_ptr[40] = _tmp1409 * _tmp2215 + _tmp1422 * _tmp2213; - hessian_value_ptr[41] = _tmp1441 * _tmp2219 + _tmp1534 * _tmp2220; - hessian_value_ptr[42] = _tmp1355 * _tmp2227 + _tmp1379 * _tmp2226; - hessian_value_ptr[43] = _tmp1525 * _tmp2233 + _tmp1545 * _tmp2231; - hessian_value_ptr[44] = _tmp1504 * _tmp2239 + _tmp1536 * _tmp2238; - hessian_value_ptr[45] = _tmp1452 * _tmp2243 + _tmp1463 * _tmp2244; - hessian_value_ptr[46] = _tmp1533 * _tmp2251 + _tmp1542 * _tmp2250; - hessian_value_ptr[47] = _tmp1427 * _tmp2256 + _tmp1535 * _tmp2257; - hessian_value_ptr[48] = _tmp1349 * _tmp2263 + _tmp1490 * _tmp2264; - hessian_value_ptr[49] = _tmp1496 * _tmp2270 + _tmp1547 * _tmp2268; - hessian_value_ptr[50] = _tmp1477 * _tmp2276 + _tmp1480 * _tmp2277; - hessian_value_ptr[51] = std::pow(_tmp1555, Scalar(2)) + std::pow(_tmp1559, Scalar(2)) + - std::pow(_tmp1563, Scalar(2)) + std::pow(_tmp1576, Scalar(2)) + - std::pow(_tmp1580, Scalar(2)) + std::pow(_tmp1582, Scalar(2)) + - std::pow(_tmp1584, Scalar(2)) + std::pow(_tmp1588, Scalar(2)) + - std::pow(_tmp1592, Scalar(2)) + std::pow(_tmp1596, Scalar(2)) + - std::pow(_tmp1597, Scalar(2)) + std::pow(_tmp1610, Scalar(2)) + - std::pow(_tmp1614, Scalar(2)) + std::pow(_tmp1615, Scalar(2)) + - std::pow(_tmp1619, Scalar(2)) + std::pow(_tmp1621, Scalar(2)) + - std::pow(_tmp1625, Scalar(2)) + std::pow(_tmp1629, Scalar(2)) + - std::pow(_tmp1630, Scalar(2)) + std::pow(_tmp1631, Scalar(2)) + - std::pow(_tmp1636, Scalar(2)) + std::pow(_tmp1637, Scalar(2)) + - std::pow(_tmp1641, Scalar(2)) + std::pow(_tmp1646, Scalar(2)) + - std::pow(_tmp1647, Scalar(2)) + std::pow(_tmp1648, Scalar(2)) + - std::pow(_tmp1652, Scalar(2)) + std::pow(_tmp1653, Scalar(2)) + - std::pow(_tmp1654, Scalar(2)) + std::pow(_tmp1655, Scalar(2)) + - std::pow(_tmp1659, Scalar(2)) + std::pow(_tmp1663, Scalar(2)) + - std::pow(_tmp1664, Scalar(2)) + std::pow(_tmp1665, Scalar(2)) + - std::pow(_tmp1669, Scalar(2)) + std::pow(_tmp1670, Scalar(2)) + - std::pow(_tmp1674, Scalar(2)) + std::pow(_tmp1675, Scalar(2)) + - std::pow(_tmp1676, Scalar(2)) + std::pow(_tmp1677, Scalar(2)) + - std::pow(_tmp1678, Scalar(2)) + std::pow(_tmp1679, Scalar(2)) + - std::pow(_tmp1680, Scalar(2)) + std::pow(_tmp1681, Scalar(2)) + - std::pow(_tmp1682, Scalar(2)) + std::pow(_tmp1683, Scalar(2)) + - std::pow(_tmp1685, Scalar(2)) + std::pow(_tmp1686, Scalar(2)) + - std::pow(_tmp1687, Scalar(2)) + std::pow(_tmp1692, Scalar(2)) + - std::pow(_tmp1693, Scalar(2)) + std::pow(_tmp1694, Scalar(2)); + _tmp1331 * _tmp2100 + _tmp1338 * _tmp2003 + _tmp1347 * _tmp2086 + _tmp1348 * _tmp2088 + + _tmp1349 * _tmp2081 + _tmp1356 * _tmp2090 + _tmp1361 * _tmp2035 + _tmp1366 * _tmp2073 + + _tmp1373 * _tmp2094 + _tmp1374 * _tmp2055 + _tmp1375 * _tmp2080 + _tmp1381 * _tmp2044 + + _tmp1389 * _tmp1994 + _tmp1396 * _tmp2012 + _tmp1405 * _tmp2102 + _tmp1410 * _tmp2024 + + _tmp1415 * _tmp2060 + _tmp1422 * _tmp2082 + _tmp1423 * _tmp2079 + _tmp1424 * _tmp2096 + + _tmp1434 * _tmp2040 + _tmp1443 * _tmp2061 + _tmp1444 * _tmp2020 + _tmp1446 * _tmp2031 + + _tmp1462 * _tmp2087 + _tmp1466 * _tmp2089 + _tmp1468 * _tmp2050 + _tmp1469 * _tmp1993 + + _tmp1475 * _tmp1984 + _tmp1476 * _tmp2025 + _tmp1478 * _tmp2039 + _tmp1479 * _tmp2095 + + _tmp1486 * _tmp2045 + _tmp1492 * _tmp2098 + _tmp1493 * _tmp1998 + _tmp1494 * _tmp1988 + + _tmp1500 * _tmp2065 + _tmp1505 * _tmp2078 + _tmp1506 * _tmp2091 + _tmp1507 * _tmp2069 + + _tmp1509 * _tmp2007 + _tmp1511 * _tmp2101 + _tmp1512 * _tmp2054 + _tmp1513 * _tmp2093 + + _tmp1515 * _tmp2068 + _tmp1516 * _tmp2092 + _tmp1517 * _tmp2066 + _tmp1518 * _tmp2014 + + _tmp2067 * _tmp2238; + hessian_value_ptr[31] = _tmp1410 * _tmp2114 + _tmp1513 * _tmp2116; + hessian_value_ptr[32] = _tmp1356 * _tmp2122 + _tmp1468 * _tmp2121; + hessian_value_ptr[33] = _tmp1492 * _tmp2127 + _tmp1493 * _tmp2128; + hessian_value_ptr[34] = _tmp1366 * _tmp2135 + _tmp1479 * _tmp2132; + hessian_value_ptr[35] = _tmp1373 * _tmp2140 + _tmp1444 * _tmp2141; + hessian_value_ptr[36] = _tmp1422 * _tmp2145 + _tmp1512 * _tmp2146; + hessian_value_ptr[37] = _tmp1434 * _tmp2153 + _tmp1478 * _tmp2152; + hessian_value_ptr[38] = _tmp1389 * _tmp2160 + _tmp1494 * _tmp2159; + hessian_value_ptr[39] = _tmp1347 * _tmp2164 + _tmp1348 * _tmp2166; + hessian_value_ptr[40] = _tmp1361 * _tmp2173 + _tmp1374 * _tmp2171; + hessian_value_ptr[41] = _tmp1396 * _tmp2178 + _tmp1506 * _tmp2179; + hessian_value_ptr[42] = _tmp1338 * _tmp2186 + _tmp1349 * _tmp2185; + hessian_value_ptr[43] = _tmp1500 * _tmp2192 + _tmp1517 * _tmp2190; + hessian_value_ptr[44] = _tmp1486 * _tmp2198 + _tmp1509 * _tmp2197; + hessian_value_ptr[45] = _tmp1415 * _tmp2202 + _tmp1424 * _tmp2203; + hessian_value_ptr[46] = _tmp1505 * _tmp2210 + _tmp1516 * _tmp2209; + hessian_value_ptr[47] = _tmp1381 * _tmp2215 + _tmp1507 * _tmp2216; + hessian_value_ptr[48] = _tmp1331 * _tmp2222 + _tmp1469 * _tmp2223; + hessian_value_ptr[49] = _tmp1475 * _tmp2229 + _tmp1518 * _tmp2227; + hessian_value_ptr[50] = _tmp1443 * _tmp2234 + _tmp1446 * _tmp2236; + hessian_value_ptr[51] = + std::pow(_tmp1526, Scalar(2)) + std::pow(_tmp1530, Scalar(2)) + + std::pow(_tmp1535, Scalar(2)) + std::pow(_tmp1539, Scalar(2)) + + std::pow(_tmp1543, Scalar(2)) + + std::pow(_tmp1550, Scalar(2)) * std::pow(priors_1_0_sqrt_info(4, 4), Scalar(2)) + + std::pow(_tmp1552, Scalar(2)) + std::pow(_tmp1556, Scalar(2)) + + std::pow(_tmp1560, Scalar(2)) + std::pow(_tmp1564, Scalar(2)) + + std::pow(_tmp1566, Scalar(2)) + std::pow(_tmp1570, Scalar(2)) + + std::pow(_tmp1574, Scalar(2)) + std::pow(_tmp1578, Scalar(2)) + + std::pow(_tmp1582, Scalar(2)) + std::pow(_tmp1583, Scalar(2)) + + std::pow(_tmp1584, Scalar(2)) + std::pow(_tmp1591, Scalar(2)) + + std::pow(_tmp1596, Scalar(2)) + std::pow(_tmp1598, Scalar(2)) + + std::pow(_tmp1599, Scalar(2)) + std::pow(_tmp1603, Scalar(2)) + + std::pow(_tmp1609, Scalar(2)) + std::pow(_tmp1613, Scalar(2)) + + std::pow(_tmp1615, Scalar(2)) + std::pow(_tmp1616, Scalar(2)) + + std::pow(_tmp1621, Scalar(2)) + std::pow(_tmp1625, Scalar(2)) + + std::pow(_tmp1626, Scalar(2)) + std::pow(_tmp1627, Scalar(2)) + + std::pow(_tmp1631, Scalar(2)) + std::pow(_tmp1632, Scalar(2)) + + std::pow(_tmp1636, Scalar(2)) + std::pow(_tmp1638, Scalar(2)) + + std::pow(_tmp1639, Scalar(2)) + std::pow(_tmp1640, Scalar(2)) + + std::pow(_tmp1641, Scalar(2)) + std::pow(_tmp1642, Scalar(2)) + + std::pow(_tmp1643, Scalar(2)) + std::pow(_tmp1644, Scalar(2)) + + std::pow(_tmp1645, Scalar(2)) + std::pow(_tmp1646, Scalar(2)) + + std::pow(_tmp1648, Scalar(2)) + std::pow(_tmp1649, Scalar(2)) + + std::pow(_tmp1654, Scalar(2)) + std::pow(_tmp1655, Scalar(2)) + + std::pow(_tmp1656, Scalar(2)) + std::pow(_tmp1657, Scalar(2)); hessian_value_ptr[52] = - _tmp1555 * _tmp1878 + _tmp1559 * _tmp1832 + _tmp1563 * _tmp1710 + _tmp1576 * _tmp1890 + - _tmp1580 * _tmp1870 + _tmp1582 * _tmp1802 + _tmp1584 * _tmp1833 + _tmp1588 * _tmp1860 + - _tmp1592 * _tmp1805 + _tmp1596 * _tmp1892 + _tmp1597 * _tmp1773 + _tmp1610 * _tmp1792 + - _tmp1614 * _tmp1703 + _tmp1615 * _tmp1815 + _tmp1619 * _tmp1757 + _tmp1621 * _tmp1748 + - _tmp1625 * _tmp1869 + _tmp1629 * _tmp1765 + _tmp1630 * _tmp1803 + _tmp1631 * _tmp1836 + - _tmp1636 * _tmp1789 + _tmp1637 * _tmp1814 + _tmp1641 * _tmp1737 + _tmp1646 * _tmp1718 + - _tmp1647 * _tmp1895 + _tmp1648 * _tmp1791 + _tmp1652 * _tmp1835 + _tmp1653 * _tmp1849 + - _tmp1654 * _tmp1719 + _tmp1655 * _tmp1801 + _tmp1659 * _tmp1839 + _tmp1663 * _tmp1838 + - _tmp1664 * _tmp1746 + _tmp1665 * _tmp1893 + _tmp1669 * _tmp1894 + _tmp1670 * _tmp1824 + - _tmp1674 * _tmp1831 + _tmp1675 * _tmp1891 + _tmp1676 * _tmp1888 + _tmp1677 * _tmp1727 + - _tmp1678 * _tmp1877 + _tmp1679 * _tmp1837 + _tmp1680 * _tmp1782 + _tmp1681 * _tmp1747 + - _tmp1682 * _tmp1728 + _tmp1683 * _tmp1879 + _tmp1685 * _tmp1756 + _tmp1686 * _tmp1800 + - _tmp1687 * _tmp1729 + _tmp1692 * _tmp1887 + _tmp1693 * _tmp1858 + _tmp1694 * _tmp1889; + _tmp1526 * _tmp1839 + _tmp1530 * _tmp1793 + _tmp1535 * _tmp1674 + _tmp1539 * _tmp1750 + + _tmp1543 * _tmp1829 + _tmp1552 * _tmp1762 + _tmp1556 * _tmp1818 + _tmp1560 * _tmp1764 + + _tmp1564 * _tmp1853 + _tmp1565 * _tmp1840 + _tmp1566 * _tmp1734 + _tmp1570 * _tmp1666 + + _tmp1574 * _tmp1716 + _tmp1578 * _tmp1826 + _tmp1582 * _tmp1725 + _tmp1583 * _tmp1794 + + _tmp1584 * _tmp1763 + _tmp1591 * _tmp1796 + _tmp1596 * _tmp1749 + _tmp1598 * _tmp1773 + + _tmp1599 * _tmp1856 + _tmp1603 * _tmp1699 + _tmp1609 * _tmp1682 + _tmp1613 * _tmp1795 + + _tmp1615 * _tmp1808 + _tmp1616 * _tmp1761 + _tmp1621 * _tmp1800 + _tmp1625 * _tmp1799 + + _tmp1626 * _tmp1706 + _tmp1627 * _tmp1854 + _tmp1631 * _tmp1855 + _tmp1632 * _tmp1784 + + _tmp1636 * _tmp1792 + _tmp1638 * _tmp1851 + _tmp1639 * _tmp1690 + _tmp1640 * _tmp1837 + + _tmp1641 * _tmp1798 + _tmp1642 * _tmp1760 + _tmp1643 * _tmp1742 + _tmp1644 * _tmp1707 + + _tmp1645 * _tmp1691 + _tmp1646 * _tmp1841 + _tmp1648 * _tmp1715 + _tmp1649 * _tmp1758 + + _tmp1654 * _tmp1849 + _tmp1655 * _tmp1817 + _tmp1656 * _tmp1850 + _tmp1657 * _tmp1774; hessian_value_ptr[53] = - _tmp1555 * _tmp1935 + _tmp1559 * _tmp1973 + _tmp1563 * _tmp2014 + _tmp1576 * _tmp2017 + - _tmp1580 * _tmp1939 + _tmp1582 * _tmp1972 + _tmp1584 * _tmp1997 + _tmp1588 * _tmp1916 + - _tmp1592 * _tmp1988 + _tmp1596 * _tmp2002 + _tmp1597 * _tmp2004 + _tmp1610 * _tmp1990 + - _tmp1614 * _tmp1951 + _tmp1615 * _tmp1966 + _tmp1619 * _tmp1962 + _tmp1621 * _tmp1923 + - _tmp1625 * _tmp1907 + _tmp1629 * _tmp1929 + _tmp1630 * _tmp2006 + _tmp1631 * _tmp1989 + - _tmp1636 * _tmp1971 + _tmp1637 * _tmp1899 + _tmp1641 * _tmp2019 + _tmp1646 * _tmp1911 + - _tmp1647 * _tmp2022 + _tmp1648 * _tmp1955 + _tmp1652 * _tmp1903 + _tmp1653 * _tmp1984 + - _tmp1654 * _tmp1922 + _tmp1655 * _tmp2021 + _tmp1659 * _tmp1978 + _tmp1663 * _tmp1994 + - _tmp1664 * _tmp1963 + _tmp1665 * _tmp1964 + _tmp1669 * _tmp1921 + _tmp1670 * _tmp2018 + - _tmp1674 * _tmp2003 + _tmp1675 * _tmp1912 + _tmp1676 * _tmp1995 + _tmp1677 * _tmp1965 + - _tmp1678 * _tmp2007 + _tmp1679 * _tmp1961 + _tmp1680 * _tmp1996 + _tmp1681 * _tmp1947 + - _tmp1682 * _tmp2016 + _tmp1683 * _tmp1983 + _tmp1685 * _tmp1943 + _tmp1686 * _tmp2023 + - _tmp1687 * _tmp1952 + _tmp1692 * _tmp2015 + _tmp1693 * _tmp2005 + _tmp1694 * _tmp1982; + _tmp1526 * _tmp1895 + _tmp1530 * _tmp1933 + _tmp1535 * _tmp1973 + _tmp1539 * _tmp1965 + + _tmp1543 * _tmp1899 + _tmp1552 * _tmp1931 + _tmp1556 * _tmp1882 + _tmp1560 * _tmp1949 + + _tmp1564 * _tmp1961 + _tmp1565 * _tmp1902 + _tmp1566 * _tmp1963 + _tmp1570 * _tmp1914 + + _tmp1574 * _tmp1920 + _tmp1578 * _tmp1871 + _tmp1582 * _tmp1891 + _tmp1583 * _tmp1943 + + _tmp1584 * _tmp1966 + _tmp1591 * _tmp1950 + _tmp1596 * _tmp1930 + _tmp1598 * _tmp1861 + + _tmp1599 * _tmp1979 + _tmp1603 * _tmp1977 + _tmp1609 * _tmp1875 + _tmp1613 * _tmp1867 + + _tmp1615 * _tmp1944 + _tmp1616 * _tmp1978 + _tmp1621 * _tmp1937 + _tmp1625 * _tmp1954 + + _tmp1626 * _tmp1921 + _tmp1627 * _tmp1922 + _tmp1631 * _tmp1886 + _tmp1632 * _tmp1976 + + _tmp1636 * _tmp1962 + _tmp1638 * _tmp1876 + _tmp1639 * _tmp1924 + _tmp1640 * _tmp1967 + + _tmp1641 * _tmp1919 + _tmp1642 * _tmp1932 + _tmp1643 * _tmp1955 + _tmp1644 * _tmp1910 + + _tmp1645 * _tmp1975 + _tmp1646 * _tmp1942 + _tmp1648 * _tmp1906 + _tmp1649 * _tmp1980 + + _tmp1654 * _tmp1974 + _tmp1655 * _tmp1964 + _tmp1656 * _tmp1941 + _tmp1657 * _tmp1915; hessian_value_ptr[54] = - _tmp1555 * _tmp2140 + _tmp1559 * _tmp2131 + _tmp1563 * _tmp2101 + _tmp1576 * _tmp2143 + - _tmp1580 * _tmp2027 + _tmp1582 * _tmp2112 + _tmp1584 * _tmp2137 + _tmp1588 * _tmp2134 + - _tmp1592 * _tmp2136 + _tmp1596 * _tmp2113 + _tmp1597 * _tmp2064 + _tmp1610 * _tmp2102 + - _tmp1614 * _tmp2040 + _tmp1615 * _tmp2111 + _tmp1619 * _tmp2079 + _tmp1621 * _tmp2065 + - _tmp1625 * _tmp2035 + _tmp1629 * _tmp2125 + _tmp1630 * _tmp2141 + _tmp1631 * _tmp2130 + - _tmp1636 * _tmp2123 + _tmp1637 * _tmp2129 + _tmp1641 * _tmp2110 + _tmp1646 * _tmp2085 + - _tmp1647 * _tmp2144 + _tmp1648 * _tmp2108 + _tmp1652 * _tmp2044 + _tmp1653 * _tmp2052 + - _tmp1654 * _tmp2066 + _tmp1655 * _tmp2094 + _tmp1659 * _tmp2036 + _tmp1663 * _tmp2060 + - _tmp1664 * _tmp2124 + _tmp1665 * _tmp2142 + _tmp1669 * _tmp2118 + _tmp1670 * _tmp2100 + - _tmp1674 * _tmp2132 + _tmp1675 * _tmp2048 + _tmp1676 * _tmp2133 + _tmp1677 * _tmp2054 + - _tmp1678 * _tmp2139 + _tmp1679 * _tmp2090 + _tmp1680 * _tmp2138 + _tmp1681 * _tmp2106 + - _tmp1682 * _tmp2070 + _tmp1683 * _tmp2135 + _tmp1685 * _tmp2080 + _tmp1686 * _tmp2031 + - _tmp1687 * _tmp2053 + _tmp1692 * _tmp2095 + _tmp1693 * _tmp2084 + _tmp1694 * _tmp2075; - hessian_value_ptr[55] = _tmp1592 * _tmp2157 + _tmp1597 * _tmp2155; - hessian_value_ptr[56] = _tmp1674 * _tmp2164 + _tmp1679 * _tmp2162; - hessian_value_ptr[57] = _tmp1614 * _tmp2170 + _tmp1630 * _tmp2169; - hessian_value_ptr[58] = _tmp1669 * _tmp2178 + _tmp1678 * _tmp2175; - hessian_value_ptr[59] = _tmp1663 * _tmp2185 + _tmp1680 * _tmp2184; - hessian_value_ptr[60] = _tmp1629 * _tmp2189 + _tmp1655 * _tmp2190; - hessian_value_ptr[61] = _tmp1619 * _tmp2195 + _tmp1685 * _tmp2196; - hessian_value_ptr[62] = _tmp1659 * _tmp2203 + _tmp1686 * _tmp2202; - hessian_value_ptr[63] = _tmp1559 * _tmp2209 + _tmp1637 * _tmp2207; - hessian_value_ptr[64] = _tmp1692 * _tmp2213 + _tmp1694 * _tmp2215; - hessian_value_ptr[65] = _tmp1588 * _tmp2220 + _tmp1653 * _tmp2219; - hessian_value_ptr[66] = _tmp1652 * _tmp2227 + _tmp1664 * _tmp2226; - hessian_value_ptr[67] = _tmp1641 * _tmp2231 + _tmp1681 * _tmp2233; - hessian_value_ptr[68] = _tmp1646 * _tmp2239 + _tmp1675 * _tmp2238; - hessian_value_ptr[69] = _tmp1555 * _tmp2244 + _tmp1670 * _tmp2243; - hessian_value_ptr[70] = _tmp1636 * _tmp2251 + _tmp1683 * _tmp2250; - hessian_value_ptr[71] = _tmp1596 * _tmp2257 + _tmp1693 * _tmp2256; - hessian_value_ptr[72] = _tmp1625 * _tmp2264 + _tmp1665 * _tmp2263; - hessian_value_ptr[73] = _tmp1580 * _tmp2270 + _tmp1677 * _tmp2268; - hessian_value_ptr[74] = _tmp1563 * _tmp2276 + _tmp1682 * _tmp2277; - hessian_value_ptr[75] = std::pow(_tmp1703, Scalar(2)) + std::pow(_tmp1710, Scalar(2)) + - std::pow(_tmp1718, Scalar(2)) + std::pow(_tmp1719, Scalar(2)) + - std::pow(_tmp1727, Scalar(2)) + std::pow(_tmp1728, Scalar(2)) + - std::pow(_tmp1729, Scalar(2)) + std::pow(_tmp1737, Scalar(2)) + - std::pow(_tmp1746, Scalar(2)) + std::pow(_tmp1747, Scalar(2)) + - std::pow(_tmp1748, Scalar(2)) + std::pow(_tmp1756, Scalar(2)) + - std::pow(_tmp1757, Scalar(2)) + std::pow(_tmp1765, Scalar(2)) + - std::pow(_tmp1773, Scalar(2)) + std::pow(_tmp1782, Scalar(2)) + - std::pow(_tmp1789, Scalar(2)) + std::pow(_tmp1791, Scalar(2)) + - std::pow(_tmp1792, Scalar(2)) + std::pow(_tmp1800, Scalar(2)) + - std::pow(_tmp1801, Scalar(2)) + std::pow(_tmp1802, Scalar(2)) + - std::pow(_tmp1803, Scalar(2)) + std::pow(_tmp1805, Scalar(2)) + - std::pow(_tmp1814, Scalar(2)) + std::pow(_tmp1815, Scalar(2)) + - std::pow(_tmp1824, Scalar(2)) + std::pow(_tmp1831, Scalar(2)) + - std::pow(_tmp1832, Scalar(2)) + std::pow(_tmp1833, Scalar(2)) + - std::pow(_tmp1835, Scalar(2)) + std::pow(_tmp1836, Scalar(2)) + - std::pow(_tmp1837, Scalar(2)) + std::pow(_tmp1838, Scalar(2)) + - std::pow(_tmp1839, Scalar(2)) + std::pow(_tmp1849, Scalar(2)) + - std::pow(_tmp1858, Scalar(2)) + std::pow(_tmp1860, Scalar(2)) + - std::pow(_tmp1869, Scalar(2)) + std::pow(_tmp1870, Scalar(2)) + - std::pow(_tmp1877, Scalar(2)) + std::pow(_tmp1878, Scalar(2)) + - std::pow(_tmp1879, Scalar(2)) + std::pow(_tmp1887, Scalar(2)) + - std::pow(_tmp1888, Scalar(2)) + std::pow(_tmp1889, Scalar(2)) + - std::pow(_tmp1890, Scalar(2)) + std::pow(_tmp1891, Scalar(2)) + - std::pow(_tmp1892, Scalar(2)) + std::pow(_tmp1893, Scalar(2)) + - std::pow(_tmp1894, Scalar(2)) + std::pow(_tmp1895, Scalar(2)); + _tmp1526 * _tmp2096 + _tmp1530 * _tmp2088 + _tmp1535 * _tmp2061 + _tmp1539 * _tmp2025 + + _tmp1543 * _tmp1984 + _tmp1552 * _tmp2068 + _tmp1556 * _tmp2091 + _tmp1560 * _tmp2093 + + _tmp1564 * _tmp2069 + _tmp1565 * _tmp2080 + _tmp1566 * _tmp2024 + _tmp1570 * _tmp1998 + + _tmp1574 * _tmp2039 + _tmp1578 * _tmp1993 + _tmp1582 * _tmp2082 + _tmp1583 * _tmp2089 + + _tmp1584 * _tmp2098 + _tmp1591 * _tmp2087 + _tmp1596 * _tmp2078 + _tmp1598 * _tmp2086 + + _tmp1599 * _tmp2102 + _tmp1603 * _tmp2066 + _tmp1609 * _tmp2045 + _tmp1613 * _tmp2003 + + _tmp1615 * _tmp2012 + _tmp1616 * _tmp2054 + _tmp1621 * _tmp1994 + _tmp1625 * _tmp2020 + + _tmp1626 * _tmp2081 + _tmp1627 * _tmp2100 + _tmp1631 * _tmp2073 + _tmp1632 * _tmp2060 + + _tmp1636 * _tmp2090 + _tmp1638 * _tmp2007 + _tmp1639 * _tmp2014 + _tmp1640 * _tmp2095 + + _tmp1641 * _tmp2050 + _tmp1642 * _tmp2079 + _tmp1643 * _tmp2094 + _tmp1644 * _tmp2065 + + _tmp1645 * _tmp2031 + _tmp1646 * _tmp2092 + _tmp1648 * _tmp2040 + _tmp1649 * _tmp1988 + + _tmp1654 * _tmp2055 + _tmp1655 * _tmp2044 + _tmp1656 * _tmp2035 + _tmp1657 * _tmp2101; + hessian_value_ptr[55] = _tmp1560 * _tmp2116 + _tmp1566 * _tmp2114; + hessian_value_ptr[56] = _tmp1636 * _tmp2122 + _tmp1641 * _tmp2121; + hessian_value_ptr[57] = _tmp1570 * _tmp2128 + _tmp1584 * _tmp2127; + hessian_value_ptr[58] = _tmp1631 * _tmp2135 + _tmp1640 * _tmp2132; + hessian_value_ptr[59] = _tmp1625 * _tmp2141 + _tmp1643 * _tmp2140; + hessian_value_ptr[60] = _tmp1582 * _tmp2145 + _tmp1616 * _tmp2146; + hessian_value_ptr[61] = _tmp1574 * _tmp2152 + _tmp1648 * _tmp2153; + hessian_value_ptr[62] = _tmp1621 * _tmp2160 + _tmp1649 * _tmp2159; + hessian_value_ptr[63] = _tmp1530 * _tmp2166 + _tmp1598 * _tmp2164; + hessian_value_ptr[64] = _tmp1654 * _tmp2171 + _tmp1656 * _tmp2173; + hessian_value_ptr[65] = _tmp1556 * _tmp2179 + _tmp1615 * _tmp2178; + hessian_value_ptr[66] = _tmp1613 * _tmp2186 + _tmp1626 * _tmp2185; + hessian_value_ptr[67] = _tmp1603 * _tmp2190 + _tmp1644 * _tmp2192; + hessian_value_ptr[68] = _tmp1609 * _tmp2198 + _tmp1638 * _tmp2197; + hessian_value_ptr[69] = _tmp1526 * _tmp2203 + _tmp1632 * _tmp2202; + hessian_value_ptr[70] = _tmp1596 * _tmp2210 + _tmp1646 * _tmp2209; + hessian_value_ptr[71] = _tmp1564 * _tmp2216 + _tmp1655 * _tmp2215; + hessian_value_ptr[72] = _tmp1578 * _tmp2223 + _tmp1627 * _tmp2222; + hessian_value_ptr[73] = _tmp1543 * _tmp2229 + _tmp1639 * _tmp2227; + hessian_value_ptr[74] = _tmp1535 * _tmp2234 + _tmp1645 * _tmp2236; + hessian_value_ptr[75] = _tmp1217 * std::pow(_tmp1325, Scalar(2)) + + std::pow(_tmp1666, Scalar(2)) + std::pow(_tmp1674, Scalar(2)) + + std::pow(_tmp1682, Scalar(2)) + std::pow(_tmp1690, Scalar(2)) + + std::pow(_tmp1691, Scalar(2)) + std::pow(_tmp1699, Scalar(2)) + + std::pow(_tmp1706, Scalar(2)) + std::pow(_tmp1707, Scalar(2)) + + std::pow(_tmp1715, Scalar(2)) + std::pow(_tmp1716, Scalar(2)) + + std::pow(_tmp1725, Scalar(2)) + std::pow(_tmp1734, Scalar(2)) + + std::pow(_tmp1742, Scalar(2)) + std::pow(_tmp1749, Scalar(2)) + + std::pow(_tmp1750, Scalar(2)) + std::pow(_tmp1758, Scalar(2)) + + std::pow(_tmp1760, Scalar(2)) + std::pow(_tmp1761, Scalar(2)) + + std::pow(_tmp1762, Scalar(2)) + std::pow(_tmp1763, Scalar(2)) + + std::pow(_tmp1764, Scalar(2)) + std::pow(_tmp1773, Scalar(2)) + + std::pow(_tmp1774, Scalar(2)) + std::pow(_tmp1784, Scalar(2)) + + std::pow(_tmp1792, Scalar(2)) + std::pow(_tmp1793, Scalar(2)) + + std::pow(_tmp1794, Scalar(2)) + std::pow(_tmp1795, Scalar(2)) + + std::pow(_tmp1796, Scalar(2)) + std::pow(_tmp1797, Scalar(2)) + + std::pow(_tmp1798, Scalar(2)) + std::pow(_tmp1799, Scalar(2)) + + std::pow(_tmp1800, Scalar(2)) + std::pow(_tmp1808, Scalar(2)) + + std::pow(_tmp1809, Scalar(2)) + std::pow(_tmp1817, Scalar(2)) + + std::pow(_tmp1818, Scalar(2)) + std::pow(_tmp1826, Scalar(2)) + + _tmp1827 * std::pow(_tmp51, Scalar(2)) + std::pow(_tmp1829, Scalar(2)) + + std::pow(_tmp1837, Scalar(2)) + std::pow(_tmp1839, Scalar(2)) + + std::pow(_tmp1840, Scalar(2)) + std::pow(_tmp1841, Scalar(2)) + + std::pow(_tmp1849, Scalar(2)) + std::pow(_tmp1850, Scalar(2)) + + std::pow(_tmp1851, Scalar(2)) + std::pow(_tmp1853, Scalar(2)) + + std::pow(_tmp1854, Scalar(2)) + std::pow(_tmp1855, Scalar(2)) + + std::pow(_tmp1856, Scalar(2)); hessian_value_ptr[76] = - _tmp1703 * _tmp1951 + _tmp1710 * _tmp2014 + _tmp1718 * _tmp1911 + _tmp1719 * _tmp1922 + - _tmp1727 * _tmp1965 + _tmp1728 * _tmp2016 + _tmp1729 * _tmp1952 + _tmp1737 * _tmp2019 + - _tmp1746 * _tmp1963 + _tmp1747 * _tmp1947 + _tmp1748 * _tmp1923 + _tmp1756 * _tmp1943 + - _tmp1757 * _tmp1962 + _tmp1765 * _tmp1929 + _tmp1773 * _tmp2004 + _tmp1782 * _tmp1996 + - _tmp1789 * _tmp1971 + _tmp1791 * _tmp1955 + _tmp1792 * _tmp1990 + _tmp1800 * _tmp2023 + - _tmp1801 * _tmp2021 + _tmp1802 * _tmp1972 + _tmp1803 * _tmp2006 + _tmp1805 * _tmp1988 + - _tmp1814 * _tmp1899 + _tmp1815 * _tmp1966 + _tmp1824 * _tmp2018 + _tmp1831 * _tmp2003 + - _tmp1832 * _tmp1973 + _tmp1833 * _tmp1997 + _tmp1835 * _tmp1903 + _tmp1836 * _tmp1989 + - _tmp1837 * _tmp1961 + _tmp1838 * _tmp1994 + _tmp1839 * _tmp1978 + _tmp1849 * _tmp1984 + - _tmp1858 * _tmp2005 + _tmp1860 * _tmp1916 + _tmp1869 * _tmp1907 + _tmp1870 * _tmp1939 + - _tmp1877 * _tmp2007 + _tmp1878 * _tmp1935 + _tmp1879 * _tmp1983 + _tmp1887 * _tmp2015 + - _tmp1888 * _tmp1995 + _tmp1889 * _tmp1982 + _tmp1890 * _tmp2017 + _tmp1891 * _tmp1912 + - _tmp1892 * _tmp2002 + _tmp1893 * _tmp1964 + _tmp1894 * _tmp1921 + _tmp1895 * _tmp2022; + _tmp1325 * _tmp1925 + _tmp1666 * _tmp1914 + _tmp1674 * _tmp1973 + _tmp1682 * _tmp1875 + + _tmp1690 * _tmp1924 + _tmp1691 * _tmp1975 + _tmp1699 * _tmp1977 + _tmp1706 * _tmp1921 + + _tmp1707 * _tmp1910 + _tmp1715 * _tmp1906 + _tmp1716 * _tmp1920 + _tmp1725 * _tmp1891 + + _tmp1734 * _tmp1963 + _tmp1742 * _tmp1955 + _tmp1749 * _tmp1930 + _tmp1750 * _tmp1965 + + _tmp1758 * _tmp1980 + _tmp1760 * _tmp1932 + _tmp1761 * _tmp1978 + _tmp1762 * _tmp1931 + + _tmp1763 * _tmp1966 + _tmp1764 * _tmp1949 + _tmp1773 * _tmp1861 + _tmp1774 * _tmp1915 + + _tmp1784 * _tmp1976 + _tmp1792 * _tmp1962 + _tmp1793 * _tmp1933 + _tmp1794 * _tmp1943 + + _tmp1795 * _tmp1867 + _tmp1796 * _tmp1950 + _tmp1797 * _tmp1956 + _tmp1798 * _tmp1919 + + _tmp1799 * _tmp1954 + _tmp1800 * _tmp1937 + _tmp1808 * _tmp1944 + _tmp1809 * _tmp1923 + + _tmp1817 * _tmp1964 + _tmp1818 * _tmp1882 + _tmp1826 * _tmp1871 + + _tmp1827 * _tmp51 * _tmp52 + _tmp1829 * _tmp1899 + _tmp1837 * _tmp1967 + + _tmp1839 * _tmp1895 + _tmp1840 * _tmp1902 + _tmp1841 * _tmp1942 + _tmp1849 * _tmp1974 + + _tmp1850 * _tmp1941 + _tmp1851 * _tmp1876 + _tmp1853 * _tmp1961 + _tmp1854 * _tmp1922 + + _tmp1855 * _tmp1886 + _tmp1856 * _tmp1979; hessian_value_ptr[77] = - _tmp1703 * _tmp2040 + _tmp1710 * _tmp2101 + _tmp1718 * _tmp2085 + _tmp1719 * _tmp2066 + - _tmp1727 * _tmp2054 + _tmp1728 * _tmp2070 + _tmp1729 * _tmp2053 + _tmp1737 * _tmp2110 + - _tmp1746 * _tmp2124 + _tmp1747 * _tmp2106 + _tmp1748 * _tmp2065 + _tmp1756 * _tmp2080 + - _tmp1757 * _tmp2079 + _tmp1765 * _tmp2125 + _tmp1773 * _tmp2064 + _tmp1782 * _tmp2138 + - _tmp1789 * _tmp2123 + _tmp1791 * _tmp2108 + _tmp1792 * _tmp2102 + _tmp1800 * _tmp2031 + - _tmp1801 * _tmp2094 + _tmp1802 * _tmp2112 + _tmp1803 * _tmp2141 + _tmp1805 * _tmp2136 + - _tmp1814 * _tmp2129 + _tmp1815 * _tmp2111 + _tmp1824 * _tmp2100 + _tmp1831 * _tmp2132 + - _tmp1832 * _tmp2131 + _tmp1833 * _tmp2137 + _tmp1835 * _tmp2044 + _tmp1836 * _tmp2130 + - _tmp1837 * _tmp2090 + _tmp1838 * _tmp2060 + _tmp1839 * _tmp2036 + _tmp1849 * _tmp2052 + - _tmp1858 * _tmp2084 + _tmp1860 * _tmp2134 + _tmp1869 * _tmp2035 + _tmp1870 * _tmp2027 + - _tmp1877 * _tmp2139 + _tmp1878 * _tmp2140 + _tmp1879 * _tmp2135 + _tmp1887 * _tmp2095 + - _tmp1888 * _tmp2133 + _tmp1889 * _tmp2075 + _tmp1890 * _tmp2143 + _tmp1891 * _tmp2048 + - _tmp1892 * _tmp2113 + _tmp1893 * _tmp2142 + _tmp1894 * _tmp2118 + _tmp1895 * _tmp2144; - hessian_value_ptr[78] = _tmp1773 * _tmp2155 + _tmp1805 * _tmp2157; - hessian_value_ptr[79] = _tmp1831 * _tmp2164 + _tmp1837 * _tmp2162; - hessian_value_ptr[80] = _tmp1703 * _tmp2170 + _tmp1803 * _tmp2169; - hessian_value_ptr[81] = _tmp1877 * _tmp2175 + _tmp1894 * _tmp2178; - hessian_value_ptr[82] = _tmp1782 * _tmp2184 + _tmp1838 * _tmp2185; - hessian_value_ptr[83] = _tmp1765 * _tmp2189 + _tmp1801 * _tmp2190; - hessian_value_ptr[84] = _tmp1756 * _tmp2196 + _tmp1757 * _tmp2195; - hessian_value_ptr[85] = _tmp1800 * _tmp2202 + _tmp1839 * _tmp2203; - hessian_value_ptr[86] = _tmp1814 * _tmp2207 + _tmp1832 * _tmp2209; - hessian_value_ptr[87] = _tmp1887 * _tmp2213 + _tmp1889 * _tmp2215; - hessian_value_ptr[88] = _tmp1849 * _tmp2219 + _tmp1860 * _tmp2220; - hessian_value_ptr[89] = _tmp1746 * _tmp2226 + _tmp1835 * _tmp2227; - hessian_value_ptr[90] = _tmp1737 * _tmp2231 + _tmp1747 * _tmp2233; - hessian_value_ptr[91] = _tmp1718 * _tmp2239 + _tmp1891 * _tmp2238; - hessian_value_ptr[92] = _tmp1824 * _tmp2243 + _tmp1878 * _tmp2244; - hessian_value_ptr[93] = _tmp1789 * _tmp2251 + _tmp1879 * _tmp2250; - hessian_value_ptr[94] = _tmp1858 * _tmp2256 + _tmp1892 * _tmp2257; - hessian_value_ptr[95] = _tmp1869 * _tmp2264 + _tmp1893 * _tmp2263; - hessian_value_ptr[96] = _tmp1727 * _tmp2268 + _tmp1870 * _tmp2270; - hessian_value_ptr[97] = _tmp1710 * _tmp2276 + _tmp1728 * _tmp2277; - hessian_value_ptr[98] = std::pow(_tmp1899, Scalar(2)) + std::pow(_tmp1903, Scalar(2)) + - std::pow(_tmp1907, Scalar(2)) + std::pow(_tmp1911, Scalar(2)) + - std::pow(_tmp1912, Scalar(2)) + std::pow(_tmp1916, Scalar(2)) + + _tmp1217 * _tmp1325 * _tmp2067 + _tmp1666 * _tmp1998 + _tmp1674 * _tmp2061 + + _tmp1682 * _tmp2045 + _tmp1690 * _tmp2014 + _tmp1691 * _tmp2031 + _tmp1699 * _tmp2066 + + _tmp1706 * _tmp2081 + _tmp1707 * _tmp2065 + _tmp1715 * _tmp2040 + _tmp1716 * _tmp2039 + + _tmp1725 * _tmp2082 + _tmp1734 * _tmp2024 + _tmp1742 * _tmp2094 + _tmp1749 * _tmp2078 + + _tmp1750 * _tmp2025 + _tmp1758 * _tmp1988 + _tmp1760 * _tmp2079 + _tmp1761 * _tmp2054 + + _tmp1762 * _tmp2068 + _tmp1763 * _tmp2098 + _tmp1764 * _tmp2093 + _tmp1773 * _tmp2086 + + _tmp1774 * _tmp2101 + _tmp1784 * _tmp2060 + _tmp1792 * _tmp2090 + _tmp1793 * _tmp2088 + + _tmp1794 * _tmp2089 + _tmp1795 * _tmp2003 + _tmp1796 * _tmp2087 + _tmp1797 * _tmp2097 + + _tmp1798 * _tmp2050 + _tmp1799 * _tmp2020 + _tmp1800 * _tmp1994 + _tmp1808 * _tmp2012 + + _tmp1809 * _tmp2099 + _tmp1817 * _tmp2044 + _tmp1818 * _tmp2091 + _tmp1826 * _tmp1993 + + _tmp1829 * _tmp1984 + _tmp1837 * _tmp2095 + _tmp1839 * _tmp2096 + _tmp1840 * _tmp2080 + + _tmp1841 * _tmp2092 + _tmp1849 * _tmp2055 + _tmp1850 * _tmp2035 + _tmp1851 * _tmp2007 + + _tmp1853 * _tmp2069 + _tmp1854 * _tmp2100 + _tmp1855 * _tmp2073 + _tmp1856 * _tmp2102 + + _tmp2239 * _tmp51; + hessian_value_ptr[78] = _tmp1734 * _tmp2114 + _tmp1764 * _tmp2116; + hessian_value_ptr[79] = _tmp1792 * _tmp2122 + _tmp1798 * _tmp2121; + hessian_value_ptr[80] = _tmp1666 * _tmp2128 + _tmp1763 * _tmp2127; + hessian_value_ptr[81] = _tmp1837 * _tmp2132 + _tmp1855 * _tmp2135; + hessian_value_ptr[82] = _tmp1742 * _tmp2140 + _tmp1799 * _tmp2141; + hessian_value_ptr[83] = _tmp1725 * _tmp2145 + _tmp1761 * _tmp2146; + hessian_value_ptr[84] = _tmp1715 * _tmp2153 + _tmp1716 * _tmp2152; + hessian_value_ptr[85] = _tmp1758 * _tmp2159 + _tmp1800 * _tmp2160; + hessian_value_ptr[86] = _tmp1773 * _tmp2164 + _tmp1793 * _tmp2166; + hessian_value_ptr[87] = _tmp1849 * _tmp2171 + _tmp1850 * _tmp2173; + hessian_value_ptr[88] = _tmp1808 * _tmp2178 + _tmp1818 * _tmp2179; + hessian_value_ptr[89] = _tmp1706 * _tmp2185 + _tmp1795 * _tmp2186; + hessian_value_ptr[90] = _tmp1699 * _tmp2190 + _tmp1707 * _tmp2192; + hessian_value_ptr[91] = _tmp1682 * _tmp2198 + _tmp1851 * _tmp2197; + hessian_value_ptr[92] = _tmp1784 * _tmp2202 + _tmp1839 * _tmp2203; + hessian_value_ptr[93] = _tmp1749 * _tmp2210 + _tmp1841 * _tmp2209; + hessian_value_ptr[94] = _tmp1817 * _tmp2215 + _tmp1853 * _tmp2216; + hessian_value_ptr[95] = _tmp1826 * _tmp2223 + _tmp1854 * _tmp2222; + hessian_value_ptr[96] = _tmp1690 * _tmp2227 + _tmp1829 * _tmp2229; + hessian_value_ptr[97] = _tmp1674 * _tmp2234 + _tmp1691 * _tmp2236; + hessian_value_ptr[98] = _tmp1217 * std::pow(_tmp1321, Scalar(2)) + + _tmp1827 * std::pow(_tmp52, Scalar(2)) + std::pow(_tmp1861, Scalar(2)) + + std::pow(_tmp1867, Scalar(2)) + std::pow(_tmp1871, Scalar(2)) + + std::pow(_tmp1875, Scalar(2)) + std::pow(_tmp1876, Scalar(2)) + + std::pow(_tmp1882, Scalar(2)) + std::pow(_tmp1886, Scalar(2)) + + std::pow(_tmp1891, Scalar(2)) + std::pow(_tmp1895, Scalar(2)) + + std::pow(_tmp1899, Scalar(2)) + std::pow(_tmp1902, Scalar(2)) + + std::pow(_tmp1906, Scalar(2)) + std::pow(_tmp1910, Scalar(2)) + + std::pow(_tmp1914, Scalar(2)) + std::pow(_tmp1915, Scalar(2)) + + std::pow(_tmp1919, Scalar(2)) + std::pow(_tmp1920, Scalar(2)) + std::pow(_tmp1921, Scalar(2)) + std::pow(_tmp1922, Scalar(2)) + - std::pow(_tmp1923, Scalar(2)) + std::pow(_tmp1929, Scalar(2)) + - std::pow(_tmp1935, Scalar(2)) + std::pow(_tmp1939, Scalar(2)) + - std::pow(_tmp1943, Scalar(2)) + std::pow(_tmp1947, Scalar(2)) + - std::pow(_tmp1951, Scalar(2)) + std::pow(_tmp1952, Scalar(2)) + - std::pow(_tmp1955, Scalar(2)) + std::pow(_tmp1961, Scalar(2)) + - std::pow(_tmp1962, Scalar(2)) + std::pow(_tmp1963, Scalar(2)) + - std::pow(_tmp1964, Scalar(2)) + std::pow(_tmp1965, Scalar(2)) + - std::pow(_tmp1966, Scalar(2)) + std::pow(_tmp1971, Scalar(2)) + - std::pow(_tmp1972, Scalar(2)) + std::pow(_tmp1973, Scalar(2)) + - std::pow(_tmp1978, Scalar(2)) + std::pow(_tmp1982, Scalar(2)) + - std::pow(_tmp1983, Scalar(2)) + std::pow(_tmp1984, Scalar(2)) + - std::pow(_tmp1988, Scalar(2)) + std::pow(_tmp1989, Scalar(2)) + - std::pow(_tmp1990, Scalar(2)) + std::pow(_tmp1994, Scalar(2)) + - std::pow(_tmp1995, Scalar(2)) + std::pow(_tmp1996, Scalar(2)) + - std::pow(_tmp1997, Scalar(2)) + std::pow(_tmp2002, Scalar(2)) + - std::pow(_tmp2003, Scalar(2)) + std::pow(_tmp2004, Scalar(2)) + - std::pow(_tmp2005, Scalar(2)) + std::pow(_tmp2006, Scalar(2)) + - std::pow(_tmp2007, Scalar(2)) + std::pow(_tmp2014, Scalar(2)) + - std::pow(_tmp2015, Scalar(2)) + std::pow(_tmp2016, Scalar(2)) + - std::pow(_tmp2017, Scalar(2)) + std::pow(_tmp2018, Scalar(2)) + - std::pow(_tmp2019, Scalar(2)) + std::pow(_tmp2021, Scalar(2)) + - std::pow(_tmp2022, Scalar(2)) + std::pow(_tmp2023, Scalar(2)); + std::pow(_tmp1923, Scalar(2)) + std::pow(_tmp1924, Scalar(2)) + + std::pow(_tmp1930, Scalar(2)) + std::pow(_tmp1931, Scalar(2)) + + std::pow(_tmp1932, Scalar(2)) + std::pow(_tmp1933, Scalar(2)) + + std::pow(_tmp1937, Scalar(2)) + std::pow(_tmp1941, Scalar(2)) + + std::pow(_tmp1942, Scalar(2)) + std::pow(_tmp1943, Scalar(2)) + + std::pow(_tmp1944, Scalar(2)) + std::pow(_tmp1949, Scalar(2)) + + std::pow(_tmp1950, Scalar(2)) + std::pow(_tmp1954, Scalar(2)) + + std::pow(_tmp1955, Scalar(2)) + std::pow(_tmp1956, Scalar(2)) + + std::pow(_tmp1961, Scalar(2)) + std::pow(_tmp1962, Scalar(2)) + + std::pow(_tmp1963, Scalar(2)) + std::pow(_tmp1964, Scalar(2)) + + std::pow(_tmp1965, Scalar(2)) + std::pow(_tmp1966, Scalar(2)) + + std::pow(_tmp1967, Scalar(2)) + std::pow(_tmp1973, Scalar(2)) + + std::pow(_tmp1974, Scalar(2)) + std::pow(_tmp1975, Scalar(2)) + + std::pow(_tmp1976, Scalar(2)) + std::pow(_tmp1977, Scalar(2)) + + std::pow(_tmp1978, Scalar(2)) + std::pow(_tmp1979, Scalar(2)) + + std::pow(_tmp1980, Scalar(2)); hessian_value_ptr[99] = - _tmp1899 * _tmp2129 + _tmp1903 * _tmp2044 + _tmp1907 * _tmp2035 + _tmp1911 * _tmp2085 + - _tmp1912 * _tmp2048 + _tmp1916 * _tmp2134 + _tmp1921 * _tmp2118 + _tmp1922 * _tmp2066 + - _tmp1923 * _tmp2065 + _tmp1929 * _tmp2125 + _tmp1935 * _tmp2140 + _tmp1939 * _tmp2027 + - _tmp1943 * _tmp2080 + _tmp1947 * _tmp2106 + _tmp1951 * _tmp2040 + _tmp1952 * _tmp2053 + - _tmp1955 * _tmp2108 + _tmp1961 * _tmp2090 + _tmp1962 * _tmp2079 + _tmp1963 * _tmp2124 + - _tmp1964 * _tmp2142 + _tmp1965 * _tmp2054 + _tmp1966 * _tmp2111 + _tmp1971 * _tmp2123 + - _tmp1972 * _tmp2112 + _tmp1973 * _tmp2131 + _tmp1978 * _tmp2036 + _tmp1982 * _tmp2075 + - _tmp1983 * _tmp2135 + _tmp1984 * _tmp2052 + _tmp1988 * _tmp2136 + _tmp1989 * _tmp2130 + - _tmp1990 * _tmp2102 + _tmp1994 * _tmp2060 + _tmp1995 * _tmp2133 + _tmp1996 * _tmp2138 + - _tmp1997 * _tmp2137 + _tmp2002 * _tmp2113 + _tmp2003 * _tmp2132 + _tmp2004 * _tmp2064 + - _tmp2005 * _tmp2084 + _tmp2006 * _tmp2141 + _tmp2007 * _tmp2139 + _tmp2014 * _tmp2101 + - _tmp2015 * _tmp2095 + _tmp2016 * _tmp2070 + _tmp2017 * _tmp2143 + _tmp2018 * _tmp2100 + - _tmp2019 * _tmp2110 + _tmp2021 * _tmp2094 + _tmp2022 * _tmp2144 + _tmp2023 * _tmp2031; - hessian_value_ptr[100] = _tmp1988 * _tmp2157 + _tmp2004 * _tmp2155; - hessian_value_ptr[101] = _tmp1961 * _tmp2162 + _tmp2003 * _tmp2164; - hessian_value_ptr[102] = _tmp1951 * _tmp2170 + _tmp2006 * _tmp2169; - hessian_value_ptr[103] = _tmp1921 * _tmp2178 + _tmp2007 * _tmp2175; - hessian_value_ptr[104] = _tmp1994 * _tmp2185 + _tmp1996 * _tmp2184; - hessian_value_ptr[105] = _tmp1929 * _tmp2189 + _tmp2021 * _tmp2190; - hessian_value_ptr[106] = _tmp1943 * _tmp2196 + _tmp1962 * _tmp2195; - hessian_value_ptr[107] = _tmp1978 * _tmp2203 + _tmp2023 * _tmp2202; - hessian_value_ptr[108] = _tmp1899 * _tmp2207 + _tmp1973 * _tmp2209; - hessian_value_ptr[109] = _tmp1982 * _tmp2215 + _tmp2015 * _tmp2213; - hessian_value_ptr[110] = _tmp1916 * _tmp2220 + _tmp1984 * _tmp2219; - hessian_value_ptr[111] = _tmp1903 * _tmp2227 + _tmp1963 * _tmp2226; - hessian_value_ptr[112] = _tmp1947 * _tmp2233 + _tmp2019 * _tmp2231; - hessian_value_ptr[113] = _tmp1911 * _tmp2239 + _tmp1912 * _tmp2238; - hessian_value_ptr[114] = _tmp1935 * _tmp2244 + _tmp2018 * _tmp2243; - hessian_value_ptr[115] = _tmp1971 * _tmp2251 + _tmp1983 * _tmp2250; - hessian_value_ptr[116] = _tmp2002 * _tmp2257 + _tmp2005 * _tmp2256; - hessian_value_ptr[117] = _tmp1907 * _tmp2264 + _tmp1964 * _tmp2263; - hessian_value_ptr[118] = _tmp1939 * _tmp2270 + _tmp1965 * _tmp2268; - hessian_value_ptr[119] = _tmp2014 * _tmp2276 + _tmp2016 * _tmp2277; - hessian_value_ptr[120] = std::pow(_tmp2027, Scalar(2)) + std::pow(_tmp2031, Scalar(2)) + - std::pow(_tmp2035, Scalar(2)) + std::pow(_tmp2036, Scalar(2)) + - std::pow(_tmp2040, Scalar(2)) + std::pow(_tmp2044, Scalar(2)) + - std::pow(_tmp2048, Scalar(2)) + std::pow(_tmp2052, Scalar(2)) + - std::pow(_tmp2053, Scalar(2)) + std::pow(_tmp2054, Scalar(2)) + - std::pow(_tmp2060, Scalar(2)) + std::pow(_tmp2064, Scalar(2)) + - std::pow(_tmp2065, Scalar(2)) + std::pow(_tmp2066, Scalar(2)) + - std::pow(_tmp2070, Scalar(2)) + std::pow(_tmp2075, Scalar(2)) + - std::pow(_tmp2079, Scalar(2)) + std::pow(_tmp2080, Scalar(2)) + - std::pow(_tmp2084, Scalar(2)) + std::pow(_tmp2085, Scalar(2)) + - std::pow(_tmp2090, Scalar(2)) + std::pow(_tmp2094, Scalar(2)) + - std::pow(_tmp2095, Scalar(2)) + std::pow(_tmp2100, Scalar(2)) + - std::pow(_tmp2101, Scalar(2)) + std::pow(_tmp2102, Scalar(2)) + - std::pow(_tmp2106, Scalar(2)) + std::pow(_tmp2108, Scalar(2)) + - std::pow(_tmp2110, Scalar(2)) + std::pow(_tmp2111, Scalar(2)) + - std::pow(_tmp2112, Scalar(2)) + std::pow(_tmp2113, Scalar(2)) + - std::pow(_tmp2118, Scalar(2)) + std::pow(_tmp2123, Scalar(2)) + - std::pow(_tmp2124, Scalar(2)) + std::pow(_tmp2125, Scalar(2)) + - std::pow(_tmp2129, Scalar(2)) + std::pow(_tmp2130, Scalar(2)) + - std::pow(_tmp2131, Scalar(2)) + std::pow(_tmp2132, Scalar(2)) + - std::pow(_tmp2133, Scalar(2)) + std::pow(_tmp2134, Scalar(2)) + - std::pow(_tmp2135, Scalar(2)) + std::pow(_tmp2136, Scalar(2)) + - std::pow(_tmp2137, Scalar(2)) + std::pow(_tmp2138, Scalar(2)) + - std::pow(_tmp2139, Scalar(2)) + std::pow(_tmp2140, Scalar(2)) + - std::pow(_tmp2141, Scalar(2)) + std::pow(_tmp2142, Scalar(2)) + - std::pow(_tmp2143, Scalar(2)) + std::pow(_tmp2144, Scalar(2)); - hessian_value_ptr[121] = _tmp2064 * _tmp2155 + _tmp2136 * _tmp2157; - hessian_value_ptr[122] = _tmp2090 * _tmp2162 + _tmp2132 * _tmp2164; - hessian_value_ptr[123] = _tmp2040 * _tmp2170 + _tmp2141 * _tmp2169; - hessian_value_ptr[124] = _tmp2118 * _tmp2178 + _tmp2139 * _tmp2175; - hessian_value_ptr[125] = _tmp2060 * _tmp2185 + _tmp2138 * _tmp2184; - hessian_value_ptr[126] = _tmp2094 * _tmp2190 + _tmp2125 * _tmp2189; - hessian_value_ptr[127] = _tmp2079 * _tmp2195 + _tmp2080 * _tmp2196; - hessian_value_ptr[128] = _tmp2031 * _tmp2202 + _tmp2036 * _tmp2203; - hessian_value_ptr[129] = _tmp2129 * _tmp2207 + _tmp2131 * _tmp2209; - hessian_value_ptr[130] = _tmp2075 * _tmp2215 + _tmp2095 * _tmp2213; - hessian_value_ptr[131] = _tmp2052 * _tmp2219 + _tmp2134 * _tmp2220; - hessian_value_ptr[132] = _tmp2044 * _tmp2227 + _tmp2124 * _tmp2226; - hessian_value_ptr[133] = _tmp2106 * _tmp2233 + _tmp2110 * _tmp2231; - hessian_value_ptr[134] = _tmp2048 * _tmp2238 + _tmp2085 * _tmp2239; - hessian_value_ptr[135] = _tmp2100 * _tmp2243 + _tmp2140 * _tmp2244; - hessian_value_ptr[136] = _tmp2123 * _tmp2251 + _tmp2135 * _tmp2250; - hessian_value_ptr[137] = _tmp2084 * _tmp2256 + _tmp2113 * _tmp2257; - hessian_value_ptr[138] = _tmp2035 * _tmp2264 + _tmp2142 * _tmp2263; - hessian_value_ptr[139] = _tmp2027 * _tmp2270 + _tmp2054 * _tmp2268; - hessian_value_ptr[140] = _tmp2070 * _tmp2277 + _tmp2101 * _tmp2276; + _tmp1861 * _tmp2086 + _tmp1867 * _tmp2003 + _tmp1871 * _tmp1993 + _tmp1875 * _tmp2045 + + _tmp1876 * _tmp2007 + _tmp1882 * _tmp2091 + _tmp1886 * _tmp2073 + _tmp1891 * _tmp2082 + + _tmp1895 * _tmp2096 + _tmp1899 * _tmp1984 + _tmp1902 * _tmp2080 + _tmp1906 * _tmp2040 + + _tmp1910 * _tmp2065 + _tmp1914 * _tmp1998 + _tmp1915 * _tmp2101 + _tmp1919 * _tmp2050 + + _tmp1920 * _tmp2039 + _tmp1921 * _tmp2081 + _tmp1922 * _tmp2100 + _tmp1923 * _tmp2099 + + _tmp1924 * _tmp2014 + _tmp1925 * _tmp2067 + _tmp1930 * _tmp2078 + _tmp1931 * _tmp2068 + + _tmp1932 * _tmp2079 + _tmp1933 * _tmp2088 + _tmp1937 * _tmp1994 + _tmp1941 * _tmp2035 + + _tmp1942 * _tmp2092 + _tmp1943 * _tmp2089 + _tmp1944 * _tmp2012 + _tmp1949 * _tmp2093 + + _tmp1950 * _tmp2087 + _tmp1954 * _tmp2020 + _tmp1955 * _tmp2094 + _tmp1956 * _tmp2097 + + _tmp1961 * _tmp2069 + _tmp1962 * _tmp2090 + _tmp1963 * _tmp2024 + _tmp1964 * _tmp2044 + + _tmp1965 * _tmp2025 + _tmp1966 * _tmp2098 + _tmp1967 * _tmp2095 + _tmp1973 * _tmp2061 + + _tmp1974 * _tmp2055 + _tmp1975 * _tmp2031 + _tmp1976 * _tmp2060 + _tmp1977 * _tmp2066 + + _tmp1978 * _tmp2054 + _tmp1979 * _tmp2102 + _tmp1980 * _tmp1988 + _tmp2239 * _tmp52; + hessian_value_ptr[100] = _tmp1949 * _tmp2116 + _tmp1963 * _tmp2114; + hessian_value_ptr[101] = _tmp1919 * _tmp2121 + _tmp1962 * _tmp2122; + hessian_value_ptr[102] = _tmp1914 * _tmp2128 + _tmp1966 * _tmp2127; + hessian_value_ptr[103] = _tmp1886 * _tmp2135 + _tmp1967 * _tmp2132; + hessian_value_ptr[104] = _tmp1954 * _tmp2141 + _tmp1955 * _tmp2140; + hessian_value_ptr[105] = _tmp1891 * _tmp2145 + _tmp1978 * _tmp2146; + hessian_value_ptr[106] = _tmp1906 * _tmp2153 + _tmp1920 * _tmp2152; + hessian_value_ptr[107] = _tmp1937 * _tmp2160 + _tmp1980 * _tmp2159; + hessian_value_ptr[108] = _tmp1861 * _tmp2164 + _tmp1933 * _tmp2166; + hessian_value_ptr[109] = _tmp1941 * _tmp2173 + _tmp1974 * _tmp2171; + hessian_value_ptr[110] = _tmp1882 * _tmp2179 + _tmp1944 * _tmp2178; + hessian_value_ptr[111] = _tmp1867 * _tmp2186 + _tmp1921 * _tmp2185; + hessian_value_ptr[112] = _tmp1910 * _tmp2192 + _tmp1977 * _tmp2190; + hessian_value_ptr[113] = _tmp1875 * _tmp2198 + _tmp1876 * _tmp2197; + hessian_value_ptr[114] = _tmp1895 * _tmp2203 + _tmp1976 * _tmp2202; + hessian_value_ptr[115] = _tmp1930 * _tmp2210 + _tmp1942 * _tmp2209; + hessian_value_ptr[116] = _tmp1961 * _tmp2216 + _tmp1964 * _tmp2215; + hessian_value_ptr[117] = _tmp1871 * _tmp2223 + _tmp1922 * _tmp2222; + hessian_value_ptr[118] = _tmp1899 * _tmp2229 + _tmp1924 * _tmp2227; + hessian_value_ptr[119] = _tmp1973 * _tmp2234 + _tmp1975 * _tmp2236; + hessian_value_ptr[120] = _tmp1217 * std::pow(_tmp2067, Scalar(2)) + + _tmp1827 * std::pow(_tmp48, Scalar(2)) + + std::pow(_tmp1984, Scalar(2)) + std::pow(_tmp1988, Scalar(2)) + + std::pow(_tmp1993, Scalar(2)) + std::pow(_tmp1994, Scalar(2)) + + std::pow(_tmp1998, Scalar(2)) + std::pow(_tmp2003, Scalar(2)) + + std::pow(_tmp2007, Scalar(2)) + std::pow(_tmp2012, Scalar(2)) + + std::pow(_tmp2014, Scalar(2)) + std::pow(_tmp2020, Scalar(2)) + + std::pow(_tmp2024, Scalar(2)) + std::pow(_tmp2025, Scalar(2)) + + std::pow(_tmp2031, Scalar(2)) + std::pow(_tmp2035, Scalar(2)) + + std::pow(_tmp2039, Scalar(2)) + std::pow(_tmp2040, Scalar(2)) + + std::pow(_tmp2044, Scalar(2)) + std::pow(_tmp2045, Scalar(2)) + + std::pow(_tmp2050, Scalar(2)) + std::pow(_tmp2054, Scalar(2)) + + std::pow(_tmp2055, Scalar(2)) + std::pow(_tmp2060, Scalar(2)) + + std::pow(_tmp2061, Scalar(2)) + std::pow(_tmp2065, Scalar(2)) + + std::pow(_tmp2066, Scalar(2)) + std::pow(_tmp2068, Scalar(2)) + + std::pow(_tmp2069, Scalar(2)) + std::pow(_tmp2073, Scalar(2)) + + std::pow(_tmp2078, Scalar(2)) + std::pow(_tmp2079, Scalar(2)) + + std::pow(_tmp2080, Scalar(2)) + std::pow(_tmp2081, Scalar(2)) + + std::pow(_tmp2082, Scalar(2)) + std::pow(_tmp2086, Scalar(2)) + + std::pow(_tmp2087, Scalar(2)) + std::pow(_tmp2088, Scalar(2)) + + std::pow(_tmp2089, Scalar(2)) + std::pow(_tmp2090, Scalar(2)) + + std::pow(_tmp2091, Scalar(2)) + std::pow(_tmp2092, Scalar(2)) + + std::pow(_tmp2093, Scalar(2)) + std::pow(_tmp2094, Scalar(2)) + + std::pow(_tmp2095, Scalar(2)) + std::pow(_tmp2096, Scalar(2)) + + std::pow(_tmp2097, Scalar(2)) + std::pow(_tmp2098, Scalar(2)) + + std::pow(_tmp2099, Scalar(2)) + std::pow(_tmp2100, Scalar(2)) + + std::pow(_tmp2101, Scalar(2)) + std::pow(_tmp2102, Scalar(2)); + hessian_value_ptr[121] = _tmp2024 * _tmp2114 + _tmp2093 * _tmp2116; + hessian_value_ptr[122] = _tmp2050 * _tmp2121 + _tmp2090 * _tmp2122; + hessian_value_ptr[123] = _tmp1998 * _tmp2128 + _tmp2098 * _tmp2127; + hessian_value_ptr[124] = _tmp2073 * _tmp2135 + _tmp2095 * _tmp2132; + hessian_value_ptr[125] = _tmp2020 * _tmp2141 + _tmp2094 * _tmp2140; + hessian_value_ptr[126] = _tmp2054 * _tmp2146 + _tmp2082 * _tmp2145; + hessian_value_ptr[127] = _tmp2039 * _tmp2152 + _tmp2040 * _tmp2153; + hessian_value_ptr[128] = _tmp1988 * _tmp2159 + _tmp1994 * _tmp2160; + hessian_value_ptr[129] = _tmp2086 * _tmp2164 + _tmp2088 * _tmp2166; + hessian_value_ptr[130] = _tmp2035 * _tmp2173 + _tmp2055 * _tmp2171; + hessian_value_ptr[131] = _tmp2012 * _tmp2178 + _tmp2091 * _tmp2179; + hessian_value_ptr[132] = _tmp2003 * _tmp2186 + _tmp2081 * _tmp2185; + hessian_value_ptr[133] = _tmp2065 * _tmp2192 + _tmp2066 * _tmp2190; + hessian_value_ptr[134] = _tmp2007 * _tmp2197 + _tmp2045 * _tmp2198; + hessian_value_ptr[135] = _tmp2060 * _tmp2202 + _tmp2096 * _tmp2203; + hessian_value_ptr[136] = _tmp2078 * _tmp2210 + _tmp2092 * _tmp2209; + hessian_value_ptr[137] = _tmp2044 * _tmp2215 + _tmp2069 * _tmp2216; + hessian_value_ptr[138] = _tmp1993 * _tmp2223 + _tmp2100 * _tmp2222; + hessian_value_ptr[139] = _tmp1984 * _tmp2229 + _tmp2014 * _tmp2227; + hessian_value_ptr[140] = _tmp2031 * _tmp2236 + _tmp2061 * _tmp2234; hessian_value_ptr[141] = - std::pow(_tmp2155, Scalar(2)) + _tmp2156 + std::pow(_tmp2157, Scalar(2)); + std::pow(_tmp2114, Scalar(2)) + _tmp2115 + std::pow(_tmp2116, Scalar(2)); hessian_value_ptr[142] = - _tmp2158 + std::pow(_tmp2162, Scalar(2)) + std::pow(_tmp2164, Scalar(2)); + _tmp2117 + std::pow(_tmp2121, Scalar(2)) + std::pow(_tmp2122, Scalar(2)); hessian_value_ptr[143] = - _tmp2165 + std::pow(_tmp2169, Scalar(2)) + std::pow(_tmp2170, Scalar(2)); + _tmp2123 + std::pow(_tmp2127, Scalar(2)) + std::pow(_tmp2128, Scalar(2)); hessian_value_ptr[144] = - std::pow(_tmp2175, Scalar(2)) + _tmp2176 + std::pow(_tmp2178, Scalar(2)); + std::pow(_tmp2132, Scalar(2)) + _tmp2133 + std::pow(_tmp2135, Scalar(2)); hessian_value_ptr[145] = - _tmp2179 + std::pow(_tmp2184, Scalar(2)) + std::pow(_tmp2185, Scalar(2)); + _tmp2136 + std::pow(_tmp2140, Scalar(2)) + std::pow(_tmp2141, Scalar(2)); hessian_value_ptr[146] = - std::pow(_tmp2189, Scalar(2)) + std::pow(_tmp2190, Scalar(2)) + _tmp2191; + std::pow(_tmp2145, Scalar(2)) + std::pow(_tmp2146, Scalar(2)) + _tmp2147; hessian_value_ptr[147] = - std::pow(_tmp2195, Scalar(2)) + std::pow(_tmp2196, Scalar(2)) + _tmp2197; + std::pow(_tmp2152, Scalar(2)) + std::pow(_tmp2153, Scalar(2)) + _tmp2154; hessian_value_ptr[148] = - _tmp2198 + std::pow(_tmp2202, Scalar(2)) + std::pow(_tmp2203, Scalar(2)); + _tmp2155 + std::pow(_tmp2159, Scalar(2)) + std::pow(_tmp2160, Scalar(2)); hessian_value_ptr[149] = - std::pow(_tmp2207, Scalar(2)) + _tmp2208 + std::pow(_tmp2209, Scalar(2)); + std::pow(_tmp2164, Scalar(2)) + _tmp2165 + std::pow(_tmp2166, Scalar(2)); hessian_value_ptr[150] = - std::pow(_tmp2213, Scalar(2)) + _tmp2214 + std::pow(_tmp2215, Scalar(2)); + std::pow(_tmp2171, Scalar(2)) + _tmp2172 + std::pow(_tmp2173, Scalar(2)); hessian_value_ptr[151] = - std::pow(_tmp2219, Scalar(2)) + std::pow(_tmp2220, Scalar(2)) + _tmp2221; + std::pow(_tmp2178, Scalar(2)) + std::pow(_tmp2179, Scalar(2)) + _tmp2180; hessian_value_ptr[152] = - _tmp2222 + std::pow(_tmp2226, Scalar(2)) + std::pow(_tmp2227, Scalar(2)); + _tmp2181 + std::pow(_tmp2185, Scalar(2)) + std::pow(_tmp2186, Scalar(2)); hessian_value_ptr[153] = - std::pow(_tmp2231, Scalar(2)) + _tmp2232 + std::pow(_tmp2233, Scalar(2)); + std::pow(_tmp2190, Scalar(2)) + _tmp2191 + std::pow(_tmp2192, Scalar(2)); hessian_value_ptr[154] = - _tmp2234 + std::pow(_tmp2238, Scalar(2)) + std::pow(_tmp2239, Scalar(2)); + _tmp2193 + std::pow(_tmp2197, Scalar(2)) + std::pow(_tmp2198, Scalar(2)); hessian_value_ptr[155] = - std::pow(_tmp2243, Scalar(2)) + std::pow(_tmp2244, Scalar(2)) + _tmp2245; + std::pow(_tmp2202, Scalar(2)) + std::pow(_tmp2203, Scalar(2)) + _tmp2204; hessian_value_ptr[156] = - _tmp2246 + std::pow(_tmp2250, Scalar(2)) + std::pow(_tmp2251, Scalar(2)); + _tmp2205 + std::pow(_tmp2209, Scalar(2)) + std::pow(_tmp2210, Scalar(2)); hessian_value_ptr[157] = - _tmp2252 + std::pow(_tmp2256, Scalar(2)) + std::pow(_tmp2257, Scalar(2)); + _tmp2211 + std::pow(_tmp2215, Scalar(2)) + std::pow(_tmp2216, Scalar(2)); hessian_value_ptr[158] = - _tmp2258 + std::pow(_tmp2263, Scalar(2)) + std::pow(_tmp2264, Scalar(2)); + _tmp2217 + std::pow(_tmp2222, Scalar(2)) + std::pow(_tmp2223, Scalar(2)); hessian_value_ptr[159] = - std::pow(_tmp2268, Scalar(2)) + _tmp2269 + std::pow(_tmp2270, Scalar(2)); + std::pow(_tmp2227, Scalar(2)) + _tmp2228 + std::pow(_tmp2229, Scalar(2)); hessian_value_ptr[160] = - _tmp2271 + std::pow(_tmp2276, Scalar(2)) + std::pow(_tmp2277, Scalar(2)); + _tmp2230 + std::pow(_tmp2234, Scalar(2)) + std::pow(_tmp2236, Scalar(2)); } } // NOLINT(readability/fn_size) diff --git a/symforce/geo/matrix.py b/symforce/geo/matrix.py index 770d1850f..846584815 100644 --- a/symforce/geo/matrix.py +++ b/symforce/geo/matrix.py @@ -664,6 +664,23 @@ def __mul__( else: return self.__class__(self.mat * right) + def upper_triangular_mul(self, v: Matrix) -> Matrix: + """ + Returns self * v. Use to avoid expressions containing the zero-valued lower triangular + portion of ut. + + Precondition: self is upper triangular, v is a vector (NOTE(brad): not essential, but + covers the use case I care about right now) + """ + if self.shape[0] != self.shape[1]: + raise ValueError(f"self is not a square matrix; instead has shape {self.shape}") + if self.shape[1] != v.shape[0]: + raise ValueError(f"self {self.shape} and v {v.shape} have incompatible shapes") + if v.shape[1] != 1: + raise ValueError("currently, only multiplication with vectors is supported") + + return Matrix([(self[k, k:] * v[k:, 0])[0] for k in range(v.shape[0])]) + @_T.overload def __rmul__( self, left: _T.Union[Matrix, sf.sympy.MutableDenseMatrix]