예제 #1
0
void harp::spec_desisim::values ( vector_double & data ) const {

  data.resize ( nglobal_ );
  data.clear();

  fitsfile * fp;

  fits::open_read ( fp, path_ );

  // read the object flux

  fits::img_seek ( fp, objhdu_ );

  fits::img_read ( fp, data, false );

  // read the sky flux and sum

  vector_double skyflux ( data.size() );

  fits::img_seek ( fp, skyhdu_ );

  fits::img_read ( fp, skyflux, false );

  fits::close ( fp );

  for ( size_t i = 0; i < data.size(); ++i ) {
    data[i] += skyflux[i];
  }

  return;
}
예제 #2
0
void harp::spec_sim::inv_variance ( vector_double & data ) const {

  data.resize ( size_ );
  data.clear();

  return;
}
예제 #3
0
/*---------------------------------------------------------------
	Fills in the representative sector
		field in the gap structure:
  ---------------------------------------------------------------*/
void  CHolonomicND::calcRepresentativeSectorForGap(
	TGap					&gap,
	const poses::CPoint2D	&target,
	const vector_double		&obstacles)
{
    int     sector;
    int     sectors_to_be_wide = round( WIDE_GAP_SIZE_PERCENT * obstacles.size());
	int		TargetSector = direction2sector( atan2(target.y(),target.x()), obstacles.size() );

    if ( (gap.end-gap.ini) < sectors_to_be_wide )
    {
#if	1
		sector = round(0.5f*gap.ini+0.5f*gap.end);
#else
		double	min_dist_obs_near_ini=1, min_dist_obs_near_end=1;
		int		i;
		for ( i= gap.ini;i>=max(0,gap.ini-2);i--)
			min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
		for ( i= gap.end;i<=min((int)obstacles.size()-1,gap.end+2);i++)
			min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
		sector = round((min_dist_obs_near_ini*gap.ini+min_dist_obs_near_end*gap.end)/(min_dist_obs_near_ini+min_dist_obs_near_end));
#endif
    }
    else
    {
        // Para gaps anchos que NO contengan al target, cerca del borde
        //  mas cercano a este:
		//if ( TargetSector < gap.ini || TargetSector > gap.end )
  //      {
            int     dir;
            int     dist_ini = abs( TargetSector - gap.ini );
            int     dist_end = abs( TargetSector - gap.end );
            if (dist_ini<dist_end) {
                    sector = gap.ini;
                    dir = +1; }
            else {
                    sector = gap.end;
                    dir = -1; }
            sector = sector + dir * sectors_to_be_wide/2 ;
    //    }
    //    else
    //    {
    //        // Es un valle ancho con el Target dentro:
    //        // Buscar la maxima "distance" en un rango cerca del target:
    //        int     ini = max( gap.ini, TargetSector - sectors_to_be_wide / 2 );
    //        int     end = min( TargetSector + sectors_to_be_wide / 2, gap.end);

    //        sector = TargetSector;
    //        for (int i = ini;i<=end;i++)
				//if ( obstacles[i] > obstacles[sector] )
				//	sector = i;
    //    }
    }

	keep_max(sector, 0);
	keep_min(sector, (int)obstacles.size()-1);

    gap.representative_sector = sector;
}
예제 #4
0
파일: CPose2D.cpp 프로젝트: DYFeng/mrpt
/*---------------------------------------------------------------
		getAsVector
---------------------------------------------------------------*/
void CPose2D::getAsVector(vector_double &v) const
{
	v.resize(3);
	v[0]=m_coords[0];
	v[1]=m_coords[1];
	v[2]=m_phi;
}
예제 #5
0
void fixGPStimestamp(CObservationGPSPtr &obs, vector_double &time_changes, std::map<std::string,double> &DeltaTimes )
{
	if (!obs->has_GGA_datum && !obs->has_RMC_datum) return;

	CObservationGPS::TUTCTime	theTime;
	bool  hasTime=false;

	if (obs->has_GGA_datum && obs->GGA_datum.fix_quality>0)
	{
		theTime = obs->GGA_datum.UTCTime;
		hasTime = true;
	}
	else
	if (obs->has_RMC_datum && obs->RMC_datum.validity_char=='A' )
	{
		theTime = obs->RMC_datum.UTCTime;
		hasTime = true;
	}

	 // The last known delta_time for this sensor name
	if (DeltaTimes.find( obs->sensorLabel )==DeltaTimes.end())
		DeltaTimes[obs->sensorLabel] = 0;

	double &DeltaTime = DeltaTimes[obs->sensorLabel];

	if ( hasTime )
	{
		TTimeParts	timparts;
		mrpt::system::timestampToParts( obs->timestamp, timparts);

		DeltaTime  = 3600*theTime.hour + 60*theTime.minute  + theTime.sec;
		DeltaTime -= 3600*timparts.hour + 60*timparts.minute + timparts.second;

		if (theTime.hour < timparts.hour-2)
		{
			// The GPS time is one day ahead the "timestamp"
			DeltaTime += 3600*24;
		}
		else if (timparts.hour > theTime.hour+2)
		{
			// The "timstamp" is one day ahead the GPS time:
			DeltaTime -= 3600*24;
		}

		// Instead of delta, just replace:
		timparts.hour 		= theTime.hour;
		timparts.minute 	= theTime.minute;
		timparts.second 	= theTime.sec;

		obs->timestamp = buildTimestampFromParts(timparts);
	}
	else
	{
		// Use last delta
		obs->timestamp += mrpt::system::secondsToTimestamp(DeltaTime);
	}

	// Fix timestamp:
	time_changes.push_back( DeltaTime );
}
예제 #6
0
파일: test.cpp 프로젝트: bcsdleweev/mrpt
// The error function F(x):
void myFunction( const vector_double &x, const vector_double &y, vector_double &out_f)
{
	out_f.resize(1);

	// 1-cos(x+1) *cos(x*y+1)
	out_f[0] = 1 - cos(x[0]+1) * cos(x[0]*x[1]+1);
}
예제 #7
0
void ffff(const vector_double &x,const CQuaternionDouble &Q, vector_double &OUT)
{
	OUT.resize(3);
	CQuaternionDouble q(x[0],x[1],x[2],x[3]);
	q.normalize();
	q.rpy(OUT[2],OUT[1],OUT[0]);
}
예제 #8
0
void harp::eigen_decompose ( matrix_double const & invcov, vector_double & D, matrix_double & W, bool regularize ) {

  D.resize ( invcov.size1() );
  W.resize ( invcov.size1(), invcov.size2() );
  matrix_double temp ( invcov );

  int nfound;

  boost::numeric::ublas::vector < int > support ( 2 * invcov.size1() );

  boost::numeric::bindings::lapack::syevr ( 'V', 'A', boost::numeric::bindings::lower ( temp ), 0.0, 0.0, 0, 0, 0.0, nfound, D, W, support );

  if ( regularize ) {
    
    double min = 1.0e100;
    double max = -1.0e100;

    for ( size_t i = 0; i < D.size(); ++i ) {
      if ( D[i] < min ) {
        min = D[i];
      }
      if ( D[i] > max ) {
        max = D[i];
      }
    }

    double rcond = min / max;

    // pick some delta that is bigger than machine precision, but still tiny
    double epsilon = 10.0 * std::numeric_limits < double > :: epsilon();

    if ( rcond < epsilon ) {

      double reg = max * epsilon - min;
      //cerr << "REG offset = " << reg << " for min / max = " << min << " / " << max << endl;

      for ( size_t i = 0; i < D.size(); ++i ) {
        D[i] += reg;
      }

    }

  }

  return;
}
예제 #9
0
void harp::apply_inverse_norm ( vector_double const & S, vector_double & vec ) {

  for ( size_t i = 0; i < vec.size(); ++i ) {
    vec[i] /= S[i];
  }

  return;
}
예제 #10
0
void harp::spec_desisim::lambda ( vector_double & lambda_vals ) const {

  lambda_vals.resize ( nlambda_ );

  for ( size_t i = 0; i < nlambda_; ++i ) {
    lambda_vals[i] = crval + cdelt * (double)i;
  }

  return;
}
예제 #11
0
void harp::column_norm ( matrix_double const & mat, vector_double & S ) {

  S.resize( mat.size1() );
  S.clear();

  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      S[ j ] += mat( j, i );
    }
  }

  // Invert

  for ( size_t i = 0; i < S.size(); ++i ) {
    S[i] = 1.0 / S[i];
  }

  return;
}
예제 #12
0
void harp::spec_desisim::sky ( vector_double & data ) const {

  data.resize ( nglobal_ );
  data.clear();

  fitsfile * fp;

  fits::open_read ( fp, path_ );

  // read the sky flux

  fits::img_seek ( fp, skyhdu_ );

  fits::img_read ( fp, data, false );

  fits::close ( fp );

  return;
}
예제 #13
0
파일: CHistogram.cpp 프로젝트: DYFeng/mrpt
/*---------------------------------------------------------------
					getHistogramNormalized
 ---------------------------------------------------------------*/
