【svopro】Image alignment align2D

1. Mathematical description

The process of aligning 2D images can be viewed as updating the feature point positions and geometric transformation parameters in the current image by minimizing the reprojection error. Specifically, this process can be divided into the following steps:

preprocessing

Define half-patch size $halfpatch\_size$ 、块的大小 $patch\_size$ 和块的面积 $patch\_area$ . Definition reference number set $ref\_patch\_dx$ 和 $ref\_patch\_dy$ , then one 4x4 zero square $H$ 。

Calculate gradient and Hessian matrix

Traverse the pixels of the reference block, calculate the gradient value of each pixel, and set the Jacobian matrix according to whether to use affine compensation $The value of J$ . At the same time, the products of the derivative matrices are accumulated to obtain the Hessian matrix $H$ 。

Specifically, for the pixels in the reference block $(x, y)$ , its gray level $I (x, y)$ ，则其梯怼为：

$\begin{aligned} \nabla I&=[I_x,I_y] \\ I_x&=\frac{1}{2} (I(x+1,y)-I(x-1,y)) \\ I_y&=\frac{1}{2} (I(x,y+1)-I(x,y-1)) \end{aligned}$

If affine compensation is not used, the Hessian matrix $The affine parameter block of H$ is set to the identity matrix, and the residual block is set to zero.

If affine compensation is used, the Jacobian matrix $J$ format:

$\left(\begin{matrix} I_x & 0 & x & 0 \\ 0 & I_y & 0 & -I \end{matrix}\right)$

in that, $x, y$ image element position, $I$ is the pixel gray value.

iterative optimization

Initialize the pixel position to the initial estimate and the update amount to zero. Enter the iteration loop, for each iteration:

3.1 Calculate the position of pixels in the current image

$u_r=\lfloor u \rfloor, v_r=\lfloor v \rfloor$

Naka, $(u, v)$ Predominant image element position.

3.2 Calculate interpolation weight

$\begin{aligned} subpix\_x&=u-u_r \\ subpix\_y&=v-v_r \\ wTL&=(1-subpix\_x)(1-subpix\_y) \\ wTR&=subpix\_x(1-subpix\_y) \\ wBL&=(1-subpix\_x)subpix\_y \\ wBR&=subpix\_xsubpix\_y \end{aligned}$

3.3 Interpolate the search block area

$search\_pixel=wTL\times I(u_r,v_r)+wTR\times I(u_r+1,v_r)+wBL\times I(u_r,v_r+1) + wBR\times I(u_r+1,v_r+1)$

3.4 Calculate residuals

$res=search\_pixel-\alpha\times ref\_patch+mean\_diff$

in that, $\alpha$ is the gain coefficient.

3.5 Calculate the Jacobian matrix

$J=\left(\begin{matrix} I_x \\ I_y \\ 1 & 0 \\ 0 & -I \end{matrix}\right)$

In that, $I_x,I_y$ is the derivative of the corresponding pixel in the reference block.

If affine compensation is not used, $The affine parameters of J$ are set to zero.

3.6 Update Hessian matrix and residuals

$\begin{aligned} H &= H + J \times J^T \\ Jres &= J^T \times res \\ \end{aligned}$

3.7 Solve the update quantity

$update=H^{-1}\times Jres$

3.8 Update pixel position and geometric transformation parameters

$\begin{aligned} u &\leftarrow u + update[0] \\ v &\leftarrow v + update[1] \\ mean\_diff &\leftarrow mean\_diff + update[2] \\ \alpha &\leftarrow \alpha + update[3] \end{aligned}$

3.9 Determine whether convergence occurs

The alignment is considered converged if the sum of the squares of the updates is less than the specified termination condition.

Last updated estimated pixel position $cur\_px\_estimate$ , whether or not the return is successful.

2. Code description

The function of this interface is to align the 2D image so that it best matches the current image within the reference block area.
cur_img: current image.
ref_patch_with_border: Reference block with border.
ref_patch: Reference block without boundaries.
n_iter: Number of iterations.
affine_est_offset: Whether to estimate translation offset.
affine_est_gain: Whether to estimate gain.
cur_px_estimate: estimate of the current pixel.
no_simd: Whether to disable SIMD instructions.
each_step: Position information of each step (optional).

bool align2D(const cv::Mat& cur_img, uint8_t* ref_patch_with_border, uint8_t* ref_patch,
    const int n_iter, const bool affine_est_offset, const bool affine_est_gain,
    Keypoint& cur_px_estimate, bool no_simd, std::vector<Eigen::Vector2f>* each_step) {
    
    

#ifdef __ARM_NEON__
  if (!no_simd)
    return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
#endif
如果支持ARM NEON指令集并且不禁用SIMD指令，则调用align2D_NEON函数进行处理。

  if (each_step)
    each_step->clear();

If a container each_step with location information for each step is provided, the container is emptied.

  const int halfpatch_size_ = 4;
  const int patch_size_ = 8;
  const int patch_area_ = 64;
  bool converged = false;

Define half block size, block size, and block area, and initialize the convergence flag to false.

  // We optimize feature position and two affine parameters.
  // compute derivative of template and prepare inverse compositional
  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
  Eigen::Matrix4f H;
  H.setZero();

We optimize the location of the feature as well as two affine parameters. Compute the derivatives of the template and prepare the inverse combination. Define a reference block derivative array with an aligned memory alignment attribute, and a 4x4 zero matrix H.

  // compute gradient and hessian
  const int ref_step = patch_size_ + 2;
  float* it_dx = ref_patch_dx;
  float* it_dy = ref_patch_dy;
  for (int y = 0; y < patch_size_; ++y) {
    
    
    uint8_t* it = ref_patch_with_border + (y + 1) * ref_step + 1;
    for (int x = 0; x < patch_size_; ++x, ++it, ++it_dx, ++it_dy) {
    
    
      Eigen::Vector4f J;
      J[0] = 0.5 * (it[1] - it[-1]);
      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);

      // If not using the affine compensation, force the jacobian to be zero.
      J[2] = affine_est_offset ? 1.0 : 0.0;
      J[3] = affine_est_gain ? -1.0 * it[0] : 0.0;

      *it_dx = J[0];
      *it_dy = J[1];
      H += J * J.transpose();
    }
  }

