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

/*---------------------------------------------------------------
						Navigate
  ---------------------------------------------------------------*/
void  CHolonomicND::navigate(
	const mrpt::math::TPoint2D &target,
	const std::vector<double>	&obstacles,
	double			maxRobotSpeed,
	double			&desiredDirection,
	double			&desiredSpeed,
	CHolonomicLogFileRecordPtr &logRecord,
	const double    max_obstacle_dist)
{
	TGapArray			gaps;
	TSituations			situation;
	unsigned int		selectedSector;
	double				riskEvaluation;
	CLogFileRecord_NDPtr log;
	double				evaluation;

	// Create a log record for returning data.
	if (!logRecord.present())
	{
		log = CLogFileRecord_ND::Create();
		logRecord = log;
	}


	// Search gaps:
	gaps.clear();
	gapsEstimator( obstacles, target, gaps);


	// Select best gap:
	searchBestGap(	obstacles,
					1.0,
					gaps,
					target,
					selectedSector,
					evaluation,
					situation,
					riskEvaluation,
					log);

	if (situation == SITUATION_NO_WAY_FOUND)
	{
		// No way found!
		desiredDirection = 0;
		desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));

		// Speed control: Reduction factors
		// ---------------------------------------------
		const double targetNearnessFactor = std::min( 1.0, target.norm()/(options.TARGET_SLOW_APPROACHING_DISTANCE));
		const double riskFactor = std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE );
		desiredSpeed = maxRobotSpeed * std::min(riskFactor,targetNearnessFactor);
	}

	m_last_selected_sector = selectedSector;

	// LOG --------------------------
	if (log)
	{
		// gaps:
		{
			int	i,n = gaps.size();
			log->gaps_ini.resize(n);
			log->gaps_end.resize(n);
			for (i=0;i<n;i++)
			{
				log->gaps_ini[i]  = gaps[i].ini;
				log->gaps_end[i]  = gaps[i].end;
			}
		}
		// Selection:
		log->selectedSector = selectedSector;
		log->evaluation = evaluation;
		log->situation = situation;
		log->riskEvaluation = riskEvaluation;
	}
}

/*---------------------------------------------------------------
						Evaluate each gap
  ---------------------------------------------------------------*/
