Jungfraujoch/image_analysis/geom_refinement/SimpleRotXtalOptimizer.cpp

// SPDX-FileCopyrightText: 2025 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include "SimpleRotXtalOptimizer.h"

#include <Eigen/Dense>

static bool SimpleRotXtalOptimizerInternal(
    SimpleRotXtalOptimizerData &data,
    const std::vector<SpotToSave> &spots,
    float tolerance)
{
    Coord vec0 = data.latt.Vec0();
    Coord vec1 = data.latt.Vec1();
    Coord vec2 = data.latt.Vec2();

    const double tol_sq = tolerance * tolerance;

    std::vector<Eigen::Vector3d> obs;
    std::vector<Eigen::Vector3d> exp;

    // Collect reflections
    for (const auto &pt : spots) {
        if (!data.index_ice_rings && pt.ice_ring)
            continue;

        Coord recip = data.geom.DetectorToRecip(pt.x, pt.y);

        double h_fp = recip * vec0;
        double k_fp = recip * vec1;
        double l_fp = recip * vec2;

        double h = std::round(h_fp);
        double k = std::round(k_fp);
        double l = std::round(l_fp);

        double d2 =
            (h - h_fp)*(h - h_fp) +
            (k - k_fp)*(k - k_fp) +
            (l - l_fp)*(l - l_fp);

        if (d2 > tol_sq)
            continue;

        obs.emplace_back(h_fp, k_fp, l_fp);
        exp.emplace_back(h, k, l);
    }

    if (obs.size() < data.min_spots)
        return false;

    const int N = static_cast<int>(obs.size());

    // Build linear system A * omega = b
    Eigen::MatrixXd A(3*N, 3);
    Eigen::VectorXd b(3*N);

    for (int i = 0; i < N; i++) {
        const auto &h = obs[i];
        const auto &e = exp[i];

        // Cross-product matrix of h_obs
        A.row(3*i + 0) <<   0.0, -h.z(),  h.y();
        A.row(3*i + 1) <<  h.z(),  0.0, -h.x();
        A.row(3*i + 2) << -h.y(),  h.x(),  0.0;

        b.segment<3>(3*i) = e - h;
    }

    // Solve least squares
    Eigen::Vector3d omega = A.colPivHouseholderQr().solve(b);

    // Optional sanity check
    if (!omega.allFinite())
        return false;

    if (omega.norm() > 0.05) {
        return false;
        // > ~3 degrees — warn or reject
    }

    // Apply small-angle rotation to lattice
    // TODO: data.latt.RotateSmallAngle(omega);

    return true;
}

bool SimpleRotXtalOptimizer(SimpleRotXtalOptimizerData &data, const std::vector<SpotToSave> &spots) {

    if (!SimpleRotXtalOptimizerInternal(data, spots, 0.3))
        return false;
    SimpleRotXtalOptimizerInternal(data, spots, 0.2);
    return SimpleRotXtalOptimizerInternal(data, spots, 0.1);
}