以下内容主要讲解estimateFundamental()和estimateHomography()的求解过程一、本质两个函数的本质都是在做相同的事情输入两帧特征方向向量输出相机的位姿 Rt.但是两个函数的路径不同。二、Homography 单应矩阵求解2.1 函数功能输入两帧特征点方向向量Bearings通过单应矩阵Homography Matrix求解相机云顶同时分解单应矩阵得到4种可能的位姿。通过“平面约束”和“Sampson误差”筛选最优解返回最优单应性矩阵包含位姿和平面法向量2.2 核心代码解析特征点格式转换Eigen向量 - OpenCV的Point2f通过OpenCV的 RANSACA 计算单应性矩阵HH的自由度为8对H矩阵进行格式转换OpenCV的Mat → Eigen的Matrix/Vector计算内点数量统计符合单位约束的特征点分解单应矩阵得到位姿和平面法向量删选最优解Homography estimateHomography(const Bearings f_cur,const Bearings f_ref,const double focal_length,const double reproj_error_thresh,const size_t min_num_inliers) { // TODO: too long. split up and write tests CHECK_EQ(f_cur.cols(), f_ref.cols()); const size_t N f_cur.cols(); const double thresh reproj_error_thresh/focal_length; // compute homography using RANSAC //1.特征点格式转换和Ffundamental法保持一致Eigen向量 → OpenCV的Point2f std::vectorcv::Point2f ref_pts(N), cur_pts(N); for(size_t i0; iN; i) { // vk::project2把3D方向向量投影到归一化平面z1得到2D坐标 (x,y) const Vector2d uv_ref(vk::project2(f_ref.col(i))); const Vector2d uv_cur(vk::project2(f_cur.col(i))); ref_pts[i] cv::Point2f(uv_ref[0], uv_ref[1]); cur_pts[i] cv::Point2f(uv_cur[0], uv_cur[1]); } //2.求解单应性矩阵HH矩阵自由度为8 cv::Mat H cv::findHomography(ref_pts, cur_pts, cv::RANSAC, thresh); //3.转换成Eigen矩阵 Matrix3d H_cur_ref; H_cur_ref H.atdouble(0,0), H.atdouble(0,1), H.atdouble(0,2), H.atdouble(1,0), H.atdouble(1,1), H.atdouble(1,2), H.atdouble(2,0), H.atdouble(2,1), H.atdouble(2,2); // compute number of inliers //4.计算内点数量统计符合单位约束的特征点 std::vectorbool inliers(N); size_t n_inliers 0; for(size_t i0; iN; i) { //用单应性矩阵投影参考帧点 到 当前帧 const Vector2d uv_cur vk::project2(H_cur_ref * f_ref.col(i)); //计算重投影误差 const Vector2d e vk::project2(f_cur.col(i)) - uv_cur; inliers[i] (e.norm() thresh); n_inliers inliers[i]; } VLOG(100) Homography has n_inliers inliers; if(n_inliers min_num_inliers) //内点不足返回空矩阵 { return Homography(); // return homography with score zero. } // compute decomposition // 5. 分解单应矩阵得到位姿和平面法向量 cv::Matx33d K(1, 0, 0, 0, 1, 0, 0, 0, 1); //构建单位相机内参矩阵因为输入是归一化坐标内参矩阵为单位矩阵 std::vectorcv::Mat rotations; //4种可能得旋转 std::vectorcv::Mat translations; //4种可能得平移 std::vectorcv::Mat normals; //4种可能的平面法向量 cv::decomposeHomographyMat(H, K, rotations, translations, normals); CHECK_EQ(rotations.size(), 4u); // 单应矩阵分解必得到4种解 // copy in decompositions struct //6.筛选最优解 std::vectorHomography decomp; for(size_t i0; i4; i) //OpenCV使用的是cv::Mat,但是工程使用的是Eigen { Homography d; //转换平移向量 const cv::Mat t translations[i]; d.t_cur_ref Vector3d(t.atdouble(0), t.atdouble(1), t.atdouble(2)); //转换平面法向量 const cv::Mat n normals[i]; d.n_cur Vector3d(n.atdouble(0), n.atdouble(1), n.atdouble(2)); //转换旋转矩阵 const cv::Mat R rotations[i]; d.R_cur_ref R.atdouble(0,0), R.atdouble(0,1), R.atdouble(0,2), R.atdouble(1,0), R.atdouble(1,1), R.atdouble(1,2), R.atdouble(2,0), R.atdouble(2,1), R.atdouble(2,2); decomp.push_back(d); } // check that plane is in front of camera // 7. 筛选1平面法向量朝向相机前方特征点深度为正 for(Homography D : decomp) { D.score 0; for(size_t i0; iN; i) { if(!inliers[i]) continue; //法向量与特征点方向向量的点积0时 平面在相机的前方 const double test f_cur.col(i).dot(D.n_cur); if(test 0.0) D.score 1.0; } } // 按得分排序得分高更多点满足平面约束 std::sort(decomp.begin(), decomp.end(), [](const Homography lhs, const Homography rhs) { return lhs.score rhs.score; }); decomp.resize(2); //保留前边两个最优解 // According to Faugeras and Lustman, ambiguity exists if the two scores are equal // but in practive, better to look at the ratio! //8.筛选2Sampson误差对极误差进一步消除歧义 if(decomp[1].score/decomp[0].score 0.9){ decomp.erase(decomp.begin() 1); // no ambiguity } else { // two-way ambiguity: resolve by sampsonus score of all points. // Sampson error can be roughly thought as the squared distance between a // point x to the corresponding epipolar line xF const double thresh_squared thresh * thresh * 4.0; for(Homography D : decomp) { D.score 0.0; // sum of sampsonus score const Matrix3d E_cur_ref D.R_cur_ref * vk::skew(D.t_cur_ref); // Essential Matrix for(size_t i0; iN; i) { const double d vk::sampsonDistance(f_cur.col(i), E_cur_ref, f_ref.col(i)); D.score std::min(d, thresh_squared); } } // 保留Sampson误差更小的解 if(decomp[0].score decomp[1].score) decomp.erase(decomp.begin() 1); else decomp.erase(decomp.begin()); } decomp[0].score n_inliers; return decomp[0]; }2.3 函数中部分概念解析1. 内点inlier是什么内点 符合当前数学模型约束的正确匹配点外点outlier 匹配错误、噪声大、不符合模型的点在单应矩阵H里正确匹配点用H把参考帧点投影到当前帧误差很小错误匹配点投影后误差很大所以误差 阈值 → 内点****误差 阈值 → 外点计算内点的核心代码for(size_t i0; iN; i) { // 用单应矩阵 H 把参考帧点投影到当前帧 const Vector2d uv_cur vk::project2(H_cur_ref * f_ref.col(i)); // 计算实际观测点 - 投影点 误差 const Vector2d e vk::project2(f_cur.col(i)) - uv_cur; // 误差 阈值 → 是内点 inliers[i] (e.norm() thresh); n_inliers inliers[i]; }2. 分解单应矩阵时K 矩阵为什么是单位矩阵关键结论因为代码输入的 Bearings 已经是【归一化相机坐标系】下的坐标正常单应公式但这里输入的Bearings不是像素坐标而是归一化相机坐标系x/z, y/z, 1也就是已经除以了 K 矩阵的坐标所以这里设置K为单位阵3. 为什么法向量 n 与特征点方向 f 的点积 0 → 平面在相机前方Homography 分解得到4组解其中2组平面在前2组平面在后。在 Homography 分解中会得到R、n、t。其中n为平面法向量表示平面朝那个方向也是垂直于平面的单位向量。f_cur表示当前帧特征点的方向向量相机中心 → 空间点的射线也就是相机看向这个点的方向。空间点 P*////O --------相机中心n为空间点P的平面法向量指向平面外侧。平面方程:(n^T) X dn 平面法向量d 相机到平面的距离空间点X λ f_cur //空间点在射线上代入得到深度计算公式空间点在相机前方λ0λ d / (n^T f_cur)因为d 0平面在相机前所以必须n^T f_cur 0三、Fundamental 本质矩阵求解3.1 函数功能输入两帧图像特征点方向向量Bearings、相机焦距、重投影误差阈值通过本质矩阵求解两帧间的相机运动R t返回内点数量。3.2 核心代码解析VisualInitResult FundamentalVisualInit::addFrameBundle( const FrameBundlePtr frames_cur) { // Track and detect features. if(!trackFeaturesAndCheckDisparity(frames_cur)) return VisualInitResult::kTracking; // Create vector of bearing vectors const FrameBundlePtr ref_frames ref_frames_; const Frame frame_ref *ref_frames-at(0); const Frame frame_cur *frames_cur-at(0); std::vectorstd::pairsize_t, size_t matches_cur_ref; getFeatureMatches(frame_cur, frame_ref, matches_cur_ref); //获取匹配的特征点 const size_t n matches_cur_ref.size(); Bearings f_cur(3, n); Bearings f_ref(3, n); //当前帧和参考帧的特征点方向向量单位向量归一化平面坐标shape: 3×nn 是特征点数量 for(size_t i 0; i n; i) { f_cur.col(i) frame_cur.f_vec_.col(matches_cur_ref[i].first); f_ref.col(i) frame_ref.f_vec_.col(matches_cur_ref[i].second); } // Compute model //使用本质矩阵法计算相机从参考帧-当前帧如何运动 //input前后帧的方向向量 //outputT_cur_ref相机移动多少num_valid内点数量 Transformation T_cur_ref; int num_valid estimateFundamental( f_cur, f_ref, ref_frames_-at(0)-getErrorMultiplier(), options_.reproj_error_thresh, T_cur_ref); if(num_valid options_.init_min_inliers) //内点太少不使用 { LOG(WARNING) Init Fundamental: Have num_valid inliers. options_.init_min_inliers inliers minimum required.; return VisualInitResult::kFailure; } // Triangulate //建立空间点 if(triangulateAndInitializePoints( frames_cur-at(0), ref_frames_-at(0), T_cur_ref, options_.reproj_error_thresh, options_.init_min_inliers, matches_cur_ref)) { return VisualInitResult::kSuccess; } // Restart frames_cur-at(0)-clearFeatureStorage(); return VisualInitResult::kTracking; }3.3 函数中部分概念解析1. 建立空间点 triangulateAndInitializePointsbool AbstractVisualInitialization::triangulateAndInitializePoints( const FramePtr frame_cur, //当前帧 const FramePtr frame_ref, //参考帧 const Transformation T_cur_ref, //相机运动 参考帧-当前帧 const double reprojection_threshold, //误差阈值 const size_t min_inliers_threshold, //内点阈值 std::vectorstd::pairsize_t, size_t matches_cur_ref, //前后帧匹配点对 const double depth_at_current_frame) //深度 { Positions points_in_cur; //1. 三角化算出3D点 triangulatePoints(*frame_cur, *frame_ref, T_cur_ref, reprojection_threshold, matches_cur_ref, points_in_cur); //2. 检查有效点够不够不够就失败 if(matches_cur_ref.size() min_inliers_threshold){ LOG(WARNING) Init WARNING: min_inliers_threshold inliers minimum required. Have only matches_cur_ref.size(); return false; } // Scale 3D points to given scene depth and initialize Points //3. 缩放3D点到真实大小并正式创建地图点 rescaleAndInitializePoints( frame_cur, frame_ref, matches_cur_ref, points_in_cur, T_cur_ref, depth_at_current_frame); return true; }2. 三角测量 triangulatePoints函数详解Vector3d triangulateFeatureNonLin( const Matrix3d R, // 旋转相机2 → 相机1 的旋转 相机1作为参考坐标系相机2旋转了R 移动了t const Vector3d t, // 平移相机2 → 相机1 的平移 const Vector3d feature1, // 相机1 看到的点方向 const Vector3d feature2 ) // 相机2 看到的点方向 { Vector3d f2 R * feature2; // 把相机 2 的方向转到相机 1 坐标系 Vector2d b; b[0] t.dot(feature1); b[1] t.dot(f2); Matrix2d A; A(0,0) feature1.dot(feature1); A(1,0) feature1.dot(f2); A(0,1) -A(1,0); A(1,1) -f2.dot(f2); Vector2d lambda A.inverse() * b; //AXb Vector3d xm lambda[0] * feature1; Vector3d xn t lambda[1] * f2; //标准的射线方程相机中心 深度 × 方向 return ( xm xn )/2; }现实场景你有两个相机或者一个相机移动了两次两个相机都拍到了同一个空间点 P每个相机只能看到这个点的方向不知道距离用两个方向射线求交点 → 得到 P 的 3D 坐标 。这就叫三角测量。相机1参考帧原点的观测公式相机 2当前帧移动了 t、旋转了 R的观测公式联立两个方程其中λ1​ 和 λ2就是我们要求的内容。最小误差模型分别对λ1​ 和 λ2 求导推到过程省略直接给出方程并写成AXB的形式为了和代码对应代码中的f2 为旋转之后的下面方程记计算出X后带回P点的两个观测公式然后求平均得到3D坐标3. rescaleAndInitializePoints 详解上一步通过三角化计算出了3D点但是单目相机不知道真实举例不知道世界有多大。比如三角化算出一个点距离相机1米但是现实中可能是10米也可能是100米。所有计算出的距离都差一个固定的倍数。这个倍数就叫尺度scale。算缩放倍数 scale缩放相机位置缩放 3D 点正式创建地图