void CHistogram::getHistogramNormalized( vector_double &x, vector_double &hits ) const
{
	const size_t N = m_bins.size();
	linspace(m_min,m_max,N, x);

	hits.resize(N);
	const double K=m_binSizeInv/m_count;
	for (size_t i=0;i<N;i++)
		hits[i]=K*m_bins[i];
}
예제 #14
0
파일: CPose3DQuat.cpp 프로젝트: DYFeng/mrpt
/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::getAsVector(vector_double &v) const
{
	v.resize(7);
	v[0] = m_coords[0];
	v[1] = m_coords[1];
	v[2] = m_coords[2];
	v[3] = m_quat[0];
	v[4] = m_quat[1];
	v[5] = m_quat[2];
	v[6] = m_quat[3];
}
예제 #15
0
void harp::sparse_mv_trans ( matrix_double_sparse const & AT, vector_double const & in, vector_double & out ) {

  // FIXME:  for now, we just use the (unthreaded) boost sparse matrix-vector product.  If this
  // operation dominates the cost in any way, we can add a threaded implementation here.

  size_t nrows = AT.size1();
  size_t ncols = AT.size2();

  if ( in.size() != nrows ) {
    std::ostringstream o;
    o << "length of input vector (" << in.size() << ") does not match number of rows in transposed matrix (" << nrows << ")";
    HARP_THROW( o.str().c_str() );
  }

  out.resize ( ncols );

  boost::numeric::ublas::axpy_prod ( in, AT, out, true );

  return;
}
예제 #16
0
void harp::spec_sim::lambda ( vector_double & lambda_vals ) const {

  lambda_vals.resize ( nlambda_ );

  double incr = (last_lambda_ - first_lambda_) / (double)( nlambda_ - 1 );

  for ( size_t j = 0; j < nlambda_; ++j ) {
    lambda_vals[j] = first_lambda_ + incr * (double)j;
  }
  
  return;
}
예제 #17
0
/*---------------------------------------------------------------
	Fills in the representative sector
		field in the gap structure:
  ---------------------------------------------------------------*/
