matrix<long> sir(MLNetworkSharedPtr& mnet, double beta, int tau, long num_iterations) { std::vector<std::string> statuses = {"S","I","R"}; std::vector<double> init_distribution({1,0,0}); std::string seed = "I"; beta_transition ISI("I","S",beta,"I"); tau_transition IR("I",tau,"R"); std::vector<transition*> transitions = {&ISI,&IR}; return run(mnet, statuses, init_distribution, seed, transitions, num_iterations); }
distribution* init_simple_distribution(int ndim, void* data, posterior_log_pdf_func* log_pdf, posterior_log_free* freef, error **err) { distribution *dist; dist = init_distribution(ndim,data,log_pdf,freef,NULL,err); forwardError(*err,__LINE__,NULL); return dist; }
distribution* init_fulllklbs_distribution(cmblkl** lkls,int nlkl, bs_struct* rbs, int *lmax, error **err) { distribution *dist; lklbs* self; self = init_fulllklbs(lkls,nlkl, rbs, lmax, err); forwardError(*err,__LINE__,NULL); dist = init_distribution(self->ndim+self->xdim,self,&lklbs_lkl,&free_lklbs,NULL,err); forwardError(*err,__LINE__,NULL); return dist; }
distribution* init_multilklbs_distribution(int ndim,cmblkl** lkls,int nlkl, void* bs, compute_cl_func* bs_compute, posterior_log_free* bs_free, int *lmax, error **err) { distribution *dist; lklbs* self; self = init_multilklbs(lkls,nlkl,ndim,bs,bs_compute,bs_free,lmax,err); forwardError(*err,__LINE__,NULL); dist = init_distribution(ndim+self->xdim,self,&lklbs_lkl,&free_lklbs,NULL,err); forwardError(*err,__LINE__,NULL); return dist; }
// deprecated stuff distribution* init_lklbs_distribution(int ndim,void* lkl, posterior_log_pdf_func* lkl_func, posterior_log_free *lkl_free, void* bs, compute_cl_func* bs_compute, posterior_log_free* bs_free, int nell, int* ell, int *lmax,error **err) { distribution *dist; lklbs* self; self = init_lklbs(lkl,lkl_func,lkl_free,ndim,bs,bs_compute,bs_free,nell,ell,lmax,err); forwardError(*err,__LINE__,NULL); dist = init_distribution(ndim+self->xdim,self,&lklbs_lkl,&free_lklbs,NULL,err); forwardError(*err,__LINE__,NULL); return dist; }
/** * @brief Main principal * @param argc El número de argumentos del programa * @param argv Cadenas de argumentos del programa * @return Nada si es correcto o algún número negativo si es incorrecto */ int main( int argc, char** argv ) { if( argc < 5 ) return -1; // Declaración de variables gsl_rng *rng; IplImage *frame, *hsv_frame; float **ref_histos, histo_aux[atoi(argv[4])][HTAM]; CvCapture *video; particle **particles, **aux, **nuevas_particulas; CvScalar color_rojo = CV_RGB(255,0,0), color_azul = CV_RGB(0,0,255); float factor = 1.0 / 255.0, sum = 0.0f; CvRect *regions; int num_objects = 0; int MAX_OBJECTS = atoi(argv[3]), PARTICLES = atoi(argv[2]); FILE *datos; char name[45], num[3], *p1, *p2; double t_ini, t_fin; video = cvCaptureFromFile( argv[1] ); if( !video ) { printf("No se pudo abrir el fichero de video %s\n", argv[1]); exit(-1); } int nFrames = (int) cvGetCaptureProperty( video , CV_CAP_PROP_FRAME_COUNT ); first_frame = cvQueryFrame( video ); num_objects = get_regions( ®ions, MAX_OBJECTS, argv[1] ); if( num_objects == 0 ) exit(-1); t_ini = omp_get_wtime(); // Inicializamos el generador de números aleatorios gsl_rng_env_setup(); rng = gsl_rng_alloc( gsl_rng_mt19937 ); gsl_rng_set(rng, (unsigned long) time(NULL)); hsv_frame = bgr2hsv( first_frame ); nuevas_particulas = (particle**) malloc( num_objects * sizeof( particle* ) ); for( int j = 0; j < num_objects; ++j ) nuevas_particulas[j] = (particle*) malloc( PARTICLES * sizeof( particle ) ); // Computamos los histogramas de referencia y distribuimos las partículas iniciales ref_histos = compute_ref_histos( hsv_frame, regions, num_objects ); particles = init_distribution( regions, num_objects, PARTICLES ); // Mostramos el tracking if( show_tracking ) { // Mostramos todas las partículas if( show_all ) for( int k = 0; k < num_objects; ++k ) for( int j = 0; j < PARTICLES; ++j ) display_particle( first_frame, particles[k][j], color_azul ); // Dibujamos la partícula más prometedora de cada objeto for( int k = 0; k < num_objects; ++k ) display_particle( first_frame, particles[k][0], color_rojo ); cvNamedWindow( "RGB", 1 ); cvShowImage( "RGB", first_frame ); cvWaitKey( 5 ); } // Exportamos los histogramas de referencia y los frames if( exportar ) { export_ref_histos( ref_histos, num_objects ); export_frame( first_frame, 1 ); for( int k = 0; k < num_objects; ++k ) { sprintf( num, "%02d", k ); strcpy( name, REGION_BASE); p1 = strrchr( argv[1], '/' ); p2 = strrchr( argv[1], '.' ); strncat( name, (++p1), p2-p1 ); strcat( name, num ); strcat( name, ".txt" ); datos = fopen( name, "a+" ); if( ! datos ) { printf("Error creando fichero para datos\n"); exit(-1);; } fprintf( datos, "%d\t%f\t%f\n", 0, particles[k][0].x, particles[k][0].y ); fclose( datos ); } } // Bucle principal!! #pragma omp parallel num_threads(atoi(argv[4])) shared(sum) for(int i = 1; i < nFrames; ++i) { // Recordar que frame no se puede liberar debido al cvQueryFrame #pragma omp master frame = cvQueryFrame( video ); #pragma omp barrier #pragma omp for schedule(guided,1) for( int f = 0 ; f < hsv_frame->height; ++f ) { float *ptrHSV = (float*) ( hsv_frame->imageData + hsv_frame->widthStep*f ); unsigned char *ptrRGB = (unsigned char*) ( frame->imageData + frame->widthStep*f ); float h; for( int col = 0, despH = 0; col < hsv_frame->width; ++col, despH+=3 ) { int despS = despH+1; int despV = despH+2; float b = ptrRGB[despH] * factor; float g = ptrRGB[despS] * factor; float r = ptrRGB[despV] * factor; float min = MIN(MIN(b, g), r); float max = MAX(MAX(b, g), r); ptrHSV[despV] = max; // v if( max != min ) { float delta = max - min; ptrHSV[despS] = delta / max; // s = (max - min) / max = 1.0 - (min / max) if( r == max ) h = ( g - b ) / delta; else if( g == max ) h = 2.0f + (( b - r ) / delta); else h = 4.0f + (( r - g ) / delta); h *= 60.0f; if(h < 0.0f) h += 360.0f; ptrHSV[despH] = h; // h } else { ptrHSV[despH] = 0.0f; // h ptrHSV[despS] = 0.0f; // s } } } // Realizamos la predicción y medición de probabilidad para cada partícula for( int k = 0; k < num_objects; ++k ) { sum = 0.0f; #pragma omp for reduction(+:sum) schedule(guided, 1) for( int j = 0; j < PARTICLES; ++j ) { transition( &particles[k][j], frame->width, frame->height, rng ); particles[k][j].w = likelihood( hsv_frame, &particles[k][j], ref_histos[k], histo_aux[omp_get_thread_num()] ); sum += particles[k][j].w; } // Normalizamos los pesos y remuestreamos un conjunto de partículas no ponderadas #pragma omp for for( int j = 0; j < PARTICLES; ++j ) particles[k][j].w /= sum; } // Remuestreamos un conjunto de partículas no ponderadas #pragma omp for for (int k = 0; k < num_objects; ++k ) resample( particles[k], PARTICLES, nuevas_particulas[k] ); #pragma omp master { aux = particles; particles = nuevas_particulas; nuevas_particulas = aux; // Mostramos el tracking if( show_tracking ) { // Mostramos todas las partículas if( show_all ) for( int k = 0; k < num_objects; ++k ) for( int j = 0; j < PARTICLES; ++j ) display_particle( frame, particles[k][j], color_azul ); // Dibujamos la partícula más prometedora de cada objeto for( int k = 0; k < num_objects; ++k ) display_particle( frame, particles[k][0], color_rojo ); cvNamedWindow( "RGB", 1 ); cvShowImage( "RGB", frame ); cvWaitKey( 5 ); } // Exportamos los histogramas de referencia y los frames if( exportar ) { export_frame( frame, i+1 ); for( int k = 0; k < num_objects; ++k ) { sprintf( num, "%02d", k ); strcpy( name, REGION_BASE); p1 = strrchr( argv[1], '/' ); p2 = strrchr( argv[1], '.' ); strncat( name, (++p1), p2-p1 ); strcat( name, num ); strcat( name, ".txt" ); datos = fopen( name, "a+" ); if( ! datos ) { printf("Error abriendo fichero para datos\n"); exit(-1); } fprintf( datos, "%d\t%f\t%f\n", i, particles[k][0].x, particles[k][0].y ); fclose( datos ); } } } } // Liberamos todos los recursos usados (mallocs, gsl y frames) cvReleaseImage( &hsv_frame ); cvReleaseCapture( &video ); gsl_rng_free( rng ); free( regions ); for( int i = 0; i < num_objects; ++i ) { free( ref_histos[i] ); free( particles[i] ); free( nuevas_particulas[i] ); } free( particles ); free( nuevas_particulas ); t_fin = omp_get_wtime(); printf("%d\t%d\t%.10g\n", PARTICLES, num_objects, (t_fin - t_ini) * 1000); }
/** * @brief Main principal * @param argc El número de argumentos del programa * @param argv Cadenas de argumentos del programa * @return Nada si es correcto o algún número negativo si es incorrecto */ int main( int argc, char** argv ) { if( argc < 4 ) return -1; // Declaración de variables gsl_rng *rng; IplImage *frame, *hsv_frame; histogram **ref_histos, *histo_aux; CvCapture *video; particle **particles, **aux, **nuevas_particulas; CvScalar color_rojo = CV_RGB(255,0,0), color_azul = CV_RGB(0,0,255); CvRect *regions; int num_objects = 0; int i = 1, MAX_OBJECTS = atoi(argv[3]), PARTICLES = atoi(argv[2]); FILE *datos; char name[45], num[3], *p1, *p2; clock_t t_ini, t_fin; double ms; video = cvCaptureFromFile( argv[1] ); if( !video ) { printf("No se pudo abrir el fichero de video %s\n", argv[1]); exit(-1); } first_frame = cvQueryFrame( video ); num_objects = get_regions( ®ions, MAX_OBJECTS, argv[1] ); if( num_objects == 0 ) exit(-1); t_ini = clock(); hsv_frame = bgr2hsv( first_frame ); histo_aux = (histogram*) malloc( sizeof(histogram) ); histo_aux->n = NH*NS + NV; nuevas_particulas = (particle**) malloc( num_objects * sizeof( particle* ) ); for( int j = 0; j < num_objects; ++j ) nuevas_particulas[j] = (particle*) malloc( PARTICLES * sizeof( particle ) ); // Computamos los histogramas de referencia y distribuimos las partículas iniciales ref_histos = compute_ref_histos( hsv_frame, regions, num_objects ); particles = init_distribution( regions, num_objects, PARTICLES ); // Mostramos el tracking if( show_tracking ) { // Mostramos todas las partículas if( show_all ) for( int k = 0; k < num_objects; ++k ) for( int j = 0; j < PARTICLES; ++j ) display_particle( first_frame, particles[k][j], color_azul ); // Dibujamos la partícula más prometedora de cada objeto for( int k = 0; k < num_objects; ++k ) display_particle( first_frame, particles[k][0], color_rojo ); cvNamedWindow( "Video", 1 ); cvShowImage( "Video", first_frame ); cvWaitKey( 5 ); } // Exportamos los histogramas de referencia y los frames if( exportar ) { export_ref_histos( ref_histos, num_objects ); export_frame( first_frame, 1 ); for( int k = 0; k < num_objects; ++k ) { sprintf( num, "%02d", k ); strcpy( name, REGION_BASE); p1 = strrchr( argv[1], '/' ); p2 = strrchr( argv[1], '.' ); strncat( name, (++p1), p2-p1 ); strcat( name, num ); strcat( name, ".txt" ); datos = fopen( name, "a+" ); if( ! datos ) { printf("Error creando fichero para datos\n"); return -1; } fprintf( datos, "%d\t%f\t%f\n", 0, particles[k][0].x, particles[k][0].y ); fclose( datos ); } } cvReleaseImage( &hsv_frame ); // Inicializamos el generador de números aleatorios gsl_rng_env_setup(); rng = gsl_rng_alloc( gsl_rng_mt19937 ); gsl_rng_set(rng, (unsigned long) time(NULL)); // Recordar que frame no se puede liberar debido al cvQueryFrame while( frame = cvQueryFrame( video ) ) { hsv_frame = bgr2hsv( frame ); // Realizamos la predicción y medición de probabilidad para cada partícula for( int k = 0; k < num_objects; ++k ) for( int j = 0; j < PARTICLES; ++j ) { transition( &particles[k][j], frame->width, frame->height, rng ); particles[k][j].w = likelihood( hsv_frame, &particles[k][j], ref_histos[k], histo_aux ); } // Normalizamos los pesos y remuestreamos un conjunto de partículas no ponderadas normalize_weights( particles, num_objects, PARTICLES ); for (int k = 0; k < num_objects; ++k ) resample( particles[k], PARTICLES, nuevas_particulas[k] ); aux = particles; particles = nuevas_particulas; nuevas_particulas = aux; // Mostramos el tracking if( show_tracking ) { // Mostramos todas las partículas if( show_all ) for( int k = 0; k < num_objects; ++k ) for( int j = 0; j < PARTICLES; ++j ) display_particle( frame, particles[k][j], color_azul ); // Dibujamos la partícula más prometedora de cada objeto for( int k = 0; k < num_objects; ++k ) display_particle( frame, particles[k][0], color_rojo ); cvNamedWindow( "Video", 1 ); cvShowImage( "Video", frame ); cvWaitKey( 5 ); } // Exportamos los histogramas de referencia y los frames if( exportar ) { export_frame( frame, i+1 ); for( int k = 0; k < num_objects; ++k ) { sprintf( num, "%02d", k ); strcpy( name, REGION_BASE); p1 = strrchr( argv[1], '/' ); p2 = strrchr( argv[1], '.' ); strncat( name, (++p1), p2-p1 ); strcat( name, num ); strcat( name, ".txt" ); datos = fopen( name, "a+" ); if( ! datos ) { printf("Error abriendo fichero para datos\n"); return -1; } fprintf( datos, "%d\t%f\t%f\n", i, particles[k][0].x, particles[k][0].y ); fclose( datos ); } } cvReleaseImage( &hsv_frame ); ++i; } // Liberamos todos los recursos usados (mallocs, gsl y frames) cvReleaseCapture( &video ); gsl_rng_free( rng ); free( histo_aux ); free( regions ); for( int i = 0; i < num_objects; ++i ) { free( ref_histos[i] ); free( particles[i] ); free( nuevas_particulas[i] ); } free( particles ); free( nuevas_particulas ); t_fin = clock(); ms = ((double)(t_fin - t_ini) / CLOCKS_PER_SEC) * 1000.0; printf("%d\t%d\t%.10g\n", PARTICLES, num_objects, ms); }
Particle_Filter(const ras::MapHolder &map_holder) : map_holder(map_holder), generator(std::chrono::system_clock::now().time_since_epoch().count()), loc_viz(map_holder, ras::MapHolder::RESOLUTION) { ros::NodeHandle nh("~"); nh.getParam("init_pos/x", x); nh.getParam("init_pos/y", y); nh.getParam("init_pos/theta", theta); nh.getParam("robot/base", base); nh.getParam("robot/wheel_radius", wheel_radius); nh.getParam("pf/particles", P); nh.getParam("pf/resample_var", resample_variance); nh.getParam("pf/var_x", var_x); nh.getParam("pf/var_y", var_y); nh.getParam("pf/var_theta", var_theta); nh.getParam("pf/use_global", use_global); nh.getParam("pf/var_q", var_q); nh.getParam("pf/var_redist_x", var_redist_x); nh.getParam("pf/var_redist_y", var_redist_y); nh.getParam("pf/var_redist_theta", var_redist_theta); nh.getParam("dw_multiplier", dw_multiplier); nh.getParam("dv_multiplier", dv_multiplier); nh.getParam("visualization/enabled", visualization_enabled); nh.getParam("visualization/use_distance", visualization_use_distance); nh.getParam("visualization/publish/particles", publish_particles); nh.getParam("visualization/publish/robot", publish_robot); nh.getParam("visualization/publish/walls", publish_walls); nh.getParam("visualization/publish/thickened_walls", publish_thickened_walls); nh.getParam("visualization/publish/path", publish_path); likelihood_const = 1.0 / (2.0 * M_PI * sqrt(pow(var_q, M))); x_noise = std::normal_distribution<double>(0.0, var_x); y_noise = std::normal_distribution<double>(0.0, var_y); theta_noise = std::normal_distribution<double>(0.0, var_theta); theta_init_distribution = std::uniform_real_distribution<double>(-M_PI, M_PI); particle_state = std::vector<ras::particle>(P); particle_state_aux = std::vector<ras::particle>(P); particle_weight = std::vector<double>(P); particle_weight_aux = std::vector<double>(P); cdf_aux = std::vector<double>(P); estimated_measurement = boost::numeric::ublas::matrix<double>(P, M); encoders_sub = nh.subscribe("/filters/encoders", 1, &Particle_Filter::encoder_callback, this); ir_sensor_sub = nh.subscribe("/filter/adc", 1, &Particle_Filter::ir_sensor_callback, this); position_pub = nh.advertise<geometry_msgs::Pose2D>("/particle/position", 1); correction_ser = nh.advertiseService("CorrectionSrvc", &Particle_Filter::CorrectionCall, this); grid_cell_pub = nh.advertise<nav_msgs::GridCells>("grid_cells", 1); particles_pub = nh.advertise<visualization_msgs::Marker>("particles", 1); robot_pub = nh.advertise<visualization_msgs::MarkerArray>("robot", 1); wall_cell_pub = nh.advertise<nav_msgs::GridCells>("wall_cells", 1); point_path_pub = nh.advertise<visualization_msgs::Marker>("point_path", 1); current_time = ros::Time::now(); last_time = ros::Time::now(); rate = new ros::Rate(HZ); // TODO this can maybe go outside of the contructor, but it can also stay here, not sure if (use_global) { init_distribution_uniform(0.0, map_holder.get_Xmax(), 0.0, map_holder.get_Ymax()); } else { init_distribution(x, y, theta); } predict = false; measure = false; }