main.cpp 22.6 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/*!
 * \page PS-CMA-ES Particle swarm CMA Evolution strategy
 *
 *
 * [TOC]
 *
 *
 * # Optimization # {#Opti_cma_es}
 *
 *
 * In this example we show how to code PS-CMA-ES. This is just a simple variation to the
 * CMA-ES, where you have multiple CMA-ES running. The the best solution across them is
 * used to produce a drift velocity toward that point.
 *
 *
 *
 */

incardon's avatar
incardon committed
19
#define EIGEN_USE_LAPACKE
20
21
22
23
24
25
#include "Vector/vector_dist.hpp"
#include "Eigen/Dense"
#include <Eigen/Eigenvalues>
#include <Eigen/Jacobi>
#include <limits>
#include "Vector/vector_dist.hpp"
incardon's avatar
incardon committed
26
27
#include <f15_cec_fun.hpp>
#include <boost/math/special_functions/sign.hpp>
28

incardon's avatar
incardon committed
29
30

constexpr int dim = 3;
31
// set this to 4+std::floor(3*log(dim))
incardon's avatar
incardon committed
32
constexpr int lambda = 7;
33
constexpr int mu = lambda/2;
incardon's avatar
incardon committed
34
constexpr int hist_size = 21;
35
36
37
38
39
40
41
42
43

constexpr int sigma = 0;
constexpr int Cov_m = 2;
constexpr int B = 3;
constexpr int D = 4;
constexpr int Zeta = 5;
constexpr int path_s = 6;
constexpr int path_c = 7;
constexpr int ord = 8;
incardon's avatar
incardon committed
44
45
46
47
48
49
50
constexpr int stop = 9;
constexpr int fithist = 10;
constexpr int weight = 11;
constexpr int validfit = 12;
constexpr int xold = 13;
constexpr int last_restart = 14;
constexpr int iniphase = 15;
51
52
53
54
55
56

const double c_m = 1.0;

double mu_eff = 1.0;
double cs = 1.0;
double cc = 1.0;
incardon's avatar
incardon committed
57
double ccov = 1.0;
58
59
60
61
double chiN;
double d_amps = 1.0;
double stop_fitness = 1.0;
int eigeneval = 0;
incardon's avatar
incardon committed
62
63
64
65
66
67
68
double t_c = 0.1;
double b = 0.1;
double psoWeight = 0.7;
// number of cma-step before pso step
int N_pso = 200;
double stopTolX = 1e-12;
double stopTolUpX = 2000.0;
69
70
71
72
73
74
75
76
77

typedef vector_dist<dim,double, aggregate<double,
										 Eigen::VectorXd[lambda],
										 Eigen::MatrixXd,
										 Eigen::MatrixXd,
										 Eigen::DiagonalMatrix<double,Eigen::Dynamic>,
										 Eigen::VectorXd[lambda],
										 Eigen::VectorXd,
										 Eigen::VectorXd,
incardon's avatar
incardon committed
78
79
80
81
82
83
84
85
										 int[lambda],
										 int,
										 double [hist_size],
										 double [dim],
										 double,
										 Eigen::VectorXd,
										 int,
										 bool> > particle_type;
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108


double generateGaussianNoise(double mu, double sigma)
{
	static const double epsilon = std::numeric_limits<double>::min();
	static const double two_pi = 2.0*3.14159265358979323846;

	thread_local double z1;
	thread_local double generate;
	generate = !generate;

	if (!generate)
	{return z1 * sigma + mu;}

	double u1, u2;
	do
	{
	   u1 = rand() * (1.0 / RAND_MAX);
	   u2 = rand() * (1.0 / RAND_MAX);
	}
	while ( u1 <= epsilon );

	double z0;
incardon's avatar
incardon committed
109
110
	z0 = sqrt(-2.0 * log(u2)) * cos(two_pi * u1);
	z1 = sqrt(-2.0 * log(u2)) * sin(two_pi * u1);
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
	return z0 * sigma + mu;
}

template<unsigned int dim>
Eigen::VectorXd generateGaussianVector()
{
	Eigen::VectorXd tmp;
	tmp.resize(dim);

	for (size_t i = 0 ; i < dim ; i++)
	{
		tmp(i) = generateGaussianNoise(0,1);
	}

	return tmp;
}

template<unsigned int dim>
void fill_vector(double (& f)[dim], Eigen::VectorXd & ev)
{
	for (size_t i = 0 ; i < dim ; i++)
	{ev(i) = f[i];}
}

void fill_vector(const double * f, Eigen::VectorXd & ev)
{
	for (size_t i = 0 ; i < ev.size() ; i++)
	{ev(i) = f[i];}
}

struct fun_index
{
	double f;
	int id;

	bool operator<(const fun_index & tmp)
	{
		return f < tmp.f;
	}
};

double wm[mu];