void  CHolonomicND::calcRepresentativeSectorForGap(
	TGap                        & gap,
	const mrpt::math::TPoint2D  & target,
	const vector_double         & obstacles)
{
	int sector;
	const unsigned int sectors_to_be_wide = round( options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
	const unsigned int target_sector = direction2sector( atan2(target.y,target.x), obstacles.size() );

	if ( (gap.end-gap.ini) < sectors_to_be_wide )	//Select the intermediate sector
	{
#if	1
		sector = round(0.5f*gap.ini+0.5f*gap.end);
#else
		double	min_dist_obs_near_ini=1, min_dist_obs_near_end=1;
		int		i;
		for ( i= gap.ini;i>=max(0,gap.ini-2);i--)
			min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
		for ( i= gap.end;i<=min((int)obstacles.size()-1,gap.end+2);i++)
			min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
		sector = round((min_dist_obs_near_ini*gap.ini+min_dist_obs_near_end*gap.end)/(min_dist_obs_near_ini+min_dist_obs_near_end));
#endif
	}
	else	//Select a sector close to the target but spaced "sectors_to_be_wide/2" from it
	{
		unsigned int dist_ini = mrpt::utils::abs_diff(target_sector, gap.ini );
		unsigned int dist_end = mrpt::utils::abs_diff(target_sector, gap.end );

		if (dist_ini > 0.5*obstacles.size())
			dist_ini = obstacles.size() - dist_ini;
		if (dist_end > 0.5*obstacles.size())
			dist_end = obstacles.size() - dist_end;

		int dir;
		if (dist_ini<dist_end) {
				sector = gap.ini;
				dir = +1; }
		else {
				sector = gap.end;
				dir = -1; }

		sector = sector + dir*static_cast<int>(sectors_to_be_wide)/2;
	}

	keep_max(sector, 0);
	keep_min(sector, static_cast<int>(obstacles.size())-1 );

	gap.representative_sector = sector;
}
예제 #18
0
void harp::spec_sim::values ( vector_double & data ) const {

  double PI = std::atan2 ( 0.0, -1.0 );

  data.resize ( size_ );

  size_t bin = 0;

  size_t halfspace = (size_t)( atmspace_ / 2 );

  for ( size_t i = 0; i < nspec_; ++i ) {

    bool dosky = ( i % skymod_ == 0 ) ? true : false;

    size_t objoff = 2 * i;

    for ( size_t j = 0; j < nlambda_; ++j ) {

      double val = 0.0;

      val += background_ * sin ( 3.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) ) * sin ( 7.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) ) * sin ( 11.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) );

      val += 2.0 * background_;

      if ( ( ( j + halfspace ) % atmspace_ ) == 0 ) {
        val += atmpeak_;
      }

      if ( ! dosky ) {
        if ( ( ( j + objoff ) % objspace_ ) == 0 ) {
          val += objpeak_;
        }
      }

      data[ bin ] = val;

      ++bin;
    }
  }
  
  return;
}
예제 #19
0
/*---------------------------------------------------------------
						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() );

            out_selDirection	= in_gaps[selected_gap].representative_sector;
            out_selEvaluation	= selected_gap_eval;

            // D4: Es un gap ancho?
            // -----------------------------------------------------
            if ( (gap.end-gap.ini) < sectors_to_be_wide )
            {
                    // S3: Small gap
                    // -------------------------------------------
                    out_situation	= SITUATION_SMALL_GAP;
            }
            else
            {
                    // S4: Wide gap
                    // -------------------------------------------
                    out_situation	= SITUATION_WIDE_GAP;
            }

			// Que el risk no salga del gap:
			min_risk_eval_sector = gap.ini;
			max_risk_eval_sector = gap.end;
        }
    }

    // Calcular minima distancia a obstaculos a corto plazo, en
    //   un intervalo de sectores en torno al sector elegido:
    int     ancho_sectores = round( RISK_EVALUATION_SECTORS_PERCENT * obstacles.size() );
    int     sec_ini = max((int)min_risk_eval_sector, out_selDirection - ancho_sectores );
    int     sec_fin = min((int)max_risk_eval_sector, out_selDirection + ancho_sectores );

    out_riskEvaluation = 0.0;
    for (int i=sec_ini;i<=sec_fin;i++) out_riskEvaluation+= obstacles[ i ];
    out_riskEvaluation /= (sec_fin - sec_ini + 1 );

}
예제 #20
0
/*---------------------------------------------------------------
						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;
	}
}
예제 #21
0
/*---------------------------------------------------------------
						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++)
    {
		double profundidad =
				gaps_temp[i].maxDistance -
				gaps_temp[i].entranceDistance;

		if ( profundidad< maxProfundidad / 10.0f )
				borrar_gap[i]=true;
    }


	// Si es muy estrecho, pero hay uno casi igual pero UN POCO mas grande,
	//  borrar el estrecho:
    for (i=0;i<gaps_temp.size();i++)
    {
        int     ini_i = gaps_temp[i].ini;
        int     fin_i = gaps_temp[i].end;
		int		ancho_i = fin_i - ini_i;

		if ( !borrar_gap[i] )
		{
			for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
			{
				if (i!=j)
				{
					int     ini_j = gaps_temp[j].ini;
					int     fin_j = gaps_temp[j].end;
					int		ancho_j = fin_j - ini_j;

					// j dentro de i y UN POCO mas grande nada mas:
					if (	!borrar_gap[j] &&
							ini_j>=ini_i &&
							fin_j<=fin_i &&
							ancho_i < (0.05f*n) &&
							ancho_j < (0.25f*n)
						)
						borrar_gap[i] = true;
				}
			}
		}
	}

	// Si dentro tiene mas de 1, borrarlo:
   for (i=0;i<gaps_temp.size();i++)
    {
        int     ini_i = gaps_temp[i].ini;
        int     fin_i = gaps_temp[i].end;
		int		nDentro = 0;

		if ( !borrar_gap[i] )
		{
			for (unsigned int j=0;j<gaps_temp.size();j++)
			{
				if (i!=j)
				{
					int     ini_j = gaps_temp[j].ini;
					int     fin_j = gaps_temp[j].end;

					// j dentro de i:
					if (    !borrar_gap[j] &&
							ini_j>=ini_i &&
							fin_j<=fin_i ) nDentro++;
				}
			}
			if (nDentro>1) borrar_gap[i] = true;
		}
	}


	// Uno dentro de otro y practicamente a la misma altura: Eliminarlo tambien:
   for (i=0;i<gaps_temp.size();i++)
    {
		if (!borrar_gap[i])
		{
            double	ent_i = gaps_temp[i].entranceDistance;
            int     ini_i = gaps_temp[i].ini;
            int     fin_i = gaps_temp[i].end;

            double MIN_GAPS_ENTR_DIST = (MaximoAbsoluto-MinimoAbsoluto)/10.0f;

            for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
				if (i!=j)
				{
                    double	ent_j = gaps_temp[j].entranceDistance;
                    int		ini_j = gaps_temp[j].ini;
                    int		fin_j = gaps_temp[j].end;

                    // j dentro de i y casi misma "altura":
                    if (    !borrar_gap[j] &&
							!borrar_gap[i] &&
                            ini_j>=ini_i &&
                            fin_j<=fin_i &&
                            fabs(ent_i-ent_j)< MIN_GAPS_ENTR_DIST )
                                    borrar_gap[i]=true;
				}
		}
    }

    // Copiar solo huecos no marcados para borrar:
    // ---------------------------------------------------
    gaps_out.clear();
	gaps_out.reserve(15);
    for (i=0;i<gaps_temp.size();i++)
            if ( !borrar_gap[i] )
			{
				// Calcular direccion representativa:
				calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);

				gaps_out.push_back( gaps_temp[i] );
			}

}
예제 #22
0
// extracted from Particles3Dcomm.cpp
//
void Collective::read_particles_restart(
    const VCtopology3D* vct,
    int species_number,
    vector_double& u,
    vector_double& v,
    vector_double& w,
    vector_double& q,
    vector_double& x,
    vector_double& y,
    vector_double& z,
    vector_double& t)const
{
#ifdef NO_HDF5
  eprintf("Require HDF5 to read from restart file.");
#else
    if (vct->getCartesian_rank() == 0 && species_number == 0)
    {
      printf("LOADING PARTICLES FROM RESTART FILE in %s/restart.hdf\n",
        getRestartDirName().c_str());
    }
    stringstream ss;
    ss << vct->getCartesian_rank();
    string name_file = getRestartDirName() + "/restart" + ss.str() + ".hdf";
    // hdf stuff
    hid_t file_id, dataspace;
    hid_t datatype, dataset_id;
    herr_t status;
    size_t size;
    hsize_t dims_out[1];        /* dataset dimensions */
    int status_n;

    // open the hdf file
    file_id = H5Fopen(name_file.c_str(), H5F_ACC_RDWR, H5P_DEFAULT);
    if (file_id < 0) {
      eprintf("couldn't open file: %s\n"
        "\tRESTART NOT POSSIBLE", name_file.c_str());
      //cout << "couldn't open file: " << name_file << endl;
      //cout << "RESTART NOT POSSIBLE" << endl;
    }


    //find the last cycle
    int lastcycle=0;
    dataset_id = H5Dopen2(file_id, "/last_cycle", H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_INT, H5S_ALL, H5S_ALL, H5P_DEFAULT, &lastcycle);
    status = H5Dclose(dataset_id);

    stringstream species_name;
    species_name << species_number;

    ss.str("");ss << "/particles/species_" << species_number << "/x/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    datatype = H5Dget_type(dataset_id);
    size = H5Tget_size(datatype);
    dataspace = H5Dget_space(dataset_id); /* dataspace handle */
    status_n = H5Sget_simple_extent_dims(dataspace, dims_out, NULL);

    // get how many particles there are on this processor for this species
    status_n = H5Sget_simple_extent_dims(dataspace, dims_out, NULL);
    const int nop = dims_out[0]; // number of particles in this process
    //Particles3Dcomm::resize_SoA(nop);
    {
      //
      // allocate space for particles including padding
      //
      const int padded_nop = roundup_to_multiple(nop,DVECWIDTH);
      u.reserve(padded_nop);
      v.reserve(padded_nop);
      w.reserve(padded_nop);
      q.reserve(padded_nop);
      x.reserve(padded_nop);
      y.reserve(padded_nop);
      z.reserve(padded_nop);
      t.reserve(padded_nop);
      //
      // define size of particle data
      //
      u.resize(nop);
      v.resize(nop);
      w.resize(nop);
      q.resize(nop);
      x.resize(nop);
      y.resize(nop);
      z.resize(nop);
      t.resize(nop);
    }
    // get x
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &x[0]);
    // close the data set
    status = H5Dclose(dataset_id);

    // get y
    ss.str("");ss << "/particles/species_" << species_number << "/y/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &y[0]);
    status = H5Dclose(dataset_id);

    // get z
    ss.str("");ss << "/particles/species_" << species_number << "/z/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &z[0]);
    status = H5Dclose(dataset_id);

    // get u
    ss.str("");ss << "/particles/species_" << species_number << "/u/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &u[0]);
    status = H5Dclose(dataset_id);

    // get v
    ss.str("");ss << "/particles/species_" << species_number << "/v/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &v[0]);
    status = H5Dclose(dataset_id);

    // get w
    ss.str("");ss << "/particles/species_" << species_number << "/w/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &w[0]);
    status = H5Dclose(dataset_id);

    // get q
    ss.str("");ss << "/particles/species_" << species_number << "/q/cycle_" << lastcycle;
    dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &q[0]);

    //if ID is not saved, read in q as ID
    status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &t[0]);

    status = H5Dclose(dataset_id);

    /* get ID
		ss.str("");ss << "/particles/species_" << species_number << "/ID/cycle_" << lastcycle;
		dataset_id = H5Dopen2(file_id, ss.str().c_str(), H5P_DEFAULT); // HDF 1.8.8
		status = H5Dread(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, &t[0]);
		status = H5Dclose(dataset_id);
    */

    status = H5Fclose(file_id);
