void
cluster(point ps[], int n, int k, int cs[])
{
ld **distances; // all the edge weights
void *clusters[n]; // array of pointers to clusters.
// note that the indexes of the array are
// the same as the indexes of the points
int point1, point2; // temporary variables that should store
// the next pair of closest points
// allocate memory for the arrays
distances = (ld **) smalloc(sizeof(ld *) * n);
// pre-calculate all the edge weights (distances
// between every point and every other point)
get_distances(distances, ps, n);
// initialize each cluster to have just 1 point
for(int i = 0; i < n; i++) {
clusters[i] = create_cluster(i);
}
// keep merging clusters until we're down to k clusters
for(int num_clusters = n; num_clusters > k; num_clusters--) {
// find the 2 closest clusters that aren't connected
next_shortest_edge(&point1, &point2, distances, n, clusters);
// merge them
merge_clusters(clusters[point1], clusters[point2]);
}
// convert the crazy cluster numbers to normal ones
normalize_clusters(cs, n, clusters);
}
void main(int argc, char *argv[])
{
int i;
if (argc != 1)
{
printf ("Sorry, i don't take command line arguments.\n");
exit(-1);
}
else
{
t_node n; // well heluuu there :)
init();
get_distances();
n=(t_node)malloc(sizeof(struct s_node));
n->path =(int *)malloc (d * sizeof (int));
n->path[1] = 1;
n->level = 1 ;
n->bound = get_bound(n);
get_paths(n);
}
//printf ( "\n");
// printf ("\n");
// printf ("# of tours %d\n",count_tours);
// printf ("* represents a non-recursive call.\n");
// printf ("** represents a recursive call.\n");
printf ( "%f\n",best_length);
for (i=1;i<=d;i++) { printf("%f %f\n",f1[best_path[i]],f2[best_path[i]]) ; } //echo best path
}
int main(int argc, char **argv)
{
CvCapture *capture = 0;
IplImage *frame = 0;
CvSeq *faces = 0;
FILE *out_fp = 0;
char key_pressed = 0;
int prev_faces = 0;
int delay_mili = 0;
int n;
clock_t start,end; /* for measuring fps */
distance_range distances[MAX_FACES];
check_cli(argc,argv);
printf("Registering camera input...\n");
if( ( capture = cvCreateCameraCapture( CAM_DEV_NUM ) ) == NULL )
{
printf("Can't connect to the camera!\n");
exit(1);
}
printf("Loading face classifier data...\n");
cascade = ( CvHaarClassifierCascade *) cvLoad( haar_data_file, 0, 0, 0 );
if( cascade == NULL )
{
printf("Can't load classifier data!\n");
exit(1);
}
printf("Initializating font...\n");
cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0, 0.0 );
printf("Opening output file...\n");
if( (out_fp = fopen(output_file,"w")) == NULL )
{
printf("Can't open output file: %s\n",output_file );
usage(argv[0]);
return -1;
}
/* Allocate calculation memory pool */
storage = cvCreateMemStorage(0);
/* Create display window */
if(!no_gui)
cvNamedWindow( WINDOW_NAME, CV_WINDOW_AUTOSIZE );
while(1)
{
start = clock();
frame = cvQueryFrame( capture );
if(!frame)
break;
/* Get all the faces on the current frame */
faces = detect_faces(frame);
/*
* Differren number of faces detected, make sure it's not a
* single frame mistake (they happen randomly).
*/
if( faces->total != prev_faces && faces->total > 0)
{
/* reopen file to clear it */
fclose(out_fp);
out_fp = fopen(output_file, "w");
prev_faces = faces->total;
continue;
}
/*
* Get distances between faces and the camera
*/
n = get_distances(faces, distances);
write_to_file(out_fp, distances, n);
/*
* Draw the result on the screen
*/
if(!no_gui)
{
select_faces(frame, faces);
draw_distances(frame, faces);
draw_fps(frame,actual_fps);
cvShowImage(WINDOW_NAME, frame);
}
/*
* Count the delay time to get wanted number of FPS. Time of execution
* of previous functions has to be considered.
*/
end = clock();
delay_mili = (1/(double)wanted_fps)*1000;
delay_mili-= (int)(end-start)/(double)CLOCKS_PER_SEC*1000;
if(delay_mili < MIN_FRAME_DELAY )
delay_mili = MIN_FRAME_DELAY;
/*
* Check if the close button was pressed
*/
key_pressed = cvWaitKey( delay_mili ) ;
if( key_pressed == CLOSE_BUTTON)
break;
end = clock();
actual_fps = (int)round(1/((double)(end-start)/(double)CLOCKS_PER_SEC));
}
cvReleaseCapture( & capture );
if(!no_gui)
cvDestroyWindow( WINDOW_NAME );
fclose(out_fp);
free_camera_data();
return 0;
}
void MyRobot::run()
{
double sum;
gps_initial[2] = 1000.0;
x = 0;
y = 0;
z = 0;
counter = 0;
_compass_angle_green[0]= 1000.0;
_compass_angle_green[1]= 1000.0;
end = 0;
metres =0;
num_person = 0;
turn = false;
back = false;
inside = false;
person = false;
following = false;
while (step(_time_step) != -1)
{
get_distances();
sum = 0;
for (int i = 0; i < NUM_DISTANCE_SENSOR; i++)
{
sum = sum + _dist_val[i];
}
//First we calculate initial gps position, the robot wont start until we know it
if(gps_initial[2] != 1000.0){
if (((gps[2]- gps_initial[2])>17) && (num_person < 2)){
//Start the identification
if ((_compass_angle_green[0] == 1000.0) || (_compass_angle_green[1] == 1000.0))
{
scaner_turn_around();
}
else
{
gps[2] = gps_initial[2] + 18;
_forward_camera->disable();
if (person == false)
{
rescue_person(_compass_angle_green[1]);
}
else
{
rescue_person(_compass_angle_green[0]);
}
}
}
else
{
//Start the going down
if (num_person == 2){
/*Checks if there is any distance sensor detecting a wall
and if this wall is close enought (200) to change the mode
from follow_compass to control */
if (sum <= 200)
{
//To follow down direction of the map
follow_compass_down(-135);
}
else
{
control_down();
}
}
else
{
//Start the going up
gps_average();
if (sum <= 200)
{
follow_compass(45);
}
else
{
control_up();
}
}
}
}else{
gps_average();
// Read the sensors
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
_compass_angle = convert_bearing_to_degrees(compass_val);
//We placed the robot in the correct direction.
if(_compass_angle >= 35 && _compass_angle < 55){
_mode = STOP;
}else{
_mode = FAST_TURN_AROUND;
}
}
// Set the mode
mode();
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
