[Asociación de datos] Asociación de características correspondiente basada en Patch, asociando el cuadro actual -> cuadro de referencia, seguimiento entre cuadros

1. WarpPixelWise (encuentre la posición del punto de característica del marco actual)

1.1 Funciones

Esto es para proyectar los píxeles alrededor de un punto característico en el marco de referencia en el marco actual de acuerdo con la pose de la cámara en el marco actual y almacenar sus valores de píxeles en una matriz determinada.

1.2 Entrada y salida de función

Entrada: La entrada de la función incluye el marco actual, el marco de referencia, los puntos característicos en el marco de referencia, los niveles de la pirámide del marco de referencia y el marco actual, y el tamaño de medio parche.Salida: Un puntero a una matriz de
valores de píxeles de salida.

1.3 Pasos del algoritmo

1. Calcule la distancia desde el punto característico en el marco de referencia hasta la cámara del marco de referencia y la distancia hasta la cámara del marco actual;
2. Retroproyecte las coordenadas de los puntos característicos en el sistema de coordenadas de la cámara del marco de referencia en el espacio tridimensional y escale de acuerdo con la distancia a la cámara para obtener las coordenadas tridimensionales;
3. Transforme las coordenadas tridimensionales en el sistema de coordenadas de la cámara del cuadro actual y proyéctelo en la imagen del cuadro actual para obtener las coordenadas de píxeles;
4. Escale las coordenadas de píxel al nivel de búsqueda y haga lo siguiente para cada píxel dentro de la mitad del tamaño del parche:
   • Transforme las coordenadas de píxeles en coordenadas tridimensionales en el sistema de coordenadas de la cámara del marco de referencia;
   • Proyecte las coordenadas tridimensionales en la imagen del marco de referencia para obtener las coordenadas de píxeles;
   • Escale las coordenadas de píxeles al nivel de la pirámide del marco de referencia;
   • Calcula el valor de este píxel en la imagen del cuadro de referencia según la interpolación bilineal y lo almacena en la matriz de salida.

La fórmula matemática se describe de la siguiente manera:

$${pag}_{cu}=T_{ctur}^{wo}{T}_{ref}^{mundool{d}^{-}}{pag}_{re}$$

$${pag}_{re}=\frac{d_{re}}{\Vert {pag}_{ref}^{estoy}\Vert }{pag}_{ref}^{estoy}$$

$${pag}_{ctur}^{buscar\_\_}=\frac{{pag}_{cu}}{2^{nivel\_\_\_{\_}_{cu}}}$$

$${pag}_{yly}^{buscar\_\_}={pag}_{yly}^{parche\_\_\_}+{pag}_{ctur}^{buscar\_\_}$$

$${pag}_{yly}^{estoy}=\frac{{pag}_{yly}^{buscar\_\_}}{2^{nivel\_\_\_{\_}_{cu}}}$$

$${pag}_{yly}^{wo}=T_{ref}^{wo}{T}_{ctur}^{mundool{d}^{-}}{pag}_{yly}^{estoy}$$

$${pag}_{yly}^{re}=\frac{}{2^{nivel\_\_\_{\_}_{re}}}{k}_{re}{pag}_{yly}^{estoy}$$

$$\begin{gathered} yo\left({pag}_{yly}^{re}\right)= \\ {w}_{}yo(\lfloor {pag}_{ele,}\rfloor ,\lfloor {pag}_{ele,y}\rfloor )+w_{}yo(\lfloor {pag}_{ele,}\rfloor ,\lfloor {pag}_{ele,y}\rfloor +1)+ \\ {w}_{}yo(\lfloor {pag}_{ele,}\rfloor +1,\lfloor {pag}_{ele,y}\rfloor )+w_{}yo(\lfloor {pag}_{ele,}\rfloor +1,\lfloor {pag}_{ele,y}\rfloor +1) \end{gathered}$$

