int main(int number_of_scan_positions) {


	NEWMAT::Matrix Pg(3,3);
	NEWMAT::Matrix P(2,2);
	double tempAx, tempAy, tempBx, tempBy;
	double Gx, Gy, fi;
	double Px, Py, Pfi;
	Tocka Xp, G;
	Razlomljena_Duzina *map_in_local_frame = new Razlomljena_Duzina[120];
	Razlomljena_Duzina* map= new Razlomljena_Duzina[400];
	int line_counter=0;
	int number_of_lines_in_map=0;
	for (int i=0; i<number_of_scan_positions; i++){

		line_counter=0;
		ifstream file_line ("map_lines_in_local_frame" +IntToString(i)+".txt");

		P << 0 << 0 << 0 << 0;        // set line variance in local frame to 0

		while(!file_line.eof()){

			file_line >> tempAx >> tempAy >> tempBx >> tempBy;

			map_in_local_frame[line_counter].A.push_back(Tocka(tempAx, tempAy));
			map_in_local_frame[line_counter].B.push_back(Tocka(tempBx, tempBy));
			map_in_local_frame[line_counter].postaviVar(P);

			line_counter++;
		}




		file_line.close();

		ifstream file_pose ("pose_with_covariance" +IntToString(i)+".txt");
		file_pose >> Gx >> Gy >> fi;
		G=Tocka (Gx, Gy);

		file_pose >> Px  >> Py >> Pfi;

		Pg << Px<< 0 << 0
		   << 0  << Py <<0
		   << 0  << 0 <<Pfi;

		file_pose.close();




		transformiraj2(map_in_local_frame,line_counter,G,fi,&Xp,Pg);

		map=duzinacat2(map, number_of_lines_in_map, map_in_local_frame, line_counter, &number_of_lines_in_map);


	}

	poklopi_kartu2(map,number_of_lines_in_map);
	Snimi ("final_map.m", map, number_of_lines_in_map);



}
예제 #2
0
int main (int argc, char const *argv[])
{
	if (argc < 2){
		std::cout << "Usage:\n./pso <filename> <pop_size>" << std::endl;
		return 0;
	}
	
	int pop_size = 2000;
	const std::string filename = argv[1];
	if (argc > 2) pop_size = int(atoi(argv[2]));
	int iterations = 500;
	if (argc > 3) iterations = int(atoi(argv[3]));
	double alpha = 0.75;
	if (argc > 4) alpha = double(atof(argv[4]));
	double beta = 0.1;
	if (argc > 5) alpha = double(atof(argv[5]));
	
	std::vector<double> optHistory(iterations);
	
	srand((unsigned)time(NULL));
	std::vector<double>* objCost = readObjFunArray(filename.c_str());
	const int n_vars = (int)(sqrt(objCost->size()));
	
	
	
	std::clock_t start;
	start = std::clock();
	// create X
	std::vector<std::vector<int> > X(pop_size, std::vector<int>(n_vars));
	// these vector will store V
	std::vector<std::vector<swap> > V(pop_size, std::vector<swap>(0));
	// these vectors will store best individual positions
	std::vector<std::vector<int> > P(pop_size, std::vector<int>(n_vars));
	// this vector will store global best position
	std::vector<int> Pg(n_vars);
	// this  will store global best obj function value
	double fevalsPg = 0;
	// this vector will store current obj function values 
	std::vector<double> fevals(pop_size);
	// this vector will store individual best obj function values
	std::vector<double> fevalsP(pop_size);
	int n_swaps = 0;
	int first = 0;
	int second = 0;

	// generate random V == swap sequences
	// also generate random initial positions
	for (int i=0; i<pop_size; i++){
		for (int j=0; j<n_vars; j++){
			X[i][j] = j;  // fill X with nodes from 0 to n_vars-1
		}
		std::random_shuffle (X[i].begin(), X[i].end());  // shuffle
		P[i] = X[i];  // deep copy
		//for (int j=0; j<n_vars; j++){std::cout << X[i][j] << " ";}
		//std::cout << std::endl;
		n_swaps = rand() % (n_vars) + 1;
		for (int j=0; j<n_swaps; j++){
			first = rand() % n_vars;
			second = rand() % n_vars;
			V[i].push_back(swap (first, second));
			//std::cout << V[i][j].first << " " <<  V[i][j].second << std::endl;
		}
		//std::cout << V[i].size() << std::endl;
	}
	
	//evaluate global best
	for (int i=0; i<pop_size; i++){
		fevals[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
		fevalsP[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
		//std::cout << evals[i] << std::endl;
	}
	
	int globBestIndex = distance(fevals.begin(), min_element(fevals.begin(), fevals.end()));
	// std::cout << globBestIndex << std::endl;
	// save global best position and value
	Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
	fevalsPg = fevals[globBestIndex];
	//start pso

	bool terminate = false;
	/*
	std::vector<int> pp1 = {1,2,3,4,5,6,7,8};
	std::vector<int> pp2 = {3,2,1,6,8,5,7,4};
	std::vector<swap> pp3 = diff(pp1, pp2);
	printarr1(pp1);
	printarr1(pp2);
	printarr2(pp3);*/
	
	for (int k=0; k<iterations && !terminate; k++){
	    for (int i=0; i<pop_size; i++){
			//3.1 calculate difference between P_i and X_i
			// A = P_i - X_i, where A is a basic sequence.
			std::vector<swap> A = diff(P[i], applySwaps(X[i], V[i]));
			std::vector<swap> B = diff(Pg, applySwaps(X[i], V[i]));
			
			// now lets compute A*alpha and B*beta
			for (int j=A.size()-1; j>=0; j--){
				double p1 = ((double)rand()/(double)RAND_MAX);
				if (p1>alpha){
					A.erase(A.begin() + j);
				}
			}
			for (int j=B.size()-1; j>=0; j--){
				double p2 = ((double)rand()/(double)RAND_MAX);
				if (p2>beta){
					B.erase(B.begin() + j);
				}
			}
			std::vector<swap> newV = V[i];
			// concatenate all the swaps in a single sequence
			newV.insert(newV.end(), A.begin(), A.end());
			newV.insert(newV.end(), B.begin(), B.end());
			// transform the swap sequence in Basic sequence form
			V[i] = toBasicSequence(newV, n_vars);
			// update position with formula X_i = X_i + V_i
			// X[i] = applySwaps(X[i], newV);
			
			// V[i].clear();
			
			// update fevals
			fevals[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
			// possibly update P
			if (fevals[i] < fevalsP[i]){
				fevalsP[i] = fevals[i];
				P[i] = applySwaps(X[i], V[i]);
			}
		}
		// possibly update Pg
		globBestIndex = distance(fevals.begin(), min_element(fevals.begin(), fevals.end()));
		Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
		
		if (fevals[globBestIndex] < fevalsPg){
			fevalsPg = fevals[globBestIndex];
			Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
		}
		optHistory[k] = fevalsPg;
	}
	double elapsedtime = (std::clock() - start) / (double)(CLOCKS_PER_SEC);
	//std::cout << "Time: " << elapsedtime << std::endl;
	//std::cout << "best: " << fevalsPg << std::endl;
	saveSolution(elapsedtime, fevalsPg, optHistory);
	//printarr1(shiftToFirst(Pg));
}
int ParticleSimulator::step(double time)
{
	Vector pos;
	double fg;
	for(int i = 0; i < m_particle->m_p.size(); i++){
		m_particle->getState(i, pos);
		//m_particle->m_p[i].force=VectorObj(0,0,0);
		fg = m_gravity * m_particle->m_p[i].mass;
		m_particle->m_p[i].force[1] = fg;
	}
	for(int i = 0; i < m_particle->m_p.size(); i++){
		for(int j = 0; j < m_no_spring; j++){
			if(m_spring[j].p1!=NULL && m_spring[j].p2!=NULL){
			if( m_particle->m_p.size()!=0 && m_spring[j].p1 == &m_particle->m_p[i]){
				computeForce(m_spring[j].p2, m_spring[j].p1, j);
				m_spring[j].p1->force+=_force;
				//animTcl::OutputMessage("force p1: %lf %lf %lf", m_spring[j].p1->force.x(), m_spring[j].p1->force.y(), m_spring[j].p1->force.z());
			}
			else if( m_spring[j].p2 == &m_particle->m_p[i]){
				computeForce(m_spring[j].p1, m_spring[j].p2, j);
				
			//	animTcl::OutputMessage("force p2: %lf %lf %lf", m_spring[j].p2->force.x(), m_spring[j].p2->force.y(), m_spring[j].p2->force.z());
				m_spring[j].p2->force+=_force;
			}
			}
			
		}
	}
	VectorObj Pg(0,-0.5,0);
	VectorObj T(1,0,0);
	VectorObj N(0,1,0);
	for(int i = 0; i < m_particle->m_p.size(); i++){
		if((m_particle->m_p[i].pos-Pg).dot(N)<=1 ){
			//if collision with ground
			m_particle->m_p[i].force = VectorObj(0,0,0);
		
			VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-
				m_ground.kd*m_particle->m_p[i].vel.normalize().dot(N)*N;
		//VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-m_ground.kd*m_particle->m_p[i].vel.dot(N)*N;
			
			//animTcl::OutputMessage("force_n dot N: %lf", force_n.dot(N));
		
		if(force_n.dot(N)>0){
			m_particle->m_p[i].force+=force_n;
			//vt = |a|cos(theta)*b
			VectorObj VN = N.normalize().dot(m_particle->m_p[i].vel)*N.normalize();
			VectorObj VT = m_particle->m_p[i].vel - VN;
			m_particle->m_p[i].vel = VT-m_ground.kd*VN;
		}
		else{
			m_particle->m_p[i].force-=force_n;
			//m_particle->m_p[i].force = -m_particle->m_p[i].force;
		}
		
			//animTcl::OutputMessage("force p2: %lf %lf %lf", m_particle->m_p[i].force.x(), m_particle->m_p[i].force.y(), m_particle->m_p[i].force.z());
		}
		if(m_integration=="euler"){
		//forward Euler
			VectorObj cur_vel = m_particle->m_p[i].vel;
			m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
			m_particle->m_p[i].pos += cur_vel*m_timestep;
		}
		//symplectic
		else if(m_integration=="symplectic"){
			m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
			m_particle->m_p[i].pos += m_timestep* m_particle->m_p[i].vel;
		}
		//verlet
		else if(m_integration=="verlet"){
			m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
			m_prev_pos = m_particle->m_p[i].pos-m_timestep*m_particle->m_p[i].vel;
			m_particle->m_p[i].pos = 2*m_particle->m_p[i].pos-m_prev_pos+pow(m_timestep,2)/m_particle->m_p[i].mass*m_particle->m_p[i].force;
				
		}
		else{
			animTcl::OutputMessage("Wrong integration method!");
			return TCL_ERROR;
		}
		
		m_particle->setState(i, pos);
	}
	return TCL_OK;

}// ParticleSimulator::step