|
| 1 | +#include <algorithm> |
| 2 | +#include <cmath> |
| 3 | +#include <vector> |
| 4 | + |
| 5 | +#include <Eigen/Dense> |
| 6 | + |
| 7 | +#include <isce3/antenna/edge_method_cost_func.h> |
| 8 | +#include <isce3/except/Error.h> |
| 9 | +#include <isce3/math/RootFind1dNewton.h> |
| 10 | +#include <isce3/math/polyfunc.h> |
| 11 | + |
| 12 | +namespace isce3 { namespace antenna { |
| 13 | + |
| 14 | +std::tuple<double, double, bool, int> rollAngleOffsetFromEdge( |
| 15 | + const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant, |
| 16 | + const isce3::core::Linspace<double>& look_ang, |
| 17 | + std::optional<poly1d_t> polyfit_weight) |
| 18 | +{ |
| 19 | + // check the input arguments |
| 20 | + if (polyfit_echo.order != 3 || polyfit_ant.order != 3) |
| 21 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 22 | + "Requires 3rd-order poly-fit object for both " |
| 23 | + "Echo and Antenna!"); |
| 24 | + constexpr double a_tol {1e-5}; |
| 25 | + if (std::abs(polyfit_echo.mean - polyfit_ant.mean) > a_tol || |
| 26 | + std::abs(polyfit_echo.norm - polyfit_ant.norm) > a_tol) |
| 27 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 28 | + "Requires same (mean, std) for Echo and Antenna Poly1d obj!"); |
| 29 | + |
| 30 | + if (!(polyfit_echo.norm > 0.0)) |
| 31 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 32 | + "Requires positive std of Echo and Antenna Poly1d obj!"); |
| 33 | + |
| 34 | + if (polyfit_weight) { |
| 35 | + if (polyfit_weight->order < 0) |
| 36 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 37 | + "The order of polyfit for weights must be " |
| 38 | + "at least 0 (constant weights)!"); |
| 39 | + if (!(polyfit_weight->norm > 0.0)) |
| 40 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 41 | + "Requires positive std of weight Poly1d obj!"); |
| 42 | + } |
| 43 | + |
| 44 | + // create a copy polyfit objects "echo" and "ant" with zero mean and unit |
| 45 | + // std |
| 46 | + auto pf_echo_cp = polyfit_echo; |
| 47 | + pf_echo_cp.mean = 0.0; |
| 48 | + pf_echo_cp.norm = 1.0; |
| 49 | + auto pf_ant_cp = polyfit_ant; |
| 50 | + pf_ant_cp.mean = 0.0; |
| 51 | + pf_ant_cp.norm = 1.0; |
| 52 | + |
| 53 | + // declare and initialize a look angle vector |
| 54 | + Eigen::ArrayXd lka_vec(look_ang.size()); |
| 55 | + for (int idx = 0; idx < look_ang.size(); ++idx) |
| 56 | + lka_vec(idx) = look_ang[idx]; |
| 57 | + |
| 58 | + // create a weighting vector from look vector and weighting Poly1d |
| 59 | + Eigen::ArrayXd wgt_vec; |
| 60 | + if (polyfit_weight) { |
| 61 | + Eigen::Map<Eigen::ArrayXd> wgt_coef( |
| 62 | + polyfit_weight->coeffs.data(), polyfit_weight->coeffs.size()); |
| 63 | + wgt_vec = isce3::math::polyval( |
| 64 | + wgt_coef, lka_vec, polyfit_weight->mean, polyfit_weight->norm); |
| 65 | + // normalize power in dB |
| 66 | + wgt_vec -= wgt_vec.maxCoeff(); |
| 67 | + // convert from dB to linear power scale |
| 68 | + wgt_vec = Eigen::pow(10, 0.1 * wgt_vec); |
| 69 | + } |
| 70 | + // centralized and scaled the look vector based on mean/std of the echo |
| 71 | + // Poly1d to be used for both antenna and echo in the cost function. |
| 72 | + lka_vec -= polyfit_echo.mean; |
| 73 | + const auto std_inv = 1.0 / polyfit_echo.norm; |
| 74 | + lka_vec *= std_inv; |
| 75 | + |
| 76 | + // form some derivatives used in the cost function |
| 77 | + auto pf_echo_der = pf_echo_cp.derivative(); |
| 78 | + auto pf_ant_der = pf_ant_cp.derivative(); |
| 79 | + auto pf_ant_der2 = pf_ant_der.derivative(); |
| 80 | + // create a memmap of the coeff for the first and second derivatives |
| 81 | + Eigen::Map<Eigen::ArrayXd> coef_ant_der( |
| 82 | + pf_ant_der.coeffs.data(), pf_ant_der.coeffs.size()); |
| 83 | + Eigen::Map<Eigen::ArrayXd> coef_ant_der2( |
| 84 | + pf_ant_der2.coeffs.data(), pf_ant_der2.coeffs.size()); |
| 85 | + Eigen::Map<Eigen::ArrayXd> coef_echo_der( |
| 86 | + pf_echo_der.coeffs.data(), pf_echo_der.coeffs.size()); |
| 87 | + // form some arrays over scaled look angles for diff of first derivatives |
| 88 | + // and for second derivative |
| 89 | + auto ant_echo_der_dif_vec = |
| 90 | + isce3::math::polyval(coef_ant_der - coef_echo_der, lka_vec); |
| 91 | + auto ant_der2_vec = isce3::math::polyval(coef_ant_der2, lka_vec); |
| 92 | + |
| 93 | + // build cost function in the form of Poly1d object (3th order polynimal!) |
| 94 | + auto cf_pf = isce3::core::Poly1d(3, 0.0, 1.0); |
| 95 | + // fill up the coeff for the derivative of the WMSE cost function: |
| 96 | + // cost(ofs) = pf_wgt*(pf_echo_der(el) - pf_ant_der(el + ofs))**2 |
| 97 | + // See section 1.1 of the cited reference. |
| 98 | + if (polyfit_weight) { |
| 99 | + auto tmp1 = wgt_vec * ant_echo_der_dif_vec; |
| 100 | + auto tmp2 = wgt_vec * ant_der2_vec; |
| 101 | + cf_pf.coeffs[0] = (tmp1 * ant_der2_vec).sum(); |
| 102 | + cf_pf.coeffs[1] = (tmp2 * ant_der2_vec).sum() + |
| 103 | + 6 * pf_ant_cp.coeffs[3] * tmp1.sum(); |
| 104 | + cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * tmp2.sum(); |
| 105 | + cf_pf.coeffs[3] = |
| 106 | + 18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] * wgt_vec.sum(); |
| 107 | + } else // no weighting |
| 108 | + { |
| 109 | + cf_pf.coeffs[0] = (ant_echo_der_dif_vec * ant_der2_vec).sum(); |
| 110 | + cf_pf.coeffs[1] = ant_der2_vec.square().sum() + |
| 111 | + 6 * pf_ant_cp.coeffs[3] * ant_echo_der_dif_vec.sum(); |
| 112 | + cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * ant_der2_vec.sum(); |
| 113 | + cf_pf.coeffs[3] = 18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] * |
| 114 | + look_ang.size(); |
| 115 | + } |
| 116 | + // form Root finding object |
| 117 | + auto rf_obj = |
| 118 | + isce3::math::RootFind1dNewton(1e-4, 20, look_ang.spacing() / 10.); |
| 119 | + // solve for the root/roll offset via Newton |
| 120 | + auto [roll, f_val, flag, n_iter] = rf_obj.root(cf_pf); |
| 121 | + // scale back the roll angle by std of original poly1d object |
| 122 | + roll *= polyfit_echo.norm; |
| 123 | + |
| 124 | + return {roll, f_val, flag, n_iter}; |
| 125 | +} |
| 126 | + |
| 127 | +std::tuple<double, double, bool, int> rollAngleOffsetFromEdge( |
| 128 | + const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant, |
| 129 | + double look_ang_near, double look_ang_far, double look_ang_prec, |
| 130 | + std::optional<poly1d_t> polyfit_weight) |
| 131 | +{ |
| 132 | + if (!(look_ang_near > 0.0 && look_ang_far > 0.0 && look_ang_prec > 0.0)) |
| 133 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 134 | + "All look angles values must be positive numbers!"); |
| 135 | + if (look_ang_near >= (look_ang_far - look_ang_prec)) |
| 136 | + throw isce3::except::InvalidArgument(ISCE_SRCINFO(), |
| 137 | + "Near-range look angle shall be smaller than " |
| 138 | + "far one by at least one prec!"); |
| 139 | + |
| 140 | + const auto ang_size = |
| 141 | + static_cast<int>((look_ang_far - look_ang_near) / look_ang_prec) + |
| 142 | + 1; |
| 143 | + auto look_ang = isce3::core::Linspace<double>::from_interval( |
| 144 | + look_ang_near, look_ang_far, ang_size); |
| 145 | + |
| 146 | + return rollAngleOffsetFromEdge( |
| 147 | + polyfit_echo, polyfit_ant, look_ang, polyfit_weight); |
| 148 | +} |
| 149 | + |
| 150 | +}} // namespace isce3::antenna |
0 commit comments