donde, $T$ representa la pose de la cámara,$p$ representa coordenadas de píxeles,$d$ representa la distancia,$\Vert \cdot \Vert$ representa el módulo del vector,$K$ representa la matriz de referencia interna de la cámara,$I(\cdot )$ representa el valor de un determinado píxel en la imagen,$w_{},w_{},w_{},w_{}$Representa pesos de interpolación bilineal.

bool WarpPixelWise(const Frame& cur_frame, const Frame& ref_frame, const FeatureWrapper& ref_ftr,
    const int level_ref, const int level_cur, const int half_patch_size, uint8_t* patch) {
    
    
  double depth_ref = (ref_frame.pos() - ref_ftr.landmark->pos()).norm();
  double depth_cur = (cur_frame.pos() - ref_ftr.landmark->pos()).norm();

  // back project to 3D points in reference frame
  Eigen::Vector3d xyz_ref;
  ref_frame.cam()->backProject3(ref_ftr.px, &xyz_ref);
  xyz_ref = xyz_ref.normalized() * depth_ref;

  // project to current frame and convert to search level
  Eigen::Vector3d xyz_cur = cur_frame.T_cam_world() * (ref_frame.T_cam_world().inverse()) * xyz_ref;
  Eigen::Vector2d px_cur;
  cur_frame.cam()->project3(xyz_cur, &px_cur);
  Eigen::Vector2d px_cur_search = px_cur / (1 << level_cur);

  // for each pixel in the patch(on search level):
  // - convert to image level
  // - back project to 3D points
  // - project to ref frame and find pixel value in ref level
  uint8_t* patch_ptr = patch;
  const cv::Mat& img_ref = ref_frame.img_pyr_[level_ref];
  const int stride = img_ref.step.p[0];

  for (int y = -half_patch_size; y < half_patch_size; ++y) {
    
    
    for (int x = -half_patch_size; x < half_patch_size; ++x, ++patch_ptr) {
    
    
      const Eigen::Vector2d ele_patch(x, y);
      Eigen::Vector2d ele_search = ele_patch + px_cur_search;
      Eigen::Vector3d ele_xyz_cur;
      cur_frame.cam()->backProject3(ele_search * (1 << level_cur), &ele_xyz_cur);
      ele_xyz_cur = ele_xyz_cur.normalized() * depth_cur;
      Eigen::Vector3d ele_xyz_ref =
          ref_frame.T_cam_world() * (cur_frame.T_cam_world().inverse()) * ele_xyz_cur;
      Eigen::Vector2d ele_ref;
      ref_frame.cam()->project3(ele_xyz_ref, &ele_ref);
      ele_ref = ele_ref / (1 << level_ref);

      const int xi = std::floor(ele_ref[0]);
      const int yi = std::floor(ele_ref[1]);
      if (xi < 0 || yi < 0 || xi + 1 >= img_ref.cols || yi + 1 >= img_ref.rows) {
    
    
        VLOG(200) << "ref image: col-" << img_ref.cols << ", row-" << img_ref.rows;
        VLOG(200) << "xi: " << xi << ", "
                  << "yi: " << yi;
        return false;
      } else {
    
    
        const float subpix_x = ele_ref[0] - xi;
        const float subpix_y = ele_ref[1] - yi;
        const float w00 = (1.0f - subpix_x) * (1.0f - subpix_y);
        const float w01 = (1.0f - subpix_x) * subpix_y;
        const float w10 = subpix_x * (1.0f - subpix_y);
        const float w11 = 1.0f - w00 - w01 - w10;
        const uint8_t* const ptr = img_ref.data + yi * stride + xi;
        *patch_ptr = static_cast<uint8_t>(
            w00 * ptr[0] + w01 * ptr[stride] + w10 * ptr[1] + w11 * ptr[stride + 1]);
      }
    }
  }

  return true;
}

2. GetWarpMatrixAffine (calcular el marco actual -> matriz de transformación afín al marco de referencia)

2.1 Funciones

