590 lines
23 KiB
C++
590 lines
23 KiB
C++
#pragma once
|
||
|
||
/****************************************************************************************
|
||
* *
|
||
* MOUNT CONTROL COMPONENTS LIBRARY *
|
||
* *
|
||
* *
|
||
* "POINTING-CORRECTION-MODEL" FITTER *
|
||
* *
|
||
****************************************************************************************/
|
||
|
||
#include <eigen3/Eigen/Dense>
|
||
|
||
#include "mcc_concepts.h"
|
||
#include "mcc_coordinate.h"
|
||
#include "mcc_pcm.h"
|
||
|
||
namespace mcc::impl
|
||
{
|
||
|
||
enum class MccDefaultPCMConstructorErrorCode : int {
|
||
ERROR_OK,
|
||
ERROR_NOT_ENOUGH_DATA,
|
||
ERROR_INVALID_INDEX,
|
||
#ifdef USE_BSPLINE_PCM
|
||
ERROR_INVALID_KNOTS_NUMBER,
|
||
ERROR_BSPLINE_FIT
|
||
#endif
|
||
};
|
||
|
||
// error category
|
||
|
||
struct MccDefaultPCMConstructorErrorCategory : std::error_category {
|
||
MccDefaultPCMConstructorErrorCategory() = default;
|
||
|
||
const char* name() const noexcept
|
||
{
|
||
return "MCC-DEFAULT-PCM-CONSTRUCTOR-ERROR-CATEGORY";
|
||
}
|
||
|
||
std::string message(int ec) const
|
||
{
|
||
MccDefaultPCMConstructorErrorCode err = static_cast<MccDefaultPCMConstructorErrorCode>(ec);
|
||
|
||
switch (err) {
|
||
case MccDefaultPCMConstructorErrorCode::ERROR_OK:
|
||
return "OK";
|
||
case MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA:
|
||
return "not enough data point";
|
||
case MccDefaultPCMConstructorErrorCode::ERROR_INVALID_INDEX:
|
||
return "invalid index of data point";
|
||
#ifdef USE_BSPLINE_PCM
|
||
case MccDefaultPCMConstructorErrorCode::ERROR_INVALID_KNOTS_NUMBER:
|
||
return "invalid number of B-spline knots";
|
||
case MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT:
|
||
return "B-spline fitting error";
|
||
#endif
|
||
default:
|
||
return "UNKNOWN";
|
||
}
|
||
}
|
||
|
||
static const MccDefaultPCMConstructorErrorCategory& get()
|
||
{
|
||
static const MccDefaultPCMConstructorErrorCategory constInst;
|
||
return constInst;
|
||
}
|
||
};
|
||
|
||
|
||
inline std::error_code make_error_code(MccDefaultPCMConstructorErrorCode ec)
|
||
{
|
||
return std::error_code(static_cast<int>(ec), MccDefaultPCMConstructorErrorCategory::get());
|
||
}
|
||
|
||
} // namespace mcc::impl
|
||
|
||
|
||
namespace std
|
||
{
|
||
|
||
template <>
|
||
class is_error_code_enum<mcc::impl::MccDefaultPCMConstructorErrorCode> : public true_type
|
||
{
|
||
};
|
||
|
||
|
||
} // namespace std
|
||
|
||
namespace mcc::impl
|
||
{
|
||
|
||
template <MccMountType MOUNT_TYPE>
|
||
class MccPCMConstructor
|
||
{
|
||
public:
|
||
using ref_coordpair_t = std::conditional_t<mcc_is_equatorial_mount<MOUNT_TYPE>,
|
||
MccSkyHADEC_OBS,
|
||
std::conditional_t<mcc_is_altaz_mount<MOUNT_TYPE>, MccSkyAZZD, void>>;
|
||
|
||
static_assert(!std::is_void_v<ref_coordpair_t>, "UNSUPPORTED MOUNT TYPE!");
|
||
|
||
struct table_t {
|
||
std::vector<double> target_colon{}, target_colat{};
|
||
std::vector<double> hw_colon{}, hw_colat{};
|
||
std::vector<double> colon_res{}, colat_res{}; // target - hw
|
||
};
|
||
|
||
struct compute_result_t {
|
||
MccDefaultPCMType pcm_type;
|
||
MccError error{}; // final model computation error
|
||
|
||
size_t final_iter{}; // number of final iteration
|
||
|
||
std::vector<double> model_colon{}, model_colat{}; // fitting model values
|
||
std::vector<double> colon_res{}, colat_res{}; // target - model
|
||
|
||
#ifdef USE_BSPLINE_PCM
|
||
int bspline_fit_err{}; // bivariate B-spline fitting exit code (see FITPACK)
|
||
|
||
// quantities below are computed only fo pcm_type == MccDefaultPCMType::PCM_TYPE_BSPLINE
|
||
std::vector<double> inv_model_colon, inv_model_colat; // fitted inverse model values
|
||
std::vector<double> 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
|
||
double tolerance{1.0E-6}; //
|
||
};
|
||
|
||
template <mcc_coord_pair_c TAG_T, mcc_coord_pair_c HW_T>
|
||
MccError addPoint(TAG_T target_coords, HW_T const& hw_counts)
|
||
requires(TAG_T::pairKind == ref_coordpair_t::pairKind && HW_T::pairKind == MccCoordPairKind::COORDS_KIND_XY)
|
||
{
|
||
//
|
||
// TODO: target_coords.epoch() != hw_counts.epoch() ?!!!!!!!!!
|
||
//
|
||
|
||
|
||
_tableEpoch.emplace_back(hw_counts.epoch());
|
||
|
||
_table.target_colon.emplace_back(target_coords.x());
|
||
_table.target_colat.emplace_back(target_coords.y());
|
||
|
||
_table.hw_colon.emplace_back(hw_counts.x());
|
||
_table.hw_colat.emplace_back(hw_counts.y());
|
||
|
||
_table.colon_res.emplace_back((double)target_coords.x() - (double)hw_counts.x());
|
||
_table.colat_res.emplace_back((double)target_coords.y() - (double)hw_counts.y());
|
||
|
||
return {};
|
||
}
|
||
|
||
MccError addPoint(mcc_skypoint_c auto const& target_coords, mcc_coord_pair_c auto const& hw_counts)
|
||
requires(std::decay_t<decltype(hw_counts)>::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);
|
||
|
||
// auto err = target_coords.to(ref_coordpair_t::pairKind, hw_counts.epoch());
|
||
// if (err) {
|
||
// return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
|
||
// }
|
||
|
||
// _tableEpoch.emplace_back(hw_counts.epoch());
|
||
|
||
// _table.target_colon.emplace_back(target_coords.co_lon());
|
||
// _table.target_colat.emplace_back(target_coords.co_lat());
|
||
|
||
// _table.hw_colon.emplace_back(hw_counts.x());
|
||
// _table.hw_colat.emplace_back(hw_counts.y());
|
||
|
||
// _table.colon_res.emplace_back(target_coords.co_lon() - hw_counts.x());
|
||
// _table.colat_res.emplace_back(target_coords.co_lat() - hw_counts.y());
|
||
|
||
// return {};
|
||
}
|
||
|
||
const table_t& getTable() const
|
||
{
|
||
return _table;
|
||
}
|
||
|
||
template <mcc_coord_pair_c TAG_T, mcc_coord_pair_c HW_T>
|
||
MccError getPoint(size_t idx, std::tuple<TAG_T, HW_T>& point)
|
||
requires(std::same_as<typename TAG_T::x_t, typename ref_coordpair_t::x_t> &&
|
||
std::same_as<typename TAG_T::y_t, typename ref_coordpair_t::y_t> &&
|
||
HW_T::pairKind == MccCoordPairKind::COORDS_KIND_XY)
|
||
{
|
||
if (idx > _table.target_colon.size()) {
|
||
return MccDefaultPCMConstructorErrorCode::ERROR_INVALID_INDEX;
|
||
}
|
||
|
||
std::get<0>(point).setX(_table.target_colon[idx]);
|
||
std::get<0>(point).setY(_table.target_colat[idx]);
|
||
std::get<0>(point).setEpoch(_tableEpoch[idx]);
|
||
|
||
std::get<1>(point).setX(_table.hw_colon[idx]);
|
||
std::get<1>(point).setY(_table.hw_colat[idx]);
|
||
std::get<1>(point).setEpoch(_tableEpoch[idx]);
|
||
|
||
return MccDefaultPCMConstructorErrorCode::ERROR_OK;
|
||
}
|
||
|
||
|
||
size_t numberOfPoints() const
|
||
{
|
||
return _table.colon_res.size();
|
||
}
|
||
|
||
void deletePoints()
|
||
{
|
||
_tableEpoch.clear();
|
||
|
||
_table.target_colon.clear();
|
||
_table.target_colat.clear();
|
||
|
||
_table.hw_colon.clear();
|
||
_table.hw_colat.clear();
|
||
|
||
_table.colon_res.clear();
|
||
_table.colat_res.clear();
|
||
}
|
||
|
||
|
||
//
|
||
// for B-splines interior knots along axes must be given in 'pcm_data'
|
||
// NOTE: the size of the interior knots array must be at least 2 as
|
||
// it are interpretated as border knots and final full knots set is:
|
||
// knots = [input_knots[0], input_knots[0], input_knots[0], input_knots[0], input_knots[1], input_knots[2],
|
||
// ..., input_knots[N-1], input_knots[N-1], input_knots[N-1], input_knots[N-1]], where N = input_knots.size()
|
||
//
|
||
// WARNING: the input knots for inverse B-spline are ignored so the direct and inverse B-spline coefficients are
|
||
// calculated on the same mesh!
|
||
compute_result_t computeModel(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data,
|
||
compute_params_t const& comp_params = {})
|
||
{
|
||
compute_result_t result{.pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK};
|
||
|
||
size_t min_data_size = 2; // 2 is for BSPLINE
|
||
|
||
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY
|
||
#ifdef USE_BSPLINE_PCM
|
||
|| pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE
|
||
#endif
|
||
) {
|
||
if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) {
|
||
min_data_size = 9;
|
||
} else {
|
||
min_data_size = 8;
|
||
}
|
||
|
||
if (numberOfPoints() < min_data_size) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA;
|
||
return result;
|
||
}
|
||
|
||
// robust linear regression with Tukey's loss function
|
||
result = robustLinearRegress(pcm_data, comp_params);
|
||
} else {
|
||
if (numberOfPoints() < min_data_size) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA;
|
||
return result;
|
||
}
|
||
}
|
||
|
||
|
||
#ifdef USE_BSPLINE_PCM
|
||
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_BSPLINE) {
|
||
return bsplineFitting(pcm_data);
|
||
} else if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
|
||
// the fitting for geometrical coefficients is already done above so
|
||
// one must fit residuals by bivariate B-splines
|
||
|
||
std::vector<double> xres = _table.colon_res, yres = _table.colat_res;
|
||
for (size_t i = 0; i < numberOfPoints(); ++i) {
|
||
_table.colon_res[i] = _table.target_colon[i] - result.model_colon[i];
|
||
_table.colat_res[i] = _table.target_colat[i] - result.model_colat[i];
|
||
}
|
||
|
||
auto r = bsplineFitting(pcm_data);
|
||
result.error = r.error;
|
||
result.bspline_fit_err = r.bspline_fit_err;
|
||
|
||
if (!r.error) {
|
||
for (size_t i = 0; i < numberOfPoints(); ++i) {
|
||
result.model_colon[i] += r.model_colon[i];
|
||
result.model_colat[i] += r.model_colat[i];
|
||
|
||
result.colon_res[i] = _table.target_colon[i] - result.model_colon[i];
|
||
result.colat_res[i] = _table.target_colat[i] - result.model_colat[i];
|
||
}
|
||
}
|
||
|
||
// restore original residuals
|
||
_table.colon_res = xres;
|
||
_table.colat_res = yres;
|
||
}
|
||
#endif
|
||
|
||
return result;
|
||
}
|
||
|
||
protected:
|
||
// std::vector<table_elem_t> _table;
|
||
table_t _table{};
|
||
std::vector<MccCelestialCoordEpoch> _tableEpoch{};
|
||
|
||
compute_result_t bsplineFitting(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data)
|
||
{
|
||
compute_result_t result{.pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK};
|
||
|
||
if (pcm_data.bspline.knotsX.size() < 2 || pcm_data.bspline.knotsY.size() < 2) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_INVALID_KNOTS_NUMBER;
|
||
|
||
return result;
|
||
}
|
||
|
||
double resi2x, resi2y; // sum of fitting residuals squares
|
||
|
||
std::vector<double> tx(pcm_data.bspline.knotsX.size() + 6), ty(pcm_data.bspline.knotsY.size() + 6);
|
||
|
||
size_t Ncoeffs = (tx.size() - 4) * (ty.size() - 4);
|
||
|
||
pcm_data.bspline.coeffsX.resize(Ncoeffs);
|
||
pcm_data.bspline.coeffsY.resize(Ncoeffs);
|
||
|
||
/*
|
||
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
|
||
|
||
*/
|
||
|
||
// direct (celestial = encoder + pcm)
|
||
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.hw_colat, _table.hw_colon, _table.colon_res, 1.0,
|
||
pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.coeffsX, resi2x);
|
||
if (result.bspline_fit_err > 0) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
|
||
return result;
|
||
}
|
||
|
||
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.hw_colat, _table.hw_colon, _table.colat_res, 1.0,
|
||
pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.coeffsY, resi2y);
|
||
if (result.bspline_fit_err > 0) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
|
||
return result;
|
||
}
|
||
|
||
|
||
bsplines::fitpack_eval_spl2d(pcm_data.bspline.knotsY, pcm_data.bspline.knotsX, pcm_data.bspline.coeffsX,
|
||
_table.hw_colat, _table.hw_colon, result.model_colon); // get fitted residuals!!!
|
||
|
||
bsplines::fitpack_eval_spl2d(pcm_data.bspline.knotsY, pcm_data.bspline.knotsX, pcm_data.bspline.coeffsY,
|
||
_table.hw_colat, _table.hw_colon, result.model_colat); // get fitted residuals!!!
|
||
|
||
|
||
result.colon_res.resize(numberOfPoints());
|
||
result.colat_res.resize(numberOfPoints());
|
||
for (size_t i = 0; i < numberOfPoints(); ++i) {
|
||
result.colon_res[i] = _table.colon_res[i] - result.model_colon[i]; // = target - model
|
||
result.colat_res[i] = _table.colat_res[i] - result.model_colat[i]; // = target - model
|
||
result.model_colon[i] += _table.hw_colon[i]; // == hw + fitted_pcmX
|
||
result.model_colat[i] += _table.hw_colat[i]; // == hw + fitted_pcmY
|
||
}
|
||
|
||
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);
|
||
|
||
// inverse (encoder = celestial + pcm)
|
||
|
||
std::vector<double> colon_res = _table.colon_res;
|
||
std::vector<double> colat_res = _table.colat_res;
|
||
for (size_t i = 0; i < colat_res.size(); ++i) {
|
||
colon_res[i] = -colon_res[i];
|
||
colat_res[i] = -colat_res[i];
|
||
}
|
||
|
||
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.target_colon, _table.target_colat, colon_res,
|
||
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.inverseCoeffsX, resi2x);
|
||
if (result.bspline_fit_err > 0) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
|
||
return result;
|
||
}
|
||
|
||
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.target_colon, _table.target_colat, colat_res,
|
||
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.inverseCoeffsY, resi2y);
|
||
if (result.bspline_fit_err > 0) {
|
||
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
|
||
return result;
|
||
}
|
||
|
||
bsplines::fitpack_eval_spl2d(pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.inverseCoeffsX, _table.hw_colat, _table.hw_colon,
|
||
result.inv_model_colon); // get fitted residuals!!!
|
||
|
||
bsplines::fitpack_eval_spl2d(pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
|
||
pcm_data.bspline.inverseCoeffsY, _table.hw_colat, _table.hw_colon,
|
||
result.inv_model_colat); // get fitted residuals!!!
|
||
|
||
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] = _table.colon_res[i] - result.inv_model_colon[i]; // = hw - model
|
||
result.inv_colat_res[i] = _table.colat_res[i] - result.inv_model_colat[i]; // = hw - model
|
||
result.inv_model_colon[i] += _table.target_colon[i]; // == target + fitted_pcmX
|
||
result.inv_model_colat[i] += _table.target_colat[i]; // == target + fitted_pcmY
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
|
||
compute_result_t robustLinearRegress(MccDefaultPCM<MOUNT_TYPE>::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 = MccDefaultPCMConstructorErrorCode::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(_table.target_colat[i]);
|
||
sinY = std::sin(_table.target_colat[i]);
|
||
rcosY = 1.0 / cosY;
|
||
tgY = std::tan(_table.target_colat[i]);
|
||
cosX = std::cos(_table.target_colon[i]);
|
||
sinX = std::sin(_table.target_colon[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) = _table.colon_res[i];
|
||
y(i + numberOfPoints()) = _table.colat_res[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());
|
||
|
||
for (size_t i = 0; i < numberOfPoints(); ++i) {
|
||
result.colon_res[i] = _table.colon_res[i] - result.model_colon[i]; // = target - model
|
||
result.colat_res[i] = _table.colat_res[i] - result.model_colat[i]; // = target - model
|
||
// result.model_colon[i] += _table.hw_colon[i]; // == hw + fitted_pcmX
|
||
// result.model_colat[i] += _table.hw_colat[i]; // == hw + fitted_pcmY
|
||
}
|
||
|
||
return result;
|
||
}
|
||
};
|
||
|
||
} // namespace mcc::impl
|