补充一篇点云处理和优化粒子群算法~

批注：代码不全，但是部分实现，由于项目方向走错了，所以这段代码就遗弃了，怪可惜的，但是可以参考。部分代码“抄袭”于github，侵权删

具体的目的是想要从已知模型的测量数据点云中，拟合出每个小曲面的法向量，通过法向量进行坐标最佳匹配，（类似三坐标测量仪），进而对该工件进行位姿测量

具体懒得改了，能参考就参考，不能就算了~

.h:
#define PI 3.14159265358979323846
#define POPSIZE 20                         //粒子个数
#define MAXINTERATION 2000                 //最大迭代次数
#define NVARS 10                           //参数个数
#define WMAX 0.9                           //惯量权重最大值
#define WMIN 0.4                           //惯量权重最小值

struct particle {                          //单个粒子
	double pBest[NVARS];//局部极值
	double v[NVARS];//速度
	double x[NVARS];
	double upper[NVARS];
	double lower[NVARS];
};

double w;                                  //惯量权重
double c1 = 2.0;                           //加速因子1
double c2 = 2.0;                           //加速因子2
double absbound;                           //上下界绝对值
double vmax;                               //最大速度
double gBest[NVARS];                       //全局最优解
particle particles[POPSIZE];               //粒子群

double evalfunc(double[], int);            //评估函数
double avg(double[], int);                 //求平均数
double stddev(double[], int);              //求标准差
void initialize(int);                      //初始化函数
double randval(double, double);            //求范围（lower，upper）之间的随机数
void evaluate(int);                        //评估粒子适应值
void update(int, int);                     //利用更新公式更新每个粒子的速度和位置以及历史最优位置
void fit(void);                            //更新群体历史最优位置



.cpp：
#include"标头.h"
#include <cstring>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <ctime>
#include <iostream>
#include <iomanip>

using namespace std;


double evalfunc(double parameter[], int FUNC = 1) {
	/*          10个评估函数            */
	/* 通过参数FUNC来选择想要的评估函数 */
	if (FUNC == 1) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			val += parameter[i] * parameter[i];
		}
		return val;
	}

	if (FUNC == 2) {
		double val1 = 0;
		double val2 = 0;
		for (int i = 0; i < NVARS; i++) {
			val1 += abs(parameter[i]);
			val2 = val2*abs(parameter[i]);
		}
		return (val1 + val2);
	}

	if (FUNC == 3) {
		double val1 = 0;
		int i, j;
		for (i = 0; i < NVARS; i++) {
			double val2 = 0;
			for (j = 0; j < i; j++) {
				val2 += parameter[j];
			}
			val1 += val2*val2;
		}
		return val1;
	}

	if (FUNC == 4) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			if (abs(parameter[i]) > val)
				val = abs(parameter[i]);
		}
	}

	if (FUNC == 5) {
		double val = 0;
		for (int i = 0; i < NVARS - 1; i++) {
			val += 100 * (parameter[i + 1] - parameter[i] * parameter[i])*(parameter[i + 1] - parameter[i] * parameter[i]) + (parameter[i] - 1)*(parameter[i] - 1);
		}
		return val;
	}

	if (FUNC == 6) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			val += floor(parameter[i] + 0.5)*floor(parameter[i] + 0.5);
		}
		return val;
	}

	if (FUNC == 7) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			val += i*pow(parameter[i], double(i));
		}
		return (val + randval(0, 1));
	}

	if (FUNC == 8) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			val += (-(parameter[i] * sin(sqrt(abs(parameter[i])) / 180 * PI)));
		}
		return val;
	}

	if (FUNC == 9) {
		double val = 0;
		for (int i = 0; i < NVARS; i++) {
			val += (parameter[i] * parameter[i] - 10 * cos(2 * PI*parameter[i] / 180 * PI) + 10.0);
		}
		return val;
	}

	if (FUNC == 10) {
		double val1 = 0;
		double val2 = 0;
		for (int i = 0; i < NVARS; i++) {
			val1 += parameter[i] * parameter[i];
			val2 += cos(2 * PI*parameter[i] / 180 * PI);
		}
		return ((-20)*exp((-0.2)*sqrt(val1 / NVARS)) - exp(val2 / NVARS) + 20 + exp(1));
	}
	return 0;
}
double avg(double parameter[], int n) {
	double num = 0;
	for (int i = 0; i < n; i++) {
		num += parameter[i];
	}
	return num / n;
}
double stddev(double parameter[], int n) {
	double num = avg(parameter, n);
	double sum = 0.0;
	for (int i = 0; i < n; i++) {
		sum += (parameter[i] - num)*(parameter[i] - num);
	}
	return sqrt(sum / n);
}


