818 lines
27 KiB
C++
818 lines
27 KiB
C++
|
// HessianTable.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
#include <vector>
|
|||
|
|
#include <cmath>
|
|||
|
|
#include <corecrt_math_defines.h>
|
|||
|
|
#include <iostream>
|
|||
|
|
#include <fstream>
|
|||
|
|
#include <algorithm>
|
|||
|
|
#include <cstring>
|
|||
|
|
//#include <stdio.h>
|
|||
|
|
//#include <stdint.h>
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
#if 0
|
|||
|
|
// 生成1D无偏高斯二阶导数掩模
|
|||
|
|
vector<double> generate_1d_mask(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double sigma_sq = sigma * sigma;
|
|||
|
|
double normalization = 1.0 / (sqrt(2 * M_PI) * sigma_sq * sigma);
|
|||
|
|
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x_low = i - 0.5;
|
|||
|
|
double x_high = i + 0.5;
|
|||
|
|
|
|||
|
|
// 计算高斯二阶导数在区间[x_low, x_high]的积分
|
|||
|
|
double term1 = x_low * exp(-x_low * x_low / (2 * sigma_sq));
|
|||
|
|
double term2 = x_high * exp(-x_high * x_high / (2 * sigma_sq));
|
|||
|
|
double integral = (term1 - term2) / sigma_sq;
|
|||
|
|
|
|||
|
|
// 积分第二部分：误差函数处理常数项
|
|||
|
|
double erf_term = erf(x_high / (sigma * sqrt(2))) - erf(x_low / (sigma * sqrt(2)));
|
|||
|
|
integral += erf_term * sigma * sqrt(M_PI / 2);
|
|||
|
|
|
|||
|
|
integral *= normalization;
|
|||
|
|
mask[i + radius] = integral;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 1D卷积函数（处理边界为0扩展）
|
|||
|
|
vector<double> convolve_1d(const vector<double>& signal, const vector<double>& mask) {
|
|||
|
|
int mask_size = mask.size();
|
|||
|
|
int radius = (mask_size - 1) / 2;
|
|||
|
|
vector<double> result(signal.size(), 0.0);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < signal.size(); ++i) {
|
|||
|
|
double sum = 0.0;
|
|||
|
|
for (int j = -radius; j <= radius; ++j) {
|
|||
|
|
int pos = i + j;
|
|||
|
|
if (pos >= 0 && pos < signal.size()) {
|
|||
|
|
sum += signal[pos] * mask[j + radius];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
result[i] = sum;
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int main() {
|
|||
|
|
// 示例参数
|
|||
|
|
double sigma = 2.0;
|
|||
|
|
int radius = static_cast<int>(ceil(3 * sigma));
|
|||
|
|
|
|||
|
|
// 生成掩模
|
|||
|
|
vector<double> mask = generate_1d_mask(sigma, radius);
|
|||
|
|
|
|||
|
|
// 输出掩模系数
|
|||
|
|
cout << "Convolution Mask Coefficients:\n";
|
|||
|
|
for (double coeff : mask) {
|
|||
|
|
cout << coeff << " ";
|
|||
|
|
}
|
|||
|
|
cout << endl;
|
|||
|
|
|
|||
|
|
// 示例信号（可根据需要修改）
|
|||
|
|
vector<double> signal = { 0, 0, 0, 1, 1, 1, 0, 0, 0 };
|
|||
|
|
|
|||
|
|
// 应用卷积
|
|||
|
|
vector<double> response = convolve_1d(signal, mask);
|
|||
|
|
|
|||
|
|
// 输出响应
|
|||
|
|
cout << "Convolution Response:\n";
|
|||
|
|
for (double val : response) {
|
|||
|
|
cout << val << " ";
|
|||
|
|
}
|
|||
|
|
cout << endl;
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#include <vector>
|
|||
|
|
#include <cmath>
|
|||
|
|
#include <iostream>
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
// 生成1D无偏高斯一阶导数掩模
|
|||
|
|
vector<double> generate_1d_first_derivative_mask(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double sigma_sq = sigma * sigma;
|
|||
|
|
double normalization = 1.0 / (sigma * sqrt(2 * M_PI)); // 高斯函数归一化因子
|
|||
|
|
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x_low = i - 0.5;
|
|||
|
|
double x_high = i + 0.5;
|
|||
|
|
|
|||
|
|
// 计算高斯函数在离散区间端点的值
|
|||
|
|
double g_low = exp(-x_low * x_low / (2 * sigma_sq)) * normalization;
|
|||
|
|
double g_high = exp(-x_high * x_high / (2 * sigma_sq)) * normalization;
|
|||
|
|
|
|||
|
|
// 积分结果 = G(x_low) - G(x_high)
|
|||
|
|
double integral = g_low - g_high;
|
|||
|
|
mask[i + radius] = integral;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 卷积函数（与之前的二阶实现相同）
|
|||
|
|
vector<double> convolve_1d(const vector<double>& signal, const vector<double>& mask) {
|
|||
|
|
int mask_size = mask.size();
|
|||
|
|
int radius = (mask_size - 1) / 2;
|
|||
|
|
vector<double> result(signal.size(), 0.0);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < signal.size(); ++i) {
|
|||
|
|
double sum = 0.0;
|
|||
|
|
for (int j = -radius; j <= radius; ++j) {
|
|||
|
|
int pos = i + j;
|
|||
|
|
if (pos >= 0 && pos < signal.size()) {
|
|||
|
|
sum += signal[pos] * mask[j + radius];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
result[i] = sum;
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int main() {
|
|||
|
|
// 示例参数
|
|||
|
|
double sigma = 2.0;
|
|||
|
|
int radius = static_cast<int>(ceil(3 * sigma));
|
|||
|
|
|
|||
|
|
// 生成一阶导数掩模
|
|||
|
|
vector<double> mask_1st = generate_1d_first_derivative_mask(sigma, radius);
|
|||
|
|
|
|||
|
|
// 输出掩模系数
|
|||
|
|
cout << "1st Derivative Mask Coefficients:\n";
|
|||
|
|
for (double coeff : mask_1st) {
|
|||
|
|
cout << coeff << " ";
|
|||
|
|
}
|
|||
|
|
cout << endl;
|
|||
|
|
|
|||
|
|
// 示例信号（阶跃边缘）
|
|||
|
|
vector<double> signal = { 0, 0, 0, 0, 1, 1, 1, 1, 1 };
|
|||
|
|
|
|||
|
|
// 计算一阶导数响应
|
|||
|
|
vector<double> response = convolve_1d(signal, mask_1st);
|
|||
|
|
|
|||
|
|
// 输出响应
|
|||
|
|
cout << "1st Derivative Response:\n";
|
|||
|
|
for (double val : response) {
|
|||
|
|
cout << val << " ";
|
|||
|
|
}
|
|||
|
|
cout << endl;
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
#define LUMA_RGN_DATA_WIN_SIZE 16
|
|||
|
|
#define LUMA_GEN_DATA_SCALE 100
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
unsigned short WinRdx;//窗口在图像内的起始坐标
|
|||
|
|
unsigned short y; //y坐标
|
|||
|
|
unsigned short Rid; //region的ID
|
|||
|
|
unsigned char Flag; //bit0是overlap标志。bit1是反光信号标志
|
|||
|
|
unsigned char PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标
|
|||
|
|
unsigned char data[LUMA_RGN_DATA_WIN_SIZE];
|
|||
|
|
double offset;
|
|||
|
|
}Luma_rgnData;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
double true_offset;
|
|||
|
|
double compute_offset;
|
|||
|
|
double nCom_offset;
|
|||
|
|
}Luma_offsetTestCompare;
|
|||
|
|
|
|||
|
|
//生成测试数据：
|
|||
|
|
//sigma:(1.0,2.0,3.0,4.0,5.0),
|
|||
|
|
//Peak:（20，40，60，100，200，250），
|
|||
|
|
//Offset: (-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4)
|
|||
|
|
//每次调用生成Offset遍历数据(-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4)
|
|||
|
|
std::vector<Luma_rgnData> _genTestData(double sigma, double peakValue)
|
|||
|
|
{
|
|||
|
|
//int size = 10;
|
|||
|
|
//double offsetTable[10] = {-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4};
|
|||
|
|
int size = 20;
|
|||
|
|
double offsetTable[20] = { -0.5,-0.45,-0.4,-0.35, -0.3,-0.25, -0.2,-0.15, -0.1, -0.05,
|
|||
|
|
0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45 };
|
|||
|
|
std::vector< Luma_rgnData> result;
|
|||
|
|
|
|||
|
|
int buffSize = (LUMA_RGN_DATA_WIN_SIZE + 3) * LUMA_GEN_DATA_SCALE;
|
|||
|
|
std::vector<double> dataBuffer(buffSize, 0);
|
|||
|
|
|
|||
|
|
int radius = (LUMA_RGN_DATA_WIN_SIZE + 3)/2;
|
|||
|
|
int id = 0;
|
|||
|
|
double sigma2 = sigma * sigma;
|
|||
|
|
for (int i = -radius; i <= radius; i++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < LUMA_GEN_DATA_SCALE; j++)
|
|||
|
|
{
|
|||
|
|
double x = (double)i + (double)j / LUMA_GEN_DATA_SCALE;
|
|||
|
|
dataBuffer[id] = exp(-x * x / (2 * sigma2));
|
|||
|
|
id++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//生成结果数据
|
|||
|
|
int dataWin = LUMA_RGN_DATA_WIN_SIZE / 2;
|
|||
|
|
int y = 0;
|
|||
|
|
for (int n = 0; n < size; n++) //取不同的offset进行计算
|
|||
|
|
{
|
|||
|
|
double offset = offsetTable[n];
|
|||
|
|
int offsetPos = (int)(-offset * LUMA_GEN_DATA_SCALE) - LUMA_GEN_DATA_SCALE/2; //位于中心
|
|||
|
|
double rsltData[LUMA_RGN_DATA_WIN_SIZE];
|
|||
|
|
for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)//第个测试样本有16个数据，高斯分布，偏离位置为offset
|
|||
|
|
{
|
|||
|
|
double sum = 0;
|
|||
|
|
int pos = offsetPos + (i-dataWin) * LUMA_GEN_DATA_SCALE + radius * LUMA_GEN_DATA_SCALE;
|
|||
|
|
for (int j = 0; j < LUMA_GEN_DATA_SCALE; j++) //过采样，间隔为0.01
|
|||
|
|
sum += dataBuffer[j + pos];
|
|||
|
|
rsltData[i] = sum;
|
|||
|
|
}
|
|||
|
|
double maxData = 0;
|
|||
|
|
for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
|
|||
|
|
{
|
|||
|
|
if (maxData < rsltData[i])
|
|||
|
|
maxData = rsltData[i];
|
|||
|
|
}
|
|||
|
|
Luma_rgnData a_sample;
|
|||
|
|
memset(&a_sample, 0, sizeof(Luma_rgnData));
|
|||
|
|
a_sample.y = y;
|
|||
|
|
y++;
|
|||
|
|
a_sample.Rid = 1;
|
|||
|
|
a_sample.PeakRltvRdx = 0xC3;
|
|||
|
|
a_sample.offset = offset;
|
|||
|
|
for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
|
|||
|
|
{
|
|||
|
|
double d = (rsltData[i] / maxData) * peakValue;
|
|||
|
|
if (d > 254.0)
|
|||
|
|
a_sample.data[i] = 254.0;
|
|||
|
|
else
|
|||
|
|
a_sample.data[i] = (unsigned char)d;
|
|||
|
|
}
|
|||
|
|
result.push_back(a_sample);
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//直接采样法
|
|||
|
|
// 高斯一阶导数掩模生成
|
|||
|
|
vector<double> generate_1st_mask_directSample(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double sigma2 = sigma * sigma;
|
|||
|
|
double norm = 1.0 / (sigma * sigma2 * sqrt(2 * M_PI));
|
|||
|
|
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x = i;
|
|||
|
|
mask[i + radius] = -x * exp(-x * x / (2 * sigma2));
|
|||
|
|
mask[i + radius] *= norm;
|
|||
|
|
}
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
// 高斯二阶导数掩模生成
|
|||
|
|
vector<double> generate_2nd_mask_directSample(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double sigma2 = sigma * sigma;
|
|||
|
|
double sigma4 = sigma2 * sigma2;
|
|||
|
|
double norm = 1.0 / (sqrt(2 * M_PI) * sigma * sigma4);
|
|||
|
|
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x = i;
|
|||
|
|
mask[i + radius] = (x * x - sigma2) * exp(-x * x / (2 * sigma2));
|
|||
|
|
mask[i + radius] *= norm;
|
|||
|
|
}
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//积分法
|
|||
|
|
// 高斯一阶导数掩模生成
|
|||
|
|
vector<double> generate_1st_mask_Integral(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double norm = 1.0 / (sigma * sqrt(2 * M_PI));
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x_low = i - 0.5;
|
|||
|
|
double x_high = i + 0.5;
|
|||
|
|
mask[i + radius] = exp(-x_high * x_high / (2 * sigma * sigma)) -
|
|||
|
|
exp(-x_low * x_low / (2 * sigma * sigma));
|
|||
|
|
mask[i + radius] *= norm;
|
|||
|
|
}
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 高斯二阶导数掩模生成
|
|||
|
|
vector<double> generate_2nd_mask_Integral(double sigma, int radius) {
|
|||
|
|
int size = 2 * radius + 1;
|
|||
|
|
vector<double> mask(size, 0.0);
|
|||
|
|
double sigma2 = sigma * sigma;
|
|||
|
|
double norm = 1.0 / (sqrt(2 * M_PI) * sigma * sigma2);
|
|||
|
|
|
|||
|
|
for (int i = -radius; i <= radius; ++i) {
|
|||
|
|
double x_low = i - 0.5;
|
|||
|
|
double x_high = i + 0.5;
|
|||
|
|
double term1 = x_low * exp(-x_low * x_low / (2 * sigma2));
|
|||
|
|
double term2 = x_high * exp(-x_high * x_high / (2 * sigma2));
|
|||
|
|
mask[i + radius] = term1 - term2;
|
|||
|
|
mask[i + radius] *= norm;
|
|||
|
|
}
|
|||
|
|
return mask;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 卷积函数
|
|||
|
|
vector<double> convolve(const vector<double>& signal, const vector<double>& mask) {
|
|||
|
|
int radius = (mask.size() - 1) / 2;
|
|||
|
|
vector<double> result(signal.size(), 0.0);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < signal.size(); ++i) {
|
|||
|
|
double sum = 0.0;
|
|||
|
|
for (int j = -radius; j <= radius; ++j) {
|
|||
|
|
int pos = i + j;
|
|||
|
|
if (pos >= 0 && pos < signal.size()) {
|
|||
|
|
sum += signal[pos] * mask[j + radius];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
result[i] = sum;
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void _genSubpixMask(
|
|||
|
|
double sigma,
|
|||
|
|
int radius,
|
|||
|
|
std::vector<double>& outMask_1st,
|
|||
|
|
std::vector<double>& outMask_2nd)
|
|||
|
|
{
|
|||
|
|
// 生成掩模
|
|||
|
|
auto mask_1st = generate_1st_mask_directSample(sigma, radius);
|
|||
|
|
auto mask_2nd = generate_2nd_mask_directSample(sigma, radius);
|
|||
|
|
auto mask_1st_integ = generate_1st_mask_Integral(sigma, radius);
|
|||
|
|
auto mask_2nd_integ = generate_2nd_mask_Integral(sigma, radius);
|
|||
|
|
//生成宽度为LUMA_RGN_DATA_WIN_SIZE的mask
|
|||
|
|
std::vector<double> objMask_1st(LUMA_RGN_DATA_WIN_SIZE, 0.0);
|
|||
|
|
int size_1st = mask_1st_integ.size();
|
|||
|
|
for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
|
|||
|
|
{
|
|||
|
|
int pos = size_1st / 2 - LUMA_RGN_DATA_WIN_SIZE / 2 + i;
|
|||
|
|
if ((pos < 0) || (pos >= size_1st))
|
|||
|
|
objMask_1st[i] = 0;
|
|||
|
|
else
|
|||
|
|
objMask_1st[i] = mask_1st_integ[pos];
|
|||
|
|
}
|
|||
|
|
objMask_1st[0] = 0;
|
|||
|
|
for (int i = 0, i_max = objMask_1st.size(); i < i_max; i++)
|
|||
|
|
outMask_1st.push_back(objMask_1st[i]);
|
|||
|
|
std::vector<double> objMask_2nd(LUMA_RGN_DATA_WIN_SIZE, 0.0);
|
|||
|
|
int size_2nd = mask_2nd_integ.size();
|
|||
|
|
for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
|
|||
|
|
{
|
|||
|
|
int pos = size_2nd / 2 - LUMA_RGN_DATA_WIN_SIZE / 2 + i;
|
|||
|
|
if ((pos < 0) || (pos >= size_2nd))
|
|||
|
|
objMask_2nd[i] = 0;
|
|||
|
|
else
|
|||
|
|
objMask_2nd[i] = mask_2nd_integ[pos];
|
|||
|
|
}
|
|||
|
|
objMask_2nd[0] = 0;
|
|||
|
|
for (int i = 0, i_max = objMask_2nd.size(); i < i_max; i++)
|
|||
|
|
outMask_2nd.push_back(objMask_2nd[i]);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 亚像素定位核心算法
|
|||
|
|
vector<Luma_offsetTestCompare> subpixel_localization(
|
|||
|
|
const std::vector<Luma_rgnData> signal,
|
|||
|
|
std::vector<double>& objMask_1st,
|
|||
|
|
std::vector<double>& objMask_2nd,
|
|||
|
|
std::vector<int>& nObjMask_1st,
|
|||
|
|
std::vector<int>& nObjMask_2nd)
|
|||
|
|
{
|
|||
|
|
// 计算导数响应
|
|||
|
|
std::vector<Luma_offsetTestCompare> subpixel;
|
|||
|
|
for (int i = 0, i_max = signal.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
double D = 0;
|
|||
|
|
double D2 = 0;
|
|||
|
|
int nD = 0;
|
|||
|
|
int nD2 = 0;
|
|||
|
|
for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE; j++)
|
|||
|
|
{
|
|||
|
|
D += (double)signal[i].data[j] * objMask_1st[j];
|
|||
|
|
D2 += (double)signal[i].data[j] * objMask_2nd[j];
|
|||
|
|
nD += (int)signal[i].data[j] * nObjMask_1st[j];
|
|||
|
|
nD2 += (int)signal[i].data[j] * nObjMask_2nd[j];
|
|||
|
|
}
|
|||
|
|
Luma_offsetTestCompare a_sub;
|
|||
|
|
a_sub.true_offset = signal[i].offset;
|
|||
|
|
a_sub.compute_offset = D / D2;
|
|||
|
|
a_sub.nCom_offset = (double)((float)nD / (float)nD2);
|
|||
|
|
subpixel.push_back(a_sub);
|
|||
|
|
}
|
|||
|
|
return subpixel;
|
|||
|
|
|
|||
|
|
#if 0
|
|||
|
|
vector<double> D = convolve(signal, mask_1st);
|
|||
|
|
vector<double> D2 = convolve(signal, mask_2nd);
|
|||
|
|
|
|||
|
|
// 寻找候选点 (过零点)
|
|||
|
|
vector<double> positions;
|
|||
|
|
for (int i = 1; i < D.size() - 1; ++i) {
|
|||
|
|
// 检测过零点
|
|||
|
|
if (D[i] * D[i + 1] < 0 || D[i] * D[i - 1] < 0) {
|
|||
|
|
// 亚像素插值
|
|||
|
|
double x0 = D[i - 1];
|
|||
|
|
double x1 = D[i];
|
|||
|
|
double x2 = D[i + 1];
|
|||
|
|
|
|||
|
|
// 抛物线插值公式
|
|||
|
|
double offset = 0.5 * (x0 - x2) / (x0 - 2 * x1 + x2);
|
|||
|
|
double subpixel_pos = i + offset;
|
|||
|
|
|
|||
|
|
// 计算二阶导数验证
|
|||
|
|
double d2_val = D2[i] + offset * (D2[i + 1] - D2[i]);
|
|||
|
|
|
|||
|
|
// 有效性检查 (排除弱响应)
|
|||
|
|
if (fabs(d2_val) > 1e-3) {
|
|||
|
|
positions.push_back(subpixel_pos);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return positions;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void _outputTestSample(char* fileName, std::vector<Luma_rgnData>& testSamples)
|
|||
|
|
{
|
|||
|
|
std::ofstream sw(fileName);
|
|||
|
|
char str[250];
|
|||
|
|
sprintf_s(str, "0x10");
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
for (int i = 0, i_max = testSamples.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
char data[250];
|
|||
|
|
sprintf_s(data, "%04x %04x %04x %01x %02x",
|
|||
|
|
testSamples[i].WinRdx, testSamples[i].y, testSamples[i].Rid, testSamples[i].Flag, testSamples[i].PeakRltvRdx);
|
|||
|
|
sw << data << std::endl;
|
|||
|
|
for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE; j++)
|
|||
|
|
{
|
|||
|
|
sprintf_s(data, "%02x", testSamples[i].data[j]);
|
|||
|
|
sw << data << std::endl;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
sw.close();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void _outputMaskData(char* fileName, double sigma, std::vector<double>& mask_1st, std::vector<double>& mask_2nd,
|
|||
|
|
double scale, std::vector<int>& nMask_1st, std::vector<int>& nMask_2nd)
|
|||
|
|
{
|
|||
|
|
std::ofstream sw(fileName);
|
|||
|
|
char str[250];
|
|||
|
|
|
|||
|
|
sprintf_s(str, "sigma=%f", sigma);
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
sprintf_s(str, "mask_1st:");
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
for (int i = 0, i_max = mask_1st.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
if (i < i_max - 1)
|
|||
|
|
sprintf_s(str, "%f, ", mask_1st[i]);
|
|||
|
|
else
|
|||
|
|
sprintf_s(str, "%f", mask_1st[i]);
|
|||
|
|
sw << str;
|
|||
|
|
}
|
|||
|
|
sw << std::endl;
|
|||
|
|
|
|||
|
|
sprintf_s(str, "mask_2nd:");
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
for (int i = 0, i_max = mask_2nd.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
if (i < i_max - 1)
|
|||
|
|
sprintf_s(str, "%f, ", mask_2nd[i]);
|
|||
|
|
else
|
|||
|
|
sprintf_s(str, "%f", mask_2nd[i]);
|
|||
|
|
sw << str;
|
|||
|
|
}
|
|||
|
|
sw << std::endl;
|
|||
|
|
|
|||
|
|
sw << std::endl;
|
|||
|
|
sprintf_s(str, "scale=%f", scale);
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
sprintf_s(str, "nMask_1st:");
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
for (int i = 0, i_max = nMask_1st.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
if (i < i_max - 1)
|
|||
|
|
sprintf_s(str, "%d, ", nMask_1st[i]);
|
|||
|
|
else
|
|||
|
|
sprintf_s(str, "%d", nMask_1st[i]);
|
|||
|
|
sw << str;
|
|||
|
|
}
|
|||
|
|
sw << std::endl;
|
|||
|
|
|
|||
|
|
sprintf_s(str, "nMask_2nd:");
|
|||
|
|
sw << str << std::endl;
|
|||
|
|
for (int i = 0, i_max = nMask_2nd.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
if (i < i_max - 1)
|
|||
|
|
sprintf_s(str, "%d, ", nMask_2nd[i]);
|
|||
|
|
else
|
|||
|
|
sprintf_s(str, "%d", nMask_2nd[i]);
|
|||
|
|
sw << str;
|
|||
|
|
}
|
|||
|
|
sw << std::endl;
|
|||
|
|
|
|||
|
|
sw.close();
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#pragma pack(push, 1)
|
|||
|
|
typedef struct {
|
|||
|
|
uint16_t file_type; // 文件类型，'BM'
|
|||
|
|
uint32_t file_size; // 文件大小
|
|||
|
|
uint16_t reserved1; // 保留字段
|
|||
|
|
uint16_t reserved2;
|
|||
|
|
uint32_t offset_data; // 数据偏移
|
|||
|
|
}BMPFileHeader;
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
uint32_t size; // 信息头大小（40字节）
|
|||
|
|
int32_t width; // 图像宽度
|
|||
|
|
int32_t height; // 图像高度（正数表示倒序）
|
|||
|
|
uint16_t planes; // 颜色平面数（必须为1）
|
|||
|
|
uint16_t bit_count; // 每像素位数（24）
|
|||
|
|
uint32_t compression; // 压缩方式（0表示无压缩）
|
|||
|
|
uint32_t size_image; // 像素数据大小
|
|||
|
|
int32_t x_pixels_per_meter;
|
|||
|
|
int32_t y_pixels_per_meter;
|
|||
|
|
uint32_t colors_used;
|
|||
|
|
uint32_t colors_important;
|
|||
|
|
} BMPInfoHeader;
|
|||
|
|
#pragma pack(pop)
|
|||
|
|
|
|||
|
|
void save_bmp(const char* filename, uint8_t* image_data, int width, int height) {
|
|||
|
|
FILE* file;
|
|||
|
|
fopen_s(&file, filename, "wb");
|
|||
|
|
if (!file) {
|
|||
|
|
std::printf("无法打开文件 %s\n", filename);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 计算每行填充字节数
|
|||
|
|
int bytes_per_row = width * 3; // 每个像素3字节（BGR）
|
|||
|
|
int padding = (4 - (bytes_per_row % 4)) % 4;
|
|||
|
|
int row_size = bytes_per_row + padding;
|
|||
|
|
|
|||
|
|
// 初始化文件头
|
|||
|
|
BMPFileHeader file_header = {
|
|||
|
|
.file_type = 0x4D42, // 'BM'
|
|||
|
|
.file_size = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + row_size * height,
|
|||
|
|
.reserved1 = 0,
|
|||
|
|
.reserved2 = 0,
|
|||
|
|
.offset_data = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader)
|
|||
|
|
};
|
|||
|
|
// 初始化信息头
|
|||
|
|
BMPInfoHeader info_header = {
|
|||
|
|
.size = sizeof(BMPInfoHeader),
|
|||
|
|
.width = width,
|
|||
|
|
.height = height, // 正数表示像素数据从下到上排列
|
|||
|
|
.planes = 1,
|
|||
|
|
.bit_count = 24,
|
|||
|
|
.compression = 0,
|
|||
|
|
.size_image = (uint32_t)(row_size * height),
|
|||
|
|
.x_pixels_per_meter = 0,
|
|||
|
|
.y_pixels_per_meter = 0,
|
|||
|
|
.colors_used = 0,
|
|||
|
|
.colors_important = 0
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 写入头信息
|
|||
|
|
fwrite(&file_header, 1, sizeof(BMPFileHeader), file);
|
|||
|
|
fwrite(&info_header, 1, sizeof(BMPInfoHeader), file);
|
|||
|
|
|
|||
|
|
// 写入像素数据（从最后一行开始）
|
|||
|
|
uint8_t padding_bytes[3] = { 0, 0, 0 };
|
|||
|
|
for (int y = height - 1; y >= 0; y--) {
|
|||
|
|
for (int x = 0; x < width; x++) {
|
|||
|
|
uint8_t* pixel = image_data + (y * width + x) * 3;
|
|||
|
|
// 将RGB转为BGR顺序
|
|||
|
|
fputc(pixel[2], file); // B
|
|||
|
|
fputc(pixel[1], file); // G
|
|||
|
|
fputc(pixel[0], file); // R
|
|||
|
|
}
|
|||
|
|
fwrite(padding_bytes, 1, padding, file); // 填充
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
fclose(file);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void save_bmp_2(const char* filename, std::vector<Luma_rgnData>& testSamples) {
|
|||
|
|
FILE* file;
|
|||
|
|
fopen_s(&file, filename, "wb");
|
|||
|
|
if (!file) {
|
|||
|
|
std::printf("无法打开文件 %s\n", filename);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 计算每行填充字节数
|
|||
|
|
int bytes_per_row = LUMA_RGN_DATA_WIN_SIZE * 3; // 每个像素3字节（BGR）
|
|||
|
|
int padding = (4 - (bytes_per_row % 4)) % 4;
|
|||
|
|
int row_size = bytes_per_row + padding;
|
|||
|
|
int height = testSamples.size();
|
|||
|
|
|
|||
|
|
// 初始化文件头
|
|||
|
|
BMPFileHeader file_header = {
|
|||
|
|
.file_type = 0x4D42, // 'BM'
|
|||
|
|
.file_size = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + row_size * height,
|
|||
|
|
.reserved1 = 0,
|
|||
|
|
.reserved2 = 0,
|
|||
|
|
.offset_data = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader)
|
|||
|
|
};
|
|||
|
|
// 初始化信息头
|
|||
|
|
BMPInfoHeader info_header = {
|
|||
|
|
.size = sizeof(BMPInfoHeader),
|
|||
|
|
.width = LUMA_RGN_DATA_WIN_SIZE,
|
|||
|
|
.height = height, // 正数表示像素数据从下到上排列
|
|||
|
|
.planes = 1,
|
|||
|
|
.bit_count = 24,
|
|||
|
|
.compression = 0,
|
|||
|
|
.size_image = (uint32_t)(row_size * height),
|
|||
|
|
.x_pixels_per_meter = 0,
|
|||
|
|
.y_pixels_per_meter = 0,
|
|||
|
|
.colors_used = 0,
|
|||
|
|
.colors_important = 0
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 写入头信息
|
|||
|
|
fwrite(&file_header, 1, sizeof(BMPFileHeader), file);
|
|||
|
|
fwrite(&info_header, 1, sizeof(BMPInfoHeader), file);
|
|||
|
|
|
|||
|
|
// 写入像素数据（从最后一行开始）
|
|||
|
|
uint8_t padding_bytes[3] = { 0, 0, 0 };
|
|||
|
|
for (int y = height - 1; y >= 0; y--) {
|
|||
|
|
for (int x = 0; x < LUMA_RGN_DATA_WIN_SIZE; x++) {
|
|||
|
|
uint8_t pixel = (uint8_t)testSamples[y].data[x];
|
|||
|
|
// 将RGB转为BGR顺序
|
|||
|
|
fputc(pixel, file); // B
|
|||
|
|
fputc(pixel, file); // G
|
|||
|
|
fputc(pixel, file); // R
|
|||
|
|
}
|
|||
|
|
fwrite(padding_bytes, 1, padding, file); // 填充
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
fclose(file);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#if 0
|
|||
|
|
// 示例使用
|
|||
|
|
int main() {
|
|||
|
|
const int WIDTH = 640;
|
|||
|
|
const int HEIGHT = 480;
|
|||
|
|
uint8_t image[HEIGHT][WIDTH][3]; // 假设图像数据为RGB格式
|
|||
|
|
|
|||
|
|
// 填充测试数据（红色）
|
|||
|
|
for (int y = 0; y < HEIGHT; y++) {
|
|||
|
|
for (int x = 0; x < WIDTH; x++) {
|
|||
|
|
image[y][x][0] = 255; // R
|
|||
|
|
image[y][x][1] = 0; // G
|
|||
|
|
image[y][x][2] = 0; // B
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
save_bmp("output.bmp", (uint8_t*)image, WIDTH, HEIGHT);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void writeBMP(const char* filename, std::vector<Luma_rgnData>& testSamples)
|
|||
|
|
{
|
|||
|
|
std::ofstream file(filename, std::ios::out | std::ios::binary);
|
|||
|
|
if (!file) return;
|
|||
|
|
|
|||
|
|
// BMP文件头结构体
|
|||
|
|
struct BMPHeader {
|
|||
|
|
uint16_t type; // Magic identifier
|
|||
|
|
uint32_t size; // File size in bytes
|
|||
|
|
uint16_t reserved1; // Not used
|
|||
|
|
uint16_t reserved2; // Not used
|
|||
|
|
uint32_t offset; // Start position of pixel data
|
|||
|
|
uint32_t DIBSize; // DIB header size
|
|||
|
|
int32_t width; // width of bitmap in pixels
|
|||
|
|
int32_t height; // width of bitmap in pixels
|
|||
|
|
uint16_t planes; // Number of color planes must be 1
|
|||
|
|
uint16_t bits; // Bits per pixel
|
|||
|
|
uint32_t compression; // Compression type
|
|||
|
|
uint32_t imagesize; // Image size must be 0 for uncompressed images
|
|||
|
|
int32_t xresolution; // Pixels per meter
|
|||
|
|
int32_t yresolution; // Pixels per meter
|
|||
|
|
uint32_t ncolor; // Number of colors
|
|||
|
|
uint32_t importantcolor; // Number of important colors
|
|||
|
|
} header;
|
|||
|
|
|
|||
|
|
header.type = 0x4D42; // 'BM' in little endian.
|
|||
|
|
header.size = 54 + (width * height * 3); // header size + data size (3 bytes per pixel for RGB)
|
|||
|
|
header.reserved1 = 0;
|
|||
|
|
header.reserved2 = 0;
|
|||
|
|
header.offset = 54; // Start of pixel data after the header.
|
|||
|
|
header.DIBSize = 40; // Size of the DIB header, in bytes.
|
|||
|
|
header.width = width;
|
|||
|
|
header.height = height;
|
|||
|
|
header.planes = 1; // Number of color planes. Must be set to 1.
|
|||
|
|
header.bits = 24; // Bits per pixel (24 bits for RGB).
|
|||
|
|
header.compression = 0; // BI_RGB, no compression used.
|
|||
|
|
header.imagesize = (width * height * 3); // Image size, in bytes. Must be 0 for BI_RGB bitmaps.
|
|||
|
|
header.xresolution = 0; // Pixels per meter (set to 0 to use default value).
|
|||
|
|
header.yresolution = 0; // Pixels per meter (set to 0 to use default value).
|
|||
|
|
header.ncolor = 0; // Number of colors in the color palette. Must be set to 0 for BI_RGB bitmaps.
|
|||
|
|
header.importantcolor = 0; // All colors are important. Must be set to 0 for BI_RGB bitmaps.
|
|||
|
|
|
|||
|
|
file.write(reinterpret_cast<const char*>(&header), sizeof(header)); // Write the header to the file.
|
|||
|
|
file.write(reinterpret_cast<const char*>(data), width * height * 3); // Write the pixel data to the file.
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
int main() {
|
|||
|
|
#if 0
|
|||
|
|
// 生成测试信号 (高斯峰位于15.3的位置)
|
|||
|
|
vector<double> signal(30, 0.0);
|
|||
|
|
double true_pos = 15.3;
|
|||
|
|
double sigma = 3.0;
|
|||
|
|
for (int i = 0; i < signal.size(); ++i) {
|
|||
|
|
double x = i - true_pos;
|
|||
|
|
signal[i] = exp(-x * x / (2 * sigma * sigma));
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
double sigmaValue[3] = { 2.0, 3.0, 4.0 };
|
|||
|
|
double pkValue[10] = {20.0, 40.0, 60.0, 100.0, 200.0, 240.0, 250.0, 260.0, 280.0, 300.0};
|
|||
|
|
int testId = 0;
|
|||
|
|
std::vector<Luma_rgnData> testSamples;
|
|||
|
|
|
|||
|
|
// 参数设置
|
|||
|
|
double width = 4;
|
|||
|
|
double sigma = width / 1.5;
|
|||
|
|
int radius = ceil(5 * sigma);
|
|||
|
|
std::vector<double> objMask_1st;
|
|||
|
|
std::vector<double> objMask_2nd;
|
|||
|
|
_genSubpixMask(
|
|||
|
|
sigma,
|
|||
|
|
radius,
|
|||
|
|
objMask_1st,
|
|||
|
|
objMask_2nd);
|
|||
|
|
|
|||
|
|
double scale = 1024.0 * 1024.0 * 8;
|
|||
|
|
std::vector<int> nObjMask_1st;
|
|||
|
|
nObjMask_1st.resize(objMask_1st.size());
|
|||
|
|
std::vector<int> nObjMask_2nd;
|
|||
|
|
nObjMask_2nd.resize(objMask_2nd.size());
|
|||
|
|
for (int i = 0; i < objMask_1st.size(); i++)
|
|||
|
|
nObjMask_1st[i] = (int)(objMask_1st[i] * scale);
|
|||
|
|
for (int i = 0; i < objMask_2nd.size(); i++)
|
|||
|
|
nObjMask_2nd[i] = (int)(objMask_2nd[i] * scale);
|
|||
|
|
|
|||
|
|
for (int sidx = 0; sidx < 3; sidx++)
|
|||
|
|
{
|
|||
|
|
double sigma = sigmaValue[sidx];
|
|||
|
|
for (int loop = 0; loop < 10; loop++)
|
|||
|
|
{
|
|||
|
|
std::vector<Luma_rgnData> testSignal = _genTestData(sigma, pkValue[loop]);
|
|||
|
|
testSamples.insert(testSamples.end(), testSignal.begin(), testSignal.end());
|
|||
|
|
// 执行亚像素定位
|
|||
|
|
auto positions = subpixel_localization(testSignal, objMask_1st, objMask_2nd, nObjMask_1st, nObjMask_2nd);
|
|||
|
|
for (int i = 0, i_max = positions.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
cout << "Test_" << testId << ": ( ";
|
|||
|
|
for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE - 1; j++)
|
|||
|
|
cout << (int)testSignal[i].data[j] << ",";
|
|||
|
|
cout << (int)testSignal[i].data[LUMA_RGN_DATA_WIN_SIZE - 1] << " )" ;
|
|||
|
|
double diff = positions[i].compute_offset - positions[i].true_offset;
|
|||
|
|
double diff2 = positions[i].compute_offset - positions[i].nCom_offset;
|
|||
|
|
cout << " true_offset=" << positions[i].true_offset << " pos=" << positions[i].compute_offset << " nComPos=" << positions[i].nCom_offset << " diff=" << diff << " diff2=" << diff2 << std::endl;
|
|||
|
|
testId++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//文件输出
|
|||
|
|
char outFile[250];
|
|||
|
|
sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\testSampe.txt");
|
|||
|
|
_outputTestSample(outFile, testSamples);
|
|||
|
|
sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\testSampe.bmp");
|
|||
|
|
save_bmp_2(outFile, testSamples);
|
|||
|
|
sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\maskData.txt");
|
|||
|
|
_outputMaskData(outFile, sigma, objMask_1st, objMask_2nd, scale, nObjMask_1st, nObjMask_2nd);
|
|||
|
|
std::printf("done!\n");
|
|||
|
|
#if 0
|
|||
|
|
// 输出结果
|
|||
|
|
cout << "True position: " << true_pos << endl;
|
|||
|
|
cout << "Detected positions:\n";
|
|||
|
|
for (double p : positions) {
|
|||
|
|
cout << p << " (error: " << fabs(p - true_pos) << ")\n";
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
return 0;
|
|||
|
|
}
|