Show as-misc.cc syntax highlighted
/* ex: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */
#include <devel/as-emstar/ds.h>
//given a Stratum node, stratify it into 4 subcells,
//output the range information in upper_left, and lower_right boundaries
//suppose the coordinate system starts from (0, 0) at the upper left corner
// (0,0)xxxxxxxxxxxxxxx
// y
// y
// y
// y
// y
// y
bool Stratify(Adaptive_Sampling *as, Stratum_Node *cur, loc_n * upper_left, loc_n * lower_right)
{
//currently divide it into 4 subcells,
//divide the space in half both horizontally and vertically
COORD_UNIT x1, x2, y1, y2;
COORD_UNIT delta_x, delta_y;
x1 = cur->upper_left.x;
y1 = cur->upper_left.y;
x2 = cur->lower_right.x;
y2 = cur->lower_right.y;
if ( (x2 - x1) < as->low_thre() )
{
//narrow in x dimension
if ( (y2 - y1) < as->low_thre() )
//too small to do further stratification
return false;
else {
//divide along y dimension
// ___
// | 1 |
// |---|
// | 2 |
// |---|
// | 3 |
// |---|
// | 4 |
// -----
delta_y = (y2 - y1 + 1) / 4;
if ( delta_y <= 0 )
//too small to do further stratification
return false;
for (int i=0; i<4; i++)
upper_left[i].x = x1;
for (int i=0; i<4; i++)
upper_left[i].y = y1 + ( i * delta_y );
for (int i=0; i<4; i++)
lower_right[i].x = x2;
for (int i=0; i<4; i++)
lower_right[i].y = y1 + (i+1) * delta_y - 1;
lower_right[3].y = y2;
}
} else {
if ( (y2 - y1) < as->low_thre() )
//narrow in y dimension
//so divide along x dimension
// -----------------
// | 1 | 2 | 3 | 4 |
// -----------------
{
delta_x = (x2 - x1 + 1) / 4;
if ( delta_x <= 0 )
//too small to do further stratification
return false;
for (int i=0; i<4; i++)
upper_left[i].x = x1 + ( i * delta_x );
for (int i=0; i<4; i++)
upper_left[i].y = y1;
for (int i=0; i<4; i++)
lower_right[i].x = x1 + ( (i+1) * delta_x ) - 1;
lower_right[3].x = x2;
for (int i=0; i<4; i++)
lower_right[i].y = y2;
} else {
//a normal cell, divide it along both x and y dimension
//-------------
//| 1 | 2 |
//| | |
//-------------
//| 3 | 4 |
//| | |
//-------------
delta_x = (x2 - x1 + 1) / 2;
delta_y = (y2 - y1 + 1) / 2;
if ( (delta_x <= 0) || (delta_y <= 0) )
//too small to do further stratification
return false;
upper_left[0].x = x1;
upper_left[0].y = y1;
lower_right[0].x = x1 + delta_x - 1;
lower_right[0].y = y1 + delta_y - 1;
upper_left[1].x = x1 + delta_x;
upper_left[1].y = y1;
lower_right[1].x = x2;
lower_right[1].y = y1 + delta_y - 1;
upper_left[2].x = x1;
upper_left[2].y = y1 + delta_y;
lower_right[2].x = x1 + delta_x - 1;
lower_right[2].y = y2;
upper_left[3].x = x1 + delta_x;
upper_left[3].y = y1 + delta_y;
lower_right[3].x = x2;
lower_right[3].y = y2;
}
}
return true;
}
//Generate samples for the current cell.
//
void gen_samples(Adaptive_Sampling *as, Stratum_Node * cur, Samples * sample_list)
{
fprintf( stderr, "enter gen_sample\n" );
fflush( stderr );
int num_x, num_y, index, total, num_samples, num_existing;
num_x = cur->lower_right.x - cur->upper_left.x + 1;
num_y = cur->lower_right.y - cur->upper_left.y + 1;
if ( ! as->migrate_samples )
num_existing = 0;
else num_existing = (( cur->samples ) ? cur->samples->size() : 0);
total = num_x * num_y;
num_samples = MIN( (int) (total * as->alpha() ), (total - num_existing));
fprintf( stderr, " %d, %d min = %d\n", (int)(total*as->alpha()), total-num_existing, num_samples );
fflush( stderr );
IntVector id_list;
COORD_UNIT loc_x, loc_y;
Sample *s_p;
IntVector::iterator find_p;
int count =0;
while (count < num_samples)
{
index = random_range(0, (total - 1));
find_p = find( id_list.begin(), id_list.end(), index);
//it already being picked before
if ( find_p != id_list.end() )
continue;
else {
//convert index to location
loc_y = cur->upper_left.y + (index / num_x) ;
loc_x = cur->upper_left.x + (index % num_x);
//loc_x, and loc_y is in terms of pixel, stratification are all in terms of pixel.
//check if (loc_x, loc_y) are already in the current node.
if ( as->migrate_samples && sample_in_node(loc_x, loc_y, cur) )
continue;
//add this sample into sample_list
s_p = new Sample;
s_p->loc.x = loc_x;
s_p->loc.y = loc_y;
s_p->stratum_p = cur;
sample_list->push_back( s_p );
id_list.push_back( index );
count ++;
}
}
fprintf( stderr, "leaving gen_sample\n" );
fflush( stderr );
}
bool sample_in_node(int loc_x, int loc_y, Stratum_Node * node)
{
Samples * list = node->samples;
if (list == NULL)
return false;
for (unsigned int i=0; i< list->size(); i++ )
{
if ( ((*list)[i]->loc.x == loc_x) && ((*list)[i]->loc.y == loc_y) )
return true;
}
return false;
}
Stratum_Node * in_child(Stratum_Node * cur, Sample * s_p)
{
Stratum_Node * cur_child;
for (int i=0; i<MAX_CHILDREN; i++)
{
cur_child = cur->child[i];
if ( in_range(s_p->loc, cur_child->upper_left, cur_child->lower_right) )
return cur_child;
}
return NULL;
}
//if point is within the bounding box defined by (upper_left, lower_right)
//return true; o/w return false;
bool in_range(loc_n point, loc_n upper_left, loc_n lower_right)
{
if ( (point.x >= upper_left.x) && (point.x <= lower_right.x)
&& (point.y >= upper_left.y) && (point.y <= lower_right.y) )
return true;
return false;
}
//currently robot move new_loc in no time,
//when move to emstar event model, will add delay
//later can also add noise to the pos = new_loc + noise;
void robot_position(Adaptive_Sampling *as, loc_world new_loc )
{
as->pos.x = new_loc.x;
as->pos.y = new_loc.y;
}
//compute mean, statistics based on samples in the current rank of the tree
//more statistics can be added later:
//Things need to change then:
//Stats defined in ds.h
void compute_statistics( Stratum_Node * cur )
{
//compute mean
elog( LOG_INFO, "compute_statistics\n" );
Sample_Type sum, diff;
if ( cur->samples == NULL )
{
cur->stat.mean = -1;
cur->stat.variance = 0;
return;
}
sum =0;
for (unsigned int i=0; i< cur->samples->size(); i++ )
sum += (* cur->samples)[i]->value;
cur->stat.mean = sum / cur->samples->size();
//compute variance
sum =0;
for (unsigned int i=0; i< cur->samples->size(); i++ )
{
diff = (* cur->samples)[i]->value - cur->stat.mean;
sum += diff * diff;
}
cur->stat.variance = sum / cur->samples->size();
if ( cur->stat.mean > 10000 )
elog( LOG_ERR, "Mean = %f, Variance = %f\n", cur->stat.mean, cur->stat.variance );
}
bool need_more_sample(Adaptive_Sampling *as, Stratum_Node *cur)
{
fprintf( stderr, "enter need_more_sample\n");
fflush( stderr );
unsigned int num_x, num_y, total;
if ( cur->samples == NULL )
return true;
if ( as->migrate_samples )
{
num_x = cur->lower_right.x - cur->upper_left.x + 1;
num_y = cur->lower_right.y - cur->upper_left.y + 1;
total = num_x * num_y;
if ( cur->samples->size() >= total )
return false;
}
if (cur->stat.variance > as->variance_thre() )
return true;
return false;
}
//take out all the stratum node with the same rank;
//fill in the cur_queue
void fill_cur_queue( Stratum_Node_Queue * work_queue, Stratum_Node_Queue * cur_queue )
{
Stratum_Node * cur;
int cur_rank;
if ( work_queue->empty() )
return;
cur = work_queue->front();
cur_rank = cur->rank;
while (cur->rank == cur_rank )
{
work_queue->pop_front();
cur_queue->push_back( cur );
if (work_queue->empty())
break;
cur = work_queue->front();
if ( cur == NULL )
break;
}
}
void stratify_n_gen_sample(Adaptive_Sampling *as, Stratum_Node_Queue * cur_queue, Samples * sample_list)
{
Stratum_Node * cur, * cur_child;
fprintf( stderr, "enter stratify_n_gen_sample\n" );
fflush( stderr );
loc_n upper_left_l[ MAX_CHILDREN ], lower_right_l[ MAX_CHILDREN ];
int cur_rank;
//cur_queue holds all the stratums in the current rank
for ( unsigned int i=0; i< cur_queue->size(); i++ )
{
cur = (* cur_queue)[i];
cur_rank = cur->rank;
//stratify the current cell into 4 cells
//
if ( ! Stratify(as, cur, upper_left_l, lower_right_l ) )
//current cell is too small to divide further
continue;
elog( LOG_ERR, "Stratify (%d, %d) , (%d, %d) to\n", cur->upper_left.x, cur->upper_left.y, cur->lower_right.x, cur->lower_right.y);
for (int i=0; i< MAX_CHILDREN; i++)
elog( LOG_ERR, "(%d, %d) , (%d, %d)\n", upper_left_l[i].x, upper_left_l[i].y, lower_right_l[i].x, lower_right_l[i].y);
// send_rec_to_matlab(as, cur->upper_left, cur->lower_right);
//send stratify results to MATLAB for visualization and monitoring
for (int i=0; i< MAX_CHILDREN; i++)
send_rec_to_matlab(as, upper_left_l[i], lower_right_l[i] );
for (int i=0; i< MAX_CHILDREN; i++)
{
cur_child = new Stratum_Node( upper_left_l[i].x, upper_left_l[i].y, lower_right_l[i].x, lower_right_l[i].y, cur, (cur_rank + 1) );
cur->child[i] = cur_child;
}
fprintf( stderr, "before migrate samples\n" );
fflush( stderr );
//migrate samples
if ( as->migrate_samples )
{
Migrate_Samples( cur );
}
fprintf( stderr, "After migrate samples\n" );
fflush( stderr );
//since to make sure total # samples in the current stratum < total number of pixels, has to do Migrate first.
for (int i=0; i< MAX_CHILDREN; i++)
{
cur_child = cur->child[i] ;
gen_samples(as, cur_child, sample_list );
}
}
}
void Migrate_Samples(Stratum_Node *cur)
{
Stratum_Node * child_p;
Sample * s_p;
if ( cur->samples == NULL )
return;
for (unsigned int i=0; i< cur->samples->size() ; i++ )
{
s_p = (* (cur->samples))[i];
//see which child it belongs to.
child_p = in_child(cur, s_p);
if ( child_p != NULL )
{
//move it to the child
if ( child_p->samples == NULL )
child_p->samples = new Samples;
if ( child_p->samples == NULL )
{
fprintf(stderr, "Out of memory\n");
exit( 1 );
}
//assert( child_p->samples != NULL );
child_p->samples->push_back( s_p );
}
}
//delete samples from the parent
//!! need to check whether the following affect the....
cur->samples->clear();
delete (cur->samples);
cur->samples = NULL;
}
void group_samples(Samples * sample_list)
{
//group samples
//suppose x is horizontal direction
//suppoes y is vertical direction
//robot is preferred to move vertically
//sort the samples: first by x, inside each x, sort by y
sort( sample_list->begin(), sample_list->end(), sample_first() );
//!!!!need to add test if the sort function work correctly..
}
void compute_stats_for_all(Adaptive_Sampling *as, Stratum_Node_Queue * work_queue, Stratum_Node_Queue * cur_queue)
{
Stratum_Node * cur;
//compute statistics for all the children Stratum of nodes in cur_queue.
//cur_queue holds all the stratums in the current rank
for ( unsigned int i=0; i< cur_queue->size(); i++ )
{
cur = (* cur_queue)[i];
if ( cur->child[0] != NULL )
{
for (unsigned int j=0; j< MAX_CHILDREN; j++)
{
compute_statistics( cur->child[j] );
if ( cur->child[j]->stat.mean == - 1)
elog( LOG_ERR, "Compute STA not touched\n" );
if ( cur->child[j]->stat.mean > 100000 )
elog( LOG_ERR, "wierd error\n" );
if ( need_more_sample(as, cur->child[j] ) )
{
elog( LOG_ERR, "Need more samples\n" );
//push it into the working queue
work_queue->push_back( cur->child[j] );
}
fprintf( stderr, "left need_more_sample\n");
fflush( stderr );
}
}
}
}
//test if the program work correctly
//print all the samples in the list
void print_samples( Samples * list, int type )
{
return;
Sample *s_p;
fprintf( stdout, "Samples:\n" );
for (unsigned int i=0; i< list->size(); i++)
{
s_p = (* list)[i];
if ( type == LOCATION )
fprintf( stdout, "(%d, %d)\n", s_p->loc.x, s_p->loc.y );
else if ( type == LOC_N_VALUE )
fprintf( stdout, "(%d, %d): %f\n", s_p->loc.x, s_p->loc.y, s_p->value );
}
}
void print_tree(FILE *fd, Adaptive_Sampling *as, Stratum_Node * t)
{
if ( t == NULL )
return;
int rank;
rank = t->rank;
Samples *list;
Sample *s_p;
loc_world loc_w;
for ( int i=0; i< rank; i++ )
{
fprintf( fd, " " );
}
if ( t->stat.mean > 10000 )
fprintf( stderr, "%f\n", t->stat.mean);
if ( t->samples != NULL )
compute_statistics( t );
fprintf( fd, "m = %f; v =%f (%d, %d) (%d, %d)\n", t->stat.mean, t->stat.variance, t->upper_left.x, t->upper_left.y, t->lower_right.x, t->lower_right.y );
if ( t->child[0] != NULL )
{
/*
for ( int i=0; i< rank; i++ )
{
fprintf( fd, " " );
}
fprintf( fd, "m = %f; v =%f\n", t->stat.mean, t->stat.variance );
*/
//leaf node, print out samples
list = t->samples;
if ( list != NULL )
{
for (unsigned int i=0; i< list->size(); i++)
{
s_p = (* list)[i];
for ( int j=0; j< rank; j++ )
{
fprintf( fd, " " );
}
//transform it to the world coordinates
as->field->pixel_2_world( &s_p->loc, &loc_w);
fprintf( fd, "(%d, %d)(%f, %f): %f\n", s_p->loc.x, s_p->loc.y, loc_w.x, loc_w.y, s_p->value );
}
}
for (unsigned int i=0; i< MAX_CHILDREN; i++)
{
print_tree(fd, as, t->child[i] );
}
} else {
//leaf node, print out samples
list = t->samples;
if ( list == NULL )
return;
for (unsigned int i=0; i< list->size(); i++)
{
s_p = (* list)[i];
for ( int j=0; j< rank; j++ )
{
fprintf( fd, " " );
}
//transform it to the world coordinates
as->field->pixel_2_world( &s_p->loc, &loc_w);
fprintf( fd, "(%d, %d)(%f, %f): %f\n", s_p->loc.x, s_p->loc.y, loc_w.x, loc_w.y, s_p->value );
}
}
fprintf( fd, "\n" );
}
void Field::SetWorldCoord(float x1, float y1, float x2, float y2)
{
world_upper_left.x = x1;
world_upper_left.y = y1;
world_lower_right.x = x2;
world_lower_right.y = y2;
}
void Field::SetVGACoord(float x1, float y1, float x2, float y2)
{
VGA_upper_left.x = x1;
VGA_upper_left.y = y1;
VGA_lower_right.x = x2;
VGA_lower_right.y = y2;
VGA_mode = true;
}
void Field::set_pixel_size(float x, float y)
{
pixel_size_x = x;
pixel_size_y = y;
pixel_upper_left.x = 1;
pixel_upper_left.y = 1;
pixel_lower_right.x = pixel_upper_left.x + (int) ( (world_lower_right.x - world_upper_left.x) / x);
pixel_lower_right.y = pixel_upper_left.y + (int) ( (world_lower_right.y - world_upper_left.y) / y);
if (pixel_lower_right.x == pixel_upper_left.x)
return;
pixel_size_x = (world_lower_right.x - world_upper_left.x) / (pixel_lower_right.x - pixel_upper_left.x);
pixel_size_y = (world_lower_right.y - world_upper_left.y) / (pixel_lower_right.y - pixel_upper_left.y);
}
//return the center of the pixel *loc* in terms of
//unit of world coordinates in loc_w
// if pixel index is out of range defined by pixel_upper_left,
//pixel_lower_right, return false, o/w return true
bool Field::pixel_2_world(loc_n * loc, loc_world * loc_w)
{
float dx, dy;
if ((loc->x < pixel_upper_left.x) || (loc->x > pixel_lower_right.x) || (loc->y < pixel_upper_left.y) || (loc->y > pixel_lower_right.y))
return false;
dx = pixel_size_x / 2;
dy = pixel_size_y / 2;
loc_w->x = world_upper_left.x + (loc->x - pixel_upper_left.x) * pixel_size_x;
loc_w->x += floor(dx / MOTOR_RESOLUTION) * MOTOR_RESOLUTION;
loc_w->y = world_upper_left.y + (loc->y - pixel_upper_left.y) * pixel_size_y;
loc_w->y += floor(dy / MOTOR_RESOLUTION) * MOTOR_RESOLUTION;
return true;
}
//return the upper left corner of the pixel *loc* in terms of
//unit of world coordinates in loc_w
// if pixel index is out of range defined by pixel_upper_left,
//pixel_lower_right, return false, o/w return true
bool Field::pixel_2_world_up_left(loc_n * loc, loc_world * loc_w)
{
if ((loc->x < pixel_upper_left.x) || (loc->x > pixel_lower_right.x) || (loc->y < pixel_upper_left.y) || (loc->y > pixel_lower_right.y))
return false;
loc_w->x = world_upper_left.x + (loc->x - pixel_upper_left.x) * pixel_size_x;
loc_w->y = world_upper_left.y + (loc->y - pixel_upper_left.y) * pixel_size_y;
return true;
}
void send_rec_to_matlab(Adaptive_Sampling *as, loc_n upper_left, loc_n lower_right)
{
Rectangle rec;
loc_world upper, lower;
as->field->pixel_2_world_up_left( &upper_left, &upper );
as->field->pixel_2_world_up_left( &lower_right, &lower );
rec.x1 = upper.x ;
rec.y1 = upper.y ;
rec.x2 = lower.x ;
rec.y2 = lower.y ;
rec.pixel_size_x = as->field->pixel_size_x;
rec.pixel_size_y = as->field->pixel_size_y;
if ( as->verbose() )
fprintf( stderr, "Send Rec to matlab: (%f %f), (%f %f) %f, %f\n", rec.x1, rec.y1, rec.x2, rec.y2, rec.pixel_size_x, rec.pixel_size_y );
elog( LOG_INFO, "Send Rec to matlab: (%f %f), (%f %f) %f, %f\n", rec.x1, rec.y1, rec.x2, rec.y2, rec.pixel_size_x, rec.pixel_size_y );
tcp_send(as->sd(), &rec, sizeof( Rectangle ) );
}
void send_sample_to_matlab(Adaptive_Sampling *as, loc_world pos, NimsSensorDataType val)
{
Rectangle rec;
rec.x1 = POINTS;
rec.y1 = pos.x;
rec.x2 = pos.y;
rec.y2 = val;
rec.pixel_size_x = 0;
rec.pixel_size_y = 0;
if ( as->verbose() )
fprintf( stderr, "send sample to matlab: (%f %f), (%f %f) %f, %f\n", rec.x1, rec.y1, rec.x2, rec.y2, rec.pixel_size_x, rec.pixel_size_y );
elog( LOG_ERR, "send sample to Matlab: (%f %f), v= %f\n", rec.y1, rec.x2, rec.y2 );
tcp_send(as->sd(), &rec, sizeof( Rectangle ) );
fprintf( as->sample_fd(), "%f %f %f\n", rec.y1, rec.x2, rec.y2 );
fflush( as->sample_fd() );
}
void send_cmd_to_matlab(Adaptive_Sampling *as, int cmd)
{
Rectangle rec;
rec.x1 = cmd;
rec.y1 = 0;
rec.x2 = 0;
rec.y2 = 0;
rec.pixel_size_x = 0;
rec.pixel_size_y = 0;
if ( as->verbose() )
fprintf( stderr, "send cmd to matlab: (%f %f), (%f %f) %f, %f\n", rec.x1, rec.y1, rec.x2, rec.y2, rec.pixel_size_x, rec.pixel_size_y );
elog( LOG_ERR, "send cmd to Matlab: (%f %f), v= %f\n", rec.y1, rec.x2, rec.y2 );
tcp_send(as->sd(), &rec, sizeof( Rectangle ) );
}
void as_misc_init(Adaptive_Sampling *as)
{
int sd;
FILE *fd;
//create the root node
as->root = new Stratum_Node(as->field->pixel_upper_left.x, as->field->pixel_upper_left.y, as->field->pixel_lower_right.x, as->field->pixel_lower_right.y, NULL, 0 );
as->stratum_node_work_queue.push_back( as->root );
//test rectangle transfering
if ( (as->sd() < 0) && (strlen(as->matlab_ip) > 0) )
{
sd = init_connection( PORT_NUMBER, as->matlab_ip);
if ( sd < 0 )
{
elog( LOG_ERR, "can not build connection to Matlab\n" );
#ifdef MATLAB_REQUIRED
exit(-1);
#endif
} else {
elog( LOG_ERR, "successfully build connection to Matlab\n" );
// exit(1);
}
as->set_sd( sd );
}
if (as->sample_fname == NULL)
{
as->sample_fname = g_new0( char, 20 );
strcpy( as->sample_fname, "/tmp/samples" );
}
if ( as->sample_fd() == NULL )
{
fd = fopen( as->sample_fname, "w");
if ( fd == NULL )
{
elog( LOG_WARNING, "open /tmp/samples ERROR\n" );
}
as->set_sample_fd(fd );
}
//send the root bounding box to matlab.
send_rec_to_matlab(as, as->root->upper_left, as->root->lower_right);
}
void as_prepare_samples(Adaptive_Sampling *as)
{
elog( LOG_ERR, "in beginning of as_prepare_samples\n" );
if ( ! as->stratum_node_work_queue.empty() )
{
elog( LOG_ERR, "as_prepare_samples: work queue is not empty\n" );
as->stratum_node_cur_queue.clear();
as->sample_list.clear();
//take out all the stratum node at the same rank from
//the head of stratum_node_work_queue to
//stratum_node_cur_queue ;
fill_cur_queue( & (as->stratum_node_work_queue), & (as->stratum_node_cur_queue) );
if ( as->stratum_node_cur_queue.empty() )
return;
//stratify the node in stratum_node_cur_queue, and generate
//samples for all its children
//stratum_node_cur_queue holds all the stratums in the current rank
stratify_n_gen_sample(as, & (as->stratum_node_cur_queue), & (as->sample_list) );
if ( as->verbose() )
{
fprintf( stdout, "Before sort:\n" );
print_samples( & (as->sample_list), LOCATION );
}
//group samples, robot sampling action optimization
//suppose x is horizontal direction
//suppoes y is vertical direction
//robot is preferred to move vertically
//sort the samples: first by x, inside each x, sort by y
if ( ! as->sample_list.empty() )
group_samples( &(as->sample_list) );
if ( as->verbose() )
{
//see if the samples are corted correctly
fprintf( stdout, "After sort:\n" );
print_samples( &(as->sample_list), LOCATION );
}
}
elog( LOG_ERR, "At the end of as_prepare_samples\n" );
}
See more files for this project here