Explicación detallada del ajuste del paquete

inserte la descripción de la imagen aquí

Taller de Visión 3D y Taller de Visión por Computador dan prioridad a la publicación de sus propios artículos (rezaga la prioridad del blog)

Historial de ajuste de paquetes

Ajuste de paquete, el nombre chino es ajuste de paquete, el propósito clásico de BA es optimizar la pose y el punto de referencia de la cámara, que juega un papel importante en el campo de SfM y SLAM. En la actualidad, la mayoría de los libros o referencias antiguas lo traducen como " ajuste de paquete" "Es un enfoque menos riguroso. El ajuste de paquete fue propuesto por primera vez por personas dedicadas a la geodesia (topografía y mapeo) en el siglo XIX. A mediados del siglo XIX, los estudiosos de la geodesia comenzaron a estudiar triangulaciones a gran escala (grandes triangulaciones) . A mediados del siglo XX, con la aparición de las cámaras y las computadoras, la fotogrametría (fotogrametría) también comenzó a estudiar el cálculo de ajuste, por lo que le dieron el nombre de ajuste de haz (crédito de los antecesores de la materia de fotogrametría). Alrededor del siglo XXI, SLAM comenzó a surgir en el campo de la robótica. Se utilizó el primer filtro bayesiano recursivo (recursive bayesian filter), posteriormente, el problema se convirtió en un gráfico y luego se resolvió por el método de mínimos cuadrados. El diagrama de desarrollo histórico de ajuste de paquete es el siguiente:
inserte la descripción de la imagen aquí

La esencia del ajuste de paquete aún es inseparable del principio de mínimos cuadrados (crédito de Gauss) (casi todos los problemas de optimización son esencialmente mínimos cuadrados). En la actualidad, los marcos de optimización de ajuste de paquete más representativos son ceres solver y g2o (aquí se presenta principalmente ceres solver ).Se dice que el nombre de ceres se debe a que el astrónomo Piazzi observó una estrella no observada cuando estaba libre, y finalmente calculó la órbita del asteroide con mínimos cuadrados, por lo que nombró al asteroide ceres.

Teoría algorítmica del ajuste Bundle

Valor de observación: coordenadas del punto de la imagen
Cantidad de optimización (cantidad de ajuste): pose y punto de referencia
Debido a que una vez que el ajuste está involucrado, debe haber la siguiente fórmula: valor de observación + número de corrección del valor de observación = valor aproximado + número de corrección del valor aproximado, luego la fórmula del paquete el ajuste sigue siendo de A partir de la ecuación condicional colineal:

inserte la descripción de la imagen aquí

$\begin{matriz}{l} x-x_{0}=-f \frac{a_{1}\left(X_{A}-X_{S}\right)+b_{1}\left (Y_{A}-Y_{S}\right)+c_{1}\left(Z_{A}-L_{S}\right)}{a_{3}\left(X_{A}-X_{S }\derecha)+b_{3}\izquierda(Y_{A}-Y_{S}\derecha)+c_{3}\izquierda(Z_{A}-Z_{S}\derecha)} \\ y-y_ {0}=-f \frac{a_{2}\left(X_{A}-X_{S}\right)+b_{2}\left(Y_{A}-Y_{S}\right)+c_ {2}\left(Z_{A}-Z_{S}\right)}{a_{3}\left(X_{A}-X_{S}\right)+b_{3}\left(Y_{A }-Y_{S}\derecha)+c_{3}\izquierda(Z_{A}-Z_{S}\derecha)} \end{matriz}$

La función de optimización (ecuación de error) es la siguiente, donde uij son las coordenadas del punto de la imagen, Cj es la matriz de proyección de la cámara y Xi son las coordenadas del punto tridimensional:

$\min \sum_{i=1}^{n} \sum_{j=1}^{m}\ izquierda(u_{ij}-\pi\izquierda(C_{j}, X_{i}\derecha)\derecha)^{2}$

Cuatro códigos de algoritmo de ajuste de paquete

El código aquí se presenta principalmente a partir de cuatro aspectos:

