三角测量原理

最新推荐文章于 2024-01-19 16:56:50 发布

置顶 td092

最新推荐文章于 2024-01-19 16:56:50 发布

阅读量4.6k

点赞数

分类专栏：机器人文章标签：自动驾驶机器学习

欢迎交流，有评论就更

本文链接：https://blog.csdn.net/weixin_38258767/article/details/106867354

版权

机器人专栏收录该内容

63 篇文章 9 订阅

订阅专栏

三角测量原理

问题概述
ORB-SLAM中的求解方法
Open-VINS中的求解方法

问题概述

在这里插入图片描述
如上图所示，三角测量是指在已知当前帧和参考帧的位姿和匹配点以及相机内参的情况下，恢复观测点深度的过程。

ORB-SLAM中的求解方法

根据当前帧和参考帧的投影方程有：
在这里插入图片描述
将上面的方程合并，并展开：

通过SVD分解就可以求解上式。
在ORB_SLAM2中有：

 {
     // Linear Triangulation Method
     cv::Mat A(4,4,CV_32F);
     A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
     A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
     A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
     A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

     cv::Mat w,u,vt;
     cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

     x3D = vt.row(3).t();

     if(x3D.at<float>(3)==0)
         continue;

     // Euclidean coordinates
     x3D = x3D.rowRange(0,3)/x3D.at<float>(3);

 }

Open-VINS中的求解方法

Open-VINS中采用多帧观测进行三角化，首先选择一帧锚点帧A，其他帧 $C_i$
对于每一个观测有:
${^{C_i}p_f =^{C_i}z_f }\begin{bmatrix} u_n\\v_n\\1\end{bmatrix}$
对于锚点帧A，有：
在这里插入图片描述
取，满足
所以，构造如下方程

叠加多帧后：

求解上式后，可得深度 $^Az_f$
相应代码：

bool FeatureInitializer::single_triangulation_1d(Feature *feat,
                                                 std::unordered_map<size_t, std::unordered_map<double, ClonePose>> &clonesCAM) {

  // Total number of measurements
  // Also set the first measurement to be the anchor frame
  int total_meas = 0;
  size_t anchor_most_meas = 0;
  size_t most_meas = 0;
  for (auto const &pair : feat->timestamps) {
    total_meas += (int)pair.second.size();
    if (pair.second.size() > most_meas) {
      anchor_most_meas = pair.first; // 观测最多的那一帧的时间戳
      most_meas = pair.second.size(); // 最多的观测帧数
    }
  }
  feat->anchor_cam_id = anchor_most_meas;
  feat->anchor_clone_timestamp = feat->timestamps.at(feat->anchor_cam_id).back(); // 共视图中最近的一帧
  size_t idx_anchor_bearing = feat->timestamps.at(feat->anchor_cam_id).size() - 1; 

  // Our linear system matrices
  double A = 0.0;
  double b = 0.0;

  // Get the position of the anchor pose
  ClonePose anchorclone = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp);
  const Eigen::Matrix<double, 3, 3> &R_GtoA = anchorclone.Rot();
  const Eigen::Matrix<double, 3, 1> &p_AinG = anchorclone.pos();

  // Get bearing in anchor frame
  Eigen::Matrix<double, 3, 1> bearing_inA;
  bearing_inA << feat->uvs_norm.at(feat->anchor_cam_id).at(idx_anchor_bearing)(0),
      feat->uvs_norm.at(feat->anchor_cam_id).at(idx_anchor_bearing)(1), 1; // 
  bearing_inA = bearing_inA / bearing_inA.norm();

  // Loop through each camera for this feature
  for (auto const &pair : feat->timestamps) {

    // Add CAM_I features
    for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {

      // Skip the anchor bearing //
      if ((int)pair.first == feat->anchor_cam_id && m == idx_anchor_bearing)
        continue;

      // Get the position of this clone in the global
      const Eigen::Matrix<double, 3, 3> &R_GtoCi = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
      const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();

      // Convert current position relative to anchor
      Eigen::Matrix<double, 3, 3> R_AtoCi;
      R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
      Eigen::Matrix<double, 3, 1> p_CiinA;
      p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);

      // Get the UV coordinate normal
      Eigen::Matrix<double, 3, 1> b_i;
      b_i << feat->uvs_norm.at(pair.first).at(m)(0), feat->uvs_norm.at(pair.first).at(m)(1), 1;
      b_i = R_AtoCi.transpose() * b_i;
      b_i = b_i / b_i.norm();
      Eigen::Matrix3d Bperp = skew_x(b_i);

      // Append to our linear system
      Eigen::Vector3d BperpBanchor = Bperp * bearing_inA;
      A += BperpBanchor.dot(BperpBanchor);
      b += BperpBanchor.dot(Bperp * p_CiinA);
    }
  }

  // Solve the linear system
  double depth = b / A;
  Eigen::MatrixXd p_f = depth * bearing_inA;

  // Then set the flag for bad (i.e. set z-axis to nan)
  if (p_f(2, 0) < _options.min_dist || p_f(2, 0) > _options.max_dist || std::isnan(p_f.norm())) {
    return false;
  }

  // Store it in our feature object
  feat->p_FinA = p_f;
  feat->p_FinG = R_GtoA.transpose() * feat->p_FinA + p_AinG;
  return true;
}