VanishingPointDetection 代码学习整理
2-Line Exhaustive Searching for Real-Time Vanishing Point Estimation in Manhattan World,Xiaohu Lu, JianYao, Haoang Li, Yahui Liu and Xiaofeng Zhang, WACV2017.
原作者代码地址：https://github.com/xiaohulugo/VanishingPointDetection
建议结合 Structure SLAM 相关论文阅读（一）：消影点提取 进行阅读。
代码中采用了一个非OpenCV官方版本的LSD线段检测库，实现方法大同小异，但定义的线特征数据结构略有不同。笔者复现的时候用的是点线结合 PL-VINS ：线特征提取(feature-tracker) 代码整理笔记的线特征提取。
main.cpp
定义的LSD线段检测函数为：
void LineDetect( cv::Mat image, double thLength, std::vector<std::vector<double> > &lines )
具备两个输入参数和一个输出参数，image为输入图像，thLengrth为有效线段的长度判定阈值，lines为所提取出的线段容器，线段用一个尺寸为4的容器来存储2个端点的像素坐标（按x1, y1, x2, y2的顺序)。
定义的一个线段聚类可视化函数为：
void drawClusters( cv::Mat &img, std::vector<std::vector<double> > &lines, std::vector<std::vector<int> > &clusters )
用不同颜色画出消影点检测后进行了线段聚类后的三类线段特征（分别属于三个不同的消影点），clusters 里分别存储有三类线段中每条的ID。
主函数int main() 用来测试单张图片的消影点检测和线段聚类效果。
int main()
{
string inPutImage = "/Users/***/github/VanishingPointDetection-master/P1020171.jpg";
cv::Mat image= cv::imread( inPutImage );
if ( image.empty() )
{
printf( "Load image error : %s\n", inPutImage.c_str() );
}
// LSD line segment detection
double thLength = 30.0;
std::vector<std::vector<double> > lines;
LineDetect( image, thLength, lines );
// Camera internal parameters
// cv::Point2d pp( 307, 251 ); // Principle point (in pixel)
// double f = 6.053 / 0.009; // Focal length (in pixel)
cv::Point2d pp( image.cols/2, image.rows/2 ); // Principle point (in pixel)
double f = 1.2*(std::max(image.cols, image.rows));
// Vanishing point detection
std::vector<cv::Point3d> vps; // Detected vanishing points (in pixel)
std::vector<std::vector<int> > clusters; // Line segment clustering results of each vanishing point
VPDetection detector;
detector.run( lines, pp, f, vps, clusters );
drawClusters( image, lines, clusters );
imshow("",image);
cv::waitKey( 0 );
return 0;
}
测试中对图片内参做了近似处理，实际中应根据相机参数进行赋值。
- cv::Point2d pp， 对应相机内参中的 c x ， c y c_x，c_y cx，cy。
- double f，对应相机内参中的 f = f x = f y f=f_x = f_y f=fx=fy。
- std::vector<cv::Point3d> vps，消影点的像素坐标。
- VPDetection detector，构建的消影点提取算子。
核心函数在VPDetection的实现中，本文将重点理解。
VPDetection.h
struct LineInfo
{
cv::Mat_<double> para;
double length;
double orientation;
};
- para，存储有线段两端点向量的叉乘积，也是相机光心、线段端点构成平面的法向量
- length，线段长度（像素）
- orientation，线段方向角（原点左上角逆时针旋转）， atan ( dy / dx ) \text{atan}(\text{dy}/\text{dx}) atan(dy/dx)计算得到
class VPDetection
{
public:
VPDetection(void);
~VPDetection(void);
void run( std::vector<std::vector<double> > &lines, cv::Point2d pp, double f, std::vector<cv::Point3d> &vps, std::vector<std::vector<int> > &clusters );
void getVPHypVia2Lines( std::vector<std::vector<cv::Point3d> > &vpHypo );
void getSphereGrids( std::vector<std::vector<double> > &sphereGrid );
void getBestVpsHyp( std::vector<std::vector<double> > &sphereGrid, std::vector<std::vector<cv::Point3d> > &vpHypo, std::vector<cv::Point3d> &vps );
void lines2Vps( double thAngle, std::vector<cv::Point3d> &vps, std::vector<std::vector<int> > &clusters );
private:
std::vector<std::vector<double> > lines;
std::vector<LineInfo> lineInfos;
cv::Point2d pp;
double f;
double noiseRatio;
};
声明了生成消影点假设、构建极坐标网格、验证消影点假设、线段分类等函数。
VPDetection.cpp
run()
消影点检测程序执行入口为：
void VPDetection::run( std::vector<std::vector<double> > &lines, cv::Point2d pp, double f, std::vector<cv::Point3d> &vps, std::vector<std::vector<int> > &clusters )
{
this->lines = lines;
this->pp = pp;
this->f = f;
this->noiseRatio = 0.5;
cout<<"get vp hypotheses . . ."<<endl;
std::vector<std::vector<cv::Point3d>> vpHypo;
getVPHypVia2Lines( vpHypo );
cout<<"get sphere grid . . ."<<endl;
std::vector<std::vector<double>> sphereGrid;
getSphereGrids( sphereGrid );
cout<<"test vp hypotheses . . ."<<endl;
getBestVpsHyp( sphereGrid, vpHypo, vps );
cout<<"get final line clusters . . ."<<endl;
double thAngle = 6.0 / 180.0 * CV_PI;
lines2Vps( thAngle, vps, clusters );
int clusteredNum = 0;
for ( int i=0; i<3; ++i )
{
clusteredNum += clusters[i].size();
}
cout<<"total: " <<lines.size()<<" clusered: "<<clusteredNum;
cout<<" X: "<<clusters[0].size()<<" Y: "<<clusters[1].size()<<" Z: "<<clusters[2].size()<<endl;
}
由注释可知，先由 getVPHypVia2Lines穷举生成所有的消影点假设，通过getSphereGrids来构建极坐标搜索网格（可以用于后续基于查表的快速假设验证），再通过getBestVpsHyp对所有假设进行验证，获取最佳假设并提取出相应的消影点vps。最终，lines2Vps将根据设定的分类阈值（ 6 ∘ 6^\circ 6∘）和已经提取出的消影点方向，对线段特征进行分类。
getVPHypVia2Lines
{
int num = lines.size();
double p = 1.0 / 3.0 * pow( 1.0 - noiseRatio, 2 );
double confEfficience = 0.9999;
int it = log( 1 - confEfficience ) / log( 1.0 - p );
int numVp2 = 360;
double stepVp2 = 2.0 * CV_PI / numVp2;
基于噪声比为0.5的假设下(即存在一半的线段没有对应的消影点，非曼哈顿世界假设下的结构线段)，若随机选取两条线段，它们属于同一个消影点的概率计算为
p
=
1
/
3
×
0.
5
2
=
1
/
12
p=1/3 \times0.5^2=1/12
p=1/3×0.52=1/12，以置信度0.9999来计算获得至少一个2-line MSS所需的迭代次数为
i
t
=
log
(
1
−
0.9999
)
/
log
(
1
−
P
)
=
105
\begin{aligned} it &= \text{log}(1-0.9999)/\text{log}(1-P) \\ &=105 \end{aligned}
it=log(1−0.9999)/log(1−P)=105第二个消影点假设将在第一个消影点在球坐标系下的外圈( great circle) 上生成（如图所示），均匀采样
numVp2
=
360
\text{numVp2}=360
numVp2=360 个假设，即采样间隔为
stepVp2
=
2
π
/
360
=
π
/
180
=
1
∘
\text{stepVp2} =2\pi/360=\pi/180=1^\circ
stepVp2=2π/360=π/180=1∘。
这一步将获取每条检测线段的参数并存储到lineInfos中。
// get the parameters of each line
lineInfos.resize( num );
for ( int i=0; i<num; ++i )
{
cv::Mat_<double> p1 = ( cv::Mat_<double>(3, 1) << lines[i][0], lines[i][1], 1.0 );
cv::Mat_<double> p2 = ( cv::Mat_<double>(3, 1) << lines[i][2], lines[i][3], 1.0 );
lineInfos[i].para = p1.cross( p2 );
double dx = lines[i][0] - lines[i][2];
double dy = lines[i][1] - lines[i][3];
lineInfos[i].length = sqrt( dx * dx + dy * dy );
lineInfos[i].orientation = atan2( dy, dx );
if ( lineInfos[i].orientation < 0 )
{
lineInfos[i].orientation += CV_PI;
}
}
关键环节，生成每次迭代的消影点假设。
// get vp hypothesis for each iteration
// 首先构造一个 360 * 105 = 37800 大小的容器，以存储共37800个消影点假设。
vpHypo = std::vector<std::vector<cv::Point3d> > ( it * numVp2, std::vector<cv::Point3d>(3) );
int count = 0;
srand((unsigned)time(NULL));
//开始迭代
for ( int i = 0; i < it; ++ i )
{
// 从线段中随机挑选不同的两条直线，num为线段数量
int idx1 = rand() % num;
int idx2 = rand() % num;
while ( idx2 == idx1 )
{
idx2 = rand() % num;
}
// get the vp1
// 假设挑选出的2条线段属于同一个消影点，叉乘计算出第一个消影点的像素坐标
cv::Mat_<double> vp1_Img = lineInfos[idx1].para.cross( lineInfos[idx2].para );
if ( vp1_Img(2) == 0 )
{
i --;
continue;
}
//把消影点像素坐标转化为相机坐标系下的归一化坐标
cv::Mat_<double> vp1 = ( cv::Mat_<double>(3, 1) << vp1_Img(0) / vp1_Img(2) - pp.x, vp1_Img(1) / vp1_Img(2) - pp.y, f );
if ( vp1(2) == 0 ) { vp1(2) = 0.0011; }
double N = sqrt( vp1(0) * vp1(0) + vp1(1) * vp1(1) + vp1(2) * vp1(2) );
vp1 *= 1.0 / N; //消影点向量单位化
// get the vp2 and vp3
cv::Mat_<double> vp2 = ( cv::Mat_<double>(3, 1) << 0.0, 0.0, 0.0 );
cv::Mat_<double> vp3 = ( cv::Mat_<double>(3, 1) << 0.0, 0.0, 0.0 );
for ( int j = 0; j < numVp2; ++ j )
{
// vp2, numVp2 = 360, stepVp2 = pi/180, 开始均匀采样
double lambda = j * stepVp2;
// 球经纬度坐标转化为空间坐标，原理和公式见详解
double k1 = vp1(0) * sin( lambda ) + vp1(1) * cos( lambda );
double k2 = vp1(2);
double phi = atan( - k2 / k1 );
double Z = cos( phi );
double X = sin( phi ) * sin( lambda );
double Y = sin( phi ) * cos( lambda );
vp2(0) = X; vp2(1) = Y; vp2(2) = Z;
if ( vp2(2) == 0.0 ) { vp2(2) = 0.0011; } //若为0则替换为小量
N = sqrt( vp2(0) * vp2(0) + vp2(1) * vp2(1) + vp2(2) * vp2(2) );
vp2 *= 1.0 / N; //归一化第二个消影点,因为不可能在相机后方，所以当vp2(2)<0时取反方向。
if ( vp2(2) < 0 ) { vp2 *= -1.0; }
// vp3
vp3 = vp1.cross( vp2 ); //第三个消影点直接叉乘获得
if ( vp3(2) == 0.0 ) { vp3(2) = 0.0011; }
N = sqrt( vp3(0) * vp3(0) + vp3(1) * vp3(1) + vp3(2) * vp3(2) );
vp3 *= 1.0 / N;
if ( vp3(2) < 0 ) { vp3 *= -1.0; }
//至此，生成了第一次迭代的一组消影点假设
vpHypo[count][0] = cv::Point3d( vp1(0), vp1(1), vp1(2) );
vpHypo[count][1] = cv::Point3d( vp2(0), vp2(1), vp2(2) );
vpHypo[count][2] = cv::Point3d( vp3(0), vp3(1), vp3(2) );
count ++;
}
}
如注释所示，首先是随机挑选2条不同线段来生成第一个消影点VP1假设，并根据VP1在球坐标系下的极坐标来判断VP2所在的平面，均匀采样生成360个VP2的假设（方向误差最大为1度），基于VP1和VP2生成第三个消影点VP3的假设。
第
i
i
i 个假设的极坐标可以结合消影点的正交性如下计算：
{
X
=
sin
(
ϕ
)
sin
(
λ
)
Y
=
sin
(
ϕ
)
cos
(
λ
)
Z
=
cos
(
ϕ
)
0
=
X
1
X
2
+
Y
1
Y
2
+
Z
1
Z
2
\begin{cases} X &=\text{sin}(\phi)\text{sin}(\lambda) \\ Y &= \text{sin}(\phi)\text{cos}(\lambda) \\ Z &= \text{cos}(\phi)\\ 0&=X_1 X_2+ Y_1 Y_2+ Z_1 Z_2 \end{cases}
⎩
⎨
⎧XYZ0=sin(ϕ)sin(λ)=sin(ϕ)cos(λ)=cos(ϕ)=X1X2+Y1Y2+Z1Z2
代码中计算流程：
double k1 = vp1(0) * sin( lambda ) + vp1(1) * cos( lambda );
double k2 = vp1(2);
double phi = atan( - k2 / k1 );
double Z = cos( phi );
double X = sin( phi ) * sin( lambda );
double Y = sin( phi ) * cos( lambda );
即由正交性得，
sin
(
ϕ
1
)
sin
(
λ
1
)
⋅
sin
(
ϕ
2
)
sin
(
λ
2
)
+
sin
(
ϕ
1
)
cos
(
λ
1
)
⋅
sin
(
ϕ
2
)
cos
(
λ
2
)
+
cos
(
ϕ
1
)
⋅
cos
(
ϕ
2
)
=
0
\text{sin}(\phi_1)\text{sin}(\lambda_1)\cdot \text{sin}(\phi_2)\text{sin}(\lambda_2)+\text{sin}(\phi_1)\text{cos}(\lambda_1)\cdot\text{sin}(\phi_2)\text{cos}(\lambda_2)+\text{cos}(\phi_1)\cdot \text{cos}(\phi_2) = 0
sin(ϕ1)sin(λ1)⋅sin(ϕ2)sin(λ2)+sin(ϕ1)cos(λ1)⋅sin(ϕ2)cos(λ2)+cos(ϕ1)⋅cos(ϕ2)=0同除以
sin
(
ϕ
2
)
\text{sin}(\phi_2)
sin(ϕ2)，得
sin
(
ϕ
1
)
sin
(
λ
1
)
⋅
sin
(
λ
2
)
+
sin
(
ϕ
1
)
cos
(
λ
1
)
⋅
cos
(
λ
2
)
+
cos
(
ϕ
1
)
⋅
cot
(
ϕ
2
)
=
0
\text{sin}(\phi_1)\text{sin}(\lambda_1)\cdot \text{sin}(\lambda_2)+\text{sin}(\phi_1)\text{cos}(\lambda_1)\cdot \text{cos}(\lambda_2)+\text{cos}(\phi_1)\cdot \text{cot}(\phi_2) = 0
sin(ϕ1)sin(λ1)⋅sin(λ2)+sin(ϕ1)cos(λ1)⋅cos(λ2)+cos(ϕ1)⋅cot(ϕ2)=0
代码中先计算，
{
k
1
=
sin
(
ϕ
1
)
sin
(
λ
1
)
⋅
sin
(
λ
2
)
+
sin
(
ϕ
1
)
cos
(
λ
1
)
⋅
cos
(
λ
2
)
k
2
=
cos
(
ϕ
1
)
\begin{cases} k_1=\text{sin}(\phi_1)\text{sin}(\lambda_1)\cdot \text{sin} (\lambda_2)+\text{sin}(\phi_1)\text{cos}(\lambda_1)\cdot \text{cos} (\lambda_2) \\ k_2=\text{cos}(\phi_1) \end{cases}
{k1=sin(ϕ1)sin(λ1)⋅sin(λ2)+sin(ϕ1)cos(λ1)⋅cos(λ2)k2=cos(ϕ1)有，
k
1
+
k
2
⋅
cot
(
ϕ
2
)
=
0
k_1+k_2\cdot\text{cot}(\phi_2) = 0
k1+k2⋅cot(ϕ2)=0即可先求出
ϕ
2
\phi_2
ϕ2，再根据假设的
λ
2
\lambda_2
λ2 得到第二个消影点VP2的坐标，进一步叉乘得到VP3。
一次迭代内的一组消影点假设生成结束。
getSphereGrids
将构建用于快速查询以验证假设的极坐标网格。
{
// build sphere grid with 1 degree accuracy
double angelAccuracy = 1.0 / 180.0 * CV_PI;
double angleSpanLA = CV_PI / 2.0;
double angleSpanLO = CV_PI * 2.0;
int gridLA = angleSpanLA / angelAccuracy;
int gridLO = angleSpanLO / angelAccuracy;
// sphereGrid = std::vector<std::vector<double> >( gridLA, gridLO );
sphereGrid = std::vector< std::vector<double> >( gridLA, std::vector<double>(gridLO) );
for ( int i=0; i<gridLA; ++i )
{
for ( int j=0; j<gridLO; ++j )
{
sphereGrid[i][j] = 0.0;
}
}
极坐标网格
ϕ
\phi
ϕ (LA)和
λ
\lambda
λ (LO)的范围分别为
[
0
,
π
/
2
]
[0,\pi/2]
[0,π/2] 和
[
0
,
2
π
]
[0,2\pi]
[0,2π]。极坐标网格
G
\mathcal{G}
G 的构建以
1
∘
1^\circ
1∘ 为间隔，把
G
\mathcal{G}
G 初始化为
90
×
360
90\times360
90×360 的零空间，i.e.
G
(
i
,
j
)
=
0
\mathcal{G}(i,j)=0
G(i,j)=0 for
i
=
1
,
2
,
.
.
.
,
90
i=1,2,...,90
i=1,2,...,90 和
j
=
1
,
2
,
.
.
.
,
360
j=1,2,...,360
j=1,2,...,360。
遍历图像中所有线段的交点，找到对应网格并更新网格的响应数值。
// put intersection points into the grid
double angelTolerance = 60.0 / 180.0 * CV_PI;
cv::Mat_<double> ptIntersect;
double x = 0.0, y = 0.0;
double X = 0.0, Y = 0.0, Z = 0.0, N = 0.0;
double latitude = 0.0, longitude = 0.0;
int LA = 0, LO = 0;
double angleDev = 0.0;
for ( int i=0; i<lines.size()-1; ++i )
{
for ( int j=i+1; j<lines.size(); ++j )
{
//计算交点
ptIntersect = lineInfos[i].para.cross( lineInfos[j].para );
if ( ptIntersect(2,0) == 0 )
{
continue;
}
//计算归一化坐标
x = ptIntersect(0,0) / ptIntersect(2,0);
y = ptIntersect(1,0) / ptIntersect(2,0);
//转换为等效球坐标,求出latitude和longtitude
X = x - pp.x;
Y = y - pp.y;
Z = f;
N = sqrt( X * X + Y * Y + Z * Z );
latitude = acos( Z / N );
longitude = atan2( X, Y ) + CV_PI;
LA = int( latitude / angelAccuracy );//找到对应的纬度网格
if ( LA >= gridLA )
{
LA = gridLA - 1;
}
LO = int( longitude / angelAccuracy ); //找到对应的经度网格
if ( LO >= gridLO )
{
LO = gridLO - 1;
}
// 计算线段的角度偏差，超出阈值则判断必定不属于同一个消影点
angleDev = abs( lineInfos[i].orientation - lineInfos[j].orientation );
angleDev = min( CV_PI - angleDev, angleDev );
if ( angleDev > angelTolerance )
{
continue;
}
// 向对应极坐标网格内增加响应数值，长度越长，角度偏差适中的,权重越高
sphereGrid[LA][LO] += sqrt( lineInfos[i].length * lineInfos[j].length ) * ( sin( 2.0 * angleDev ) + 0.2 ); // 0.2 is much robuster
}
}
极坐标网格
ϕ
\phi
ϕ 和
λ
\lambda
λ 的范围分别为
[
0
,
π
/
2
]
[0,\pi/2]
[0,π/2] 和
[
0
,
2
π
]
[0,2\pi]
[0,2π]。因此，极坐标网格
G
\mathcal{G}
G 可以通过以下步骤来构建：
- 以 1 ∘ 1^\circ 1∘ 为间隔，把 G \mathcal{G} G 初始化为 90 × 360 90\times360 90×360 的零空间，i.e. G ( i , j ) = 0 \mathcal{G}(i,j)=0 G(i,j)=0 for i = 1 , 2 , . . . , 90 i=1,2,...,90 i=1,2,...,90 和 j = 1 , 2 , . . . , 360 j=1,2,...,360 j=1,2,...,360。
- 对每个图像上的线段对 l 1 l_1 l1和 l 2 l_2 l2, 计算出交叉点 p \textbf{p} p 和相应的经纬度 ( ϕ , λ ) (\phi, \lambda) (ϕ,λ)。
- 更新网格数值：
G ( ϕ d e g , λ d e g ) = G ( ϕ d e g , λ d e g ) + ∥ l 1 ∥ × ∥ l 2 ∥ × sin ( 2 θ ) \mathcal{G}(\phi_{deg}, \lambda_{deg})=\mathcal{G}(\phi_{deg}, \lambda_{deg})+\|\textbf{l}_1\|\times\|\textbf{l}_2\|\times\text{sin}(2\theta) G(ϕdeg,λdeg)=G(ϕdeg,λdeg)+∥l1∥×∥l2∥×sin(2θ) ϕ d e g \phi_{deg} ϕdeg和 λ d e g \lambda_{deg} λdeg是 ( ϕ , λ ) (\phi, \lambda) (ϕ,λ) 对应的边界， θ \theta θ 是小的那个角度偏差。此公式的设计目的是为了给予那些长度更长、方向偏差适中的线段组合更高权重，才更有可能属于同一个消影点。
Tips
极坐标网格的构建，本质是预先建立一种加权投票机制，正确的消影点必定其对应的网格数值越高，这样在后续验证 37800 个消影点假设的时候，就可以迅速的以查表的方式找出网格响应最大、也就是最为符合的一组消影点。
最后又做了一个3x3的高斯平滑滤波器以去除噪声，达到更鲁棒精确的检测效果。
//
int halfSize = 1;
int winSize = halfSize * 2 + 1;
int neighNum = winSize * winSize;
// get the weighted line length of each grid
std::vector< std::vector<double> > sphereGridNew = std::vector< std::vector<double> >( gridLA, std::vector<double>(gridLO) );
for ( int i=halfSize; i<gridLA-halfSize; ++i )
{
for ( int j=halfSize; j<gridLO-halfSize; ++j )
{
double neighborTotal = 0.0;
for ( int m=0; m<winSize; ++m )
{
for ( int n=0; n<winSize; ++n )
{
neighborTotal += sphereGrid[i-halfSize+m][j-halfSize+n];
}
}
sphereGridNew[i][j] = sphereGrid[i][j] + neighborTotal / neighNum;
}
}
sphereGrid = sphereGridNew;
}
getBestVpsHyp
将验证之前产生的所有消影点假设。
{
int num = vpHypo.size();
double oneDegree = 1.0 / 180.0 * CV_PI;
// get the corresponding line length of every hypotheses
std::vector<double> lineLength( num, 0.0 );
for ( int i = 0; i < num; ++ i )
{
std::vector<cv::Point2d> vpLALO( 3 );
for ( int j = 0; j < 3; ++ j )
{
if ( vpHypo[i][j].z == 0.0 )
{
continue;
}
if ( vpHypo[i][j].z > 1.0 || vpHypo[i][j].z < -1.0 )
{
cout<<1.0000<<endl;
}
//转换为极坐标，找到对应网格
double latitude = acos( vpHypo[i][j].z );
double longitude = atan2( vpHypo[i][j].x, vpHypo[i][j].y ) + CV_PI;
int gridLA = int( latitude / oneDegree );
if ( gridLA == 90 )
{
gridLA = 89;
}
int gridLO = int( longitude / oneDegree );
if ( gridLO == 360 )
{
gridLO = 359;
}
//计算对应的lineLength，也就是响应数值
lineLength[i] += sphereGrid[gridLA][gridLO];
}
}
// get the best hypotheses 找到响应最大的一组假设
int bestIdx = 0;
double maxLength = 0.0;
for ( int i = 0; i < num; ++ i )
{
if ( lineLength[i] > maxLength )
{
maxLength = lineLength[i];
bestIdx = i;
}
}
vps = vpHypo[bestIdx];
}
lines2Vps
将根据各线段与三个不同消影点的角度偏差来进行分类。
{
clusters.clear();
clusters.resize( 3 );
//get the corresponding vanish points on the image plane
//获取消影点在像素坐标系下的投影坐标。
std::vector<cv::Point2d> vp2D( 3 );
for ( int i = 0; i < 3; ++ i )
{
vp2D[i].x = vps[i].x * f / vps[i].z + pp.x;
vp2D[i].y = vps[i].y * f / vps[i].z + pp.y;
}
for ( int i = 0; i < lines.size(); ++ i )
{
double x1 = lines[i][0];
double y1 = lines[i][1];
double x2 = lines[i][2];
double y2 = lines[i][3];
//计算线段的中点
double xm = ( x1 + x2 ) / 2.0;
double ym = ( y1 + y2 ) / 2.0;
double v1x = x1 - x2;
double v1y = y1 - y2;
double N1 = sqrt( v1x * v1x + v1y * v1y );
v1x /= N1; v1y /= N1;
double minAngle = 1000.0;
int bestIdx = 0;
for ( int j = 0; j < 3; ++ j )
{
//从线段中点引出一条到消影点的线段
double v2x = vp2D[j].x - xm;
double v2y = vp2D[j].y - ym;
double N2 = sqrt( v2x * v2x + v2y * v2y );
v2x /= N2; v2y /= N2;
double crossValue = v1x * v2x + v1y * v2y;
if ( crossValue > 1.0 )
{
crossValue = 1.0;
}
if ( crossValue < -1.0 )
{
crossValue = -1.0;
}
// |a||b|cos<a,b>,获取线段与该消影点的角度偏差
double angle = acos( crossValue );
angle = min( CV_PI - angle, angle );
//找到偏差最小的那个消影点
if ( angle < minAngle )
{
minAngle = angle;
bestIdx = j;
}
}
//角度偏差小于阈值，则判定分类有效。
if ( minAngle < thAngle )
{
clusters[bestIdx].push_back( i );
}
}
}
角度偏差的定义如图所示。最终，我们把图像线段根据三个消影点进行了分类。
最终可达成的效果为下图所示，红圈位置为能在像素平面看到的消影点，并基于检测到的三个消影点对图像线段进行了分类。
存在问题
不过在EuRoc数据集的MH_03_mediam的测试中，若想要实时进行消影点检测并用于定位的辅助信息，仍然存在有以下几个方面的问题需要解决：
- 线段分类错误，比如消影点附近的一条竖直线，被识别成了水平线；同一块木板的两侧边缘被分类给了不同消影点（红、绿）。
- 只能区分出三类线段，但由于第一个消影点假设的生成是随机抽取的，因此无法特定的给某个消影点所属的线段进行分类。
同一水平线即分类成了绿色也分类成了红色。
解决办法：属于竖直方向的结构线段可以直接确定并固定，对于水平方向的两个消影点方向的结构线段，可以采取在系统初始化的时候就进行固定，即按第一帧的消影点排列顺序进行分类固定，递归的对后续单帧生成的消影点进行重新排列组合。效果如下图所示。
扫一扫
5381
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