Optimice los parámetros internos de la cámara y los coeficientes de distorsión. La posición de la cámara (6dof) y las
funciones de costo de puntos de referencia se escriben de la siguiente manera:

template <typename CameraModel>
class BundleAdjustmentCostFunction {
 public:
  explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D)
      : observed_x_(point2D(0)), observed_y_(point2D(1)) {}
//构造函数传入的是观测值
  static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {
    return (new ceres::AutoDiffCostFunction<
            BundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 3,
            CameraModel::kNumParams>(
        new BundleAdjustmentCostFunction(point2D)));
  }
//优化量:2代表误差方程个数;4代表pose中的姿态信息,用四元数表示;3代表pose中的位置信息;3代表landmark
自由度;CameraModel::kNumParams是相机内参和畸变系数,其取决于相机模型是what


// opertator 重载函数的参数即是待优化的量
  template <typename T>
  bool operator()(const T* const qvec, const T* const tvec,
                  const T* const point3D, const T* const camera_params,
                  T* residuals) const {
    // Rotate and translate.
    T projection[3];
    ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
    projection[0] += tvec[0];
    projection[1] += tvec[1];
    projection[2] += tvec[2];

    // Project to image plane.
    projection[0] /= projection[2];
    projection[1] /= projection[2];

    // Distort and transform to pixel space.
    CameraModel::WorldToImage(camera_params, projection[0], projection[1],
                              &residuals[0], &residuals[1]);

    // Re-projection error.
    residuals[0] -= T(observed_x_);
    residuals[1] -= T(observed_y_);

    return true;
  }

 private:
  const double observed_x_;
  const double observed_y_;
};

Después de escribir la función de costo, lo siguiente es la necesidad de agregar todos los parámetros al bloque residual, para que ceres se pueda derivar automáticamente. El código es el siguiente:

ceres::Problem problem;
ceres::CostFunction* cost_function = nullptr; 
ceres::LossFunction * p_LossFunction =
    ceres_options_.bUse_loss_function_ ?
      new ceres::HuberLoss(Square(4.0))
      : nullptr; // 关于为何使用损失函数,因为现实中并不是所有观测过程中的噪声都服从 
      //gaussian noise的（或者可以说几乎没有），
      //遇到有outlier的情况，这些方法非常容易挂掉，
      //这时候就得用到robust statistics里面的
      //robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，
      //比较常用的有huber, cauchy等等。
cost_function = BundleAdjustmentCostFunction<CameraModel>::Create(point2D.XY()); 
//将优化量加入残差块
problem_->AddResidualBlock(cost_function, p_LossFunction, qvec_data,
                                 tvec_data, point3D.XYZ().data(),
                                 camera_params_data);

¡Como arriba, el ajuste del paquete del caso 1 está completo!

Optimice los parámetros internos de la cámara y los coeficientes de distorsión, plantee la parametrización de subconjuntos (optimización de parámetros parciales de información de pose) y punto de referencia 3D. Cuando
solo se optimiza la información de actitud , el código que debe agregarse al problema es el siguiente:

    //这里姿态又用欧拉角表示
    map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};

    double * parameter_block = &map_poses.at(indexPose)[0];
    problem.AddParameterBlock(parameter_block, 6);
    std::vector<int> vec_constant_extrinsic;
    vec_constant_extrinsic.insert(vec_constant_extrinsic.end(), {3,4,5});
    if (!vec_constant_extrinsic.empty())
     {
         // 主要用到ceres的SubsetParameterization函数
        ceres::SubsetParameterization *subset_parameterization =
          new ceres::SubsetParameterization(6, vec_constant_extrinsic);
        problem.SetParameterization(parameter_block, subset_parameterization);
     }

