target.cpp

来自「用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控」· C++ 代码 · 共 312 行

/*
    Robot Interface Remote Client
    (C) 2006 Jason Hunt
    [e-mail]

    FILE: target.cpp
    
    Simple target tracking system

*/


#include <windows.h>
#include <stdio.h>
#include <math.h>

#pragma hdrstop

#include "display.h"
#include "target.h"
#include "network.h"
#include "robot.h"
#include "image.h"

// buffer for image processing
color_type *last_frame;
color_type *processed_frame;


unsigned int update = 0;    // used to keep track of camera sweep

bool targeting = false;     // targeting enabled
bool d_frame = false;       // display difference frame


unsigned int diff_thresh = 50;


/* Allocate or resize internal buffers */
void resize_target_buffers( void )
{
    if (last_frame) free(last_frame);
    if (processed_frame) free(last_frame);

    
    last_frame = (color_type *) malloc(image_x_size * image_y_size * sizeof(color_type));

    if (!last_frame)
        add_line("Unable to get buffer memory");
    
    processed_frame = (color_type *) malloc(image_x_size * image_y_size * sizeof(color_type));
    
    if (!processed_frame)
        add_line("Unable to get buffer memory");
    



}
/* End of resize_target_buffer */


/* Cmpute change between frames */
unsigned char get_change(unsigned int x, unsigned int y)
{
    unsigned int index = x + y * image_x_size;

    unsigned int change = (int)(abs(image_buffer[index].r - last_frame[index].r) + 
                                abs(image_buffer[index].g - last_frame[index].g) +
                                abs(image_buffer[index].b - last_frame[index].b)) / 3;


    if (change > diff_thresh)
        change = 255;
    else
        change = 0;


/*  if (change < 75) // 50 works
      change = 0;
      else
      change -= 75;*/

if (change > 255) change = 255;


    return((unsigned char) change);
}
/* end of get_change */
 
 
/* Compute change between frames */
color_type get_change_rgb(unsigned int x, unsigned int y)
{

    color_type c;
        
    unsigned int index = x + y * image_x_size;

    unsigned int r_diff = abs(image_buffer[index].r - last_frame[index].r);
    unsigned int g_diff = abs(image_buffer[index].g - last_frame[index].g);
    unsigned int b_diff = abs(image_buffer[index].b - last_frame[index].b);
    
    
    unsigned int change = (r_diff + g_diff + b_diff) / 3;
    

    if (change > 100)
    {
        if (r_diff > 255) r_diff = 255;
        if (g_diff > 255) g_diff = 255;
        if (b_diff > 255) b_diff = 255;
    
        c.r = r_diff;
        c.g = g_diff;
        c.b = b_diff;
    }
    else
    {
        c.r = 0;
        c.g = 0;
        c.b = 0;
    }
        
    

    return(c);
}
/* end of get_change */
 

/* draw a box */
void draw_box(unsigned int x1, unsigned int y1, 
              unsigned int x2, unsigned int y2, 
              color_type *m)
{
    // range check
    if (x1 < 0) x1 = 0;
    if (x1 > image_x_size-1) x1 = image_x_size - 1;

    if (x2 < 0) x2 = 0;
    if (x2 > image_x_size-1) x2 = image_x_size - 1;

    if (y1 < 0) y1 = 0;
    if (y1 > image_y_size-1) y1 = image_y_size - 1;

    if (y2 < 0) y2 = 0;
    if (y2 > image_y_size-1) y2 = image_y_size - 1;
            
    for (unsigned int y = y1; y < y2 ; y++) 
    {
        unsigned int index = x1 + y * image_x_size;

        for (unsigned int x = x1; x < x2 ; x++)
        {
            m[index].r = 255;
            m[index].g = 0;
            m[index].b = 0;
            index++;
        }
    }
}
/* end of draw box */



/* update pan and tilt based on camera data */
void process_frame( void )
{

    unsigned int sum_x = 0, sum_y = 0, num = 0, index = 0;

    // compute centroid of difference frame
    // with weighted averages
    for (unsigned int y = 0; y < image_y_size; y++)
        for (unsigned int x = 0; x < image_x_size; x++)
        {
            unsigned int  v = get_change(x, y);

            if (d_frame) // if we need to display the difference frame
            {
               // unsigned int tmp = v;
               // if (tmp > 255) tmp = 255;

//                processed_frame[index].g = (unsigned char)tmp;
//                processed_frame[index].r = 0;        
//                processed_frame[index].b = 0;

                

                  processed_frame[index] = get_change_rgb(x, y);

                index++;
            }

            sum_x += x*v;
            sum_y += y*v;
            num+=v;       
        }



    // waiting for camera to stabilize
   /* if (update++ <30)//< fps) 
    {
        // save the frame
        memcpy(last_frame, image_buffer, image_x_size*image_y_size * sizeof(color_type));  
        
        if (d_frame)
            memcpy(image_buffer, processed_frame, sizeof(color_type)*image_x_size*image_y_size);

        return;
    }*/

update = 0;

    // center of image
    unsigned int center_x, center_y;
    bool found = false;

    // make sure we have enough change to even bother moving
//   if (num > 1500)
//    if (num > 10000)


//   if (num > 10000)     // 10000 works
   
//   if (num > 1000)     // 10000 works
   
      if (num > 100000)     // 10000 works
   
    {
        center_x = sum_x / num;
        center_y = sum_y / num;
        found = true;
    } 
    else
    {
        center_x = image_x_size / 2;
        center_y = image_y_size / 2;
    }

    // display for debugging
    char b[300];
    sprintf(b, "num: %d ", num);
    add_line(b);
    
    // done with image, save to old image
    memcpy(last_frame, image_buffer, image_x_size*image_y_size * sizeof(color_type));   

    if (d_frame)
        memcpy(image_buffer, processed_frame, sizeof(color_type)*image_x_size*image_y_size);



    if (found)
    {
        unsigned int index = center_x;

        // draw lines
        for (unsigned int y = 0; y < image_y_size; y++, index+=image_x_size)
            image_buffer[index].g = 255;        

        index = center_y * image_x_size;

        for (unsigned int x = 0; x < image_x_size; x++)
            image_buffer[index++].g = 255;        


        unsigned int dead_zone = 20;

       
        
        int offset_x = image_x_size/2 - center_x;
        int offset_y = image_y_size/2 - center_y;


        int update_speed_x= offset_x / 10;
        int update_speed_y= offset_y / 10;
        
//        char b[400];
//        sprintf(b, "x: %d  y: %d", update_speed_x, update_speed_y);
//        add_line(b);
        
        int new_pan, new_tilt;
                      
               
        new_pan = robot_data.pan + update_speed_x;
        new_tilt = robot_data.tilt - update_speed_y;
        
        
        if (abs(update_speed_x) > 0 || abs(update_speed_y) > 0)
        update= 0;
        

        // draw targetting box
        draw_box(center_x-5,center_y-5, center_x+5, center_y+5, image_buffer);
        
       // fire_on();
    }
    else
    {
      //  fire_off();
       //send_value(0x64);   
    }    

}
/* End of process frame */