File indexing completed on 2026-05-10 08:00:12
0001
0002
0003
0004
0005
0006
0007
0008
0009 #pragma once
0010
0011 #include <algorithm>
0012 #include <array>
0013 #include <cmath>
0014 #include <concepts>
0015 #include <cstdint>
0016 #include <functional>
0017 #include <vector>
0018
0019 namespace Acts::Experimental {
0020
0021
0022 class HoughAccumulatorSection {
0023 public:
0024
0025 enum class Decision {
0026 Discard,
0027 Accept,
0028
0029 Drill,
0030
0031 DrillAndExpand,
0032
0033
0034 };
0035
0036 HoughAccumulatorSection() = default;
0037
0038
0039
0040
0041
0042
0043
0044
0045
0046 HoughAccumulatorSection(float xw, float yw, float xBegin, float yBegin,
0047 int div = 0,
0048 const std::vector<std::uint32_t> &indices = {},
0049 const std::vector<float> &history = {});
0050
0051
0052
0053 Decision decision() const { return m_decision; }
0054
0055
0056
0057
0058
0059 Decision updateDecision(Decision d) { return m_decision = d; }
0060
0061
0062
0063
0064
0065
0066
0067 void updateDimensions(float xw, float yw, float xBegin, float yBegin);
0068
0069
0070
0071
0072
0073 void expand(float xs, float ys);
0074
0075
0076
0077 inline const std::vector<std::uint32_t> &indices() const { return m_indices; }
0078
0079
0080
0081 inline std::vector<std::uint32_t> &indices() { return m_indices; }
0082
0083
0084
0085 std::size_t count() const { return m_indices.size(); }
0086
0087
0088
0089
0090
0091
0092
0093
0094
0095 HoughAccumulatorSection bottom(bool copyIndices = false) const;
0096
0097
0098
0099
0100
0101
0102
0103
0104
0105 HoughAccumulatorSection top(bool copyIndices = false) const;
0106
0107
0108
0109
0110
0111
0112
0113 HoughAccumulatorSection left(bool copyIndices = false) const;
0114
0115
0116
0117
0118
0119
0120
0121 HoughAccumulatorSection right(bool copyIndices = false) const;
0122
0123
0124
0125
0126
0127
0128
0129
0130
0131
0132
0133
0134
0135 HoughAccumulatorSection bottomRight(bool copyIndices = false) const;
0136
0137
0138
0139
0140
0141 HoughAccumulatorSection bottomLeft(bool copyIndices = false) const;
0142
0143
0144
0145
0146
0147 HoughAccumulatorSection topLeft(bool copyIndices = false) const;
0148
0149
0150
0151
0152
0153 HoughAccumulatorSection topRight(bool copyIndices = false) const;
0154
0155
0156
0157
0158 template <typename F>
0159 bool isLineInside(F &&function) const &
0160 requires std::invocable<F, float>;
0161
0162
0163
0164
0165
0166
0167 template <typename F>
0168 bool isCrossingInside(F &&line1, F &&line2) const &
0169 requires std::invocable<F, float>;
0170
0171
0172
0173 float xSize() const { return m_xSize; }
0174
0175
0176 float ySize() const { return m_ySize; }
0177
0178
0179 float xBegin() const { return m_xBegin; }
0180
0181
0182 float yBegin() const { return m_yBegin; }
0183
0184
0185 std::uint32_t divisionLevel() const { return m_divisionLevel; }
0186
0187
0188
0189
0190 void setHistory(std::size_t index, float value) {
0191 m_history.resize(std::max(index + 1, m_history.size()));
0192 m_history.at(index) = value;
0193 }
0194
0195
0196
0197 float history(std::uint32_t index) const { return m_history.at(index); }
0198
0199 private:
0200 Decision m_decision = Decision::Drill;
0201 float m_xSize = 0;
0202 float m_ySize = 0;
0203 float m_xBegin = 0;
0204 float m_yBegin = 0;
0205
0206 std::uint32_t m_divisionLevel = 0;
0207
0208 std::vector<std::uint32_t> m_indices;
0209
0210 std::vector<float> m_history;
0211 };
0212
0213 template <typename F>
0214 inline bool HoughAccumulatorSection::isLineInside(F &&function) const &
0215 requires std::invocable<F, float>
0216 {
0217 const float yB = function(m_xBegin);
0218 const float yE = function(m_xBegin + m_xSize);
0219 return (yE > yB) ? yB < m_yBegin + m_ySize && yE > m_yBegin
0220 : yB > m_yBegin && yE < m_yBegin + m_ySize;
0221 }
0222
0223 template <typename F>
0224 inline bool HoughAccumulatorSection::isCrossingInside(F &&line1,
0225 F &&line2) const &
0226 requires std::invocable<F, float>
0227 {
0228
0229
0230
0231
0232
0233
0234
0235
0236
0237
0238
0239
0240
0241
0242
0243
0244
0245
0246
0247 const float xL = xBegin();
0248 const float xR = xBegin() + xSize();
0249
0250
0251 const float y1L = line1(xL);
0252 const float y1R = line1(xR);
0253 const float y2L = line2(xL);
0254 const float y2R = line2(xR);
0255
0256
0257
0258 const float dL = y1L - y2L;
0259 const float dR = y1R - y2R;
0260
0261 if (dL * dR > 0) {
0262 return false;
0263 }
0264
0265
0266 const auto checkVerticalOverlap = [this](float y) {
0267 return yBegin() < y && y < yBegin() + ySize();
0268 };
0269
0270 if (checkVerticalOverlap(y1L) && checkVerticalOverlap(y1R)) {
0271 return true;
0272 }
0273
0274 if (checkVerticalOverlap(y2L) && checkVerticalOverlap(y2R)) {
0275 return true;
0276 }
0277
0278
0279
0280 const float abs_dL = std::abs(dL);
0281 const float abs_dR = std::abs(dR);
0282
0283
0284 if (abs_dL + abs_dR == 0.0f) {
0285 return false;
0286 }
0287
0288 const float t = abs_dL / (abs_dL + abs_dR);
0289 const float xCross = std::lerp(xL, xR, t);
0290
0291
0292 const float yCross1 = line1(xCross);
0293 const float yCross2 = line2(xCross);
0294 const float yCross = 0.5f * (yCross1 + yCross2);
0295
0296 return checkVerticalOverlap(yCross);
0297 }
0298
0299
0300
0301 template <typename Measurement>
0302 struct HoughExplorationOptions {
0303
0304
0305 float xMinBinSize = 1.0f;
0306
0307
0308
0309 float yMinBinSize = 1.0f;
0310
0311
0312
0313 float expandX = 1.1f;
0314
0315
0316
0317 float expandY = 1.1f;
0318
0319
0320 using LineFunctor = std::function<float(const Measurement &, float)>;
0321
0322
0323
0324 LineFunctor lineFunctor;
0325
0326 using DecisionFunctor = std::function<HoughAccumulatorSection::Decision(
0327 const HoughAccumulatorSection &, const std::vector<Measurement> &)>;
0328
0329
0330
0331 DecisionFunctor decisionFunctor;
0332 };
0333
0334
0335
0336
0337
0338
0339
0340
0341 template <typename Measurement>
0342 void exploreHoughParametersSpace(
0343 std::vector<HoughAccumulatorSection> §ionsStack,
0344 const std::vector<Measurement> &measurements,
0345 const HoughExplorationOptions<Measurement> &opt,
0346 std::vector<HoughAccumulatorSection> &results) {
0347 while (!sectionsStack.empty()) {
0348 HoughAccumulatorSection thisSection = std::move(sectionsStack.back());
0349 sectionsStack.pop_back();
0350
0351 std::vector<HoughAccumulatorSection> newSections;
0352 const bool splitX = thisSection.xSize() > opt.xMinBinSize;
0353 const bool splitY = thisSection.ySize() > opt.yMinBinSize;
0354 if (splitX && splitY) {
0355
0356 newSections.push_back(thisSection.bottomLeft());
0357 newSections.push_back(thisSection.topLeft());
0358 newSections.push_back(thisSection.bottomRight());
0359 newSections.push_back(thisSection.topRight());
0360 } else if (!splitX && splitY) {
0361
0362 newSections.push_back(thisSection.bottom());
0363 newSections.push_back(thisSection.top());
0364 } else if (splitX && !splitY) {
0365
0366 newSections.push_back(thisSection.left());
0367 newSections.push_back(thisSection.right());
0368 }
0369
0370 if (thisSection.decision() ==
0371 HoughAccumulatorSection::Decision::DrillAndExpand) {
0372 for (HoughAccumulatorSection &s : newSections) {
0373 s.expand(opt.expandX, opt.expandY);
0374 }
0375 }
0376
0377 for (const std::uint32_t idx : thisSection.indices()) {
0378 const auto &m = measurements[idx];
0379 const auto line = [&](float x) { return opt.lineFunctor(m, x); };
0380
0381 for (HoughAccumulatorSection &s : newSections) {
0382 if (s.isLineInside(line)) {
0383 s.indices().push_back(idx);
0384 }
0385 }
0386 }
0387 for (HoughAccumulatorSection &s : newSections) {
0388 s.updateDecision(opt.decisionFunctor(s, measurements));
0389 if (s.decision() == HoughAccumulatorSection::Decision::Accept) {
0390 results.push_back(std::move(s));
0391 } else if (s.decision() == HoughAccumulatorSection::Decision::Drill ||
0392 s.decision() ==
0393 HoughAccumulatorSection::Decision::DrillAndExpand) {
0394 sectionsStack.push_back(std::move(s));
0395 }
0396 }
0397 }
0398 }
0399
0400
0401
0402
0403
0404
0405
0406
0407
0408 template <typename Measurement, typename Functor>
0409 bool passIntersectionsCheck(const HoughAccumulatorSection §ion,
0410 const std::vector<Measurement> &measurements,
0411 const Functor &lineFunctor,
0412 const std::uint32_t threshold) {
0413 const std::size_t count = section.count();
0414 const float xLeft = section.xBegin();
0415 const float xRight = xLeft + section.xSize();
0416 const auto &indices = section.indices();
0417
0418
0419 constexpr std::size_t kMaxStackLines = 64;
0420
0421 if (count <= kMaxStackLines) {
0422 std::array<float, kMaxStackLines> yLeft{};
0423 std::array<float, kMaxStackLines> yRight{};
0424
0425 for (std::size_t i = 0; i < count; ++i) {
0426 const auto &m = measurements[indices[i]];
0427 yLeft[i] = lineFunctor(m, xLeft);
0428 yRight[i] = lineFunctor(m, xRight);
0429 }
0430
0431 std::uint32_t inside = 0;
0432 for (std::uint32_t i = 0; i < count; ++i) {
0433 for (std::uint32_t j = i + 1; j < count; ++j) {
0434 if ((yLeft[i] - yLeft[j]) * (yRight[i] - yRight[j]) < 0.0f) {
0435 inside++;
0436 if (inside >= threshold) {
0437 return true;
0438 }
0439 }
0440 }
0441 }
0442 return false;
0443 } else {
0444 std::vector<float> yLeft(count);
0445 std::vector<float> yRight(count);
0446 for (std::uint32_t i = 0; i < count; ++i) {
0447 const auto &m = measurements[indices[i]];
0448 yLeft[i] = lineFunctor(m, xLeft);
0449 yRight[i] = lineFunctor(m, xRight);
0450 }
0451 std::uint32_t inside = 0;
0452 for (std::uint32_t i = 0; i < count; ++i) {
0453 for (std::uint32_t j = i + 1; j < count; ++j) {
0454 if ((yLeft[i] - yLeft[j]) * (yRight[i] - yRight[j]) < 0.0f) {
0455 inside++;
0456 if (inside >= threshold) {
0457 return true;
0458 }
0459 }
0460 }
0461 }
0462 return inside >= threshold;
0463 }
0464 }
0465
0466 }