diff --git a/image_analysis/geom_refinement/GeometryRefiner.cpp b/image_analysis/geom_refinement/GeometryRefiner.cpp index c1835b121..581aebfd7 100644 --- a/image_analysis/geom_refinement/GeometryRefiner.cpp +++ b/image_analysis/geom_refinement/GeometryRefiner.cpp @@ -167,80 +167,125 @@ struct BeamBlock { } }; -// The residual of one spot (XtalResidual) on the blocks this refinement can move: the beam (read through -// BeamBlock), the distance, the frame's orientation and the cell lengths, plus the module's move (dx, dy, rot) -// where the spot's module is placed. The rotation axis and the cell angles are never refined here, and the -// tilt only on request (TiltedSpotResidual): they enter as plain numbers, not as blocks held constant, -// because Ceres sizes its dual numbers from the declared blocks - 17 to 20 derivatives per number, 6 to 8 of -// them always zero. The arithmetic on the values is the same either way. +// The residual of one spot (XtalResidual) on the blocks this refinement can move, in this order: the beam +// (read through BeamBlock) and the distance as one block, the tilt where it is refined, the frame's orientation, +// the cell lengths, and the module's move (dx, dy, rot) where the spot's module is placed. The rotation axis, the +// cell angles and a held tilt enter as plain numbers. +// +// The residual is the difference of two halves that share no block: the observed reciprocal vector (beam, +// distance, tilt, move) and the predicted one (orientation, cell). Ceres' AutoDiffCostFunction would carry the +// derivatives of every block through both halves; here each half is differentiated on its own blocks only, which +// gives the same values and the same derivatives with half as many of them per number. // // A placed module: moving the spot by R(rot) u - u + (dx, dy), with (ux, uy) its offset from the module // centre, shifts its offset from the beam exactly as moving the beam the other way does, so the beam handed // to XtalResidual carries the move and everything else is the residual of an unmoved spot. -struct SpotResidual { - XtalResidual residual; - BeamBlock beam_block; - const double *detector_rot; // the tilt where it is held - const double *rotation_axis; - const double *cell_ang; - double ux = 0, uy = 0; +class SpotCost : public ceres::CostFunction { +public: + SpotCost(const XtalResidual &residual, const BeamBlock &beam_block, const double *detector_rot, + const double *rotation_axis, const double *cell_ang, bool tilt, bool module, double ux = 0, + double uy = 0) + : residual(residual), beam_block(beam_block), detector_rot(detector_rot), rotation_axis(rotation_axis), + cell_ang(cell_ang), tilt(tilt), module(module), ux(ux), uy(uy) { + set_num_residuals(3); + std::vector &sizes = *mutable_parameter_block_sizes(); + sizes.push_back(3); + if (tilt) + sizes.push_back(2); + sizes.push_back(3); + sizes.push_back(3); + if (module) + sizes.push_back(3); + } + + bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override { + if (tilt) + return module ? EvaluateWith(parameters, residuals, jacobians) + : EvaluateWith(parameters, residuals, jacobians); + return module ? EvaluateWith(parameters, residuals, jacobians) + : EvaluateWith(parameters, residuals, jacobians); + } + +private: + template + bool EvaluateWith(double const *const *p, double *out, double **jac) const { + constexpr int N_OBS = 3 + (TILT ? 2 : 0) + (MODULE ? 3 : 0); + using JO = ceres::Jet; + using JP = ceres::Jet; + constexpr int TILT_BLOCK = 1, P0_BLOCK = TILT ? 2 : 1, P1_BLOCK = P0_BLOCK + 1, MOVE_BLOCK = P1_BLOCK + 1; + + // The observed half, on the beam and distance, the tilt and the move. + JO bd[3], t[2], m[3]; + for (int j = 0; j < 3; ++j) + bd[j] = JO(p[0][j], j); + if constexpr (TILT) + for (int j = 0; j < 2; ++j) + t[j] = JO(p[TILT_BLOCK][j], 3 + j); + if constexpr (MODULE) + for (int j = 0; j < 3; ++j) + m[j] = JO(p[MOVE_BLOCK][j], N_OBS - 3 + j); + JO obs[3]; + if constexpr (TILT) + Observed(bd, t, MODULE ? m : nullptr, obs); + else + Observed(bd, detector_rot, MODULE ? m : nullptr, obs); + + // The predicted half, on the orientation and the cell lengths. + JP p0[3], p1[3]; + for (int j = 0; j < 3; ++j) { + p0[j] = JP(p[P0_BLOCK][j], j); + p1[j] = JP(p[P1_BLOCK][j], 3 + j); + } + const JP p2[3] = {JP(cell_ang[0]), JP(cell_ang[1]), JP(cell_ang[2])}; + JP recip_unrot[3]; + residual.PredictedRecipUnrot(p1, p2, recip_unrot); + JP pred[3]; + const AngleAxisRotator rot_p0(p0); + rot_p0.Rotate(recip_unrot, pred); + + for (int r = 0; r < 3; ++r) + out[r] = obs[r].a - pred[r].a; + if (!jac) + return true; + // Row-major 3 x size blocks; a block held constant has no Jacobian asked for. + auto fill = [](double *block, int size, auto derivative) { + if (block) + for (int r = 0; r < 3; ++r) + for (int c = 0; c < size; ++c) + block[r * size + c] = derivative(r, c); + }; + fill(jac[0], 3, [&](int r, int c) { return obs[r].v[c]; }); + if constexpr (TILT) + fill(jac[TILT_BLOCK], 2, [&](int r, int c) { return obs[r].v[3 + c]; }); + if constexpr (MODULE) + fill(jac[MOVE_BLOCK], 3, [&](int r, int c) { return obs[r].v[N_OBS - 3 + c]; }); + fill(jac[P0_BLOCK], 3, [&](int r, int c) { return -pred[r].v[c]; }); + fill(jac[P1_BLOCK], 3, [&](int r, int c) { return -pred[r].v[3 + c]; }); + return true; + } template - bool Evaluate(const T *const beam, const T *const distance_mm, const R *const tilt, const T *const p0, - const T *const p1, const T *const move, T *out) const { + void Observed(const T *const beam_distance, const R *const tilt, const T *const move, T *recip_obs) const { using std::cos; using std::sin; T b[2]; - beam_block.Poni(beam, distance_mm[0], tilt, b); + beam_block.Poni(beam_distance, beam_distance[2], tilt, b); if (move) { const T c = cos(move[2]), s = sin(move[2]); b[0] = b[0] - ((c - T(1)) * T(ux) - s * T(uy) + move[0]); b[1] = b[1] - (s * T(ux) + (c - T(1)) * T(uy) + move[1]); } const R axis[3] = {R(rotation_axis[0]), R(rotation_axis[1]), R(rotation_axis[2])}; - T recip_obs[3]; - residual.ObservedRecip(b, distance_mm[0], tilt, axis, recip_obs); - - const T p2[3] = {T(cell_ang[0]), T(cell_ang[1]), T(cell_ang[2])}; - T recip_unrot[3]; - residual.PredictedRecipUnrot(p1, p2, recip_unrot); - - T e_pred_recip[3]; - const AngleAxisRotator rot_p0(p0); - rot_p0.Rotate(recip_unrot, e_pred_recip); - - out[0] = recip_obs[0] - e_pred_recip[0]; - out[1] = recip_obs[1] - e_pred_recip[1]; - out[2] = recip_obs[2] - e_pred_recip[2]; - return true; + residual.ObservedRecip(b, beam_distance[2], tilt, axis, recip_obs); } - // beam_distance: the beam (x, y) and the distance, as one block. - template - bool operator()(const T *const beam_distance, const T *const p0, const T *const p1, T *out) const { - return Evaluate(beam_distance, beam_distance + 2, detector_rot, p0, p1, static_cast(nullptr), out); - } - - template - bool operator()(const T *const beam_distance, const T *const p0, const T *const p1, const T *const move, - T *out) const { - return Evaluate(beam_distance, beam_distance + 2, detector_rot, p0, p1, move, out); - } -}; - -// The same with the tilt refined, as a block after the beam and distance. -struct TiltedSpotResidual : SpotResidual { - template - bool operator()(const T *const beam_distance, const T *const tilt, const T *const p0, const T *const p1, - T *out) const { - return Evaluate(beam_distance, beam_distance + 2, tilt, p0, p1, static_cast(nullptr), out); - } - - template - bool operator()(const T *const beam_distance, const T *const tilt, const T *const p0, const T *const p1, - const T *const move, T *out) const { - return Evaluate(beam_distance, beam_distance + 2, tilt, p0, p1, move, out); - } + const XtalResidual residual; + const BeamBlock beam_block; + const double *detector_rot; // the tilt where it is held + const double *rotation_axis; + const double *cell_ang; + const bool tilt, module; + const double ux, uy; }; // The residual of a fit that moves only the crystal orientations (refine_beam_distance off, no module @@ -478,32 +523,24 @@ GeometryRefinerResult RefineGlobalGeometry(const DiffractionGeometry &nominal_ge } else if (s.module >= 0 && static_cast(s.module) < modules.size() && refined[s.module]) { // The spot as found: the module's move is a block of the residual. const auto &m = modules[s.module]; - const SpotResidual sr{XtalResidual(s.x, s.y, lambda, pixel, cos_rot3, sin_rot3, 0.0, - h, k, l, system, orientation), - beam_block, detector_rot, rot_vec, cell_ang, s.x - m.x0, s.y - m.y0}; + const XtalResidual found(s.x, s.y, lambda, pixel, cos_rot3, sin_rot3, 0.0, h, k, l, system, + orientation); + auto *cost = new SpotCost(found, beam_block, detector_rot, rot_vec, cell_ang, refine_tilt, true, + s.x - m.x0, s.y - m.y0); if (refine_tilt) - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new TiltedSpotResidual{sr}), - &loss, - beam_distance, detector_rot, orient[i].data(), cell_len, move[s.module].data()); + problem.AddResidualBlock(cost, &loss, beam_distance, detector_rot, orient[i].data(), + cell_len, move[s.module].data()); else - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new SpotResidual{sr}), - &loss, - beam_distance, orient[i].data(), cell_len, move[s.module].data()); - } else if (refine_tilt) - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new TiltedSpotResidual{{xr, beam_block, detector_rot, rot_vec, cell_ang}}), - &loss, - beam_distance, detector_rot, orient[i].data(), cell_len); - else - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new SpotResidual{xr, beam_block, detector_rot, rot_vec, cell_ang}), - &loss, - beam_distance, orient[i].data(), cell_len); + problem.AddResidualBlock(cost, &loss, beam_distance, orient[i].data(), cell_len, + move[s.module].data()); + } else { + auto *cost = new SpotCost(xr, beam_block, detector_rot, rot_vec, cell_ang, refine_tilt, false); + if (refine_tilt) + problem.AddResidualBlock(cost, &loss, beam_distance, detector_rot, orient[i].data(), + cell_len); + else + problem.AddResidualBlock(cost, &loss, beam_distance, orient[i].data(), cell_len); + } ++used_in_frame; ++spots_used; }