Zhiyuan LIU

2. 核心算法

2.1 起始

随机采样一致（Random SAmple Consensus）的实质是随机选取一些能够建立所需模型的样本点，构建模型，并判断该模型是否满足需求，循环重复直至满足退出条件。这里的目的是从点云中拟合平面，所以取三个随机点，并计算平面。

首先，从点云中选取一定数量的子集作为平面的候选点。为了在重复迭代时节省时间，这里使用原数据的一个随机子集进行计算，减少计算量。子集的点的数量将影响计算速度。

// pick a batch of n vertices randomly from the vertex slice
vertexBatch := make([]plyfile.VertexMono, 0)
indexBatch := make([]int, 0)
for {
	indexRandom := r.Intn(len(vlist))

	// check if this vertex is already chosen. If not, append the vertex and its index to the batch
	if mymath.ExistIntList(indexBatch, indexRandom) {...}
	vertexBatch = append(vertexBatch, vlist[indexRandom])
	indexBatch = append(indexBatch, indexRandom)
	// end condition
	if len(vertexBatch) == volumeBatch {...}
}

2.2 RANSAC迭代

随机选取三个点并形成平面，计算上一步的子集中的所有点和此平面的距离，如果距离小于给定值则记为内点，否则记为外点。如果内点的数量达到一定比例，则认为此平面存在，放入验证组。如果没有达到此比例，则重新选择三个点并计算。如果达到一定迭代次数仍未找到拥有内点的数量达到一定比例的平面，则返回拥有内点数量最多的平面。

// pick 3 vertices randomly
	p1 := 0
	p2 := 0
	p3 := 0
	// check if the 3 vertices are different one from another
	for {...}

	// calculate the inlines for the plane formed by these 3 vertices (if the distance to the plane is inferior to the given threshold, this point would be regarded as inline) and take the score
	p := New_p_by_verticesMono(vlist[p1], vlist[p2], vlist[p3])
	for _, vertex := range vlist {
		if mymath.DistPointPlane(vertex.Ply_x, vertex.Ply_y, vertex.Ply_z, p.A, p.B, p.C, p.D) < minDistance {...}
	}
	NumIteration += 1

	// update the score (inline rate). If there is a higher score, note down the plane and 3 vertices which forms it
	score := InlineCount / float64(n)
	if score > highscore {...}

	// quit condition : the high score surpass the given parameter, or the number of iteration reach the maximal given in parameter too
	if NumIteration > minNforLoop && highscore > minScore || NumIteration >= maxNforLoop {...}
}

2.3 全局拟合

计算上一步中得到的平面到所有点的距离，将小于给定距离的点记为全局内点，使用所有全局内点通过最小二乘法拟合一个新的平面，并将此平面放入平面池，同时将全局内点移出下次迭代使用的数据集。

// Fit several points to x plane, using the least square method by solving a linear system Ax=b
. A : matrix of the size n rows and 3 columns. Rows are (xi, yi, 1) in which xi and yi are coordinates x and y of the points
A := mat.NewDense(n, 3, nil)
// B : matrix of the size n rows and 1 columns, (zi), the coordinates z of the points
b := mat.NewDense(n, 1, nil)
// initializing
for i := 0; i < n; i++ {
	A.Set(i, 0, vlist[i].Ply_x)
	A.Set(i, 1, vlist[i].Ply_y)
	A.Set(i, 2, 1)
	b.Set(i, 0, vlist[i].Ply_z)
}

//  compute : A * x = b --> ATA * x = AT * b --> x = (ATA)^-1 * AT * b

AT := A.T()
// multiply A by AT at the left side to prepare for the inverse
var ATA mat.Dense
ATA.Mul(AT, A)

// compute the inverse
var ATAInv mat.Dense
err := ATAInv.Inverse(&ATA)
if err != nil {
	log.Fatalf("A is not invertible: %v", err)
}

var ATAInvAT mat.Dense
ATAInvAT.Mul(&ATAInv, AT)
var X mat.Dense
X.Mul(&ATAInvAT, b)

P := New_p(X.At(0, 0), X.At(1, 0), -1, X.At(2, 0))
return P

2.4 重复检测

检测是否存在重复的平面。对于所有平面池内的平面，两两计算夹角，如夹角小于给定值，则视为平行，计算平面距离，如再小于给定值，则确认重复。使用两平面所有的内点以最小二乘法拟合出新的平面，替代两平面放入平面池。

// check that if the new plane belongs to the same planar facade as one of the planes already obtained. If it is the case, mix the vertices and re-adjust the two planes using least square method
if len(planes) > 0 {
	for index, planeObtained := range planes {
		// first check : if the angle formed by the two planes are inferior to the given value
		// second check : if the average distance from the points forming the old plane and the new plane is inferior to the given value
		if mymath.VectorsAngle32(planeObtained.A, planeObtained.B, planeObtained.C, newP.A, newP.B, newP.C) < maxAnglePlanes && planeObtained.DistAvrPointPlaneMono32(vetexInline) < 4 * maxDistance {...}
	}
}

// if vetexInline != nil (empty slice) means that no similar planes found, we add the new plane to the slice. Otherwise there was a similar plane
if vetexInline != nil {...}

// remove the inline points from the original slice, color these vertices and move them to a new slice to prepare for the painting
for i, index := range indexInline {
	vlist[index], vlist[len(vlist) - 1 - i] = vlist[len(vlist) - 1 - i], vlist[index]
}

vlist = append(vlist[:0], vlist[0:len(vlist) - inlineCount]...)

2.5 退出条件

迭代以上所有步骤，直到迭代次数大于给定值或剩余外点数量小于给定值，返回平面数量，所有平面方程和剩余外点数量。

// decide whether the iteration continues
numIter++
if len(vlist) < maxVtoQuit || numIter > iterMax {
	ifContinue = false
}
return planes, len(planes), verticesOfPlanes

2.6 参数

主要参数如下，需要根据点云的类型手动调整。

func PlaneMonoConsecRANSAC32(
	vlist []plyfile.VertexMono,	//所有的点（全集）
	maxDistance float32, 		//平面与内点的最大距离
	minScoreforRAN float32,		//平面的内点在子集中的最低比例
	minVforPlane int,			//一个平面在全集中最低内点数量
	maxAnglePlanes float32,		//平行平面的最大夹角
	maxVtoQuit int,				//程序结束时不属于任何平面的点的最大数量
	iterMax int,				//程序结束前的最多迭代次数
	volumeBatch int 			//构成子集的点的数量
	) ([]Plane32, int, [][]plyfile.VertexMono)

例如 ：

Planes, numPlanes, _ := calculator.PlaneMonoConsecRANSAC32(vlist, 0.003,
																0.1,
																500,
																0.1,
																50,
																80,
																500)