El papel de la función es calcular la matriz de transformación afín del marco actual al marco de referencia , de modo que la proyección de un punto característico en el marco de referencia en el marco actual coincida con el parche correspondiente en el marco actual.

2.2 Entrada y salida de función

Entrada: la entrada de la función incluye los parámetros de la cámara del marco de referencia y el marco actual, los puntos característicos en el marco de referencia, el vector de dirección de los puntos característicos, la distancia desde los puntos característicos hasta la cámara del marco de referencia, la pose transformación entre el marco actual y el marco de referencia, y la referencia El nivel de pirámide del marco.
salida: un puntero a la matriz de transformación afín de salida.

2.3 Pasos del algoritmo

Los pasos de implementación de la función son los siguientes:

1. Calcule las coordenadas tridimensionales según el vector de dirección y la distancia de los puntos característicos;
2. Mueva la mitad del tamaño del parche a lo largo de las direcciones horizontal y vertical en el marco de referencia y retroproyecte las coordenadas de píxeles movidos en el espacio tridimensional para obtener vectores tridimensionales en dos direcciones;
3. Transforme el vector tridimensional en el sistema de coordenadas de la cámara del cuadro actual y proyéctelo en el cuadro actual para obtener tres coordenadas de píxeles;
4. Calcula una matriz de transformación afín a partir de tres coordenadas de píxeles.

La fórmula matemática se describe de la siguiente manera:

$$\begin{array}{rl}{X}_{re}& =F_{re}{d}_{re}\\ X_{du,re}& =j_{du,re}{X}_{re}\\ X_{dv,re}& =j_{dv,re}{X}_{re}\\ X_{cu}& =T_{cur\_re}{X}_{re}\\ X_{du,cu}& =T_{cur\_re}{X}_{du,re}\\ X_{dv,cu}& =T_{cur\_re}{X}_{dv,re}\\ A_{cur\_re}& =[\begin{array}{cc}\frac{X_{du,cu}-x_{cu}}{kMediotamañodeparche\_\_\_\_\_\_\_\_}& \frac{X_{dv,cu}-x_{cu}}{kMediotamañodeparche\_\_\_\_\_\_\_\_}\end{array}\end{array}$$

donde $F_{re}$Representa el vector de dirección del punto característico, $d_{re}$Indica la distancia desde el punto característico hasta la cámara del marco de referencia, $X_{re}$Indica las coordenadas tridimensionales del punto característico en el sistema de coordenadas de la cámara del marco de referencia, $X_{du,re}$和$X_{dv,re}$Indica las coordenadas tridimensionales en el marco de referencia después de mover la mitad del tamaño del parche en las direcciones horizontal y vertical, $j_{du,re}$和$j_{dv,re}$Representar respectivamente el vector tridimensional correspondiente a las coordenadas de píxel correspondientes en el sistema de coordenadas de la cámara del marco de referencia después de mover la mitad del tamaño del parche en las direcciones horizontal y vertical, T cur _ ref T_{cur\_ref $T_{cur\_re}$Representa la transformación de pose entre el cuadro actual y el cuadro de referencia, $X_{cu}$、$X_{du,cu}$和$X_{dv,cu}$Representar respectivamente $X_{re}$、$X_{du,re}$和$X_{dv,re}$Las coordenadas tridimensionales obtenidas después de transformar al sistema de coordenadas de la cámara del cuadro actual, $\frac{X_{du,cu}-x_{cu}}{kMediotamañodeparche\_\_\_\_\_\_\_\_}$和$\frac{X_{dv,cu}-x_{cu}}{kMediotamañodeparche\_\_\_\_\_\_\_\_}$representan respectivamente el vector tridimensional correspondiente a las coordenadas de píxeles correspondientes en el sistema de coordenadas de la cámara de cuadro actual después de mover la mitad del tamaño del parche en las direcciones horizontal y vertical, $A_{cur\_re}$Representa la matriz de transformación afín del marco actual al marco de referencia.

void GetWarpMatrixAffine(const CameraPtr& cam_ref, const CameraPtr& cam_cur,
    const Eigen::Ref<Keypoint>& px_ref, const Eigen::Ref<BearingVector>& f_ref,
    const double depth_ref, const Transformation& T_cur_ref, const int level_ref,
    AffineTransform* A_cur_ref) {
    
    
  CHECK_NOTNULL(A_cur_ref);

  // Compute affine warp matrix A_ref_cur
  const int kHalfPatchSize = 5;
  const Position xyz_ref = f_ref * depth_ref;
  Position xyz_du_ref, xyz_dv_ref;
  // NOTE: project3 has no guarantee that the returned vector is unit length
  // - for pinhole: z component is 1 (unit plane)
  // - for omnicam: norm is 1 (unit sphere)
  cam_ref->backProject3(
      px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref), &xyz_du_ref);
  cam_ref->backProject3(
      px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref), &xyz_dv_ref);
  if (cam_ref->getType() == Camera::Type::kPinhole) {
    
    
    xyz_du_ref *= xyz_ref[2];
    xyz_dv_ref *= xyz_ref[2];
  } else {
    
    
    xyz_du_ref.normalize();
    xyz_dv_ref.normalize();
    xyz_du_ref *= depth_ref;
    xyz_dv_ref *= depth_ref;
  }

  Keypoint px_cur, px_du_cur, px_dv_cur;
  cam_cur->project3(T_cur_ref * xyz_ref, &px_cur);
  cam_cur->project3(T_cur_ref * xyz_du_ref, &px_du_cur);
  cam_cur->project3(T_cur_ref * xyz_dv_ref, &px_dv_cur);
  A_cur_ref->col(0) = (px_du_cur - px_cur) / kHalfPatchSize;
  A_cur_ref->col(1) = (px_dv_cur - px_cur) / kHalfPatchSize;
}

