OpenCV灰度图像平移和旋转算法（平面内）

 旋转重心为图像中心；

为简化计算，旋转角度为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;
}