Calculate the gradient and Hessian matrix. Traverse the pixels of the reference block, calculate the gradient value of each pixel, and set the value of the Jacobian matrix J according to whether to use affine compensation. At the same time, the products of the derivative matrices are accumulated to obtain the Hessian matrix H.

  // If not use affine compensation, force update to be zero by
  // * setting the affine parameter block in H to identity
  // * setting the residual block to zero (see below)
  if (!affine_est_offset) {
    
    
    H(2, 2) = 1.0;
  }
  if (!affine_est_gain) {
    
    
    H(3, 3) = 1.0;
  }
  Eigen::Matrix4f Hinv = H.inverse();
  float mean_diff = 0;
  float alpha = 1.0;

If affine compensation is not used, the affine parameter block of the Hessian matrix H is set to the identity matrix and the residual block is set to zero. Calculate the inverse matrix Hinv of the Hessian matrix, and initialize the mean difference mean_diff and alpha.

  // Compute pixel location in new image:
  float u = cur_px_estimate.x();
  float v = cur_px_estimate.y();

  if (each_step)
    each_step->push_back(Eigen::Vector2f(u, v));

Calculate the position of the pixel in the new image and add it to the position information at each step (if a container is provided).

  // termination condition
  const float min_update_squared =
      0.03 * 0.03;  // TODO I suppose this depends on the size of the image (ate)
  const int cur_step = cur_img.step.p[0];
  Eigen::Vector4f update;
  update.setZero();
  for (int iter = 0; iter < n_iter; ++iter) {
    
    
    int u_r = std::floor(u);
    int v_r = std::floor(v);
    if (u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols - halfpatch_size_ ||
        v_r >= cur_img.rows - halfpatch_size_)
      break;

    if (std::isnan(u) || std::isnan(v))
      return false;

Define the iteration termination condition, the step size of the current image, and the initial value of the update amount. Enter an iterative loop, and for each iteration, calculate the position of the pixel in the current image (rounded to an integer). If the image bounds are exceeded or an invalid position occurs, exit the iteration and return false.


    // compute interpolation weights
    float subpix_x = u - u_r;
    float subpix_y = v - v_r;
    float wTL = (1.0 - subpix_x) * (1.0 - subpix_y);
    float wTR = subpix_x * (1.0 - subpix_y);
    float wBL = (1.0 - subpix_x) * subpix_y;
    float wBR = subpix_x * subpix_y;

    // loop through search_patch, interpolate
    uint8_t* it_ref = ref_patch;
    float* it_ref_dx = ref_patch_dx;
    float* it_ref_dy = ref_patch_dy;
    Eigen::Vector4f Jres;
    Jres.setZero();
    for (int y = 0; y < patch_size_; ++y) {
    
    
      uint8_t* it =
          (uint8_t*)cur_img.data + (v_r + y - halfpatch_size_) * cur_step + u_r - halfpatch_size_;
      for (int x = 0; x < patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy) {
    
    
        float search_pixel =
            wTL * it[0] + wTR * it[1] + wBL * it[cur_step] + wBR * it[cur_step + 1];
        float res = search_pixel - alpha * (*it_ref) + mean_diff;
        Jres[0] -= res * (*it_ref_dx);
        Jres[1] -= res * (*it_ref_dy);

        if (affine_est_offset) {
    
    
          Jres[2] -= res;
        }

        if (affine_est_gain) {
    
    
          Jres[3] -= (-1) * res * (*it_ref);
        }
      }
    }

    if (!affine_est_offset) {
    
    
      Jres[2] = 0.0;
    }
    if (!affine_est_gain) {
    
    
      Jres[3] = 0.0;
    }

Calculate interpolation weights and interpolate the search block area. The residual Jres is calculated based on the weights and pixel differences. If affine compensation is not used, the affine parameter of Jres is set to zero.

    update = Hinv * Jres;
    u += update[0];
    v += update[1];
    mean_diff += update[2];
    alpha += update[3];

    if (each_step)
      each_step->push_back(Eigen::Vector2f(u, v));

    if (update[0] * update[0] + update[1] * update[1] < min_update_squared) {
    
    
      converged = true;
      break;
    }
  }

  cur_px_estimate << u, v;
  (void)no_simd;

  return converged;
}

According to the inverse matrix Hinv of the Hessian matrix, the residual Jres and the update amount update, the position, average difference and gain coefficient alpha of the pixel are updated. The alignment is considered converged if the sum of the squares of the updates is less than the specified termination condition. Finally, the estimated pixel position cur_px_estimate is updated, and the result of convergence is returned.

3. References:

L. Lucchi, C. Gasser, T. Hofmann, and V. Friebel, “Efficient deformable registration of non-linearly distorted 3D point-sets,” in Proc. IEEE Int. Conf. Comput. Vis., 2013, pp. 3059–3066.
OpenCV official documentation