void init_weight()
{
	for (size_t i = 0 ; i < mu ; i++)
incardon's avatar
incardon committed
157
	{wm[i] = log(mu+1.0) - log(i+1);}
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178

	double tot = 0.0;

	for (size_t i = 0 ; i < mu ; i++)
	{tot += wm[i];}

	double sum = 0.0;
	double sum2 = 0.0;

	for (size_t i = 0 ; i < mu ; i++)
	{
		wm[i] /= tot;
		sum += wm[i];
		sum2 += wm[i]*wm[i];
	}

	// also set mu_eff
    mu_eff=sum*sum/sum2;

}

incardon's avatar
incardon committed
179
double weight_sample(int i)
180
181
182
183
{
	return wm[i];
}

incardon's avatar
incardon committed
184
185

void create_rotmat(Eigen::VectorXd & S,Eigen::VectorXd & T, Eigen::MatrixXd & R)
186
{
incardon's avatar
incardon committed
187
188
189
190
	Eigen::VectorXd S_work(dim);
	Eigen::VectorXd T_work(dim);
	Eigen::VectorXd S_sup(dim);
	Eigen::VectorXd T_sup(dim);
191

incardon's avatar
incardon committed
192
193
194
195
196
197
198
	Eigen::MatrixXd R_tar(dim,dim);
	Eigen::MatrixXd R_tmp(dim,dim);
	Eigen::MatrixXd R_sup(dim,dim);
	double G_S,G_C;
	Eigen::MatrixXd S_tmp(2,2);
	Eigen::MatrixXd T_tmp(2,2);
	int p,q,i;
199

incardon's avatar
incardon committed
200
201
	S_work = S;
	T_work = T;
202

incardon's avatar
incardon committed
203
204
205
	R.setIdentity();
	R_tar = R;
	R_tmp = R;
206

incardon's avatar
incardon committed
207
	for (p = dim - 2; p >= 0 ; p -= 1)
208
209
	{

incardon's avatar
incardon committed
210
		for (q = dim - 1 ; q >= p+1 ; q-= 1)
211
		{
incardon's avatar
incardon committed
212
213
214
215
			T_tmp(0) = T_work(p);
			T_tmp(1) = T_work(q);
			S_tmp(0) = S_work(p);
			S_tmp(1) = S_work(q);
216

incardon's avatar
incardon committed
217
			// Perform Givens Rotation on start vector
218

incardon's avatar
incardon committed
219
220
221
			Eigen::JacobiRotation<double> G;
			double z;
			G.makeGivens(S_tmp(0), S_tmp(1),&z);
222

incardon's avatar
incardon committed
223
224
225
226
			// Check direction of rotation
			double sign = 1.0;
			if (z < 0.0)
			{sign = -1.0;}
227

incardon's avatar
incardon committed
228
229
230
231
232
233
			// Build a Rotation Matrix out of G_C and G_S
			R_tmp.setIdentity();
			R_tmp(p,p) = sign*G.c();
			R_tmp(q,q) = sign*G.c();
			R_tmp(p,q) = sign*-G.s();
			R_tmp(q,p) = sign*G.s();
234

incardon's avatar
incardon committed
235
236
			// Rotate start vector and update R
			// S_work = R_tmp*S_work
237

incardon's avatar
incardon committed
238
239
240
			S_work = R_tmp*S_work;
			// R = R_tmp*R
			R = R_tmp*R;
241

incardon's avatar
incardon committed
242
			// Perform Givens Rotation on target vector
243

incardon's avatar
incardon committed
244
			G.makeGivens(T_tmp(0), T_tmp(1),&z);
245

incardon's avatar
incardon committed
246
247
248
			sign = 1.0;
			if (z < 0.0)
			{sign = -1.0;}
249

incardon's avatar
incardon committed
250
251
252
253
254
			R_tmp.setIdentity();
			R_tmp(p,p) = sign*G.c();
			R_tmp(q,q) = sign*G.c();
			R_tmp(p,q) = sign*-G.s();
			R_tmp(q,p) = sign*G.s();
255

incardon's avatar
incardon committed
256
			// Rotate target vector and update R_tar
257

incardon's avatar
incardon committed
258
259
260
261
			T_work = R_tmp*T_work;
			R_tar = R_tmp*R_tar;
		}
	}
262

incardon's avatar
incardon committed
263
	R = R_tar.transpose()*R;
264

incardon's avatar
incardon committed
265
	// Check the rotation
266

incardon's avatar
incardon committed
267
268
269
	Eigen::VectorXd Check(dim);
	Check = R*S;
}
270

