pcl粒子群PSO计算平面度

        在求平面度的时候,LS对于数据量小的点云,比较适合.当数据量变大的时候,达到千万级的时候,LS就会崩,他所需要的矩阵会溢出,最后求出非常离谱的数值.这里采用粒子群来求超大数据量的点云平面度.

设平面方程

点到直线的距离为

平面度:

平面度与c无关,与a,b有关

所以在该问题下粒子维度是2维的.适应度函数是

粒子群部分:

1,初始化种群参数,粒子群规模,并随机生成a,b的值,因为平面点云数据中心化过,所以a,b的值尽量生成的小一点,我限制在(-1,1)之间;初始化迭代次数,惯性权重(我设置的是一个范围,后续惯性权重不是一成不变的,是非线性变化的).

2,初始化每个粒子的位置和速度.

3,迭代:

        计算每个粒子所对应的平面度,也就是适应值.

//计算适应度(输入矩阵版),目前未用
VectorXf PSO::fitness_cal(MatrixXf group_a_b) {
		
	int groupsize = group_a_b.cols();
	group_a_b.colwise().normalize();
	MatrixXf id_xyz_centroid = xyz_id_centroid.transpose();             //n*3
	MatrixXf distance_all = id_xyz_centroid * group_a_b;                //n*p = n*3   *  3*p
	VectorXf fitness_s = VectorXf::Random(group_a_b.cols());
	for (int i = 0; i < groupsize; i++) {
		fitness_s(i) = distance_all.col(i).maxCoeff() - distance_all.col(i).minCoeff();
	}
	return fitness_s;
}

//计算适应度(输入nodePSO版本)
float PSO::fitness_cal_node(nodePSO& node) {
	Vector3f plane = node.pos;
	plane.normalize();
	MatrixXf fitnesss = plane.transpose() * xyz_id_centroid;
	return fitnesss.maxCoeff() - fitnesss.minCoeff();
}

        更新每个粒子的速度和位置.

void PSO::move_node_pos(nodePSO& node)
{
	float r1 = random_N(0, 1), r2 = random_N(0, 1);
	//cout << r1 << " " << r2 << " " << node.w << " " << c1 << " " << c2 << endl;
	node.speed = node.w * node.speed + c1 * r1 * (node.pos_best - node.speed) + c2 * r2 * (pos_group_best - node.speed);
	node.pos += node.speed;
	node.pos /= -node.pos(2);
	//node.pos(2) = -1;
	node.pos_history.push_back(node.pos);
	//cout << "旧:" << endl << node.pos_history[node.pos_history.size() - 2]<< endl << "新:" << endl << node.pos << endl;

}

在这个过程中我们要记录/更新:粒子个体历史最优位置及最优适应值,群体历史最优位置及最优适应值.

void PSO::move_node_fitness(nodePSO &node) {//适应度计算+个体最好适应度的值和位置更新
	node.fitness_node = fitness_cal_node(node);
	//cout << node.fitness_node << endl;
	//个体最优更新,群体最优更新
	if (node.fitness_node < node.fitness_node_best) {
		node.fitness_node_best = node.fitness_node;
		node.pos_best = node.pos;
		if (node.fitness_node_best < fitness_group_best) {
			fitness_group_best = node.fitness_node_best;
			pos_group_best = node.pos_best;
		}
	}
	node.fitness_history.push_back(node.fitness_node_best);
	//cout << node.fitness_history.back() << endl;
}

        更新其他参数,我更新了w,c1,c2(这里因人而异)

        重复3

void PSO::PSO_w_nonlinear_process() {
	Wmax = 0.9;
	Wmin = 0.2;
#ifdef  time_lxr
	pcl::console::TicToc time; time.tic();
#endif
	for (temp_id = epoch - 1; temp_id >= 0; temp_id--) {
#ifdef c1_c2_process
		if (temp_id % 30 == 15) {
			c1 *= 0.8;
			c2 *= 0.8;
		}
#endif
		for (auto& node : group_node) {
			move_node_pos(node);
			move_node_fitness(node);
		}
		move_group_w();
	}
#ifdef  time_lxr
	std::cout << "PSO_w用时：" << time.toc() << "ms" << endl;
#endif

}

4,迭代结束,输出最优解,输出最优解的平面度(适应度),看需求画图

实验一下,当数据量在百万级的时候,LS要快于PSO.当数据量在千万级的时候,PSO远远快于LS.