Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记

VanishingPointDetection 代码学习整理

  • main.cpp
  • VPDetection.h
  • VPDetection.cpp
    • run()
    • getVPHypVia2Lines
    • getSphereGrids
    • getBestVpsHyp
    • lines2Vps
  • 存在问题

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 cxcy
  • double f,对应相机内参中的 f = f x = f y f=f_x = f_y f=fx=fy
  • std::vector 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(10.9999)/log(1P)=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
Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第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+k2cot(ϕ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 >( 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 ∘ 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
  2. 对每个图像上的线段对 l 1 l_1 l1 l 2 l_2 l2, 计算出交叉点 p \textbf{p} p 和相应的经纬度 ( ϕ , λ ) (\phi, \lambda) (ϕ,λ)
  3. 更新网格数值:
    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,获取线段与该消影点的角度偏差
			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 );
		}
	}
}

角度偏差的定义如图所示。最终,我们把图像线段根据三个消影点进行了分类。
Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第2张图片

最终可达成的效果为下图所示,红圈位置为能在像素平面看到的消影点,并基于检测到的三个消影点对图像线段进行了分类。

存在问题

不过在EuRoc数据集的MH_03_mediam的测试中,若想要实时进行消影点检测并用于定位的辅助信息,仍然存在有以下几个方面的问题需要解决:

  1. 线段分类错误,比如消影点附近的一条竖直线,被识别成了水平线;同一块木板的两侧边缘被分类给了不同消影点(红、绿)。
    Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第3张图片
  2. 只能区分出三类线段,但由于第一个消影点假设的生成是随机抽取的,因此无法特定的给某个消影点所属的线段进行分类。
    Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第4张图片
    同一水平线即分类成了绿色也分类成了红色。
    Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第5张图片

解决办法:属于竖直方向的结构线段可以直接确定并固定,对于水平方向的两个消影点方向的结构线段,可以采取在系统初始化的时候就进行固定,即按第一帧的消影点排列顺序进行分类固定,递归的对后续单帧生成的消影点进行重新排列组合。效果如下图所示。

Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第6张图片
Vanishing Point Detection 消影点/消失点/灭点检测代码学习整理笔记_第7张图片


新建了一个点线SLAM/结构化SLAM交流群,可以私信我加入,欢迎新人同学讨论和行业大佬指导

你可能感兴趣的:(资料学习,深度学习,python,人工智能)