incardon's avatar
incardon committed
271
272
273
274
275
276
277
278
void updatePso(openfpm::vector<double> & best_sol,
			   double sigma,
			   Eigen::VectorXd & xmean,
			   Eigen::MatrixXd & B,
			   Eigen::DiagonalMatrix<double,Eigen::Dynamic> & D,
			   Eigen::MatrixXd & C_pso)
{
	Eigen::VectorXd best_sol_ei(dim);
279

incardon's avatar
incardon committed
280
281
282
283
284
285
286
	double bias_weight = psoWeight;
	fill_vector(&best_sol.get(0),best_sol_ei);
	Eigen::VectorXd gb_vec = best_sol_ei-xmean;
	double gb_vec_length = sqrt(gb_vec.transpose() * gb_vec);
	Eigen::VectorXd b_main = B.col(dim-1);
	Eigen::VectorXd bias(dim);
	bias.setZero();
287

incardon's avatar
incardon committed
288
289
290
291
292
293
	// Rotation Matrix
	Eigen::MatrixXd R(dim,dim);

	if (gb_vec_length > 0.0)
	{
	    if(sigma < gb_vec_length)
294
	    {
incardon's avatar
incardon committed
295
296
297
298
	    	if(sigma/gb_vec_length <= t_c*gb_vec_length)
	    	{bias = 0.5*gb_vec;}
	    	else
	    	{bias = sigma*gb_vec/gb_vec_length;}
299
	    }
incardon's avatar
incardon committed
300
301
302
	    else
	    {bias.setZero();}
	}
303

incardon's avatar
incardon committed
304
	  xmean = xmean + bias;
305

incardon's avatar
incardon committed
306
307
308
309
	  if (psoWeight < 1.0)
	  {
		  Eigen::MatrixXd B_rot(dim,dim);
		  Eigen::DiagonalMatrix<double,Eigen::Dynamic> D_square(dim);
310

incardon's avatar
incardon committed
311
312
313
314
315
316
317
318
319
320
321
322
		  create_rotmat(b_main,gb_vec,R);
		  for (size_t i = 0 ; i < dim ; i++)
		  {B_rot.col(i) = R*B.col(i);}

		  for (size_t i = 0 ; i < dim ; i++)
		  {D_square.diagonal()[i] = D.diagonal()[i] * D.diagonal()[i];}
		  C_pso = B_rot * D_square * B_rot.transpose();

		  Eigen::MatrixXd trUp = C_pso.triangularView<Eigen::Upper>();
		  Eigen::MatrixXd trDw = C_pso.triangularView<Eigen::StrictlyUpper>();
		  C_pso = trUp + trDw.transpose();
	  }
323
324
}

incardon's avatar
incardon committed
325
326
327
328
329
330

void broadcast_best_solution(particle_type & vd,
							 openfpm::vector<double> & best_sol,
							 double & best,
							 double best_sample,
							 openfpm::vector<double> & best_sample_sol)
331
332
333
334
{
	best_sol.resize(dim);
	auto & v_cl = create_vcluster();

incardon's avatar
incardon committed
335
336
	double best_old = best_sample;
	v_cl.min(best_sample);
337
338
	v_cl.execute();

incardon's avatar
incardon committed
339
340
341
342
343
344
	// The old solution remain the best
	if (best < best_sample)
	{return;}

	best = best_sample;

345
	size_t rank;
incardon's avatar
incardon committed
346
	if (best_old == best_sample)
347
348
349
350
351
352
353
354
355
356
	{
		rank = v_cl.getProcessUnitID();

		// we own the minimum and we decide who broad cast
		v_cl.min(rank);
		v_cl.execute();

		if (rank == v_cl.getProcessUnitID())
		{
			for (size_t i = 0 ; i < dim ; i++)
incardon's avatar
incardon committed
357
			{best_sol.get(i) = best_sample_sol.get(i);}
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
		}
	}
	else
	{
		rank = std::numeric_limits<size_t>::max();

		// we do not own  decide who broad cast
		v_cl.min(rank);
		v_cl.execute();
	}

	// now we broad cast the best solution across processors

	v_cl.Bcast(best_sol,rank);
	v_cl.execute();
}

incardon's avatar
incardon committed
375
void cmaes_myprctile(openfpm::vector<fun_index> & f_obj, double (& perc)[2], double (& res)[2])
376
{
incardon's avatar
incardon committed
377
378
379
380
	double sar[lambda];
	double availablepercentiles[lambda];
	int idx[hist_size];
	int i,k;
381

incardon's avatar
incardon committed
382
	for (size_t i = 0 ; i < lambda ; i++)
383
	{
incardon's avatar
incardon committed
384
385
		availablepercentiles[i] = 0.0;
		sar[i] = f_obj.get(i).f;
386
	}
incardon's avatar
incardon committed
387
	std::sort(&sar[0],&sar[lambda]);
388

incardon's avatar
incardon committed
389
390
391
392
393
394
395
396
397
398
	for (size_t i = 0 ; i < 2 ; i++)
	{
		if (perc[i] <= (100.0*0.5/lambda))
		{res[i] = sar[0];}
		else if (perc[i] >= (100.0*(lambda-0.5)/lambda) )
		{res[i] = sar[lambda-1];}
		else
		{
			for (size_t j = 0 ; j < lambda ; j++)
			{availablepercentiles[j] = 100.0 * ((double(j)+1.0)-0.5) / lambda;}
399

incardon's avatar
incardon committed
400
401
402
			for (k = 0 ; k < lambda ; k++)
			{if(availablepercentiles[k] >= perc[i]) {break;}}
			k-=1;
403

incardon's avatar
incardon committed
404
405
406
407
408
409
410
			res[i] = sar[k] + (sar[k+1]-sar[k]) * (perc[i]
							-availablepercentiles[k]) / (availablepercentiles[k+1] - availablepercentiles[k]);
		}
	}
}

