diff --git a/CMakeLists.txt b/CMakeLists.txt index 03270bc..40b0a78 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -284,7 +284,11 @@ if(USE_BSPLINE_PCM) endif() if(USE_EIGEN3) - list(APPEND MCC_SRC include/mcc/mcc_pcm_construct.h) + list( + APPEND MCC_SRC + include/mcc/mcc_pcm_construct.h + include/mcc/mcc_pcm_fit.h + ) endif() add_library(${PROJECT_NAME} INTERFACE ${MCC_SRC}) diff --git a/include/mcc/mcc_pcm_fit.h b/include/mcc/mcc_pcm_fit.h new file mode 100644 index 0000000..b5a894d --- /dev/null +++ b/include/mcc/mcc_pcm_fit.h @@ -0,0 +1,563 @@ +#pragma once + + +/**************************************************************************************** + * * + * MOUNT CONTROL COMPONENTS LIBRARY * + * * + * * + * "POINTING-CORRECTION-MODEL" FITTER * + * * + ****************************************************************************************/ + +#include + +#include "mcc_concepts.h" +#include "mcc_coordinate.h" +#include "mcc_pcm.h" + + +namespace mcc::impl +{ + +enum class MccPCMFitterErrorCode : int { + ERROR_OK, + ERROR_INVALID_EPOCH, + ERROR_NOT_ENOUGH_DATA, + ERROR_INVALID_INDEX, +#ifdef USE_BSPLINE_PCM + ERROR_INVALID_KNOTS_NUMBER, + ERROR_BSPLINE_FIT +#endif +}; + +// error category + +struct MccPCMFitterErrorCategory : std::error_category { + MccPCMFitterErrorCategory() = default; + + const char* name() const noexcept + { + return "MCC-DEFAULT-PCM-CONSTRUCTOR-ERROR-CATEGORY"; + } + + std::string message(int ec) const + { + MccPCMFitterErrorCode err = static_cast(ec); + + switch (err) { + case MccPCMFitterErrorCode::ERROR_OK: + return "OK"; + case MccPCMFitterErrorCode::ERROR_INVALID_EPOCH: + return "invalid epoch"; + case MccPCMFitterErrorCode::ERROR_NOT_ENOUGH_DATA: + return "not enough data point"; + case MccPCMFitterErrorCode::ERROR_INVALID_INDEX: + return "invalid index of data point"; +#ifdef USE_BSPLINE_PCM + case MccPCMFitterErrorCode::ERROR_INVALID_KNOTS_NUMBER: + return "invalid number of B-spline knots"; + case MccPCMFitterErrorCode::ERROR_BSPLINE_FIT: + return "B-spline fitting error"; +#endif + default: + return "UNKNOWN"; + } + } + + static const MccPCMFitterErrorCategory& get() + { + static const MccPCMFitterErrorCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccPCMFitterErrorCode ec) +{ + return std::error_code(static_cast(ec), MccPCMFitterErrorCategory::get()); +} + +} // namespace mcc::impl + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + + +namespace mcc::impl +{ + +template +class MccPCMFitter +{ +public: + using ref_coordpair_t = std::conditional_t, + MccSkyHADEC_OBS, + std::conditional_t, MccSkyAZZD, void>>; + + static_assert(!std::is_void_v, "UNSUPPORTED MOUNT TYPE!"); + + struct compute_result_t { + MccDefaultPCMType pcm_type; + MccError error{}; // final model computation error + + size_t final_iter{}; // number of final iteration + + std::vector model_colon{}, model_colat{}; // fitting model values + std::vector colon_res{}, colat_res{}; // target - model + std::vector colon_weight{}, colat_weight; // Tukey's weights + +#ifdef USE_BSPLINE_PCM + std::array bspline_fit_err{ + 0, 0, 0, 0}; // bivariate B-spline fitting exit code (see FITPACK) + // 0 - pcmX(X,Y), 1 - pcmY(X,Y), 2 - revPcmX(COLON, COLAT), 3 - revPcmY(COLON, COLAT) + + // quantities below are computed only fo pcm_type == MccDefaultPCMType::PCM_TYPE_BSPLINE + std::vector inv_model_colon{}, inv_model_colat{}; // fitted inverse model values + std::vector inv_colon_res{}, inv_colat_res{}; // encoder - model +#endif + }; + + struct compute_params_t { + size_t max_iter{100}; // max number of iterations for robust linear regression method + double c{4.685}; // Tukey's parameter for robust linear regression method + double tolerance{1.0E-6}; // tolerance value for bivariare B-spline fitting + }; + + + size_t numberOfPoints() const + { + return _targetCOLON.size(); + } + + + void deletePoints() + { + _epoch.clear(); + _targetCOLON.clear(); + _targetCOLAT.clear(); + _colatRES.clear(); + _colonRES.clear(); + } + + template + MccError addPoint(TAG_T const& target_coords, HW_T const& hw_counts) + requires(TAG_T::pairKind == ref_coordpair_t::pairKind && HW_T::pairKind == MccCoordPairKind::COORDS_KIND_XY) + { + if (hw_counts.epoch() != target_coords.epoch()) { + return MccPCMFitterErrorCode::ERROR_INVALID_EPOCH; + } + + _epoch.emplace_back(hw_counts.epoch()); + + // to ensure the input celestial coordinates are normalized + typename ref_coordpair_t::x_t ang_x{(double)target_coords.x()}; + typename ref_coordpair_t::y_t ang_y{(double)target_coords.y()}; + + _targetCOLON.emplace_back((double)ang_x); + _targetCOLAT.emplace_back((double)ang_y); + + // normalize encoder coordinates to the range of corresponding celestial ones + + ang_x = (double)hw_counts.x(); + ang_y = (double)hw_counts.y(); + _hwX.emplace_back((double)ang_x); + _hwY.emplace_back((double)ang_y); + + _colonRES.emplace_back(_targetCOLON.back() - _hwX.back()); + _colatRES.emplace_back(_targetCOLAT.back() - _hwY.back()); + + return MccPCMFitterErrorCode::ERROR_OK; + } + + + MccError addPoint(mcc_skypoint_c auto const& target_coords, mcc_coord_pair_c auto const& hw_counts) + requires(std::decay_t::pairKind == MccCoordPairKind::COORDS_KIND_XY) + { + ref_coordpair_t cp; + + cp.setEpoch(hw_counts.epoch()); + auto err = target_coords.to(cp); + if (err) { + return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE); + } + + return addPoint(cp, hw_counts); + } + + template + MccError addPoint(COLON_T const& colon, COLAT_T const& colat, mcc_coord_pair_c auto const& hw_counts) + requires(std::is_arithmetic_v && std::is_arithmetic_v && + (std::decay_t::pairKind == MccCoordPairKind::COORDS_KIND_XY)) + { + ref_coordpair_t cp{typename ref_coordpair_t::x_t{colon}, typename ref_coordpair_t::y_t{colat}, + hw_counts.epoch()}; + + return addPoint(cp, hw_counts); + } + + + // something like: + // auto [tag, hw, err] = getPoint(7); + template + std::tuple getPoint(size_t idx) + requires(std::same_as && + std::same_as && + HW_T::pairKind == MccCoordPairKind::COORDS_KIND_XY) + { + std::tuple point; + + if (idx > numberOfPoints()) { + std::get<2>(point) = MccPCMFitterErrorCode::ERROR_INVALID_INDEX; + } else { + std::get<2>(point) = MccPCMFitterErrorCode::ERROR_OK; + + std::get<0>(point).setX(_targetCOLON[idx]); + std::get<0>(point).setY(_targetCOLAT[idx]); + std::get<0>(point).setEpoch(_epoch[idx]); + + std::get<1>(point).setX(_hwX[idx]); + std::get<1>(point).setY(_hwY[idx]); + std::get<1>(point).setEpoch(_epoch[idx]); + } + + return point; + } + +protected: + // store coordinates in radians + std::vector _targetCOLON{}, _targetCOLAT{}, _hwX, _hwY; + std::vector _colonRES{}, _colatRES{}; // target - hw + std::vector _epoch{}; + + + void computeModelResi(compute_result_t& result) + { + result.colon_res.resize(numberOfPoints()); + result.colat_res.resize(numberOfPoints()); + + for (size_t i = 0; i < numberOfPoints(); ++i) { + result.colon_res[i] = _colonRES[i] - result.model_colon[i]; // = target - model + result.colat_res[i] = _colatRES[i] - result.model_colat[i]; // = target - model + } + +#ifdef USE_BSPLINE_PCM + if (result.pcm_type == MccDefaultPCMType::PCM_TYPE_BSPLINE) { + result.inv_colon_res.resize(numberOfPoints()); + result.inv_colat_res.resize(numberOfPoints()); + for (size_t i = 0; i < numberOfPoints(); ++i) { + result.inv_colon_res[i] = _colonRES[i] - result.inv_model_colon[i]; // = hw - model + result.inv_colat_res[i] = _colatRES[i] - result.inv_model_colat[i]; // = hw - model + } + } +#endif + } + + + compute_result_t bsplineFitting(MccDefaultPCM::pcm_data_t& pcm_data) + { + compute_result_t result{.pcm_type = pcm_data.type, .error = MccPCMFitterErrorCode::ERROR_OK}; + + // at least border knots must be given ([t_min, t_max]) + if (pcm_data.bspline.knotsX.size() < 2 || pcm_data.bspline.knotsY.size() < 2) { + result.error = MccPCMFitterErrorCode::ERROR_INVALID_KNOTS_NUMBER; + + return result; + } + + double resi2x, resi2y; // sum of fitting residuals squares + + // full knot vectors: [t_min, t_min, t_min, t_min, ..., t_1, t_2, ..., t_max, t_max, t_max, t_max] + std::vector tx(pcm_data.bspline.knotsX.size() + 6), ty(pcm_data.bspline.knotsY.size() + 6); + + std::vector&theta_hw = _hwY, theta_tag = _targetCOLAT, phi_hw = _hwX, phi_tag = _targetCOLON; + std::vector theta, phi; + + /* + WARNING: + FITPACK B-spline on sphere: in the fitting routines the first angle argument is THETA - co-latitude + coordinate and the second one is PHI - co-longitude!!! + + In the fitting routines the first angle argument THETA must be in the range of [0.0, PI] + (while celestial declination is in the range of [-PI/2, PI/2])!!! + The second angle argument is the PHI and its valid range is [0, 2*PI] + (while celestial hour angle is assumed to be in the range of [-PI, PI]) + */ + + if constexpr (std::same_as) { + theta.resize(numberOfPoints()); + phi.resize(numberOfPoints()); + theta_hw = theta; + phi_hw = phi; + + for (size_t i = 0; i < numberOfPoints(); ++i) { + theta_hw[i] = _hwY[i] + MCC_HALF_PI; + phi_hw[i] = _hwX[i] + std::numbers::pi; + } + } + + tx[0] = tx[1] = tx[2] = pcm_data.bspline.knotsX[0]; + tx[tx.size() - 1] = tx[tx.size() - 2] = tx[tx.size() - 3] = + pcm_data.bspline.knotsX[pcm_data.bspline.knotsX.size() - 1]; + + ty[0] = ty[1] = ty[2] = pcm_data.bspline.knotsY[0] + MCC_HALF_PI; + ty[ty.size() - 1] = ty[ty.size() - 2] = ty[ty.size() - 3] = + pcm_data.bspline.knotsY[pcm_data.bspline.knotsY.size() - 1] + MCC_HALF_PI; + + for (size_t i = 0; i < pcm_data.bspline.knotsX.size(); ++i) { + tx[3 + i] = pcm_data.bspline.knotsX[i]; + } + for (size_t i = 0; i < pcm_data.bspline.knotsY.size(); ++i) { + ty[3 + i] = pcm_data.bspline.knotsY[i] + MCC_HALF_PI; + } + + size_t Ncoeffs = (tx.size() - 4) * (ty.size() - 4); + + pcm_data.bspline.coeffsX.resize(Ncoeffs); + pcm_data.bspline.coeffsY.resize(Ncoeffs); + + + // direct (celestial = encoder + pcm) + result.bspline_fit_err[0] = + bsplines::fitpack_sphere_fit(theta_hw, phi_hw, _colonRES, 1.0, ty, tx, pcm_data.bspline.coeffsX, resi2x); + if (result.bspline_fit_err[0] > 0) { + result.error = MccPCMFitterErrorCode::ERROR_BSPLINE_FIT; + return result; + } + + result.bspline_fit_err[1] = + bsplines::fitpack_sphere_fit(theta_hw, phi_hw, _colatRES, 1.0, ty, tx, pcm_data.bspline.coeffsY, resi2y); + if (result.bspline_fit_err[1] > 0) { + result.error = MccPCMFitterErrorCode::ERROR_BSPLINE_FIT; + return result; + } + + + bsplines::fitpack_eval_spl2d(ty, tx, pcm_data.bspline.coeffsX, theta_hw, phi_hw, + result.model_colon); // get fitted residuals!!! + + bsplines::fitpack_eval_spl2d(ty, tx, pcm_data.bspline.coeffsY, theta_hw, phi_hw, + result.model_colat); // get fitted residuals!!! + + + if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_BSPLINE) { + // here both direct and inverse coefficients will be calculated + pcm_data.inverseBspline.coeffsX.resize(Ncoeffs); + pcm_data.inverseBspline.coeffsY.resize(Ncoeffs); + + std::vector theta_tag(numberOfPoints()); + + // inverse (encoder = celestial + pcm) + + std::vector colon_res = _colonRES; + std::vector colat_res = _colatRES; + if constexpr (std::same_as) { + theta_tag = theta; + phi_tag = phi; + } + + for (size_t i = 0; i < colat_res.size(); ++i) { + colon_res[i] = -colon_res[i]; + colat_res[i] = -colat_res[i]; + + if constexpr (std::same_as) { + theta_tag[i] = _targetCOLAT[i] + MCC_HALF_PI; + phi_tag[i] = _targetCOLON[i] + std::numbers::pi; + } + } + + result.bspline_fit_err[2] = bsplines::fitpack_sphere_fit(theta_tag, phi_tag, colon_res, 1.0, ty, tx, + pcm_data.bspline.inverseCoeffsX, resi2x); + if (result.bspline_fit_err[2] > 0) { + result.error = MccPCMFitterErrorCode::ERROR_BSPLINE_FIT; + return result; + } + + result.bspline_fit_err[3] = bsplines::fitpack_sphere_fit(theta_tag, phi_tag, colat_res, 1.0, ty, tx, + pcm_data.bspline.inverseCoeffsY, resi2y); + if (result.bspline_fit_err[3] > 0) { + result.error = MccPCMFitterErrorCode::ERROR_BSPLINE_FIT; + return result; + } + + bsplines::fitpack_eval_spl2d(ty, tx, pcm_data.bspline.inverseCoeffsX, theta_tag, phi_tag, + result.inv_model_colon); // get fitted residuals!!! + + bsplines::fitpack_eval_spl2d(ty, tx, pcm_data.bspline.inverseCoeffsY, theta_tag, phi_tag, + result.inv_model_colat); // get fitted residuals!!! + } + + computeModelResi(result); + + return result; + } + + + compute_result_t robustLinearRegress(MccDefaultPCM::pcm_data_t& pcm_data, + compute_params_t const& comp_params) + { + auto tukey_weight = [](Eigen::VectorXd const& resi, double tukey_c, Eigen::VectorXd& tukey_weight) { + Eigen::ArrayXd u = (resi / tukey_c).cwiseAbs(); + auto mask = (u < 1.0); + + Eigen::VectorXd r = mask.select(1.0 - u * u, 0.0); + + tukey_weight = r.array() * r.array(); + }; + + auto gastwirth_est = [](Eigen::VectorXd const& r) { + // auto gastwirth_est = [](Eigen::VectorXd const& resi) { + auto resi = r; + std::nth_element(resi.begin(), resi.begin() + resi.size() / 2, resi.end()); + double md = resi(resi.size() / 2); + resi = resi.array() - md; + + // Gastwirth’s estimation: 0.3*Q(1/3) + 0.4*Q(1/2) + 0.3*Q(2/3) + + Eigen::ArrayXd arr = resi.cwiseAbs(); + + std::nth_element(arr.begin(), arr.begin() + 2 * arr.size() / 3, arr.end()); + + // add 1.0E-8 to avoid 0.0 + return (0.3 * arr(arr.size() / 3) + 0.4 * arr(arr.size() / 2) + 0.3 * arr(2 * arr.size() / 3)) / 0.6745 + + 1.0E-8; + + // std::nth_element(arr.begin(), arr.begin() + arr.size() / 2, arr.end()); + + // return arr(arr.size() / 2) / 0.6745 + 1.0E-8; + }; + + compute_result_t result{.pcm_type = pcm_data.type, .error = MccPCMFitterErrorCode::ERROR_OK}; + + const size_t n_coeffs = MOUNT_TYPE == MccMountType::FORK_TYPE ? 9 : 8; + + Eigen::MatrixXd X(2 * numberOfPoints(), n_coeffs); + Eigen::VectorXd y(2 * numberOfPoints()), beta = Eigen::VectorXd::Zero(n_coeffs), new_beta(n_coeffs); + Eigen::VectorXd model; + + Eigen::RowVectorXd row(n_coeffs); + double tgY, sinY, cosY, rcosY, cosX, sinX, cosPhi = std::cos(pcm_data.siteLatitude), + sinPhi = std::sin(pcm_data.siteLatitude); + + // system matrix and right-hand vector + for (size_t i = 0; i < numberOfPoints(); ++i) { + cosY = std::cos(_targetCOLAT[i]); + sinY = std::sin(_targetCOLAT[i]); + rcosY = 1.0 / cosY; + tgY = std::tan(_targetCOLAT[i]); + cosX = std::cos(_targetCOLON[i]); + sinX = std::sin(_targetCOLON[i]); + + + // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl, forkfl] or + // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl] + if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) { + if (utils::isEqual(cosY, 0.0)) { + row << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } else { + row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY, + -(cosPhi * cosX + sinPhi * tgY), 0.0; + } + } else { + if (utils::isEqual(cosY, 0.0)) { + row << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } else { + row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY, + -(cosPhi * cosX + sinPhi * tgY); + } + } + + X.row(i) = row; + + if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) { + if (utils::isEqual(cosX, 0.0)) { + row << 0.0, 1.0, 0.0, 0.0, sinX, cosX, cosPhi * cosX * sinY - sinPhi * cosY, 0.0, 0.0; + } else { + row << 0.0, 1.0, 0.0, 0.0, sinX, cosX, cosPhi * cosX * sinY - sinPhi * cosY, 0.0, 1.0 / cosX; + } + } else { + row << 0.0, 1.0, 0.0, 0.0, sinX, cosX, cosPhi * cosX * sinY - sinPhi * cosY, 0.0; + } + + X.row(i + numberOfPoints()) = row; + + y(i) = _colonRES[i]; + y(i + numberOfPoints()) = _colatRES[i]; + } + + Eigen::VectorXd resi = y; + Eigen::VectorXd weights(2 * numberOfPoints()); + + double scale; + + Eigen::MatrixXd WX; + Eigen::VectorXd Wy; + + // initial parameters estimation (least squares) + beta = X.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y); + model = X * beta; + resi = y - model; + + // resi = y; + + for (result.final_iter = 1; result.final_iter <= comp_params.max_iter; ++result.final_iter) { + scale = gastwirth_est(resi); + + tukey_weight(resi / scale, comp_params.c, weights); + + // Eigen::MatrixXd XtW = X.transpose() * weights.asDiagonal(); + // Eigen::MatrixXd XtWX = XtW * X; + // Eigen::VectorXd XtWy = XtW * y; + + // new_beta = XtWX.ldlt().solve(XtWy); + + WX = weights.asDiagonal() * X; + Wy = weights.asDiagonal() * y; + new_beta = WX.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Wy); + + if ((new_beta - beta).norm() < comp_params.tolerance) { + break; + } + + beta = new_beta; + + model = X * beta; + resi = y - model; + } + + pcm_data.geomCoefficients.zeroPointX = new_beta(0); + pcm_data.geomCoefficients.zeroPointY = new_beta(1); + pcm_data.geomCoefficients.collimationErr = new_beta(2); + pcm_data.geomCoefficients.nonperpendErr = new_beta(3); + pcm_data.geomCoefficients.misalignErr1 = new_beta(4); + pcm_data.geomCoefficients.misalignErr2 = new_beta(5); + pcm_data.geomCoefficients.tubeFlexure = new_beta(6); + pcm_data.geomCoefficients.DECaxisFlexure = new_beta(7); + if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) { + pcm_data.geomCoefficients.forkFlexure = new_beta(8); + } + + result.model_colon = {model.begin(), model.begin() + numberOfPoints()}; + result.model_colat = {model.begin() + numberOfPoints(), model.end()}; + result.colon_res.resize(numberOfPoints()); + result.colat_res.resize(numberOfPoints()); + + result.colon_weight = {weights.begin(), weights.begin() + numberOfPoints()}; + result.colat_weight = {weights.begin() + numberOfPoints(), weights.end()}; + + computeModelResi(result); + + return result; + } +}; + +} // namespace mcc::impl \ No newline at end of file