Exemple #1
0
int main(int argc, char* argv[]) {
    /* Arguments */
    if ( argc != 2 ) {
        printf("\nUsage: %s <file>\n\n", argv[0]);
        exit(0);
    }

    pgm_t* src = pgm_read(argv[1]);
    
    dmap_t* sobx = sobel_x(src);
    dmap_t* soby = sobel_y(src);
    dmap_t* sobm = magnitude(src, sobx, soby);
    dmap_t* mxx = multiply(src, sobx, sobx);
    dmap_t* myy = multiply(src, soby, soby);
    dmap_t* mxy = multiply(src, sobx, soby);
    dmap_t* mxxf = binomial_filter(mxx);
    dmap_t* myyf = binomial_filter(myy);
    dmap_t* mxyf = binomial_filter(mxy);
    dmap_t* harris1 = harris(src, mxxf, myyf, mxyf, 1);
    dmap_t* harris1f = harris(src, mxxf, myyf, mxyf, 1);

    dmap_write(sobx, argv[1], "sobel_x");
    dmap_write(soby, argv[1], "sobel_y");
    dmap_write(sobm, argv[1], "sobel_magnitude");

    dmap_write(mxx, argv[1], "xx");
    dmap_write(myy, argv[1], "yy");
    dmap_write(mxy, argv[1], "xy");

    dmap_write(mxxf, argv[1], "xxf");
    dmap_write(myyf, argv[1], "yyf");
    dmap_write(mxyf, argv[1], "xyf");

    dmap_write(harris1, argv[1], "harris1");
    dmap_write(harris1f, argv[1], "harris1f");

    pgm_free(src);
    dmap_free(sobx);
    dmap_free(soby);
    dmap_free(sobm);
    dmap_free(mxx);
    dmap_free(myy);
    dmap_free(mxy);
    dmap_free(harris1);

    return 0;
}
Exemple #2
0
int main()
{
  void *buffer1,*buffer2,*buffer3,*buffer4;
  unsigned short *image;
  unsigned char *grayscale;
  unsigned char current_mode;
  unsigned char mode;
  init_LCD();
  init_camera();
  vga_set_swap(VGA_QuarterScreen|VGA_Grayscale);
  printf("Hello from Nios II!\n");
  cam_get_profiling();
  buffer1 = (void *) malloc(cam_get_xsize()*cam_get_ysize());
  buffer2 = (void *) malloc(cam_get_xsize()*cam_get_ysize());
  buffer3 = (void *) malloc(cam_get_xsize()*cam_get_ysize());
  buffer4 = (void *) malloc(cam_get_xsize()*cam_get_ysize());
  cam_set_image_pointer(0,buffer1);
  cam_set_image_pointer(1,buffer2);
  cam_set_image_pointer(2,buffer3);
  cam_set_image_pointer(3,buffer4);
  enable_continues_mode();
  init_sobel_arrays(cam_get_xsize()>>1,cam_get_ysize());
  do {
	  if (new_image_available() != 0) {
		  if (current_image_valid()!=0) {
			  current_mode = DIPSW_get_value();
			  mode = current_mode&(DIPSW_SW1_MASK|DIPSW_SW3_MASK|DIPSW_SW2_MASK);
			  image = (unsigned short*)current_image_pointer();
		      switch (mode) {
		      case 0 : transfer_LCD_with_dma(&image[16520],
		                	cam_get_xsize()>>1,
		                	cam_get_ysize(),0);
		      	  	   if ((current_mode&DIPSW_SW8_MASK)!=0) {
		      	  		  vga_set_swap(VGA_QuarterScreen);
		      	  		  vga_set_pointer(image);
		      	  	   }
		      	  	   break;
		      case 1 : conv_grayscale((void *)image,
		    		                  cam_get_xsize()>>1,
		    		                  cam_get_ysize());
		               grayscale = get_grayscale_picture();
		               transfer_LCD_with_dma(&grayscale[16520],
		      		                	cam_get_xsize()>>1,
		      		                	cam_get_ysize(),1);
		      	  	   if ((current_mode&DIPSW_SW8_MASK)!=0) {
		      	  		  vga_set_swap(VGA_QuarterScreen|VGA_Grayscale);
		      	  		  vga_set_pointer(grayscale);
		      	  	   }
		      	  	   break;
		      case 2 : conv_grayscale((void *)image,
		    		                  cam_get_xsize()>>1,
		    		                  cam_get_ysize());
		               grayscale = get_grayscale_picture();
		               sobel_x_with_rgb(grayscale);
		               image = GetSobel_rgb();
		               transfer_LCD_with_dma(&image[16520],
		      		                	cam_get_xsize()>>1,
		      		                	cam_get_ysize(),0);
		      	  	   if ((current_mode&DIPSW_SW8_MASK)!=0) {
		      	  		  vga_set_swap(VGA_QuarterScreen);
		      	  		  vga_set_pointer(image);
		      	  	   }
		      	  	   break;
		      case 3 : conv_grayscale((void *)image,
		    		                  cam_get_xsize()>>1,
		    		                  cam_get_ysize());
		               grayscale = get_grayscale_picture();
		               sobel_x(grayscale);
		               sobel_y_with_rgb(grayscale);
		               image = GetSobel_rgb();
		               transfer_LCD_with_dma(&image[16520],
		      		                	cam_get_xsize()>>1,
		      		                	cam_get_ysize(),0);
		      	  	   if ((current_mode&DIPSW_SW8_MASK)!=0) {
		      	  		  vga_set_swap(VGA_QuarterScreen);
		      	  		  vga_set_pointer(image);
		      	  	   }
		      	  	   break;
		      default: conv_grayscale((void *)image,
	                                  cam_get_xsize()>>1,
	                                  cam_get_ysize());
                       grayscale = get_grayscale_picture();
                       sobel_x(grayscale);
                       sobel_y(grayscale);
                       sobel_threshold(128);
                       grayscale=GetSobelResult();
		               transfer_LCD_with_dma(&grayscale[16520],
		      		                	cam_get_xsize()>>1,
		      		                	cam_get_ysize(),1);
		      	  	   if ((current_mode&DIPSW_SW8_MASK)!=0) {
		      	  		  vga_set_swap(VGA_QuarterScreen|VGA_Grayscale);
		      	  		  vga_set_pointer(grayscale);
		      	  	   }
		      	  	   break;
		      }
		  }
	  }
  } while (1);
  return 0;
}
/***********************************************************
* @fn imageCallback(const sensor_msgs::ImageConstPtr& msg)
* @brief segments image
* @pre takes in a ros message of a raw or cv image
* @post publishes a CV_32FC1 image using cv_bridge
* @param takes in a ros message of a raw or cv image 
***********************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{

    cv_bridge::CvImagePtr cv_ptr_src;

    //takes in the image
    try
    {
      cv_ptr_src = cv_bridge::toCvCopy(msg, "rgb8");
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    
    
    //create internal images
    cv::Mat sobel_x(cv_ptr_src->image.size(), CV_32FC3);
    cv::Mat sobel_y(cv_ptr_src->image.size(), CV_32FC3);
    cv::Mat dbg(cv_ptr_src->image.size(), CV_8UC3);
    cv_bridge::CvImage dbg_msg;
    
    
    //set debug to zero
    dbg = cv::Mat::zeros(cv_ptr_src->image.size(), CV_8UC3);
    
    //preforms x and y sobels
    cv::Sobel(cv_ptr_src->image, sobel_x, cv_ptr_src->image.depth(), params.sobel_order, 0, params.sobel_size, params.sobel_scaler);
    cv::Sobel(cv_ptr_src->image, sobel_y, cv_ptr_src->image.depth(), 0, params.sobel_order, params.sobel_size, params.sobel_scaler);
    
    //create pixel pointer data
    //TODO could be created globaly?
    pixel pixels[cv_ptr_src->image.cols][cv_ptr_src->image.rows];
    for(int y = 0; y < cv_ptr_src->image.rows; y++)
    {
        for(int x = 0; x < cv_ptr_src->image.cols; x++)
        {
            
            pixels[x][y].x = x;
            pixels[x][y].y = y;
        }
    }
    
    //run lengh vectors
    std::vector<run_length> x_run_lengths;
    std::vector<run_length> y_run_lengths;
    
    //find xrun lengths
    for (int y = 0; y < sobel_x.rows; y++)
    {
        run_length current;
        current.start = &pixels[0][y];
        current.members.push_back(&pixels[0][y]);
        
        current.avg.y = y;
        current.avg.x = 0;
        
        //calculate sums 
        for (int c = 0; c < sobel_x.depth(); c++)
        {
            //start average 
            current.avg.chanel[c] = cv_ptr_src->image.at<int>(0,y,c);
        }
        
        
        //initalize slope_var and pre_val
        int pre_val[sobel_x.depth()];
        double slope_var = 0;
        for (int c = 0; c < sobel_x.depth(); c++)
        {
            pre_val[c] = sobel_x.at<int>(0,y,c);
        }
        
        
        //move along cols
        for(int x = 1; x < sobel_x.cols; x++)
        {
            for (int c = 0; c < sobel_x.depth(); c++)
            {
                //calculate variance accoss image
                slope_var += abs(sobel_x.at<int>(x,y,c) - pre_val[c]);
            }
            
            //check to see if we need a new run
            if(slope_var > params.slope_var_thresh)
            {
                //set end as previous pixel
                current.end = &pixels[x-1][y];
                
                current.linked = false;
                
                /*do this after linking
                //calculate average
                for (int c = 0; c < sobel_x.depth)
                {
                    //divide sum by nuber of pixels 
                    current.avg.chanel[c] /= current.members.size;
                }
                */
                
                
                //push back run_lengh
                x_run_lengths.push_back(current);
                
                //link members to run_lengh
                for (unsigned int i = 0; i < x_run_lengths.back().members.size() ; i++ )
                {
                    x_run_lengths.back().members.at(i)->x_parent = &x_run_lengths.back();
                } 
                
                //start new run
                current.start = &pixels[x][y];
                current.members.push_back(&pixels[x][y]);
                
                current.avg.y = y;
                current.avg.x = x;
                //calculate sums 
                for (int c = 0; c < sobel_x.depth(); c++)
                {
                    //start average 
                    current.avg.chanel[c] = cv_ptr_src->image.at<int>(x,y,c);
                }
                
                
                //initalize slope_var and pre_val
                slope_var = 0;
                for (int c = 0; c < sobel_x.depth(); c++)
                {
                    pre_val[c] = sobel_x.at<int>(0,y,c);
                }
                
            }
            else
            {
                //pixel is part of current run
                
                //add pixel as a member
                current.members.push_back(&pixels[x][y]);
                
                //calculate sums 
                for (int c = 0; c < sobel_x.depth(); c++)
                {
                    //add average 
                    current.avg.chanel[c] += cv_ptr_src->image.at<int>(x,y,c);
                }
                current.avg.x += x;
            }
        }
    
    }
    
    ROS_INFO("color: x_run_lengths:%i", x_run_lengths.size());
    
    //find yrun lengths
    for (int x = 0; x < sobel_y.rows; x++)
    {
        run_length current;
        current.start = &pixels[x][0];
        current.members.push_back(&pixels[x][0]);
        
        current.avg.x = x;
        current.avg.y = 0;
        //calculate sums 
        for (int c = 0; c < sobel_y.depth(); c++)
        {
            //start average 
            current.avg.chanel[c] = cv_ptr_src->image.at<int>(x,0,c);
        }
        
        
        //initalize slope_var and pre_val
        int pre_val[sobel_y.depth()];
        double slope_var = 0;
        for (int c = 0; c < sobel_y.depth(); c++)
        {
            pre_val[c] = sobel_y.at<int>(x,0,c);
        }
        
        
        //move along cols
        for(int y = 1; y < sobel_y.cols; y++)
        {
            for (int c = 0; c < sobel_y.depth(); c++)
            {
                //calculate variance accoss image
                slope_var += abs(sobel_y.at<int>(x,y,c) - pre_val[c]);
            }
            
            //check to see if we need a new run
            if(slope_var > params.slope_var_thresh)
            {
                //set end as previous pixel
                current.end = &pixels[x][y-1];
                
                current.linked = false;
                
                /* do this after linking
                //calculate average
                for (int c = 0; c < sobel_y.depth)
                {
                    //divide sum by nuber of pixels 
                    current.avg.chanel[c] /= current.members.size;
                }
                */
                
                //push back run_lengh
                y_run_lengths.push_back(current);
                
                //link members to run_lengh
                for (unsigned int i = 0; i < y_run_lengths.back().members.size() ; i++ )
                {
                    y_run_lengths.back().members.at(i)->y_parent = &y_run_lengths.back();
                }
                
                //start new run
                current.start = &pixels[x][y];
                current.members.push_back(&pixels[x][y]);
                
                current.avg.y = y;
                current.avg.x = x;
                //calculate sums 
                for (int c = 0; c < sobel_y.depth(); c++)
                {
                    //start average 
                    current.avg.chanel[c] = cv_ptr_src->image.at<int>(x,y,c);
                }
                
                
                //initalize slope_var and pre_val
                slope_var = 0;
                for (int c = 0; c < sobel_y.depth(); c++)
                {
                    pre_val[c] = sobel_y.at<int>(0,y,c);
                }
                
            }
            else
            {
                //pixel is part of current run
                
                //add pixel as a member
                current.members.push_back( &pixels[x][y] );
                
                //calculate sums 
                for(int c = 0; c < sobel_y.depth(); c++)
                {
                    //add average 
                    current.avg.chanel[c] += cv_ptr_src->image.at<int>(x,y,c);
                }
                current.avg.y += y;
            }
        }
    
    }
    
    ROS_INFO("color: y_run_lengths:%i", y_run_lengths.size());
    
    
    //call recursive function to link the runs
    std::vector<segment> segments;
    
    //for all unlinked runs create segments
    for(unsigned int i = 0; i < x_run_lengths.size(); i++)
    {
        if(!x_run_lengths.at(i).linked)
        {
            //intialize segment
            segment seg;
            seg.avg.x=0;
            seg.avg.y=0;
            for (int c = 0; c < 4; c++)
            {
                seg.avg.chanel[c] = 0;
            }
            
            pixel  *point = pixels[0];
            
            link_runs(&x_run_lengths, &y_run_lengths, cv_ptr_src->image.cols, cv_ptr_src->image.rows , point, &seg, &x_run_lengths.at(i));
            
            
            for (int c = 0; c < 4; c++)
            {
                //divide sum by nuber of pixels 
                seg.avg.chanel[c] /= seg.members.size();
            }
            seg.avg.x /= seg.members.size();
            seg.avg.y /= seg.members.size();
            
            segments.push_back(seg);
        }
    }
    
    ROS_INFO("color: segments:%i", segments.size());
    
    
    //paint debug image with average values
    for( unsigned int s=0; s < segments.size(); s++)
    {
        for(unsigned int p=0; p < segments.at(s).members.size(); p++)
        {
            for(int c=0; c < 3; c++)
            {
                dbg.at<int>(segments.at(s).members.at(p)->x,segments.at(s).members.at(p)->x,c) = segments.at(s).avg.chanel[c];
            }
        }
    }
    
    
    //setup and publish message for debug image
    dbg_msg.header = cv_ptr_src->header;
    dbg_msg.encoding = "rgb8";
    dbg_msg.image = dbg ;
    
    //publish debug
    dbg_pub.publish(dbg_msg.toImageMsg());
    

    
}