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;
}
