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]