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 }