void FORTE_TrkEclipse::executeEvent(int pa_nEIID){
QO() = QI();
switch (pa_nEIID){
case scm_nEventREQID:
if(true == QI()){
//TODO: test with and without type casts
track_update(); // get new image data
COUNT() = static_cast<TForteInt16>(track_count(CH()));
if(COUNT() > I_BLOB()){ // there is at least I_BLOBs on CH()
SIZE() = static_cast<TForteInt16>(track_size(CH(), I_BLOB())); //
FRAME() = static_cast<TForteInt16>(track_get_frame()); //
CONFIDENCE() = static_cast<TForteInt16>(track_confidence(CH(), I_BLOB()));
X() = static_cast<TForteInt16>(track_x(CH(), I_BLOB())); // get image data
Y() = static_cast<TForteInt16>(track_y(CH(), I_BLOB())); // and data
MAJOR() = static_cast<TForteInt16>(track_major_axis(CH(), I_BLOB())); // get image data
MINOR() = static_cast<TForteInt16>(track_minor_axis(CH(), I_BLOB())); // and data
if(true == INFO()){
if(COUNT() > I_BLOB()){ // there is a blob
printf("On Channel=%d Eclipse Blob#%d is at (X,Y,MAJOR,MINOR)=(%d,%d,%d,%d)\n", (int) CH(), (int) I_BLOB(), (int) X(), (int) Y(), (int) MAJOR(), (int) MINOR());
printf("\t Confidence=%d; Size=%d;Frame=%d\n", (int) CONFIDENCE(), (int) SIZE(), (int) FRAME());
}
else{
printf("No Blob#%i in sight on Color Channel %i\n", (int) I_BLOB(), (int) CH());
}
}
}
}
sendOutputEvent(scm_nEventCNFID);
break;
}
}
int main ()
{
float CENTER = 79.5;
float OFFSET = 0.05;
drone_connect();
enable_drone_vision();
drone_front_camera();
sleep(2);
drone_takeoff();
printf("Black button to exit test loop and land the drone");
float x, y;
track_update();
while(!black_button())
{
x = OFFSET * ((track_x(0,0) - CENTER) / CENTER);
y = OFFSET * ((track_y(0,0) - CENTER) / CENTER);
if(track_count(0) > 0)
{
printf("Items tracking: %d\n", track_count(0));
drone_move(0,0, x, y);
}
els
{
printf("No items: Hovering \n");
drone_hover();
}
sleep(4);
track_update();
}
printf("Landing");
drone_move(0,0,0,-.25);
sleep(0.5);
drone_land();
drone_disconnect();
return 0;
}
/* Global variables go below (if you absolutely need them).*/
int look_for_color_green() {
int x, y, color=2; // set up for color channel 2 (green)
while (black_button() == 0) //run till button is pressed
{
track_update(); // process the most recent image
if (track_count(color) > 0)
{ // get x, y for the biggest blob the channel sees
x = track_x(color,2); y = track_y(color,2);
printf("Biggest blob at (%d,%d)\n",x,y);
}
else
{ printf("No color match in Frame\n"); }
sleep(0.2); // give print time to register
}
printf("Program is done.\n");
}
int main()
{
//Init Variables
int color = RED;
int lastSeen = right;
int pumpTime = 0;
//Enable Servos
enable_servos();
sleep(.2);
beep();
while(1) {
//Check if we have time left on our Pump Counter
if(pumpTime > 0) {
//Turn Pump On
set_servo_position(servoPump, pumpOn);
}
else {
//Turn Pump Off
set_servo_position(servoPump, pumpOff);
}
//Image Capture
track_update();
//Check if We See a blob
if(track_count(color) > 0 ) {
//Find out where blobs are
//Left
if( track_x(color, 0) < 80) {
move(diagLeft, 500);
lastSeen = left;
}
//Right
else if( track_x(color, 0) > 120 ) {
move(diagRight, 500);
lastSeen = right;
}
//Middle
else {
move(forward, 500);
}
//Check if the Blob is close
if( track_bbox_top(color, 0) > 80) {
//Motors Off
ao();
//Extend Arm (Triggers Pneumatics)
set_servo_position(servoSwitch, 44);
sleep(1);
//Retract Arm (Triggers Pneumatics)
set_servo_position(servoSwitch, 1024);
sleep(1);
//We need to refill tanks
pumpTime += 100;
}
//****Screen Debug Info****
//printf("red blobs: %d\n", track_count(color));
printf("Blob X Cord: %d\n", track_x(color,0));
printf("Blob Y Cord: %d\n", track_y(color,0));
//printf("Blob Box Height: %d\n", track_bbox_height(color,0));
//printf("Blob Box Width: %d\n", track_bbox_width(color,0));
printf("track_size: %d\n", track_size(color, 0));
printf( "top y: %d\n\n", track_bbox_top(color, 0));
}
//Else We didn't find a blob
else{
printf( "Don't see the color blob.\n" );
// Search for ball using last known direction
move(lastSeen, 500);
}
//Decrement PumpTime
if(pumpTime > 0) pumpTime--;
}
}
/**
* process_and_show_images
*
* show the different images and save a video file with the results
*
*/
int
process_and_show_images (const char* name_output_video,
track_t list_of_tracks [MAX_CAM - MIN_CAM][MAX_FRAMES])
{
IplImage *ground_plane_image = NULL, *cam[MAX_CAM - MIN_CAM];
char cam_name [5][255];
IplImage *DispImage;
int current_frame, i, j ,k;
float x_fused = 0.0, y_fused = 0.0, x_fused_errors = 0.0, y_fused_errors = 0.0;
CvVideoWriter *writer = NULL;
CvSize s = cvSize (X_RESOLUTION, Y_RESOLUTION);
int contcam[MAX_CAM - MIN_CAM] = {0, 0, 0, 0, 0};
float acumcam_x[MAX_CAM - MIN_CAM] = {0.0, 0.0, 0.0, 0.0, 0.0};
float acumcam_y[MAX_CAM - MIN_CAM] = {0.0, 0.0, 0.0, 0.0, 0.0};
#ifdef DEBUG
printf ("process_and_show_images() \n");
#endif
#ifdef SHOW_IMAGE
printf ("Creating output video file: %s\n", name_output_video);
writer = cvCreateVideoWriter (name_output_video, CV_FOURCC ('M', 'J', 'P', 'G'), 15, s, 1);
//writer = cvCreateVideoWriter (name_output_video, -1, 15, s, 1);
cvNamedWindow ("Apidis", CV_WINDOW_AUTOSIZE);
DispImage = cvCreateImage (cvSize (X_RESOLUTION, Y_RESOLUTION), IPL_DEPTH_8U, 3);
#endif
/* for each frame */
for (current_frame = 0; current_frame < MAX_FRAMES; current_frame++) {
printf (".");
fflush (stdout);
#ifdef SHOW_IMAGE
sprintf (cam_name[0], "%s-%04d.jpeg", LOCATION_CAM1_IMAGES, current_frame);
sprintf (cam_name[1], "%s-%04d.jpeg", LOCATION_CAM2_IMAGES, current_frame);
sprintf (cam_name[2], "%s-%04d.jpeg", LOCATION_CAM4_IMAGES, current_frame);
/* Note that cam3 and cam4 are mapped to camera 6 and camera 7 images */
sprintf (cam_name[3], "%s-%04d.jpeg", LOCATION_CAM6_IMAGES, current_frame);
sprintf (cam_name[4], "%s-%04d.jpeg", LOCATION_CAM7_IMAGES, current_frame);
for (i = 0; i < MAX_CAM; i++) {
cam[i] = cvLoadImage (cam_name[i], CV_LOAD_IMAGE_COLOR);
if (cam[i] == NULL) {
printf ("Error opening image: %s \n", cam_name[i]);
exit (-1);
}
}
if (ground_plane_image == NULL)
ground_plane_image = cvLoadImage (LOCATION_GROUND_PLANE_IMAGE, CV_LOAD_IMAGE_COLOR);
cvSetImageROI (DispImage, cvRect (0, 0, 400, 300));
#endif
for (i = 0; i < MAX_CAM; i++) {
if (track_x (i, current_frame) != 0) { /* if camera 1 in current frame have information */
#ifdef SHOW_IMAGE
cvRectangle (cam[i], cvPoint ((int) track_x (i, current_frame), (int) track_y (i, current_frame)),
cvPoint ((int) track_x (i, current_frame) + track_w (i, current_frame),
(int) track_y (i, current_frame) + track_h (i, current_frame)), CV_RGB (255,255,0), 2, 8, 0);
cvRectangle (cam[i], cvPoint ((int) track_x_kalman (i, current_frame), (int) track_y_kalman (i, current_frame)),
cvPoint ((int) track_x_kalman (i, current_frame) + track_w_kalman (i, current_frame),
(int) track_y_kalman (i, current_frame) + track_h_kalman (i, current_frame)), CV_RGB (255,0,0), 2, 8, 0);
#endif
if (contcam[i] < 5) /* wait for having 5 values */
contcam[i]++;
if (contcam[i] == 5) {/* save stats */
/* get the current mean */
acumcam_x[i] = 0.0;
acumcam_y[i] = 0.0;
for (j = 0; j< 5; j++) {
acumcam_x[i] += track_px_global_kalman (i, current_frame - j);
acumcam_y[i] += track_py_global_kalman (i, current_frame - j);
}
px_global_mean_5 (i, current_frame) = acumcam_x[i] / (float) contcam[i];
py_global_mean_5 (i, current_frame) = acumcam_y[i] / (float) contcam[i];
for (j = 0; j < 5; j++) {
px_global_var_5 (i, current_frame) +=
pow(track_px_global (i, current_frame-j) - px_global_mean_5 (i, current_frame),2);
py_global_var_5 (i, current_frame) +=
pow(track_py_global (i, current_frame-j) - py_global_mean_5 (i, current_frame),2);
}
px_global_var_5 (i, current_frame) = px_global_var_5 (i, current_frame) / 5.0;
py_global_var_5 (i, current_frame) = py_global_var_5 (i, current_frame) / 5.0;
}
}
/* if we lost the objective initialize */
if (track_x (i, current_frame) == 0)
contcam[i] = 0;
#ifdef SHOW_IMAGE
cvResize (cam[i], DispImage, CV_INTER_AREA);
cvResetImageROI (DispImage);
if (i == 0)
cvSetImageROI (DispImage, cvRect (400, 0, 400, 300));
if (i == 1)
cvSetImageROI (DispImage, cvRect (800, 0, 400, 300));
if (i == 2)
cvSetImageROI (DispImage, cvRect (0, 300, 400, 300));
if (i == 3)
cvSetImageROI (DispImage, cvRect (400, 300, 400, 300));
if (i == 4)
cvSetImageROI (DispImage, cvRect (800, 300, 400, 300));
#endif
} /* for each camera */
x_fused = fused_px_global (current_frame, list_of_tracks);
y_fused = fused_py_global (current_frame, list_of_tracks);
x_fused_errors = fused_px_global_errors (current_frame, list_of_tracks);
y_fused_errors = fused_py_global_errors (current_frame, list_of_tracks);
/* for each camera, save the fused data in the data structure list_of_tracks */
for (k = 0; k < (MAX_CAM - MIN_CAM); k++) {
track_fused_x (k, current_frame) = x_fused;
track_fused_y (k, current_frame) = y_fused;
}
/* save the fused error (difference between filtered data and fused data) */
for (k = 0; k < (MAX_CAM - MIN_CAM); k++) {
if (track_py_local (k, current_frame) > 0) {
track_error_x (k, current_frame) = abs (track_fused_x (k, current_frame) - track_px_global_kalman (k, current_frame));
track_error_y (k, current_frame) = abs (track_fused_y (k, current_frame) - track_py_global_kalman (k, current_frame));
}
}
/* get the absolute errors between local data and fused data */
obtain_track_errors (current_frame, list_of_tracks);
#ifdef SHOW_IMAGE
/* drawing the player positions on the ground plane */
cvCircle (ground_plane_image, cvPoint (((int)(track_px_global (1, current_frame)/SCALE_X)),
((int) (track_py_global(1, current_frame)/SCALE_Y))) , 4, CV_RGB(255, 0, 0), CV_FILLED, CV_AA, 0);
printf ("Frame:%d, establishing player @: %f,%f --%f,%f --%f,%f\n", current_frame, track_fused_x(0, current_frame),
track_fused_y(0, current_frame), x_fused, y_fused, track_px_global(1, current_frame), track_py_global (1, current_frame));
cvResize (ground_plane_image, DispImage, CV_INTER_AREA);
cvResetImageROI (DispImage);
cvShowImage ("Apidis", DispImage);
cvWriteFrame (writer, DispImage);
/* free the memory */
for (i = 0; i < MAX_CAM; i++)
cvReleaseImage (&cam[i]);
cvWaitKey(2);
#endif
} /* for each frame */
/* get the mean errors of each camera */
obtain_mean_errors (list_of_tracks);
#ifdef SHOW_IMAGE
cvReleaseImage (&ground_plane_image);
cvReleaseVideoWriter (&writer);
cvDestroyWindow ("Apidis");
#endif
return 0;
}
