Explicação detalhada do ajuste do pacote

insira a descrição da imagem aqui

O 3D Vision Workshop e o Computer Vision Workshop dão prioridade à publicação de seus próprios artigos (a prioridade do blog fica para trás)

Histórico de ajuste de pacote

Ajuste de pacote, o nome chinês é ajuste de pacote, o objetivo clássico do BA é otimizar a pose e o marco da câmera, que desempenha um papel importante no campo de SfM e SLAM. Atualmente, a maioria dos livros ou referências antigas o traduzem em " ajuste de pacote" "É uma abordagem menos rigorosa. O ajuste de pacote foi proposto pela primeira vez por pessoas envolvidas em geodésia (levantamento e mapeamento) no século 19. Em meados do século 19, estudiosos da geodésica começaram a estudar triangulações de grande escala (grandes triangulações) . Em meados do século 20, com o surgimento de câmeras e computadores, a fotogrametria (fotogrametria) também começou a estudar a computação de ajuste, por isso deram um nome chamado ajuste de pacote (o crédito dos predecessores do assunto da fotogrametria). Por volta do século 21, o SLAM começou a surgir no campo da robótica. O primeiro filtro bayesiano recursivo (filtro recursivo bayesiano) foi usado. Posteriormente, o problema foi transformado em gráfico e resolvido pelo método dos mínimos quadrados. O diagrama de desenvolvimento histórico de ajuste do pacote é a seguinte:
insira a descrição da imagem aqui

A essência do ajuste de pacote ainda é inseparável do princípio dos mínimos quadrados (crédito de Gauss) (quase todos os problemas de otimização são essencialmente quadrados mínimos). ) .Diz-se que o nome de ceres é que o astrônomo Piazzi observou uma estrela não observada quando ele estava livre, e finalmente calculou a órbita do asteróide com mínimos quadrados, então ele nomeou o asteróide ceres.

Teoria algorítmica de ajuste de Bundle

Valor de observação: coordenadas do ponto da imagem
Quantidade de otimização (quantidade de ajuste): pose e marco
Como uma vez que o ajuste está envolvido, deve haver a seguinte fórmula: valor de observação + número de correção do valor de observação = valor aproximado + número de correção do valor aproximado, então a fórmula do pacote o ajuste ainda é de A partir da equação condicional colinear:

insira a descrição da imagem aqui

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

A função de otimização (equação de erro) é a seguinte, onde uij são as coordenadas do ponto da imagem, Cj é a matriz de projeção da câmera e Xi são as coordenadas do ponto tridimensional:

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

Quatro códigos de algoritmo de ajuste de pacote

O código aqui é apresentado principalmente a partir de quatro aspectos:

Otimize os parâmetros internos e os coeficientes de distorção da câmera. A pose da câmera (6dof) e as
funções de custo de ponto de referência são escritas da seguinte forma:

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_;
};

Após escrever a função de custo, segue a necessidade de adicionar todos os parâmetros ao bloco residual, para que os ceres possam ser derivados automaticamente. O código é o seguinte:

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 acima, o ajuste do pacote do case1 está concluído!

Otimize os parâmetros internos da câmera e os coeficientes de distorção, a parametrização do subconjunto de poses (otimização parcial dos parâmetros das informações de pose) e o marco 3D. Quando
apenas as informações de atitude são otimizadas , o código que precisa ser adicionado ao problema é o seguinte:

    //这里姿态又用欧拉角表示
    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);
     }

Otimize os parâmetros internos da câmera e os coeficientes de distorção, a parametrização do subconjunto de poses (otimização parcial dos parâmetros das informações de pose) e o marco 3D. Ao
otimizar apenas as informações de posição , o código que precisa ser adicionado ao problema é o seguinte (igual ao código acima, apenas um linha precisa 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);
     }

Otimize os parâmetros internos da câmera e os coeficientes de distorção, a pose é uma constante sem otimização e marco 3D.
A função de custo é escrita da seguinte forma:

//相机模型
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_;
};

Em seguida, adicione o problema ao código do bloco residual da seguinte maneira:

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点和相机内参

Acima estão os quatro casos de BA. Claro, existem muitas variantes, como BA com restrição de GPS (ou seja, ajuste indireto com restrições), como fixação de pontos de referência 3D, otimização de pose e parâmetros de câmera e
coeficientes de distorção.

Referências

código-fonte colmap openmvg, endereço do github:

https://github.com/openMVG/openMVG

https://github.com/colmap/colmap
Shan Jie. Breve história e resumo do ajuste do pacote. Journal of Wuhan University Information Science Edition, 2018, 43(12): 1797-1810.