3. GetWarpMatrixAffine (calcular el marco actual -> matriz de transformación afín al marco de referencia)

3.1 Funciones

La función de esta función es calcular en qué nivel de pirámide buscar puntos coincidentes para obtener el mejor rendimiento .

3.2 Entrada y salida de función

Entrada: Su entrada incluye una matriz de transformación afín y un nivel máximo de pirámide.
Salida: La salida es un nivel de pirámide.

3.3 Pasos del algoritmo

1. Calcular el determinante D de la matriz de transformación afín;
2. Si D es mayor que 3,0 y el nivel de búsqueda es menor que el nivel máximo de la pirámide, agregue 1 al nivel de búsqueda y multiplique D por 0,25 hasta que D sea menor o igual a 3,0 o el nivel de búsqueda alcance el nivel máximo de la pirámide.

La fórmula matemática se describe de la siguiente manera:

$$niveldebúsqueda\_\_\_\_\_\_\_=\mathrm{mínimo}(\mathrm{máximo}\_nivel,\_\_\_\_\mathrm{máx}\{norte|re({UN}_{cur\_re})>3.0^{norte}\})$$

其中，$re\left({un}_{cur\_re}\right)$ representa la matriz de transformación afín$A_{cur\_re}$determinante de .

El principio de esta función es elegir el nivel de la pirámide a buscar según la incertidumbre de la matriz de transformación afín. Cuando la matriz de transformación afín es más incierta, es necesario buscar en los niveles superiores de la pirámide para obtener un mejor rendimiento de coincidencia. Cuando la matriz de transformación afín es más definida, la búsqueda se puede realizar en un nivel de pirámide más bajo para mejorar la eficiencia computacional.

int GetBestSearchLevel(const AffineTransform& A_cur_ref, const int max_level) {
    
    
  // Compute patch level in other image
  int search_level = 0;
  double D = A_cur_ref.determinant();
  while (D > 3.0 && search_level < max_level) {
    
    
    search_level += 1;
    D *= 0.25;
  }
  return search_level;
}