gistfile1.txt

if(!cube_initiation && CubeCenter.size() >= 2){
		cubes.push_back(CubeCenter[0]);
		cubes.push_back(CubeCenter[1]);
		missing_association.push_back(0);
		missing_association.push_back(0);
		cube_initiation = true;
	}
	else if(cube_initiation ){
		for(int index = 0; index < 2; index++){
			ROS_INFO("No seg");
			if(CubeCenter.size() >= 1){
				int nearest_id = -1;
				//double near
				for(int i = 0; i < CubeCenter.size(); i++){

					ROS_INFO("No seg");
					if((nearest_id == -1) || // Take first element if not initiated
					   (CubeCenter[i] - cubes[index]).norm() <= (CubeCenter[nearest_id] - cubes[index]).norm()){
						ROS_INFO("No seg");
						nearest_id = i;
					}
				}
				if((CubeCenter[nearest_id] - cubes[index]).norm() < max_translation 
					//|| missing_association[index] > max_tick_ghost
				    ){
					//We assign the vector to the cube and remove it from the stack
					missing_association[index] = 0;
					cubes[index] = CubeCenter[nearest_id];
					CubeCenter.erase(CubeCenter.begin() + nearest_id);
				}
				else{
					missing_association[index]++;
				}
			}
			else{
				missing_association[index]++;
			}
		}
		// if there is still dots
		if(CubeCenter.size() > 0){
			for(int index = 0; index < 2; index++){
				if(missing_association[index] > max_tick_ghost){
					ROS_INFO("We are not lucky");

					int nearest_id = -1;
					for(int i = 0; i < CubeCenter.size(); i++){
						if((nearest_id == -1) || // Take first element if not initiated
						   (CubeCenter[i] - cubes[index]).norm() <= (CubeCenter[nearest_id] - cubes[index]).norm()){
							nearest_id = i;
						}
					}
					if(nearest_id != -1){
						ROS_INFO("We find the one");
						cubes[index] = CubeCenter[nearest_id];
						CubeCenter.erase(CubeCenter.begin() + nearest_id);
						missing_association[index] = 0;
					}
				}
			}

		}
	}