当前位置：首页 > news >正文

LineSlam线特征投影融合（Fuse）中pML-＞GetLineNormalVector()的理解代码理解

news 2025/11/1 9:22:10

理解LineSlam中线段融合部分，里面有很多限制条件，但是比较明显，但是对于线段的Normal，有些不容易理解，因此研究了一下，以下是个人理解，并且记录

其中fuse的代码代码如下（部分自己已经修改，用于LineSlam的工程中）：

int LSDmatcher::Fuse(KeyFrame *pKF, const std::vector<MapLine *> &vpMapLines, const float th){Eigen::Matrix3f Rcw_eigen = pKF->GetRotation();Eigen::Vector3f tcw_eigen = pKF->GetTranslation();cv::Mat Rcw = Converter::toCvMat(Rcw_eigen);;cv::Mat tcw = (Mat_<float>(3, 1) << tcw_eigen(0), tcw_eigen(1), tcw_eigen(2));const float &fx = pKF->fx;const float &fy = pKF->fy;const float &cx = pKF->cx;const float &cy = pKF->cy;const float &bf = pKF->mbf;Eigen::Vector3f Ow_eigen = pKF->GetCameraCenter();cv::Mat Ow = (Mat_<float>(3, 1) << Ow_eigen(0), Ow_eigen(1), Ow_eigen(2));int nFused=0;const int nLines = vpMapLines.size();// For each candidate MapPoint project and matchfor(int iML=0; iML<nLines; iML++){MapLine* pML = vpMapLines[iML];// Discard Bad MapLines and already foundif(!pML || pML->isBad())continue;//Vector6d P = pML->GetWorldPos();std::pair<Eigen::Vector3f, Eigen::Vector3f> lineWorldPosition = pML->GetLineWorldPos();cv::Mat SP = (Mat_<float>(3, 1) << lineWorldPosition.first(0), lineWorldPosition.first(1), lineWorldPosition.first(2));cv::Mat EP = (Mat_<float>(3, 1) << lineWorldPosition.second(0), lineWorldPosition.second(1), lineWorldPosition.second(2));const cv::Mat SPc = Rcw * SP + tcw;const auto &SPcX = SPc.at<float>(0);const auto &SPcY = SPc.at<float>(1);const auto &SPcZ = SPc.at<float>(2);const cv::Mat EPc = Rcw * EP + tcw;const auto &EPcX = EPc.at<float>(0);const auto &EPcY = EPc.at<float>(1);const auto &EPcZ = EPc.at<float>(2);if (SPcZ < 0.0f || EPcZ < 0.0f)continue;const float invz1 = 1.0f / SPcZ;const float u1 = fx * SPcX * invz1 + cx;const float v1 = fy * SPcY * invz1 + cy;if (u1 < pKF->mnMinX || u1 > pKF->mnMaxX)continue;if (v1 < pKF->mnMinY || v1 > pKF->mnMaxY)continue;const float invz2 = 1.0f / EPcZ;const float u2 = fx * EPcX * invz2 + cx;const float v2 = fy * EPcY * invz2 + cy;if (u2 < pKF->mnMinX || u2 > pKF->mnMaxX)continue;if (v2 < pKF->mnMinY || v2 > pKF->mnMaxY)continue;const float maxDistance = pML->GetMaxDistanceInvariance();const float minDistance = pML->GetMinDistanceInvariance();const cv::Mat OM = 0.5 * (SP + EP) - Ow;const float dist = cv::norm(OM);if (dist < minDistance || dist > maxDistance)continue;// Check viewing angle 这个LineNormal是啥意思？Eigen::Vector3f Pn = pML->GetLineNormalVector();cv::Mat pn = (Mat_<float>(3, 1) << Pn(0), Pn(1), Pn(2));if(OM.dot(pn)<0.5*dist)continue;const int nPredictedLevel = pML->PredictScale(dist, pKF);const float radius = th*pKF->mvScaleFactors[nPredictedLevel];const vector<size_t> vIndices = pKF->GetLinesInArea(u1,v1, u2, v2, radius);if(vIndices.empty())continue;const cv::Mat dML = pML->GetLineDescriptor();int bestDist=INT_MAX;int bestIdx =-1 ;for(unsigned long idx : vIndices){const int &klLevel = pKF->mvKeyLines[idx].octave;if(klLevel<nPredictedLevel-1 || klLevel>nPredictedLevel)continue;const cv::Mat &dKF = pKF->mLineDescriptors.row(idx);const int dist = DescriptorDistance(dML,dKF);if(dist<bestDist){bestDist = dist;bestIdx = idx;}}if(bestDist<=TH_LOW){MapLine* pMLinKF = pKF->GetMapLine(bestIdx);if(pMLinKF){if(!pMLinKF->isBad()) {if(pMLinKF->Observations()>pML->Observations())pML->Replace(pMLinKF);elsepMLinKF->Replace(pML);}}else{pML->AddLineObservation(pKF,bestIdx);pKF->AddMapLine(pML,bestIdx);}nFused++;}}return nFused;}

本来对于这段代码pML->GetLineNormal();，没有那么好理解，因此记录
先分析它附近的代码，如下：

// Check viewing angle 这个LineNormal是啥意思？
Eigen::Vector3f Pn = pML->GetLineNormalVector();
cv::Mat pn = (Mat_<float>(3, 1) << Pn(0), Pn(1), Pn(2));
if(OM.dot(pn)<0.5*dist)continue;

🧠 背景：MapLine 与其法向量 (Normal)

在 ORB-SLAM 或 LSD-SLAM 系列中，每个 MapLine（世界坐标系下的线段）都维护了一条空间线的信息：

起点 P1
终点 P2
平均观测方向（normal vector）Pn

而 GetLineNormal() 返回的 Pn 是线段的平均观测方向法向量，用来判断当前相机是否处在这条线的“前方”或“后方”。

在这里插入图片描述

判断逻辑

if (OM.dot(pn) < 0.5 * dist)continue;

这行的作用是：
判断当前相机视角是否位于线段的可观测半空间内
解释：

OM.dot(pn) 表示相机中心到线段中点方向与线段平均法向量之间的夹角。
若点积过小，说明相机与线段法向量方向几乎相反（即在线段背面），该线在当前视角下不可见或方向反转。
阈值 0.5 * dist 是一个启发式条件，过滤掉大角度背向的线条。

直观理解：

假设线段像一根小棍子，
法向量 Pn 表示线段“朝向相机的方向”；
OM 是相机到这根棍子的位置方向；
点积较大 → 说明相机正看向线段；
点积较小 → 说明相机在反方向或斜后方，线段不可见

其中线段
这正是理解线特征几何一致性的关键之一。
在 ORB-SLAM / PL-SLAM / Line3D++ 等系统中，
MapLine::GetLineNormal() 所返回的法向量 (normal, 它的变量为：mNormalVector)(这点非常重要)
并不是线段本身的方向向量，而是它的平均观测方向（Viewing Direction）。
我们来分两步说明：
一、Normal 在 MapLine 中的含义

在 ORB-SLAM 的点特征中，MapPoint 存储一个 mNormalVector，
用于表示从地图点指向所有观测该点的相机的平均方向。

线特征（MapLine）也同理：
当某个关键帧观测到这条线段时，线段在世界坐标下的中点记作 Pc；相机在世界坐标下的位置记作 Ow；
从线段到相机的方向为：
在这里插入图片描述
每次有新的关键帧观测该线段，就把这个方向向量累积：

然后归一化：

这就是 mNormalVector —— “线段的平均观测方向”

通常在 ORB-SLAM 或基于它**（典型 MapLine::UpdateNormalAndDepth）**的系统中，这样写：

void MapLine::UpdateNormalAndDepth()
{if (mbBad)return;std::map<KeyFrame*, std::tuple<int, int>> observations;{std::unique_lock<std::mutex> lock(mMutexFeatures);observations = mLineObservations;}if (observations.empty())return;cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);int n = 0;for (auto &obs : observations){KeyFrame* pKF = obs.first;if (pKF->isBad())continue;// 相机中心cv::Mat Ow = pKF->GetCameraCenter();// 世界坐标下线段起点与终点Eigen::Vector3f SP, EP;std::tie(SP, EP) = GetLineWorldPos();cv::Mat Pc = (cv::Mat_<float>(3,1) << 0.5f*(SP(0)+EP(0)), 0.5f*(SP(1)+EP(1)), 0.5f*(SP(2)+EP(2)));cv::Mat v = Ow - Pc;v = v / cv::norm(v);normal += v;n++;}mNormalVector = normal / static_cast<float>(n);mNormalVector = mNormalVector / cv::norm(mNormalVector);
}