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;
}
void harp::spec_sim::inv_variance ( vector_double & data ) const {
data.resize ( size_ );
data.clear();
return;
}
/*---------------------------------------------------------------
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;
}
/*---------------------------------------------------------------
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;
}
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 );
}
// 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);
}
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]);
}
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;
}
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;
}
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;
}
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;
}
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;
}
/*---------------------------------------------------------------
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];
}
/** 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];
}
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;
}
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;
}
/*---------------------------------------------------------------
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;
}
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;
}
/*---------------------------------------------------------------
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 );
}
/*---------------------------------------------------------------
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;
}
}
/*---------------------------------------------------------------
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] );
}
}
// 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
}
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;
}
/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
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] );
}
}
/*---------------------------------------------------------------
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 );
}
/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
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
}