#endif
}
예제 #23
0
void harp::spec_sim::sky_truth ( vector_double & data ) const {

  double PI = std::atan2 ( 0.0, -1.0 );

  size_t halfspace = (size_t)( atmspace_ / 2 );

  size_t nreduced = 0;

  for ( size_t i = 0; i < nspec_; ++i ) {
    if ( i % skymod_ != 0 ) {
      ++nreduced;
    }
  }

  ++nreduced;

  size_t nbins = nreduced * nlambda_;

  data.resize ( nbins );

  size_t bin = 0;

  for ( size_t i = 0; i < nspec_; ++i ) {

    if ( i % skymod_ != 0 ) {

      size_t objoff = 2 * i;

      for ( size_t j = 0; j < nlambda_; ++j ) {

        double val = 0.0;

        if ( ( ( j + objoff ) % objspace_ ) == 0 ) {
          val += objpeak_;
        }

        data[ bin ] = val;

        ++bin;
      }

    }

  }

  for ( size_t j = 0; j < nlambda_; ++j ) {

    double val = 0.0;

    val += background_ * sin ( 3.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) ) * sin ( 7.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) ) * sin ( 11.0 * (double)j / ( (double)atmspace_ * 2.0 * PI ) );

    val += 2.0 * background_;

    if ( ( ( j + halfspace ) % atmspace_ ) == 0 ) {
      val += atmpeak_;
    }

    data[ bin ] = val;

    ++bin;
  }

  return;
}
예제 #24
0
/*---------------------------------------------------------------
						Evaluate each gap
  ---------------------------------------------------------------*/