double maxval(double (& buf)[hist_size], bool (& mask)[hist_size])
411
{
incardon's avatar
incardon committed
412
413
414
415
416
417
	double max = 0.0;
	for (size_t i = 0 ; i < hist_size ; i++)
	{
		if (buf[i] > max && mask[i] == true)
		{max = buf[i];}
	}
418

incardon's avatar
incardon committed
419
420
	return max;
}
421

incardon's avatar
incardon committed
422
423
424
425
426
427
428
429
double minval(double (& buf)[hist_size], bool (& mask)[hist_size])
{
	double min = std::numeric_limits<double>::max();
	for (size_t i = 0 ; i < hist_size ; i++)
	{
		if (buf[i] < min && mask[i] == true)
		{min = buf[i];}
	}
430

incardon's avatar
incardon committed
431
432
	return min;
}
433

incardon's avatar
incardon committed
434
435
436
437
void cmaes_intobounds(Eigen::VectorXd & x, Eigen::VectorXd & xout,bool (& idx)[dim], bool & idx_any)
{
	idx_any = false;
	for (size_t i = 0; i < dim ; i++)
438
	{
incardon's avatar
incardon committed
439
		if(x(i) < -5.0)
440
		{
incardon's avatar
incardon committed
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
			xout(i) = -5.0;
			idx[i] = true;
			idx_any = true;
		}
		else if (x(i) > 5.0)
		{
			xout(i) = 5.0;
			idx[i] = true;
			idx_any = true;
		}
		else
		{
			xout(i) = x(i);
			idx[i] = false;
		}
	}
}
458

