分析一下这个函数:bool MultiCentreLaserExtract(const cv::Mat& src_img, std::vector<cv::Point2f>& centre_points,
std::vector<uint8_t>& gray_values, int gray_threshold,
int search_range, LaserSearchDirection search_direction,
CentreExtractMode mode, const BlurConfig& blur_config,
int filter_half_size, float laser_half_width) {
if (src_img.type() != CV_8UC1) {
LOG_ERROR_AND_RETURN_FALSE("src_img should be CV_8UC1");
}
cv::Mat src_img_float;
// Transpose to use the same coordinates
if (search_direction == LaserSearchDirection::kVertical) {
#ifdef _DEBUG
cv::transpose(src_img, src_img_float);
#else
// 多线程分块转置
if (!BlockTransposeGrayImg(src_img, src_img_float, 128)) {
LOG_ERROR_AND_RETURN_FALSE("BlockTransposeGrayImg failed");
}
#endif // DEBUG
} else {
src_img_float = src_img.clone();
}
// #ifdef _DEBUG
// // 图像预处理
// if (blur_config.enabled && blur_config.kernel_size.width > 0 &&
// blur_config.kernel_size.height > 0) {
// algo::image_process::CvBlur(src_img_float, src_img_float, blur_config.kernel_size,
// CV_8UC1,
// BlurMode::kGaussianBlur);
// }
// // src_img_float.convertTo(src_img_float, CV_32FC1);
// #else
// 图像预处理
// src_img_float.convertTo(src_img_float, CV_32FC1);
// 和 FPGA 一致, 没有先转为浮点数再滤波
if (blur_config.enabled && blur_config.kernel_size.width > 0 &&
blur_config.kernel_size.height > 0) {
algo::image_process::CvBlur(src_img_float, src_img_float, blur_config.kernel_size, CV_8UC1,
BlurMode::kGaussianBlur);
}
// #endif // DEBUG
int resolution_width = src_img_float.cols;
int resolution_height = src_img_float.rows;
// 初始化输出
auto centre_points_internal = std::vector<cv::Point2f>(resolution_height);
for (int h = 0; h < resolution_height; h++) {
centre_points_internal[h] = cv::Point2f(0.f, h);
}
auto gray_values_internal = std::vector<uint8_t>(resolution_height);
switch (mode) {
case CentreExtractMode::kCaliper: {
//先进行高斯滤波 sigma需要大于等于激光线半宽除以sqrt(3)
//详情见《An Unbiased Detector of Curvilinear Structures》
cv::GaussianBlur(src_img_float, src_img_float, cv::Size(0, 0), laser_half_width/sqrt(3));
//用高斯一阶导进行滤波
//每行中正响应最大点和负响应最大点构成激光线的边缘
//滤波核的计算方法见《An Unbiased Detector of Curvilinear Structures》
cv::Mat kernel(1, 2 * filter_half_size + 1, CV_32F);
float sigma = filter_half_size / 1.0;
for (int i = 0; i < kernel.cols; i++) {
int x = i - filter_half_size;
kernel.at<float>(0, i) =
exp(-(pow(x + 0.5, 2) / (2 * sigma * sigma))) / (sqrt(2 * CV_PI) * sigma) -
exp(-(pow(x - 0.5, 2) / (2 * sigma * sigma))) / (sqrt(2 * CV_PI) * sigma);
}
cv::Mat filtered;
cv::filter2D(src_img_float, filtered, CV_32F, kernel);
for (int h = 0; h < resolution_height; h++) {
std::vector<float> data(&filtered.at<float>(h, 0),
&filtered.at<float>(h, 0) + resolution_width);
cv::Point pMax, pMin;
cv::minMaxLoc(data, NULL, NULL, &pMin, &pMax);
int width = fabs(pMax.x - pMin.x) + 1;
//激光线太细 数据量不够 不计算
if (width < 3) continue;
int start = pMax.x < pMin.x ? pMax.x : pMin.x;
int end = pMax.x > pMin.x ? pMax.x : pMin.x;
//计算[start,end]区间内平均灰度
float avg =
cv::sum(std::vector<uchar>(&src_img_float.at<uchar>(h, start),
&src_img_float.at<uchar>(h, start) + width))[0] / (float)width;
//平均灰度太低 不计算
if (avg < gray_threshold) continue;
//对[start,end]内的点进行高斯拟合,方法见
//https://blog.csdn.net/qq_33487700/article/details/121998149
cv::Mat A(width, 3, CV_64F);
cv::Mat b(width, 1, CV_64F);
for (int r = 0; r < A.rows; r++) {
A.at<double>(r, 0) = 1;
int x = start + r;
int y = src_img_float.at<uchar>(h, x);
A.at<double>(r, 1) = x;
A.at<double>(r, 2) = x * x;
b.at<double>(r, 0) = log(y);
}
//cv::Mat x;
//cv::solve(A, b, x, cv::DecompTypes::DECOMP_SVD);
//和fpga一致
cv::Mat x = (A.t() * A).inv() * A.t() * b;
centre_points_internal[h].x = -x.at<double>(1, 0) / (2 * x.at<double>(2, 0));
gray_values_internal[h] =
src_img_float.at<uchar>(h, centre_points_internal[h].x);
}
break;
}
default: {
std::vector<std::vector<BrightSection>> bright_ranges_cand =
std::vector<std::vector<BrightSection>>(resolution_height);
constexpr int kNumCandSectionsPerRow = 5;
for (auto& ranges : bright_ranges_cand) {
ranges.reserve(kNumCandSectionsPerRow);
}
#ifdef _DEBUG
const int kNumThreads = 1;
#else
const int kNumThreads = std::max(1, omp_get_num_procs() / 2);
#pragma omp parallel for num_threads(kNumThreads) schedule(static)
#endif
for (int h = 0; h < resolution_height; h++) {
// #ifdef _DEBUG
auto* data_src = src_img_float.ptr<uchar>(h);
// #else
// auto* data_src = src_img_float.ptr<float>(h);
// #endif
std::vector<BrightSection> bright_ranges;
for (int w = 0; w < resolution_width; w++) {
// Search The Largest Value In Area
// [w - search_range, w + search_range + 1]
if (data_src[w] <= gray_threshold) continue;
auto start_w = std::max(0, w - search_range);
auto end_w = std::min(resolution_width, w + search_range + 1);
// 确保 w 位置的灰度是局部最大值
if (std::any_of(data_src + start_w, data_src + end_w,
[&](const auto& val) { return data_src[w] < val; })) {
continue;
}
// Find Gravity Sub-pixel Centre for Candidate
double x_index_weighted = 0, intensity_weighted = 0;
double sum_loc_intensity = 0;
double sum_intensity_intensity = 0;
double sum_intensity = 0;
double max_intensity = 0;
start_w = std::max(0, w - search_range);
end_w = std::min(resolution_width, w + search_range + 1);
for (int index_w = start_w; index_w < end_w;
index_w++) // Find Sub_centre around max_pos
{
double intensity = data_src[index_w];
sum_loc_intensity += (index_w * intensity);
sum_intensity_intensity += intensity * intensity;
sum_intensity += intensity;
max_intensity = std::max(max_intensity, intensity);
}
if (sum_intensity <= 0) continue;
// Sorting according to x_index not gray_val
if (bright_ranges.size() < kNumCandSectionsPerRow) {
bright_ranges.emplace_back(
sum_intensity_intensity / sum_intensity, max_intensity,
cv::Point2f{static_cast<float>(sum_loc_intensity / sum_intensity),
static_cast<float>(h)});
} else {
auto min_it =
std::min_element(bright_ranges.begin(), bright_ranges.end(),
[](const BrightSection& a, const BrightSection& b) {
return a.max_intensity < b.max_intensity;
});
size_t min_index = min_it - bright_ranges.begin();
// The coming value is greater than min gray_val && x_index greater than all
// the candidates
if (bright_ranges[min_index].max_intensity < max_intensity) {
// 将min_index后面的元素前移一位
std::copy(bright_ranges.begin() + min_index + 1, bright_ranges.end(),
bright_ranges.begin() + min_index);
// 在最后放入新值
bright_ranges.back() = BrightSection{
intensity_weighted, max_intensity,
cv::Point2f{static_cast<float>(sum_loc_intensity / sum_intensity),
static_cast<float>(h)}};
}
}
w += search_range; // Avoid repeated traversal
}
bright_ranges_cand[h] = std::move(bright_ranges);
// 对于只有一个点的情况, 先赋值
if (bright_ranges_cand[h].size() == 1) {
centre_points_internal[h].x = bright_ranges_cand[h][0].center.x;
gray_values_internal[h] = bright_ranges_cand[h][0].mean_intensity;
}
}
for (int h = 0; h < resolution_height; h++) {
if (bright_ranges_cand[h].size() <= 1) continue;
switch (mode) {
case CentreExtractMode::kMaxValue: {
auto max_it = std::max_element(
bright_ranges_cand[h].begin(), bright_ranges_cand[h].end(),
[](const BrightSection& a, const BrightSection& b) {
return a.max_intensity <
b.max_intensity; // 不同区域比较使用该区域的灰度最大值
});
centre_points_internal[h].x = max_it->center.x;
gray_values_internal[h] = max_it->mean_intensity; // 返回灰度使用加权均值
break;
}
case CentreExtractMode::kNearLeft:
centre_points_internal[h].x = bright_ranges_cand[h][0].center.x;
gray_values_internal[h] = bright_ranges_cand[h][0].mean_intensity;
break;
case CentreExtractMode::kFarRight:
centre_points_internal[h].x = bright_ranges_cand[h].back().center.x;
gray_values_internal[h] = bright_ranges_cand[h].back().mean_intensity;
break;
case CentreExtractMode::kContinuous: {
int index_select = -1;
const int kSearchRange = 5;
const float kHWeights[kSearchRange + 1] = {0.0, 1.0, 0.9, 0.8, 0.7, 0.5};
float dist_min = std::numeric_limits<float>::max(); // 初始化最小距离,
// 确保第一个区域能命中
for (int index = 0; index < bright_ranges_cand[h].size(); index++) {
float dist_temp = 0.f;
int start_h = std::max(0, h - kSearchRange);
int end_h = std::min(resolution_height, h + kSearchRange + 1);
for (int index_h = start_h; index_h < end_h; index_h++) {
if (centre_points_internal[index_h].x > 0) {
dist_temp += (fabs(bright_ranges_cand[h][index].center.x -
centre_points_internal[index_h].x) *
kHWeights[abs(index_h - h)]);
}
}
if (dist_temp <= dist_min) {
dist_min = dist_temp;
index_select = index;
}
}
centre_points_internal[h].x = bright_ranges_cand[h][index_select].center.x;
gray_values_internal[h] =
bright_ranges_cand[h][index_select].mean_intensity;
break;
}
case CentreExtractMode::kRemove:
centre_points_internal[h].x = 0;
gray_values_internal[h] = 0;
break;
default:
break;
}
}
break;
}
}
// result
if (search_direction == LaserSearchDirection::kVertical) {
// x <-> y
for (auto& point : centre_points_internal) {
std::swap(point.x, point.y);
}
}
assert(centre_points_internal.size() == gray_values_internal.size());
centre_points = std::move(centre_points_internal);
gray_values = std::move(gray_values_internal);
return true;
}