C++ TGapArray::reserve方法代码示例

/*---------------------------------------------------------------
				Find gaps in the obtacles (Beta version)
  ---------------------------------------------------------------*/
void  CHolonomicND::gapsEstimator(
	const std::vector<double>         & obstacles,
	const mrpt::math::TPoint2D  & target,
	TGapArray                   & gaps_out)
{
	const size_t n = obstacles.size();
	ASSERT_(n>2);

	// ================ Parameters ================
	const int     GAPS_MIN_WIDTH = ceil(n*0.01); // was: 3
	const double  GAPS_MIN_DEPTH_CONSIDERED = 0.6;
	const double  GAPS_MAX_RELATIVE_DEPTH = 0.5;
	// ============================================

	// Find the maximum distances to obstacles:
	// ----------------------------------------------------------
	float overall_max_dist = std::numeric_limits<float>::min(), overall_min_dist = std::numeric_limits<float>::max();
	for (size_t i=1;i<(n-1);i++)
	{
		mrpt::utils::keep_max(overall_max_dist, obstacles[i]);
		mrpt::utils::keep_min(overall_min_dist, obstacles[i]);
	}
	double max_depth = overall_max_dist - overall_min_dist;

	//  Build list of "GAPS":
	// --------------------------------------------------------
	TGapArray gaps_temp;
	gaps_temp.reserve( 150 );

	for (double threshold_ratio = 0.95;threshold_ratio>=0.05;threshold_ratio-=0.05)
	{
			const double  dist_threshold = threshold_ratio* overall_max_dist + (1.0f-threshold_ratio)*min(target.norm(), GAPS_MIN_DEPTH_CONSIDERED);

			bool    is_inside = false;
			size_t  sec_ini=0, sec_end=0;
			double  maxDist=0.;

			for (size_t i=0;i<n;i++)
			{
				if ( !is_inside && ( obstacles[i]>=dist_threshold) )	//A gap begins
				{
					sec_ini = i;
					maxDist = obstacles[i];
					is_inside = true;
				}
				else if (is_inside && (i==(n-1) || obstacles[i]<dist_threshold ))	//A gap ends
				{
					if (obstacles[i]<dist_threshold)
						sec_end = i-1;
					else
						sec_end = i;

					is_inside = false;

					if ( (sec_end-sec_ini) >= (size_t)GAPS_MIN_WIDTH )
					{
						// Add new gap:
						gaps_temp.resize( gaps_temp.size() + 1 );
						TGap	& newGap = *gaps_temp.rbegin();

						newGap.ini				= sec_ini;
						newGap.end				= sec_end;
						newGap.minDistance		= min( obstacles[sec_ini], obstacles[sec_end] );
						newGap.maxDistance		= maxDist;
					}
				}

				if (is_inside)
					maxDist = std::max( maxDist, obstacles[i] );
			}
	}

	//Start to filter the gap list
	//--------------------------------------------------------------

	const size_t nTempGaps = gaps_temp.size();

	std::vector<bool> delete_gaps;
	delete_gaps.assign( nTempGaps, false);

	// First, remove redundant gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i] == 1)
			continue;

		for (size_t j=i+1;j<nTempGaps;j++)
		{
			if (gaps_temp[i].ini == gaps_temp[j].ini || gaps_temp[i].end == gaps_temp[j].end)
				delete_gaps[j] = 1;
		}
	}

	// Remove gaps with a big depth
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i] == 1)
//.........这里部分代码省略.........

/*---------------------------------------------------------------
						Find gaps in the obtacles.
  ---------------------------------------------------------------*/
void  CHolonomicND::gapsEstimator(
		vector_double		&obstacles,
		poses::CPoint2D		&target,
		TGapArray			&gaps_out )
{
	unsigned int	i,n;
	int				nMaximos=0;
    double			MaximoAbsoluto = -100;
	double			MinimoAbsoluto = 100;
    vector_int		MaximoIdx;
    vector_double	MaximoValor;

    // Hacer una lista con los maximos de las distancias a obs:
    // ----------------------------------------------------------
	MaximoIdx.resize(obstacles.size());
	MaximoValor.resize(obstacles.size());
	n = obstacles.size();

    for (i=1;i<(n-1);i++)
    {
		// Actualizar max. y min. absolutos:
		MaximoAbsoluto= max( MaximoAbsoluto, obstacles[i] );
		MinimoAbsoluto= min( MinimoAbsoluto, obstacles[i] );

		// Buscar maximos locales:
		if ( ( obstacles[i] >= obstacles[i+1] &&
			  obstacles[i] > obstacles[i-1] ) ||
			  ( obstacles[i] > obstacles[i+1] &&
			  obstacles[i] >= obstacles[i-1] ) )
		{
				MaximoIdx[nMaximos] = i;
				MaximoValor[nMaximos++] = obstacles[i];
		}
    }

    //  Crear GAPS:
    // --------------------------------------------------------
	TGapArray    gaps_temp;
   	gaps_temp.reserve( 150 );

	for (double factorUmbral = 0.975f;factorUmbral>=0.04f;factorUmbral-=0.05f)
	{
            double   umbral = factorUmbral* MaximoAbsoluto + (1.0f-factorUmbral)*MinimoAbsoluto;
			bool	dentro = false;
			int		sec_ini=0, sec_end;
			double	maxDist=0;

			for (i=0;i<n;i++)
			{
				if ( !dentro && (!i || obstacles[i]>=umbral) )
				{
					sec_ini = i;
					maxDist = obstacles[i];
					dentro = true;
				}
				else if (dentro && (i==(n-1) || obstacles[i]<umbral ))
				{
					sec_end = i;
					dentro = false;

					if ( (sec_end-sec_ini) > 2 )
					{
						// Add new gap:
						TGap	newGap;
						newGap.ini				= sec_ini;
						newGap.end				= sec_end;
						newGap.entranceDistance = min( obstacles[sec_ini], obstacles[sec_end] );
						newGap.maxDistance		= maxDist;

						gaps_temp.push_back(newGap);
					}
				}

				if (dentro) maxDist = max( maxDist, obstacles[i] );
			}
	}

    // Proceso de eliminacion de huecos redundantes:
    // -------------------------------------------------------------
	std::vector<bool>	borrar_gap;
	borrar_gap.resize( gaps_temp.size() );
        for (i=0;i<gaps_temp.size();i++)
			borrar_gap[i] = false;


    // Eliminar huecos con muy poca profundidad si estan dentro de otros:
	double	maxProfundidad = 0;
	for (i=0;i<gaps_temp.size();i++)
    {
		double profundidad =
				gaps_temp[i].maxDistance -
				gaps_temp[i].entranceDistance;
		maxProfundidad = max(maxProfundidad, profundidad);
	}

	for (i=0;i<gaps_temp.size();i++)
    {
//.........这里部分代码省略.........