incardon's avatar
incardon committed
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
void cmaes_handlebounds(openfpm::vector<fun_index> & f_obj,
						double sigma,
						double & validfit,
						Eigen::VectorXd (& arxvalid)[lambda],
						Eigen::VectorXd (& arx)[lambda],
						Eigen::MatrixXd & C,
						Eigen::VectorXd & xmean,
						Eigen::VectorXd & xold,
						double (& weight)[dim],
						double (& fithist)[hist_size],
						bool & iniphase,
						double & validfitval,
						double mu_eff,
						int step,
						int last_restart)
{
	double val[2];
	double value;
	double diag[dim];
	double meandiag;
	int i,k,maxI;
	bool mask[hist_size];
	bool idx[dim];
	Eigen::VectorXd tx(dim);
	int dfitidx[hist_size];
	double dfitsort[hist_size];
	double prct[2] = {25.0,75.0};
	bool idx_any;

	for (size_t i = 0 ; i < hist_size ; i++)
	{
		dfitsort[i] = 0.0;
		dfitidx[i] = 0;
492

incardon's avatar
incardon committed
493
494
495
496
497
		if (fithist[i] > 0.0)
		{mask[i] = true;}
		else
		{mask[i] = false;}
	}
498

incardon's avatar
incardon committed
499
500
	for (size_t i = 0 ; i < dim ; i++)
	{diag[i] = C(i,i);}
501

incardon's avatar
incardon committed
502
	maxI = 0;
503

incardon's avatar
incardon committed
504
	meandiag = C.trace()/dim;
505

incardon's avatar
incardon committed
506
507
	cmaes_myprctile(f_obj, prct, val);
	value = (val[1] - val[0]) / dim / meandiag / (sigma*sigma);
508

incardon's avatar
incardon committed
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
	if (value >= std::numeric_limits<double>::max())
	{
		auto & v_cl = create_vcluster();
		std::cout << "Process " << v_cl.rank() << " warning: Non-finite fitness range" << std::endl;
		value = maxval(fithist,mask);
	}
	else if(value == 0.0)
	{
		value = minval(fithist,mask);
	}
	else if (validfit == 0.0)
	{
		for (size_t i = 0 ; i < hist_size ; i++)
		{fithist[i] = -1.0;}
		validfit = 1;
	}
525

incardon's avatar
incardon committed
526
527
528
529
530
531
532
533
534
535
536
537
538
539
	for (size_t i = 0; i < hist_size ; i++)
	{
		if(fithist[i] < 0.0)
		{
			fithist[i] = value;
			maxI = i;
			break;
		}
		else if(i == hist_size-1)
		{
			for (size_t k = 0 ; k < hist_size-1 ; k++)
			{fithist[k] = fithist[k+1];}
			fithist[i] = value;
			maxI = i;
540
541
542
		}
	}

incardon's avatar
incardon committed
543
	cmaes_intobounds(xmean,tx,idx,idx_any);
544

incardon's avatar
incardon committed
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
	if (iniphase)
	{
		if (idx_any)
		{
			if(maxI == 1)
			{value = fithist[0];}
			else
			{
				openfpm::vector<fun_index> fitsort(maxI+1);
				for (size_t i = 0 ; i <= maxI; i++)
				{
					fitsort.get(i).f = fithist[i];
					fitsort.get(i).id = i;
				}

				fitsort.sort();
				for (size_t k = 0; k <= maxI ; k++)
				{fitsort.get(k).f = fithist[fitsort.get(k).id];}

				if ((maxI+1) % 2 == 0)
				{value = (fitsort.get(maxI/2).f+fitsort.get(maxI/2+1).f)/2.0;}
				else
				{value = fitsort.get(maxI/2).f;}
			}
			for (size_t i = 0 ; i < dim ; i++)
			{
				diag[i] = diag[i]/meandiag;
				weight[i] = 2.0002 * value / diag[i];
			}
			if (validfitval == 1.0 && step-last_restart > 2)
			{
				iniphase = false;
			}
		}
	}
580

incardon's avatar
incardon committed
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
	if(idx_any)
	{
		tx = xmean - tx;
		for(size_t i = 0 ; i < dim ; i++)
		{
			idx[i] = (idx[i] && (fabs(tx(i)) > 3.0*std::max(1.0,sqrt(dim)/mu_eff) * sigma * sqrt(diag[i])));
			idx[i] = (idx[i] && (std::copysign(1.0,tx(i)) == std::copysign(1.0,(xmean(i)-xold(i)))) );
		}
		for (size_t i = 0 ; i < dim ; i++)
		{
			if (idx[i] == true)
			{
				weight[i] = pow(1.2,(std::max(1.0,mu_eff/10.0/dim)))*weight[i];
			}
		}
	}
	double arpenalty[lambda];
	for (size_t i = 0 ; i < lambda ; i++)
	{
		arpenality[i] = 0.0;
		for (size_t j = 0 ; j < dim ; j++)
		{
			arpenalty[i] += weight[j] * (arxvalid[i](j) - arx[i](j))*(arxvalid[i](j) - arx[i](j));
		}
		f_obj.get(i).f += arpenalty[i];
	}
//	fitness%sel = fitness%raw + bnd%arpenalty;
608
609
}

incardon's avatar
incardon committed
610
611
612
void cma_step(particle_type & vd, int step,  double & best,
			  int & best_i, openfpm::vector<double> & best_sol,
			  int & stop_cond)
