旋转重心为图像中心;
为简化计算,旋转角度为int值,单位为度;
DIM_SAMPLE_POINTS_X, DIM_SAMPLE_POINTS_Y 为图像大小,建议为相等;
图像的x轴与实际坐标相等,而y轴方向相反,因此shift_y之前有个负号;
为了加快计算速度,常用度数的正弦和余弦值保存在数组VIEW_COS_LOOKUP中;
float pos_x, pos_y;
int pos_x1, pos_x2, pos_y1, pos_y2;
float sum = 0.0;
float center_x=(DIM_SAMPLE_POINTS_X-1)/2.0;
float center_y=(DIM_SAMPLE_POINTS_Y-1)/2.0;
float shift_x = shift(0)/SENSOR_RANGE/2.0*DIM_SAMPLE_POINTS_X;
float shift_y = -shift(1)/SENSOR_RANGE/2.0*DIM_SAMPLE_POINTS_Y;
std::vector<float> temp_new_data;
temp_new_data.reserve(SAMPLE_POINTS_SIZE);
for (int t_i = 0; t_i < SAMPLE_POINTS_SIZE; t_i++)//clear all temp data
temp_new_data[t_i] = 0;
for (int x = 0; x < DIM_SAMPLE_POINTS_X; x++)
{
for (int y = 0; y < DIM_SAMPLE_POINTS_Y; y++)
{
float data_xy = (temp_s_data)[x+y* DIM_SAMPLE_POINTS_X];
if(data_xy>0.01)
{
pos_x = VIEW_COS_LOOKUP[rot]*(x-center_x) - (center_y-y)*VIEW_SIN_LOOKUP[rot] + center_x + shift_x;
pos_y = center_y - VIEW_SIN_LOOKUP[rot]*(x-center_x) - (center_y-y)*VIEW_COS_LOOKUP[rot] + shift_y;
if(pos_x<0 || pos_y<0 || pos_x> DIM_SAMPLE_POINTS_X-1 || pos_y>= DIM_SAMPLE_POINTS_Y-1) continue;
pos_x1 = floor(pos_x);
pos_x2 = ceil(pos_x);
pos_y1 = floor(pos_y);
pos_y2 = ceil(pos_y);
double w_x, w_y;
if(pos_x1==pos_x) w_x = 1.0;
else w_x = pos_x2 - pos_x;
if(pos_y1==pos_y) w_y = 1.0;
else w_y = pos_y2 - pos_y;
temp_new_data[pos_x1 + pos_y1 * DIM_SAMPLE_POINTS_X] += w_x * w_y * data_xy;
temp_new_data[pos_x1 + pos_y2 * DIM_SAMPLE_POINTS_X] += w_x * (1 - w_y) * data_xy;
temp_new_data[pos_x2 + pos_y1 * DIM_SAMPLE_POINTS_X] += (1 - w_x) * w_y * data_xy;
temp_new_data[pos_x2 + pos_y2 * DIM_SAMPLE_POINTS_X] += (1 - w_x) * (1 - w_y) * data_xy;
sum += data_xy;
}
}
}
下面为匹配算法, 数组中保存有历史数据,用于匹配
float error = 0.0;
std::vector<float> show_err_data;
show_err_data.reserve(SAMPLE_POINTS_SIZE);//to show error data in window
for (int t_i = 0; t_i < SAMPLE_POINTS_SIZE; t_i++)
{
error += abs(temp_new_data[t_i] - memory_sdata[t_i]);
show_err_data[t_i] = abs(temp_new_data[t_i] - memory_sdata[t_i]);
}
return error/sum/2.0;
}