double randval(double low, double high) {
	return (low + (high - low)*rand()*1.0 / RAND_MAX);
}
void initialize(int FUNC = 1) {
	/*  初始化每个粒子的速度、位置并将  */
	/*  该位置设定为当前历史最优位置找  */
	/*  到所有粒子的最优位置并设定为当  */
	/*  前群体历史最优位置              */
	int i, j;
	double lbound[NVARS];
	double ubound[NVARS];

	for (i = 0; i < NVARS; i++) {
		lbound[i] = (-absbound);
		ubound[i] = absbound;
	}

	for (i = 0; i < NVARS; i++) {
		for (j = 0; j < POPSIZE; j++) {
			particles[j].upper[i] = ubound[i];
			particles[j].lower[i] = lbound[i];
			particles[j].v[i] = randval(lbound[i], ubound[i]);
			particles[j].x[i] = randval(lbound[i], ubound[i]);
			particles[j].pBest[i] = particles[j].x[i];
		}
	}

	double pval = evalfunc(particles[0].pBest);
	int num;
	for (i = 1; i < POPSIZE; i++) {
		if (pval > evalfunc(particles[i].pBest)) {
			pval = evalfunc(particles[i].pBest);
			num = i;
		}
		else continue;
	}
	for (j = 0; j < NVARS; j++) {
		gBest[j] = particles[num].pBest[j];
	}
}
void evaluate(int FUNC = 1) {
	/*  通过传入参数FUNC来调用不同的评  */
	/*  估函数并对粒子位置进行评估并更  */
	/*  新粒子历史最优位置              */
	int i, j;
	double pval[POPSIZE], nval[POPSIZE];

	for (i = 0; i < POPSIZE; i++) {
		pval[i] = evalfunc(particles[i].pBest, FUNC);
		nval[i] = evalfunc(particles[i].x, FUNC);

		if (pval[i] > nval[i]) {
			for (j = 0; j < NVARS; j++) {
				particles[i].pBest[j] = particles[i].x[j];
			}
		}
	}
}
void update(int interation, int w_change_method = 1) {
	/*  通过参数w_change_method来选择不 */
	/*  同的惯性权重衡量规则来根据群体  */
	/*  历史最优位置更新粒子速度以及位置*/
	int i, j;
	double v, x;

	if (w_change_method == 1) {
		w = WMAX - (WMAX - WMIN) / MAXINTERATION*(double)interation;
	}
	else if (w_change_method == 2) {
		w = randval(0.4, 0.6);
	}
	else if (w_change_method == 3) {
		w = WMAX - (WMAX - WMIN)*((double)interation / MAXINTERATION)*((double)interation / MAXINTERATION);
	}
	else if (w_change_method == 4) {
		w = WMIN + (WMAX - WMIN)*((double)interation / MAXINTERATION - 1)*((double)interation / MAXINTERATION - 1);
	}
	else {
		cout << "Dont have this weight change method!";
		return;
	}

	for (i = 0; i < NVARS; i++) {
		for (j = 0; j < POPSIZE; j++) {
			v = w*particles[j].v[i] + c1*randval(0, 1)*(particles[j].pBest[i] - particles[j].x[i]) + c2*randval(0, 1)*(gBest[i] - particles[j].x[i]);
			if (v > vmax)particles[j].v[i] = vmax;
			else if (v < (-vmax))particles[j].v[i] = (-vmax);
			else particles[j].v[i] = v;

			x = particles[j].x[i] + particles[j].v[i];
			if (x > particles[j].upper[i])particles[j].x[i] = particles[j].upper[i];
			else if (x < particles[j].lower[i])particles[j].x[i] = particles[j].lower[i];
			else particles[j].x[i] = x;
		}
	}
}
void fit(void) {
	/*         更新群体最优位置         */
	int i, j;
	double gval = evalfunc(gBest);
	double pval[POPSIZE];

	for (i = 0; i < POPSIZE; i++) {
		pval[i] = evalfunc(particles[i].pBest);

		if (gval > pval[i]) {
			for (j = 0; j < NVARS; j++) {
				gBest[j] = particles[i].pBest[j];
			}
		}
	}
}
int main() {
	srand((unsigned)time(NULL));//随机数发生器初始化
	double all_gBest[100];//全局最优解
	int  j;
	int FUNC;
	int w_change_method;//权重因子变化方法
	//std::cout << "*********************************************************************************************************************************" << endl;
	//cout << "*******************************  十个" << NVARS << "维函数用4种不同的权重控制方法运行一百次的平均结果及标准差" << "  *******************************" << endl;
	//cout << "                线性递减法" << "                      随机权重法" << "                      凹函数递减法" << "                    凸函数递减法" << endl;
	//cout << " 函数" << endl;
	//cout << "           平均值" << "          标准差"
		//<< "          平均值" << "          标准差"
		//<< "          平均值" << "          标准差"
		//<< "          平均值" << "          标准差";
	//cout << endl << endl;
	for (FUNC = 1; FUNC <= 10; FUNC++) {
		cout << " " << std::left << "f" << setw(4) << FUNC;
		switch (FUNC)
		{
		case 1:absbound = 100.0; break;
		case 2:absbound = 10.0; break;
		case 3:absbound = 100.0; break;
		case 4:absbound = 100.0; break;
		case 5:absbound = 30.0; break;
		case 6:absbound = 100.0; break;
		case 7:absbound = 1.28; break;
		case 8:absbound = 500.0; break;
		case 9:absbound = 5.12; break;
		case 10:absbound = 32.0; break;
		default:
			break;
		}
		vmax = absbound*0.15;
		for (w_change_method = 1; w_change_method <= 4; w_change_method++) {
			for (j = 0; j < 100; j++) {
				initialize();
				evaluate(FUNC);
				for (int i = 0; i < MAXINTERATION; i++) {
					update(i, w_change_method);
					evaluate(FUNC);
					fit();
				}
				double evalue = evalfunc(gBest);
				if (evalue == 0) {
					j--;
					continue;
				}
				all_gBest[j] = evalue;
			}
			cout << setw(16) << avg(all_gBest, 100);
			cout << setw(16) << stddev(all_gBest, 100);
		}
		cout << endl;
	}
	cout << "*********************************************************  算法运行结束  ********************************************************" << endl;
	cout << "*********************************************************************************************************************************" << endl;
	system("pause");
}