void  CHolonomicND::evaluateGaps(
	const std::vector<double>	&obstacles,
	const double maxObsRange,
	const TGapArray		&gaps,
	const unsigned int	target_sector,
	const float		target_dist,
	std::vector<double>		&out_gaps_evaluation )
{
	out_gaps_evaluation.resize( gaps.size());

	double	targetAng = M_PI*(-1 + 2*(0.5+target_sector)/double(obstacles.size()));
	double	target_x =  target_dist*cos(targetAng);
	double	target_y =  target_dist*sin(targetAng);

	for (unsigned int i=0;i<gaps.size();i++)
	{
		// Short cut:
		const TGap *gap = &gaps[i];

		const float d = min3(
			obstacles[ gap->representative_sector ],
			maxObsRange,
			0.95*target_dist );

		// The TP-Space representative coordinates for this gap:
		const double	phi = M_PI*(-1 + 2*(0.5+gap->representative_sector)/double(obstacles.size()));
		const double	x =  d*cos(phi);
		const double	y =  d*sin(phi);


		// Factor #1: Maximum reachable distance with this PTG:
		// -----------------------------------------------------
		// It computes the average free distance of the gap:
		float	meanDist = 0.f;
		for (unsigned int j=gap->ini;j<=gap->end;j++)
			meanDist+= obstacles[j];
		meanDist/= ( gap->end - gap->ini + 1);

		double factor1;
		if (mrpt::utils::abs_diff(gap->representative_sector,target_sector)<=1 && target_dist<1)
		      factor1 = std::min(target_dist,meanDist) / target_dist;
		else  factor1 = meanDist;



		// Factor #2: Distance to target in "sectors"
		// -------------------------------------------
		unsigned int dif = mrpt::utils::abs_diff(target_sector, gap->representative_sector );

		// Handle the -PI,PI circular topology:
		if (dif> 0.5*obstacles.size())
			dif = obstacles.size() - dif;

		const double factor2= exp(-square( dif / (obstacles.size()*0.25))) ;



		// Factor #3: Punish paths that take us far away wrt the target:  **** I don't understand it *********
		// -----------------------------------------------------
		double	closestX,closestY;
		double dist_eucl = math::minimumDistanceFromPointToSegment(
			target_x, target_y, // Point
			0,0,  x,y,          // Segment
			closestX,closestY   // Out
			);

		const float factor3 = ( maxObsRange - std::min(maxObsRange ,dist_eucl) ) / maxObsRange;



		// Factor #4: Stabilizing factor (hysteresis) to avoid quick switch among very similar paths:
		// ------------------------------------------------------------------------------------------
		double factor_AntiCab;


		if (m_last_selected_sector != std::numeric_limits<unsigned int>::max() )
		{
			unsigned int dist = mrpt::utils::abs_diff(m_last_selected_sector, gap->representative_sector);

			if (dist > unsigned(0.1*obstacles.size()))
				factor_AntiCab = 0.0;
			else
				factor_AntiCab = 1.0;
		}
		else
		{
			factor_AntiCab = 0;
		}


		ASSERT_(options.factorWeights.size()==4);

		if ( obstacles[gap->representative_sector] < options.TOO_CLOSE_OBSTACLE ) // Too close to obstacles
				out_gaps_evaluation[i] = 0;
		else	out_gaps_evaluation[i] = (
				  options.factorWeights[0] * factor1 +
				  options.factorWeights[1] * factor2 +
//.........这里部分代码省略.........

/*---------------------------------------------------------------
						Search the best gap.
  ---------------------------------------------------------------*/
void  CHolonomicND::searchBestGap(
	const std::vector<double>         & obstacles,
	const double                  maxObsRange,
	const TGapArray             & in_gaps,
	const mrpt::math::TPoint2D  & target,
	unsigned int                & out_selDirection,
	double                      & out_selEvaluation,
	TSituations                 & out_situation,
	double                      & out_riskEvaluation,
	CLogFileRecord_NDPtr	      log)
{
	// For evaluating the "risk":
	unsigned int min_risk_eval_sector = 0;
	unsigned int max_risk_eval_sector = obstacles.size()-1;
	const unsigned int target_sector  = direction2sector(atan2(target.y,target.x),obstacles.size());
	const double target_dist          = std::max(0.01,target.norm());
	// (Risk is evaluated at the end, for all the situations)

	// D1 : Straight path?
	// --------------------------------------------------------
	const int freeSectorsNearTarget = ceil(0.02*obstacles.size());
	bool theyAreFree = true, caseD1 = false;
	if (target_sector>static_cast<unsigned int>(freeSectorsNearTarget) &&
		target_sector<static_cast<unsigned int>(obstacles.size()-freeSectorsNearTarget) )
	{
		const double min_free_dist = std::min(1.05*target_dist, 0.95*maxObsRange);
		for (int j=-freeSectorsNearTarget;theyAreFree && j<=freeSectorsNearTarget;j++)
			if (obstacles[ (int(target_sector) + j) % obstacles.size()]<min_free_dist)
				theyAreFree = false;
		caseD1 = theyAreFree;
	}

	if (caseD1)
	{
		// S1: Move straight towards target:
		out_selDirection	= target_sector;

		// In case of several paths, the shortest:
		out_selEvaluation   =	1.0 + std::max( 0.0, (maxObsRange - target_dist) / maxObsRange );
		out_situation		=	SITUATION_TARGET_DIRECTLY;
	}
	else
	{
		// Evaluate all gaps (if any):
		std::vector<double>  gaps_evaluation;
		int            selected_gap			=-1;
		double         selected_gap_eval	= -100;

		evaluateGaps(
				obstacles,
				maxObsRange,
				in_gaps,
				target_sector,
				target_dist,
				gaps_evaluation );

		if (log) log->gaps_eval = gaps_evaluation;

		// D2: is there any gap "beyond" the target (and not too far away)?   (Not used)
		// ----------------------------------------------------------------

		//unsigned int dist;
		//for ( unsigned int i=0;i<in_gaps.size();i++ )
		//{
		//	dist = mrpt::utils::abs_diff(target_sector, in_gaps[i].representative_sector );
		//	if (dist > 0.5*obstacles.size())
		//		dist = obstacles.size() - dist;
		//
		//	if ( in_gaps[i].maxDistance >= target_dist && dist <= (int)floor(options.MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size()) )
		//
		//		if ( gaps_evaluation[i]>selected_gap_eval )
		//		{
		//			selected_gap_eval = gaps_evaluation[i];
		//			selected_gap = i;
		//		}
		//}


		// Keep the best gaps (if none was picked up to this point)
		if ( selected_gap==-1 )
			for ( unsigned int i=0;i<in_gaps.size();i++ )
				if ( gaps_evaluation[i]>selected_gap_eval )
				{
					selected_gap_eval = gaps_evaluation[i];
					selected_gap = i;
				}

		//  D3: Wasn't a good enough gap (or there were none)?
		// ----------------------------------------------------------
		if ( selected_gap_eval <= 0 )
		{
			// S2: No way found
			// ------------------------------------------------------
			out_selDirection	= 0;
			out_selEvaluation	= 0.0; // Worst case
			out_situation		= SITUATION_NO_WAY_FOUND;
		}
//.........这里部分代码省略.........

/*---------------------------------------------------------------
						Navigate
  ---------------------------------------------------------------*/
void  CHolonomicND::navigate(
				poses::CPoint2D	&target,
				vector_double	&obstacles,
				double			maxRobotSpeed,
				double			&desiredDirection,
				double			&desiredSpeed,
				CHolonomicLogFileRecordPtr &logRecord)
{
	TGapArray			gaps;
	TSituations			situation;
	int					selectedSector;
	double				riskEvaluation;
	CLogFileRecord_NDPtr log;
	double				evaluation;

	// Create a log record for returning data.
	if (!logRecord.present())
	{
		log = CLogFileRecord_ND::Create();
		logRecord = log;
	}


	// Search gaps:
    gaps.clear();
	gapsEstimator(	obstacles,
					target,
					gaps );


	// Select best gap:
	searchBestGap(	obstacles,
					1.0f,
					gaps,
					target,
					selectedSector,
					evaluation,
					situation,
					riskEvaluation,
					log);

	if (situation == SITUATION_NO_WAY_FOUND)
	{
		// No way found!
		desiredDirection = 0;
		desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));

		// Speed control: Reduction factors
		// ---------------------------------------------
		double		targetNearnessFactor = max(0.20, min(1.0, 1.0-exp(-(target.norm()+0.01)/TARGET_SLOW_APPROACHING_DISTANCE)));
		//printf(" TARGET NEARNESS = %f\n",targetNearnessFactor);
		double		riskFactor = min(1.0, riskEvaluation / RISK_EVALUATION_DISTANCE );

		desiredSpeed = maxRobotSpeed * min(riskFactor,targetNearnessFactor);
	}

	last_selected_sector = selectedSector;

	// LOG --------------------------
	if (log)
	{
		// gaps:
		if (situation != SITUATION_TARGET_DIRECTLY )
		{
			int	i,n = gaps.size();
			log->gaps_ini.resize(n);
			log->gaps_end.resize(n);
			for (i=0;i<n;i++)
			{
				log->gaps_ini[i]  = gaps[i].ini;
				log->gaps_end[i]  = gaps[i].end;
			}
		}
        // Selection:
        log->selectedSector = selectedSector;
        log->evaluation = evaluation;
        log->situation = situation;
        log->riskEvaluation = riskEvaluation;
	}
}

/*---------------------------------------------------------------
						Evaluate each gap
  ---------------------------------------------------------------*/
void  CHolonomicND::evaluateGaps(
	const vector_double	&obstacles,
	const double		maxObsRange,
	const TGapArray		&gaps,
	const int			TargetSector,
	const double		TargetDist,
	vector_double		&out_gaps_evaluation )
{
	out_gaps_evaluation.resize( gaps.size());

	double	targetAng = M_PI*(-1 + 2*(0.5+TargetSector)/double(obstacles.size()));
	double	target_x =  TargetDist*cos(targetAng);
	double	target_y =  TargetDist*sin(targetAng);

    for (unsigned int i=0;i<gaps.size();i++)
    {
        // Para referenciarlo mas facilmente:
        const TGap	*gap = &gaps[i];

        double   d;
        d = min( obstacles[ gap->representative_sector ],
				min( maxObsRange,  0.95f*TargetDist) );

		// Las coordenadas (en el TP-Space) representativas del gap:
		double	phi = M_PI*(-1 + 2*(0.5+gap->representative_sector)/double(obstacles.size()));
		double	x =  d*cos(phi);
		double	y =  d*sin(phi);

        // Factor 1: Distancia hasta donde llego por esta GPT:
        // -----------------------------------------------------
		double factor1;
/*		if (gap->representative_sector == TargetSector )
				factor1 = min(TargetDist,obstacles[gap->representative_sector]) / TargetDist;
		else
		{
			if (TargetDist>1)
					factor1 = obstacles[gap->representative_sector] / TargetDist;
			else	factor1 = obstacles[gap->representative_sector];
		}
*/
		// Calcular la distancia media a donde llego por este gap:
		double	meanDist = 0;
		for (int j=gap->ini;j<=gap->end;j++)
			meanDist+= obstacles[j];
		meanDist/= ( gap->end - gap->ini + 1);

		if (abs(gap->representative_sector-TargetSector)<=1 && TargetDist<1)
				factor1 = min(TargetDist,meanDist) / TargetDist;
		else	factor1 = meanDist;

        // Factor 2: Distancia en sectores:
        // -------------------------------------------
        double   dif = fabs(((double)( TargetSector - gap->representative_sector )));
//		if (dif> (0.5f*obstacles.size()) ) dif = obstacles.size() - dif;
		// Solo si NO estan el target y el gap atravesando el alfa = "-pi" o "pi"
		if (dif> (0.5f*obstacles.size()) && (TargetSector-0.5f*obstacles.size())*(gap->representative_sector-0.5f*obstacles.size())<0 )
			dif = obstacles.size() - dif;

        double   factor2= exp(-square( dif / (obstacles.size()/4))) ;

        // Factor3: Para evitar cabeceos entre 2 o mas caminos que sean casi iguales:
        // -------------------------------------------
		double dist = (double)(abs(last_selected_sector - gap->representative_sector));
		//
		if (dist> (0.5f*obstacles.size()) ) dist = obstacles.size() - dist;

		double factor_AntiCab;
		if (last_selected_sector==-1)
				factor_AntiCab = 0;
		else	factor_AntiCab = (dist > 0.10f*obstacles.size()) ? 0.0f:1.0f;

        // Factor3: Minima distancia entre el segmento y el target:
		//  Se valora negativamente el alejarse del target
        // -----------------------------------------------------
		double	closestX,closestY;
        double dist_eucl = math::minimumDistanceFromPointToSegment(
					target_x,
					target_y,
					0,0,
					x,y,
					closestX,closestY);

        double factor3=  ( maxObsRange - min(maxObsRange ,dist_eucl) ) / maxObsRange;

		ASSERT_(factorWeights.size()==4);

		if ( obstacles[gap->representative_sector] < TOO_CLOSE_OBSTACLE ) // Too close to obstacles
				out_gaps_evaluation[i] = 0;
		else	out_gaps_evaluation[i] = (
				  factorWeights[0] * factor1 +
				  factorWeights[1] * factor2 +
				  factorWeights[2] * factor3 +
				  factorWeights[3] * factor_AntiCab ) / (math::sum(factorWeights)) ;

	} // for each gap

}

/*---------------------------------------------------------------
						Search the best gap.
  ---------------------------------------------------------------*/
void  CHolonomicND::searchBestGap(
			vector_double		&obstacles,
			double				maxObsRange,
			TGapArray			&in_gaps,
			poses::CPoint2D		&target,
			int					&out_selDirection,
			double				&out_selEvaluation,
			TSituations			&out_situation,
			double				&out_riskEvaluation,
			CLogFileRecord_NDPtr log)
{
	// Para evaluar el risk:
	unsigned int min_risk_eval_sector	= 0;
	unsigned int max_risk_eval_sector	= obstacles.size()-1;
	unsigned int TargetSector		= direction2sector(atan2(target.y(),target.x()),obstacles.size());
	const double TargetDist	= std::max(0.01,target.norm());

    // El "risk" se calcula al final para todos los casos.

    // D1 : Camino directo?
    // --------------------------------------------------------
    const int freeSectorsNearTarget = 10;  // 3
    bool theyAreFree = true, caseD1 = false;
    if (TargetSector>(unsigned int)freeSectorsNearTarget &&
		TargetSector<(unsigned int)(obstacles.size()-freeSectorsNearTarget) )
    {
        for (int j=-freeSectorsNearTarget;j<=freeSectorsNearTarget;j++)
                if (obstacles[ TargetSector + j ]<0.95*TargetDist)
                        theyAreFree = false;
        caseD1 = theyAreFree;
    }

    if (caseD1)
    {
        // S1: Camino libre hacia target:
        out_selDirection	= TargetSector;

		// Si hay mas de una, la que llegue antes
		out_selEvaluation   =	1.0 + std::max( 0.0, (maxObsRange - TargetDist) / maxObsRange );
        out_situation		=	SITUATION_TARGET_DIRECTLY;
    }
    else
    {
        // Evaluar los GAPs (Si no hay ninguno, nada, claro):
        vector_double	gaps_evaluation;
        int				selected_gap		=-1;
        double			selected_gap_eval	= -100;

        evaluateGaps(
                obstacles,
				maxObsRange,
                in_gaps,
                TargetSector,
                TargetDist,
                gaps_evaluation );

		if (log) log->gaps_eval = gaps_evaluation;

        // D2: Hay algun gap que pase por detras del target?
        //   ( y no este demasiado lejos):
        // -------------------------------------------------
        for ( unsigned int i=0;i<in_gaps.size();i++ )
			if ( in_gaps[i].maxDistance >= TargetDist &&
				 abs((int)(in_gaps[i].representative_sector-(int)TargetSector)) <= (int)floor(MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size()) )
					if ( gaps_evaluation[i]>selected_gap_eval )
					{
						selected_gap_eval = gaps_evaluation[i];
						selected_gap = i;
					}


        // Coger el mejor GAP:
        //  (Esto ya solo si no se ha cogido antes)
        if ( selected_gap==-1 )
			for ( unsigned int i=0;i<in_gaps.size();i++ )
				if ( gaps_evaluation[i]>selected_gap_eval )
				{
						selected_gap_eval = gaps_evaluation[i];
						selected_gap = i;
				}
        //  D3:  No es suficientemente bueno? ( o no habia ninguno?)
        // ------------------------------------------------------------
        if ( selected_gap_eval <= 0 )
        {
            // S2: No way found
            // ------------------------------------------------------
            out_selDirection	= 0;
            out_selEvaluation	= 0.0f; // La peor
            out_situation		= SITUATION_NO_WAY_FOUND;
        }
        else
        {
            // El seleccionado:
            TGap    gap = in_gaps[selected_gap];

            int     sectors_to_be_wide = round( WIDE_GAP_SIZE_PERCENT * obstacles.size() );

//.........这里部分代码省略.........