void global_planner::VoronoiExpansion::setNewMap(cv::Mat _map, Eigen::Vector2d _origin, float _resoltuion,
                                                 int _optimization)
{
  ROS_INFO("debug got map");
  size_t new_hash = getHash(_map, _origin, _resoltuion);

  if (currentHash_ != new_hash)
  {
    ROS_INFO("Global Planner: Computing distance field ...");
    origin_[0] = _origin[0];
    origin_[1] = _origin[1];
    resolution_ = _resoltuion;

    prepareMap(_map, map_, _optimization);
    computeDistanceField(map_, distfield_);

    ROS_INFO("Global Planner: Computing voronoi graph ...");
    computeVoronoiMap(distfield_, voronoi_);
    currentHash_ = new_hash;
  }

  gotMap_ = true;
}
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
	if (road.size() < 3)
		return 0;

	// To avoid moving the rotation element attached to the last
	int lastP;
	if (road.last().hasRotation)
		lastP = road.size() - 2;
	else
		lastP = road.size() - 1;

	// Go through all points in the road
	float totalChange = 0.f;
	for (int i = 1; i < lastP; i++)
	{
		if (road[i].isVisible == false)
			break;

		WayPoint &w0 = road[i - 1];
		WayPoint &w1 = road[i];
		WayPoint &w2 = road[i + 1];

		// Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature
		QVec atractionForce(3);
		float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext);
		atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos);

		//Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs
		computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT);

		QVec repulsionForce = QVec::zeros(3);
		QVec jacobian(3);

		// space interval to compute the derivative. Related to to robot's size
		float h = DELTA_H;
		if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ )
		{
			jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX,
			                      0,
			                      w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h));

			// repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist.
			repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist);

		}

		float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes
		float beta = 0.55;  //Posibite values between  0.1 and 1	 The bigger in magnitude, more separation from obstacles

		QVec change = (atractionForce * alpha) + (repulsionForce * beta);
		if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z()))
		{
			road.print();
			qDebug() << atractionForce << repulsionForce;
			qFatal("change");
		}
		//Now we remove the tangencial component of the force to avoid recirculation of band points
		//QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector();
		//QVec nChange = pp * (pp * change);

		w1.pos = w1.pos - change;
		totalChange = totalChange + change.norm2();
	}
	return totalChange;
}