c++学习【13】3D-2D高斯牛顿BA迭代求解相机外参-Toy模板网

这篇具有很好参考价值的文章主要介绍了c++学习【13】3D-2D高斯牛顿BA迭代求解相机外参。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }

  cout << "pose by g-n: \n" << pose.matrix() << endl;

    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

pc表示当前坐标，通过将points_3d[i]与pose相乘，得到的结果pc是该点在相机坐标系下的表示。
proj是一个二维向量，表示在相机坐标系下，通过相机投影将三维点投影到二维平面上得到的坐标。

inv_z在绑定调整迭代求解相机位姿中，用于后续的优化目标函数计算或者高斯牛顿法求解过程中的中间计算。

      Eigen::Vector2d e = points_2d[i] - proj;
      cost += e.squaredNorm();

对于误差squaredNorm()是一个Eigen库的函数，用于计算向量的平方范数（二范数）的平方。在这种情况下，误差e是一个向量，通过调用squaredNorm()函数，会计算误差向量的平方范数的平方并将结果累加到cost中。
平方范数是指向量的各个元素值的平方和再开根号，而squaredNorm()则直接计算平方范数的平方，避免了开根号的计算，可以提高计算速度，并且结果与平方范数相同。
通过将误差的平方范数的平方累加到cost中，可以构建一个优化问题的目标函数，优化算法可以通过最小化目标函数来得到最优的解。

j的计算公式，bundleAdjustmentGaussNewton方法，计算雅可比矩阵Ji，

      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

H和b是什么？通过雅可比矩阵Eigen::Matrix<double, 2, 6> J;

    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

      H += J.transpose() * J;
      b += -J.transpose() * e;

对每一个点求完之后。dx用于存储线性系统Hx = b的解

    Vector6d dx;
    dx = H.ldlt().solve(b);

更新。使用指数映射将位姿增量转换为相机位姿变换矩阵。这个变换矩阵表示相机从一个位姿移动到另一个位姿的变换。
新的位姿是通过将当前位姿与位姿增量相乘得到的。

pose = Sophus::SE3d::exp(dx) * pose;

相机位姿更新原理，与雅可比矩阵有什么关系？用于估计相机位姿以最小化重投影误差。重投影误差关于相机位姿的雅可比矩阵---是一个函数的偏导数矩阵。这个雅可比矩阵描述了相机位姿变化对重投影误差的影响。在每一次迭代中，通过计算雅可比矩阵，可以将相机位姿调整为使得重投影误差最小化的方向。
具体地，雅可比矩阵参与到高斯牛顿优化的迭代更新过程中，用于构建线性方程组的系数矩阵和右侧向量。求解这个线性方程组可以得到更新后的相机位姿。文章来源地址https://www.toymoban.com/news/detail-511769.html

到了这里，关于c++学习【13】3D-2D高斯牛顿BA迭代求解相机外参的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！