void  CHolonomicND::evaluateGaps(
	const vector_double	&obstacles,
	const double		maxObsRange,
	const TGapArray		&gaps,
	const unsigned int	target_sector,
	const double		target_dist,
	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 double 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:
		double	meanDist = 0;
		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 double 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 +
				  options.factorWeights[2] * factor3 +
				  options.factorWeights[3] * factor_AntiCab ) / (math::sum(options.factorWeights)) ;
	} // for each gap
}
예제 #25
0
/*---------------------------------------------------------------
				Find gaps in the obtacles (Beta version)
  ---------------------------------------------------------------*/
void  CHolonomicND::gapsEstimator(
	const vector_double         & obstacles,
	const mrpt::math::TPoint2D  & target,
	TGapArray                   & gaps_out)
{
	const size_t n = obstacles.size();

	// ================ Parameters ================
	const int		GAPS_MIN_WIDTH = 3;
	const double	GAPS_MIN_DEPTH_CONSIDERED = 0.6;
	const double	GAPS_MAX_RELATIVE_DEPTH = 0.5;
	// ============================================

	// Find the maximum distances to obstacles:
	// ----------------------------------------------------------
	double overall_max_dist = std::numeric_limits<double>::min(), overall_min_dist = std::numeric_limits<double>::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) >= 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)
			continue;

		if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) > max_depth*GAPS_MAX_RELATIVE_DEPTH)
			delete_gaps[i] = 1;
	}

	//Delete gaps which contain more than one other gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i]) 
			continue;

		unsigned int inner_gap_count = 0;

		for (unsigned int j=0;j<nTempGaps;j++)
		{
			if (i==j || delete_gaps[j])
				continue;

			// j is inside of i?
			if (gaps_temp[j].ini >= gaps_temp[i].ini && gaps_temp[j].end <= gaps_temp[i].end )
				if (++inner_gap_count>1)
				{
					delete_gaps[i] = 1;
					break;
				}
		}
	}

	//Delete gaps included in other gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i]) 
			continue;

		for (unsigned int j=0;j<nTempGaps;j++)
		{
			if (i==j || delete_gaps[j])
				continue;
			if (gaps_temp[i].ini <= gaps_temp[j].ini && gaps_temp[i].end >= gaps_temp[j].end)
				delete_gaps[j] = 1;
		}
	}


	// Copy as result only those gaps not marked for deletion:
	// --------------------------------------------------------
	gaps_out.clear();
	gaps_out.reserve( nTempGaps/2 );
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i]) continue;

		// Compute the representative direction ("sector") for this gap:
		calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);

		gaps_out.push_back( gaps_temp[i] );
	}

}
예제 #26
0
/*---------------------------------------------------------------
						Search the best gap.
  ---------------------------------------------------------------*/