613
{
incardon's avatar
incardon committed
614
615
616
617
	Eigen::VectorXd xmean(dim);
	Eigen::VectorXd mean_z(dim);
	Eigen::VectorXd arxvalid[lambda];
	Eigen::VectorXd arx[lambda];
618

incardon's avatar
incardon committed
619
620
621
622
623
	for (size_t i = 0 ; i < lambda ; i++)
	{
		arx[i].resize(dim);
		arxvalid[i].resize(dim);
	}
624

incardon's avatar
incardon committed
625
626
627
628
629
630
	double best_sample = std::numeric_limits<double>::max();
	openfpm::vector<double> best_sample_sol(dim);

	openfpm::vector<fun_index> f_obj(lambda);

	int counteval = step*lambda;
631

incardon's avatar
incardon committed
632
	auto it = vd.getDomainIterator();
633
634
635
636
	while (it.isNext())
	{
		auto p = it.get();

incardon's avatar
incardon committed
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
		if (vd.getProp<stop>(p) == true)
		{++it;continue;}

		// fill the mean vector;

		fill_vector(vd.getPos(p),xmean);

		for (size_t j = 0 ; j < lambda ; j++)
		{
			vd.getProp<Zeta>(p)[j] = generateGaussianVector<dim>();

			Eigen::VectorXd & debug4 = vd.getProp<Zeta>(p)[j];

			arx[j] = xmean + vd.getProp<sigma>(p)*vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<Zeta>(p)[j];

			double & debug6 = vd.getProp<sigma>(p);
653

incardon's avatar
incardon committed
654
			Eigen::MatrixXd & debug7 = vd.getProp<B>(p);
655

incardon's avatar
incardon committed
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
			// sample point has to be inside -5.0 and 5.0
			for (size_t i = 0 ; i < dim ; i++)
			{
				if (arx[j](i) < -5.0)
				{arxvalid[j](i) = -5.0;}
				else if (arx[j](i) > 5.0)
				{arxvalid[j](i) = 5.0;}
				else
				{arxvalid[j](i) = arx[j](i);}
			}

			f_obj.get(j).f = hybrid_composition<dim>(arxvalid[j]);
			f_obj.get(j).id = j;

			// Get the best ever
			if (f_obj.get(0).f < best_sample)
			{
				best_sample = f_obj.get(0).f;

			    // Copy the new mean as position of the particle
			    for (size_t i = 0 ; i < dim ; i++)
			    {best_sample_sol.get(i) = arxvalid[j](i);}
			}
		}

		// Add penalities for out of bound points
		cmaes_handlebounds(f_obj,vd.getProp<sigma>(p),
						   vd.getProp<validfit>(p),arxvalid,
						   arx,vd.getProp<Cov_m>(p),
						   xmean,vd.getProp<xold>(p),vd.getProp<weight>(p),
						   vd.getProp<fithist>(p),vd.getProp<iniphase>(p),
						   vd.getProp<validfit>(p),mu_eff,
						   step,vd.getProp<last_restart>(p));

		f_obj.sort();

		for (size_t j = 0 ; j < lambda ; j++)
		{vd.getProp<ord>(p)[j] = f_obj.get(j).id;}
694

incardon's avatar
incardon committed
695
		// Calculate weighted mean
696

incardon's avatar
incardon committed
697
698
699
		if (step == 5)
		{
			Eigen::VectorXd (& debug4)[lambda] = vd.getProp<Zeta>(p);
700

incardon's avatar
incardon committed
701
702
			int debug = 0;
		}
703

incardon's avatar
incardon committed
704
705
706
707
708
709
710
		xmean.setZero();
		mean_z.setZero();
		for (size_t j = 0 ; j < mu ; j++)
		{
			xmean += weight_sample(j)*arx[vd.getProp<ord>(p)[j]];
			mean_z += weight_sample(j)*vd.getProp<Zeta>(p)[vd.getProp<ord>(p)[j]];
		}
711

incardon's avatar
incardon committed
712
		vd.getProp<xold>(p) = xmean;
713
714
715

		++it;
	}
incardon's avatar
incardon committed
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865

	// Find the best point across processors
	broadcast_best_solution(vd,best_sol,best,best_sample,best_sample_sol);

	stop_cond = 1;

	// Particle swarm update

	auto it2 = vd.getDomainIterator();
	while (it2.isNext())
	{
		auto p = it2.get();

		if (vd.getProp<stop>(p) == true)
		{++it2;continue;}

		// There are still particles to process
		stop_cond = 0;

		vd.getProp<path_s>(p) = vd.getProp<path_s>(p)*(1.0 - cs) + sqrt(cs*(2.0-cs)*mu_eff)*vd.getProp<B>(p)*mean_z;

		double hsig = vd.getProp<path_s>(p).norm()/(1-pow(1-cs,2*counteval/lambda))/dim < 2.0 + 4.0/(dim+1);

		vd.getProp<path_c>(p) = (1-cc)*vd.getProp<path_c>(p) + hsig * sqrt(cc*(2-cc)*mu_eff)*(vd.getProp<B>(p)*vd.getProp<D>(p)*mean_z);

		if (step % N_pso == 0)
		{
			Eigen::MatrixXd C_pso(dim,dim);
			updatePso(best_sol,vd.getProp<sigma>(p),xmean,vd.getProp<B>(p),vd.getProp<D>(p),C_pso);

			// Adapt covariance matrix C
			vd.getProp<Cov_m>(p) = (1.0-ccov+(1.0-hsig)*ccov*cc*(2.0-cc)/mu_eff)*vd.getProp<Cov_m>(p) +
									ccov*(1.0/mu_eff)*(vd.getProp<path_c>(p)*vd.getProp<path_c>(p).transpose());

			for (size_t i = 0 ; i < mu ; i++)
			{vd.getProp<Cov_m>(p) += ccov*(1.0-1.0/mu_eff)*(vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<Zeta>(p)[vd.getProp<ord>(p)[i]])*weight_sample(i)*
										  (vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<Zeta>(p)[vd.getProp<ord>(p)[i]]).transpose();
			}

	    	vd.getProp<Cov_m>(p) = psoWeight*vd.getProp<Cov_m>(p) + (1.0 - psoWeight)*C_pso;
	    }
	    else
	    {
			// Adapt covariance matrix C
			vd.getProp<Cov_m>(p) = (1.0-ccov+(1.0-hsig)*ccov*cc*(2.0-cc)/mu_eff)*vd.getProp<Cov_m>(p) +
									ccov*(1.0/mu_eff)*(vd.getProp<path_c>(p)*vd.getProp<path_c>(p).transpose());

			for (size_t i = 0 ; i < mu ; i++)
			{vd.getProp<Cov_m>(p) += ccov*(1.0-1.0/mu_eff)*(vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<Zeta>(p)[vd.getProp<ord>(p)[i]])*weight_sample(i)*
				                          (vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<Zeta>(p)[vd.getProp<ord>(p)[i]]).transpose();
			}
	    }

		// Numeric error

		double smaller = std::numeric_limits<double>::max();
		for (size_t i = 0 ; i < dim ; i++)
		{
			if (vd.getProp<sigma>(p)*sqrt(vd.getProp<D>(p).diagonal()[i]) > 5.0)
			{
				if (smaller > 5.0/sqrt(vd.getProp<D>(p).diagonal()[i]))
				{smaller = 5.0/sqrt(vd.getProp<D>(p).diagonal()[i]);}
			}
		}
		if (smaller != std::numeric_limits<double>::max())
		{vd.getProp<sigma>(p) = smaller;}

		//Adapt step-size sigma
		vd.getProp<sigma>(p) = vd.getProp<sigma>(p)*exp((cs/d_amps)*(vd.getProp<path_s>(p).norm()/chiN - 1));

		auto & v_cl = create_vcluster();
		std::cout << vd.getProp<sigma>(p) <<  "  " << v_cl.rank() << std::endl;

		// Update B and D from C

		if (counteval - eigeneval > lambda/(ccov)/dim/10)
		{
			eigeneval = counteval;

			Eigen::MatrixXd trUp = vd.getProp<Cov_m>(p).triangularView<Eigen::Upper>();
			Eigen::MatrixXd trDw = vd.getProp<Cov_m>(p).triangularView<Eigen::StrictlyUpper>();
			vd.getProp<Cov_m>(p) = trUp + trDw.transpose();

			// Eigen decomposition
			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig_solver;

			eig_solver.compute(vd.getProp<Cov_m>(p));

			for (size_t i = 0 ; i < eig_solver.eigenvalues().size() ; i++)
			{vd.getProp<D>(p).diagonal()[i] = sqrt(eig_solver.eigenvalues()[i]);}
			vd.getProp<B>(p) = eig_solver.eigenvectors();

			// Make first component always positive
			for (size_t i = 0 ; i < dim ; i++)
			{
				if (vd.getProp<B>(p)(0,i) < 0)
				{vd.getProp<B>(p).col(i) = - vd.getProp<B>(p).col(i);}
			}

			Eigen::MatrixXd tmp = vd.getProp<B>(p).transpose();
			Eigen::MatrixXd & debug3 = vd.getProp<Cov_m>(p);

			int debug = 0;
		}

		Eigen::MatrixXd & debug1 = vd.getProp<B>(p);
		Eigen::DiagonalMatrix<double,Eigen::Dynamic> & debug2 = vd.getProp<D>(p);

	    // Escape flat fitness, or better terminate?
	    if (f_obj.get(0).f == f_obj.get(std::ceil(0.7*lambda)).f )
	    {
	    	vd.getProp<sigma>(p) = vd.getProp<sigma>(p)*exp(0.2+cs/d_amps);
	    	std::cout << "warning: flat fitness, consider reformulating the objective";
	    }

	    // Copy the new mean as position of the particle
	    for (size_t i = 0 ; i < dim ; i++)
	    {
	    	// Check we do not go out od bound

	    	vd.getPos(p)[i] = xmean(i);
	    }

//	    std::cout << "Best solution: " << f_obj.get(0).f << "   " << vd.getProp<sigma>(p) << std::endl;

	    double debug_sigma = vd.getProp<sigma>(p);
	    Eigen::DiagonalMatrix<double,Eigen::Dynamic> & debug_d = vd.getProp<D>(p);

	    // Stop conditions
	    bool stop_tol = true;
	    bool stop_tolX = true;
	    for (size_t i = 0 ; i < dim ; i++)
	    {
	    	stop_tol &= vd.getProp<sigma>(p)*std::max(fabs(vd.getProp<path_c>(p)(i)),sqrt(vd.getProp<D>(p).diagonal()[i])) < stopTolX;
	    	stop_tolX &= vd.getProp<sigma>(p)*sqrt(vd.getProp<D>(p).diagonal()[i]) > stopTolUpX;
	    }

	    vd.getProp<stop>(p) = stop_tol | stop_tolX;

	    if (vd.getProp<stop>(p) == true)
	    {
	    	std::cout << "Stopped" << std::endl;
	    }

		++it2;
	}

	auto & v_cl = create_vcluster();
	v_cl.min(stop_cond);
	v_cl.execute();
866
867
868
}