Optimice los parámetros internos de la cámara y los coeficientes de distorsión, plantee la parametrización del subconjunto (optimización parcial de los parámetros de la información de la pose) y el punto de referencia 3D. Cuando
solo se optimiza la información de posición , el código que debe agregarse al problema es el siguiente (igual que el código anterior, solo uno la línea necesita ser modificada):

    //这里姿态又用欧拉角表示
    map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};

    double * parameter_block = &map_poses.at(indexPose)[0];
    problem.AddParameterBlock(parameter_block, 6);
    std::vector<int> vec_constant_extrinsic;
    vec_constant_extrinsic.insert(vec_constant_extrinsic.end(), {1,2,3});
    if (!vec_constant_extrinsic.empty())
     {
        ceres::SubsetParameterization *subset_parameterization =
          new ceres::SubsetParameterization(6, vec_constant_extrinsic);
        problem.SetParameterization(parameter_block, subset_parameterization);
     }

Optimice los parámetros internos de la cámara y los coeficientes de distorsión, la pose es una constante sin optimización y un punto de referencia 3D.
La función de costo se escribe de la siguiente manera:

//相机模型
template <typename CameraModel>  
class BundleAdjustmentConstantPoseCostFunction {
 public:
  BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec,
                                           const Eigen::Vector3d& tvec,
                                           const Eigen::Vector2d& point2D)
      : qw_(qvec(0)),
        qx_(qvec(1)),
        qy_(qvec(2)),
        qz_(qvec(3)),
        tx_(tvec(0)),
        ty_(tvec(1)),
        tz_(tvec(2)),
        observed_x_(point2D(0)),
        observed_y_(point2D(1)) {}

  static ceres::CostFunction* Create(const Eigen::Vector4d& qvec,
                                     const Eigen::Vector3d& tvec,
                                     const Eigen::Vector2d& point2D) {
    return (new ceres::AutoDiffCostFunction<
            BundleAdjustmentConstantPoseCostFunction<CameraModel>, 2, 3,
            CameraModel::kNumParams>(
        new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D)));
  }

  template <typename T>
  bool operator()(const T* const point3D, const T* const camera_params,
                  T* residuals) const {
    const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)};

    // Rotate and translate.
    T projection[3];
    ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
    projection[0] += T(tx_);
    projection[1] += T(ty_);
    projection[2] += T(tz_);

    // Project to image plane.
    projection[0] /= projection[2];
    projection[1] /= projection[2];

    // Distort and transform to pixel space.
    CameraModel::WorldToImage(camera_params, projection[0], projection[1],
                              &residuals[0], &residuals[1]);

    // Re-projection error.
    residuals[0] -= T(observed_x_);
    residuals[1] -= T(observed_y_);

    return true;
  }

 private:
  const double qw_;
  const double qx_;
  const double qy_;
  const double qz_;
  const double tx_;
  const double ty_;
  const double tz_;
  const double observed_x_;
  const double observed_y_;
};

A continuación, agregue el problema al código de bloque residual de la siguiente manera:

ceres::Problem problem;
ceres::CostFunction* cost_function = nullptr; 
ceres::LossFunction * p_LossFunction =
    ceres_options_.bUse_loss_function_ ?
      new ceres::HuberLoss(Square(4.0))
      : nullptr; // 关于为何使用损失函数,因为现实中并不是所有观测过程中的噪声都服从 
      //gaussian noise的（或者可以说几乎没有），
      //遇到有outlier的情况，这些方法非常容易挂掉，
      //这时候就得用到robust statistics里面的
      //robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，
      //比较常用的有huber, cauchy等等。
cost_function = BundleAdjustmentConstantPoseCostFunction<CameraModel>::Create( \
            image.Qvec(), image.Tvec(), point2D.XY());//观测值输入  
//将优化量加入残差块
 problem_->AddResidualBlock(cost_function, loss_function, \
              point3D.XYZ().data(), camera_params_data);//被优化量加入残差-3D点和相机内参

Los anteriores son los cuatro casos de BA. Por supuesto, hay muchas variantes, como BA con restricciones de GPS (es decir, ajuste indirecto con restricciones), como la fijación de puntos de referencia 3D, la optimización de los parámetros de pose y cámara
y los coeficientes de distorsión.

Referencias

código fuente colmap openmvg, dirección github:

https://github.com/openMVG/openMVG

https://github.com/colmap/colmap
Shan Jie. Breve historia y resumen del ajuste del paquete. Revista de la edición de ciencia de la información de la Universidad de Wuhan, 2018, 43(12): 1797-1810.