rewrite PCM fitter algorithm (mcc_pcm_fit.h, MccPcmFitter class)
This commit is contained in:
@@ -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})
|
||||
|
||||
563
include/mcc/mcc_pcm_fit.h
Normal file
563
include/mcc/mcc_pcm_fit.h
Normal file
@@ -0,0 +1,563 @@
|
||||
#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 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<MccPCMFitterErrorCode>(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<int>(ec), MccPCMFitterErrorCategory::get());
|
||||
}
|
||||
|
||||
} // namespace mcc::impl
|
||||
|
||||
|
||||
namespace std
|
||||
{
|
||||
|
||||
template <>
|
||||
class is_error_code_enum<mcc::impl::MccPCMFitterErrorCode> : public true_type
|
||||
{
|
||||
};
|
||||
|
||||
} // namespace std
|
||||
|
||||
|
||||
|
||||
namespace mcc::impl
|
||||
{
|
||||
|
||||
template <MccMountType MOUNT_TYPE>
|
||||
class MccPCMFitter
|
||||
{
|
||||
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 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
|
||||
std::vector<double> colon_weight{}, colat_weight; // Tukey's weights
|
||||
|
||||
#ifdef USE_BSPLINE_PCM
|
||||
std::array<int, 4> 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<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 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 <mcc_coord_pair_c TAG_T, mcc_coord_pair_c HW_T>
|
||||
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<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);
|
||||
}
|
||||
|
||||
template <typename COLON_T, typename COLAT_T>
|
||||
MccError addPoint(COLON_T const& colon, COLAT_T const& colat, mcc_coord_pair_c auto const& hw_counts)
|
||||
requires(std::is_arithmetic_v<COLON_T> && std::is_arithmetic_v<COLAT_T> &&
|
||||
(std::decay_t<decltype(hw_counts)>::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 <mcc_coord_pair_c TAG_T, mcc_coord_pair_c HW_T>
|
||||
std::tuple<TAG_T, HW_T, MccError> getPoint(size_t idx)
|
||||
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)
|
||||
{
|
||||
std::tuple<TAG_T, HW_T, MccError> 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<double> _targetCOLON{}, _targetCOLAT{}, _hwX, _hwY;
|
||||
std::vector<double> _colonRES{}, _colatRES{}; // target - hw
|
||||
std::vector<MccCelestialCoordEpoch> _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<MOUNT_TYPE>::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<double> tx(pcm_data.bspline.knotsX.size() + 6), ty(pcm_data.bspline.knotsY.size() + 6);
|
||||
|
||||
std::vector<double>&theta_hw = _hwY, theta_tag = _targetCOLAT, phi_hw = _hwX, phi_tag = _targetCOLON;
|
||||
std::vector<double> 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<ref_coordpair_t, MccSkyHADEC_OBS>) {
|
||||
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<double> theta_tag(numberOfPoints());
|
||||
|
||||
// inverse (encoder = celestial + pcm)
|
||||
|
||||
std::vector<double> colon_res = _colonRES;
|
||||
std::vector<double> colat_res = _colatRES;
|
||||
if constexpr (std::same_as<ref_coordpair_t, MccSkyHADEC_OBS>) {
|
||||
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<ref_coordpair_t, MccSkyHADEC_OBS>) {
|
||||
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<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 = 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
|
||||
Reference in New Issue
Block a user