void  CHolonomicND::searchBestGap(
	const 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 = 0.05*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);
		int index_obstacles;

		for (int j=-freeSectorsNearTarget;j<=freeSectorsNearTarget;j++)
		{
			if (target_sector + j < 0)
				index_obstacles = obstacles.size() + (target_sector + j);
			else if (target_sector + j >= obstacles.size())
				index_obstacles = (target_sector + j) - obstacles.size();
			else
				index_obstacles = target_sector + j;

			if (obstacles[ index_obstacles ]<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):
		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;
		}
		else
		{
			// The selected gap:
			const TGap & gap = in_gaps[selected_gap];

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

			out_selDirection	= in_gaps[selected_gap].representative_sector;
			out_selEvaluation	= selected_gap_eval;

			// D4: Is it a WIDE gap?
			// -----------------------------------------------------
			if ( (gap.end-gap.ini) < sectors_to_be_wide )
			{
				// S3: Narrow gap
				// -------------------------------------------
				out_situation	= SITUATION_SMALL_GAP;
			}
			else
			{
				// S4: Wide gap
				// -------------------------------------------
				out_situation	= SITUATION_WIDE_GAP;
			}

			// Evaluate the risk only within the gap:
			min_risk_eval_sector = gap.ini;
			max_risk_eval_sector = gap.end;
		}
	}

	// Evaluate short-term minimum distance to obstacles, in a small interval around the selected direction:
	const unsigned int risk_eval_nsectors = round( options.RISK_EVALUATION_SECTORS_PERCENT * obstacles.size() );
	const unsigned int sec_ini = std::max(min_risk_eval_sector, risk_eval_nsectors<out_selDirection ? out_selDirection-risk_eval_nsectors : 0 );
	const unsigned int sec_fin = std::min(max_risk_eval_sector, out_selDirection + risk_eval_nsectors );

	out_riskEvaluation = 0.0;
	for (unsigned int i=sec_ini;i<=sec_fin;i++) out_riskEvaluation+= obstacles[ i ];
	out_riskEvaluation /= (sec_fin - sec_ini + 1 );
}
예제 #27
0
/*---------------------------------------------------------------
						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

}
예제 #28
0
/*---------------------------------------------------------------
	HornMethod
  ---------------------------------------------------------------*/
