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;
}
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());
}