in IsometricPatternMatcher/HexGridFitting.cpp [775:818]
int HexGridFitting::findOffset(
const std::array<Eigen::MatrixXi, 6>& patternReference,
const Eigen::MatrixXi& pattern, Eigen::Vector2i& bestOffset,
int& bestIndx) const {
const int R = pattern.rows();
const int C = pattern.cols();
const int referenceR = patternReference[0].rows();
const int referenceC = patternReference[0].cols();
int bestMatch = 0;
for (int patternIndx = 0; patternIndx < 6; ++patternIndx) {
// For all offsets
for (int offsetR = -R; offsetR < R; ++offsetR) {
for (int offsetC = -C; offsetC < C; ++offsetC) {
int numberMatch = 0;
for (int r = 0; r < R; ++r) {
for (int c = 0; c < C; ++c) {
if (r + offsetR >= 0 && r + offsetR < referenceR &&
c + offsetC >= 0 && c + offsetC < referenceC) {
if (patternReference[patternIndx](r + offsetR, c + offsetC) ==
pattern(r, c) &&
pattern(r, c) != 2)
numberMatch += 1;
} // end if
} // end c
} // end r
if (numberMatch > bestMatch) {
bestMatch = numberMatch;
bestOffset << offsetR, offsetC;
bestIndx = patternIndx;
}
} // end offsetC
} // end offsetR
}
if (bfsProcessSeq_.size() == 0 ||
(double(bestMatch) / double(bfsProcessSeq_.size())) < 0.7) {
LOG(INFO) << fmt::format(
"Pattern matching failed, only {} points can be matched from {} "
"processed points.",
bestMatch, bfsProcessSeq_.size());
}
return bestMatch;
}