incardon's avatar
incardon committed
869

870
871
872
873
874
int main(int argc, char* argv[])
{
    // initialize the library
	openfpm_init(&argc,&argv);

incardon's avatar
incardon committed
875
	auto & v_cl = create_vcluster();
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890

	// Here we define our domain a 2D box with internals from 0 to 1.0 for x and y
	Box<dim,double> domain;

	for (size_t i = 0 ; i < dim ; i++)
	{
		domain.setLow(i,0.0);
		domain.setHigh(i,1.0);
	}

	// Here we define the boundary conditions of our problem
	size_t bc[dim];
	for (size_t i = 0 ; i < dim ; i++)
    {bc[i] = NON_PERIODIC;};

incardon's avatar
incardon committed
891
892
	prepare_f15<dim>();

893
894
895
	// extended boundary around the domain, and the processor domain
	Ghost<dim,double> g(0.0);

incardon's avatar
incardon committed
896
    particle_type vd(v_cl.size(),domain,bc,g);
897
898
899
900
901
902
903
904
905
906

    // Initialize constants

    stop_fitness = 1e-10;
    size_t stopeval = 1e3*dim*dim;

    // Strategy parameter setting: Selection
    init_weight();

    // Strategy parameter setting: Adaptation
incardon's avatar
incardon committed
907
908
909
910
911
    cc = 4.0 / (dim+4.0);
    cs = (mu_eff+2.0) / (dim+mu_eff+3.0);
    ccov = (1.0/mu_eff) * 2.0/((dim+1.41)*(dim+1.41)) +
    	   (1.0 - 1.0/mu_eff)* std::min(1.0,(2.0*mu_eff-1.0)/((dim+2.0)*(dim+2.0) + mu_eff));
    d_amps = 1 + 2*std::max(0.0, sqrt((mu_eff-1.0)/(dim+1))-1) + cs;
912
913
914
915
916

    chiN = sqrt(dim)*(1.0-1.0/(4.0*dim)+1.0/(21.0*dim*dim));

	//! \cond [assign position] \endcond

incardon's avatar
incardon committed
917
918
919
920
921
922
923
924
925

	// initialize the srand
	int seed = 24756*v_cl.rank()*v_cl.rank();
	srand(seed);

	for (size_t k = 0 ; k < 100 ; k++)
	{

		auto it = vd.getDomainIterator();
926
927
928
929
930
931
932
933

	while (it.isNext())
	{
		auto p = it.get();

		for (size_t i = 0 ; i < dim ; i++)
		{
			// we define x, assign a random position between 0.0 and 1.0
incardon's avatar
incardon committed
934
			vd.getPos(p)[i] = 10.0*(double)rand() / RAND_MAX - 5.0;
935
936
		}

incardon's avatar
incardon committed
937
		vd.getProp<sigma>(p) = 2.0;
938
939
940
941
942
943
944
945
946
947
948
949
950

		// Initialize the covariant Matrix,B and D to identity

		vd.getProp<D>(p).resize(dim);
		for (size_t i = 0 ; i < vd.getProp<D>(p).diagonal().size() ; i++)
		{vd.getProp<D>(p).diagonal()[i] = 1.0;}
		vd.getProp<B>(p).resize(dim,dim);
		vd.getProp<B>(p).setIdentity();
		vd.getProp<Cov_m>(p) = vd.getProp<B>(p)*vd.getProp<D>(p)*vd.getProp<D>(p)*vd.getProp<B>(p);
		vd.getProp<path_s>(p).resize(dim);
		vd.getProp<path_s>(p).setZero(dim);
		vd.getProp<path_c>(p).resize(dim);
		vd.getProp<path_c>(p).setZero(dim);
incardon's avatar
incardon committed
951
952
953
954
955
956
957
958
959
960
		vd.getProp<stop>(p) = false;
		vd.getProp<iniphase>(p) = true;
		vd.getProp<last_restart>(p) = 0;

		// Initialize the bound history

		for (size_t i = 0 ; i < hist_size ; i++)
		{vd.getProp<fithist>(p)[i] = -1.0;}
		vd.getProp<fithist>(p)[0] = 1.0;
		vd.getProp<validfit>(p) = 0.0;
961
962
963
964
965
966
967
968

		// next particle
		++it;
	}

	double best = 0.0;
	int best_i = 0;

incardon's avatar
incardon committed
969
970
	best = std::numeric_limits<double>::max();
	openfpm::vector<double> best_sol(dim);
971
972
	// now do several iteration

incardon's avatar
incardon committed
973
974
975
	int stop_cond = 0;
	int i = 0;
	while (stop_cond == 0)
976
977
	{
		// sample offspring
incardon's avatar
incardon committed
978
		cma_step(vd,i+1,best,best_i,best_sol,stop_cond);
979

incardon's avatar
incardon committed
980
981
		i++;
	}
982

incardon's avatar
incardon committed
983
	std::cout << "Best solution: " << best << std::endl;
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
	}

	openfpm_finalize();

	//! \cond [finalize] \endcond

	/*!
	 * \page Vector_0_simple Vector 0 simple
	 *
	 * ## Full code ## {#code_e0_sim}
	 *
	 * \include Vector/0_simple/main.cpp
	 *
	 */
}