double scanmatching::HornMethod(
	const vector_double		&inVector,
	vector_double			&outVector,				// The output vector
	bool forceScaleToUnity )
{
	MRPT_START

	vector_double input;
	input.resize( inVector.size() );
	input = inVector;
	outVector.resize( 7 );

	// Compute the centroids
	TPoint3D	cL(0,0,0), cR(0,0,0);

	const size_t nMatches = input.size()/6;
	ASSERT_EQUAL_(input.size()%6, 0)

	for( unsigned int i = 0; i < nMatches; i++ )
	{
		cL.x += input[i*6+3];
		cL.y += input[i*6+4];
		cL.z += input[i*6+5];

		cR.x += input[i*6+0];
		cR.y += input[i*6+1];
		cR.z += input[i*6+2];
	}

	ASSERT_ABOVE_(nMatches,0)
	const double F = 1.0/nMatches;
	cL *= F;
	cR *= F;

	CMatrixDouble33 S; // S.zeros(); // Zeroed by default

	// Substract the centroid and compute the S matrix of cross products
	for( unsigned int i = 0; i < nMatches; i++ )
	{
		input[i*6+3] -= cL.x;
		input[i*6+4] -= cL.y;
		input[i*6+5] -= cL.z;

		input[i*6+0] -= cR.x;
		input[i*6+1] -= cR.y;
		input[i*6+2] -= cR.z;

		S.get_unsafe(0,0) += input[i*6+3]*input[i*6+0];
		S.get_unsafe(0,1) += input[i*6+3]*input[i*6+1];
		S.get_unsafe(0,2) += input[i*6+3]*input[i*6+2];

		S.get_unsafe(1,0) += input[i*6+4]*input[i*6+0];
		S.get_unsafe(1,1) += input[i*6+4]*input[i*6+1];
		S.get_unsafe(1,2) += input[i*6+4]*input[i*6+2];

		S.get_unsafe(2,0) += input[i*6+5]*input[i*6+0];
		S.get_unsafe(2,1) += input[i*6+5]*input[i*6+1];
		S.get_unsafe(2,2) += input[i*6+5]*input[i*6+2];
	}

	// Construct the N matrix
	CMatrixDouble44 N; // N.zeros(); // Zeroed by default

	N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
	N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
	N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
	N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );

	N.set_unsafe( 1,0,N.get_unsafe(0,1) );
	N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
	N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
	N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );

	N.set_unsafe( 2,0,N.get_unsafe(0,2) );
	N.set_unsafe( 2,1,N.get_unsafe(1,2) );
	N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
	N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );

	N.set_unsafe( 3,0,N.get_unsafe(0,3) );
	N.set_unsafe( 3,1,N.get_unsafe(1,3) );
	N.set_unsafe( 3,2,N.get_unsafe(2,3) );
	N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );

	// q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
	CMatrixDouble44 Z, D;
	vector_double v;

	N.eigenVectors( Z, D );
	Z.extractCol( Z.getColCount()-1, v );

	ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );

	// Make q_r > 0
	if( v[0] < 0 ){ v[0] *= -1;	v[1] *= -1;	v[2] *= -1;	v[3] *= -1;	}

	CPose3DQuat q;		// Create a pose rotation with the quaternion
	for(unsigned int i = 0; i < 4; i++ )			// insert the quaternion part
		outVector[i+3] = q[i+3] = v[i];

	// Compute scale
	double	num = 0.0;
	double	den = 0.0;
	for( unsigned int i = 0; i < nMatches; i++ )
	{
		num += square( input[i*6+0] ) + square( input[i*6+1] ) + square( input[i*6+2] );
		den += square( input[i*6+3] ) + square( input[i*6+4] ) + square( input[i*6+5] );
	} // end-for

	// The scale:
	double s = std::sqrt( num/den );

	// Enforce scale to be 1
	if( forceScaleToUnity )
		s = 1.0;

	TPoint3D pp;
	q.composePoint( cL.x, cL.y, cL.z, pp.x, pp.y, pp.z );
	pp*=s;

	outVector[0] = cR.x - pp.x;	// X
	outVector[1] = cR.y - pp.y;	// Y
	outVector[2] = cR.z - pp.z;	// Z

	return s; // return scale
	MRPT_END
}