//
// Generate the segmented image and send to a given port.
// - it gets image from processed sequence.
int
YARPFlowTracker::GenerateAndSend(YARPOutputPortOf<YARPGenericImage>& port)
{
// segmentation_mask_copy
// frame from seq
// port from reader
// YARPImageSequence& seq = reader->GetSequenceRef();
// YARPOutputPortOf<YARPGenericImage>& port = reader->GetSegmentedImagePort();
YARPImageOf<YarpPixelBGR>& frame = seq->GetImageRef (contact_frame);
YARPImageOf<YarpPixelMono>& mask = segmentation_mask_copy;
#ifdef LOGGING_SEGMENTATIONS
// awful to just hack this in here... bound to cause trouble...
// but I'm going to do it anyway.
#ifdef SENDING_SEGMENTATIONS
{
// send frame and mask
out_seg.Content().c1.PeerCopy(frame);
out_seg.Content().c2.PeerCopy(frame);
out_seg.Content().c2.CastCopy(mask);
out_seg.Write();
}
#endif
{
char buf[256];
long int tick = (long int) YARPTime::GetTimeAsSeconds();
sprintf(buf,"%s/%ld.ppm", LOGGING_DIR, tick);
printf("Saving frame %s\n", buf);
YARPImageFile::Write(buf,frame);
sprintf(buf,"%s/%ld.pgm", LOGGING_DIR, tick);
printf("Saving mask %s\n", buf);
YARPImageFile::Write(buf,mask);
}
#endif
YarpPixelBGR black;
black.r = black.g = black.b = 0;
YarpPixelBGR black1;
black1.r = black1.g = black1.b = 1;
for (int i = 0; i < mask.GetHeight(); i++)
for (int j = 0; j < mask.GetWidth(); j++)
{
if (frame(j,i).r != 0 ||
frame(j,i).g != 0 ||
frame(j,i).b != 0)
output_image(j,i) = (mask(j,i) != 0) ? frame(j,i) : black;
else
output_image(j,i) = (mask(j,i) != 0) ? frame(j,i) : black1;
}
// send.
port.Content().Refer (output_image);
port.Write ();
return 0;
}
void RaytracerApplication::handle_event( const SDL_Event& event )
{
int width, height;
if ( !raytracing ) {
camera_control.handle_event( this, event );
}
switch ( event.type )
{
case SDL_KEYDOWN:
switch ( event.key.keysym.sym )
{
case KEY_RAYTRACE:
get_dimension( &width, &height );
toggle_raytracing( width, height );
break;
case KEY_SCREENSHOT:
output_image();
break;
default:
break;
}
default:
break;
}
}
void RaytracerApplication::handle_event( const SDL_Event& event )
{
int width, height;
if ( !raytracing ) {
camera_control.handle_event( this, event );
}
switch ( event.type )
{
case SDL_KEYDOWN:
switch ( event.key.keysym.sym )
{
case KEY_RAYTRACE:
get_dimension( &width, &height );
toggle_raytracing( width, height );
break;
case KEY_SEND_PHOTONS:
raytracer.initialize(&scene, options.num_samples, 0, 0);
queue_render_photon=true;
case KEY_SCREENSHOT:
output_image();
break;
default:
break;
}
default:
break;
}
}
int
main( int argc,
char * argv[] )
{
ProgName = argv[0];
switch (setup ( argc, argv ))
{
default:
case SYSBUILD_FAIL: break;
case SYSBUILD_OK:
parse_config_file (ConfigData.config_file);
update_configdata ();
print_configdata ();
if (!ShowOnly)
{
make_nucleus ();
check_bootstrap ();
output_image ();
}
break;
case SYSBUILD_OLD:
sysbuild( argc, argv );
break;
}
tidyup ();
}
void rose_of_directions::draw_rod(const std::vector<float>& rose_vec) const {
std::cerr << " --> compute_from_..._in(...)" << std::endl;
std::srand(time(NULL));
std::stringstream ss;
ss << "rod_" << std::size_t(rand()) << ".png";
std::string output_fname = ss.str();
std::size_t rose_vec_size = rose_vec.size();
float rod_min = 1e30f;
float rod_max = 0.0f;
for (std::size_t j = 0; j < rose_vec_size; ++j) {
rod_min = std::min(rod_min,rose_vec[j]);
rod_max = std::max(rod_max,rose_vec[j]);
}
int width = int(rose_vec_size);
int height = int(rose_vec_size*3/4);
image_t output_image(height,width,CV_8UC3);
output_image.setTo(cv::Vec3b(0,0,0));
for (std::size_t j = 0; j < rose_vec_size; ++j) {
float value = float(height-1) * (rose_vec[j] - rod_min) / (rod_max - rod_min);
cv::Point p0, p1;
p0.x = j;
p1.x = j;
p0.y = height;
p1.y = height - int(value);
cv::line(output_image,p0,p1,cv::Scalar(0,255,0),1);
}
cv::imwrite(output_fname.c_str(),output_image);
std::cerr << " - writing rod image - " << output_fname << std::endl;
}
int main(int argc, char* argv[])
{
process_options(argc, argv);
create_image_preprocess();
create_image();
output_image();
return 0;
}
bool write_argb_image(const cv::Mat& argb_image, const std::string& output_filename)
{
cv::Mat output_image(argb_image.size(), CV_8UC3);
int argb_to_bgr[] = { 3,0, // blue
2,1, // green
1,2 // red
};
cv::mixChannels(&argb_image, 1, &output_image, 1, argb_to_bgr, 3);
return write_bgr_image(output_image, output_filename);
}
void DrawWindow::on_pushButton_exportDiagram_clicked()
{
QString output_filename = QFileDialog::getSaveFileName( this,
"Choose location...",
"Auton Diagram.png",
"PNG images (*.png)");
QSize output_size = field.itemsBoundingRect().size().toSize()*8;
QImage output_image(output_size, QImage::Format_ARGB32);
output_image.fill(Qt::transparent);
QPainter output_painter(&output_image);
output_painter.setRenderHint(QPainter::Antialiasing);
ui->graphicsView->render( &output_painter,
QRect(),
QRect(ui->graphicsView->mapFromScene(-10, -10), ui->graphicsView->mapFromScene(154, 154)),
Qt::KeepAspectRatio);
output_image.save(output_filename, "png", 0);
}
int main(int argc, char** argv){
int c;
long double x = 2.1;
long double y = 2.3;
long double d = 5;
int w = 800;
int h = 600;
int i = 200;
while((c = getopt(argc, argv, "x:y:w:h:d:i:")) != -1) {
switch(c) {
case 'x':
x = atof(optarg);
break;
case 'y':
y = atof(optarg);
break;
case 'd':
d = atof(optarg);
break;
case 'w':
w = atoi(optarg);
break;
case 'h':
h = atoi(optarg);
break;
case 'i':
i = atoi(optarg);
break;
case ':':
switch(optopt) {
case '?':
printf(usage);
return 1;
default:
break;
}
break;
default:
printf(usage);
return 1;
}
}
output_image(x, y, d, w, h, i);
}
void export_bitmap(const char *fname, const Buffer *buffer, int channels)
{
int width_i = static_cast<int>(buffer->buffer_width_f);
int height_i = static_cast<int>(buffer->buffer_height_f);
shared_ptr<uint8_t> processed_buffer(new uint8_t[4 * width_i * height_i]);
uint8_t default_channel_vals[] = {0, 0, 0, 255};
uint8_t* out_ptr = processed_buffer.get();
const T* in_ptr = reinterpret_cast<T*>(buffer->buffer);
const float *bc_comp = buffer->auto_buffer_contrast_brightness();
float color_scale = get_multiplier<T>();
const float maxIntensity = get_max_intensity<T>();
for(int y = height_i - 1; y >= 0; --y) {
for(int x = width_i - 1; x >= 0; --x) {
int c;
for(c = 0; c < channels; ++c) {
float in_val = static_cast<float>(in_ptr[y * channels * width_i + x * channels + c]);
out_ptr[y * 4 * width_i + x * 4 + c] = static_cast<uint8_t>(
clamp((in_val * bc_comp[c] + bc_comp[4 + c]*maxIntensity)*color_scale, 0.f, 255.f));
}
if(channels == 1) {
// Grayscale: Repeat first channel into G and B
for(; c < 3; ++c) {
out_ptr[y * 4 * width_i + x * 4 + c] = out_ptr[y * 4 * width_i + x * 4];
}
out_ptr[y * 4 * width_i + x * 4 + c] = 255;
} else {
for(; c < 4; ++c) {
out_ptr[y * 4 * width_i + x * 4 + c] = default_channel_vals[c];
}
}
}
}
const int bytes_per_line = width_i * 4;
QImage output_image(processed_buffer.get(), width_i, height_i, bytes_per_line, QImage::Format_RGBA8888);
output_image.save(fname, "png");
}
int main(int argc, char* argv[]) {
// check if the argument number is valid
if (argc < 4) {
std::cerr << "Usage: "
<< argv[0]
<< " input-file output-file [operations] [parameters]"
<< std::endl;
return 1;
}
// check if the input file is valid
std::ifstream input(argv[1]);
if (!input) {
std::cerr << "Can not open the grades in file " << argv[1] << std::endl;
return 1;
}
// read in the image
std::vector<std::string> image;
read_in_image(input, image);
if (image.size() == 0) {
std::cout << "Cannot import the image "
<< argv[1]
<< ". Try again please. "
<< std::endl;
return 1;
}
std::ofstream output(argv[2]); // the output file
std::string op = argv[3]; // the operation name
if (op.compare("replace") == 0 && argc == 6) { // replace
char before = argv[4][0];
char after = argv[5][0];
replace(image, before, after);
} else if (op.compare("dilation") == 0 &&
(argc == 5 || argc == 6)) { // dilation operation
char color = argv[4][0];
if (argc == 5) {
// When plus-signed structing element
dilation(image, color, false);
} else if (argc == 6 && std::string(argv[5]).compare("square") == 0) {
// when square-signed structing element
dilation(image, color, true);
} else {
std::cerr << "Usage: "
<< argv[0]
<< " input-file output-file dilation color [square]"
<< std::endl;
return 1;
}
} else if (op.compare("erosion") == 0 &&
(argc == 6 || argc == 7)) { // erosion operation
char color = argv[4][0];
char pixel = argv[5][0];
if (argc == 6) {
// When plus-signed structing element
erosion(image, color, pixel, false);
} else if (argc == 7 && std::string(argv[6]).compare("square") == 0) {
// when square-signed structing element
erosion(image, color, pixel, true);
} else {
std::cerr << "Usage: " << argv[0]
<< " input-file output-file erosion pixel [square]"
<< std::endl;
return 1;
}
} else if (op.compare("floodfill") == 0 && argc == 7) {
// floodfill operation
int row = atoi(argv[4]);
int col = atoi(argv[5]);
if (!isValid(image, row, col)) {
std::cerr << "Error: invalid arguments "
<< row << " and " << col
<< std::endl;
return 1;
}
char filler = argv[6][0];
flood_fill(image, row, col, filler);
} else { // invalid input command
std::cerr << "Invalid command." << std::endl;
std::cerr << "Usage: " << argv[0]
<< " input-file output-file [operations] [parameters]"
<< std::endl;
return 1;
}
// write the modified image to a file
if (output.is_open()) {
output_image(image, output);
output.close();
} else {
std::cerr << "Cannot output to " << argv[3] << std::endl;
return 1;
}
return 0;
}
int main(int argc,char** argv)
{
int width;
int height;
const char* source; // Source code
cl_platform_id platform; // Platform { CPU, GPU, FPGA }
cl_device_id device; // Device { GPU_ID }
cl_context context; // Context
cl_command_queue queue; // Queue
cl_int err; // for error checking
cl_event event[NR_KERNELS-1]; // must be ok for second kernel to run
cl_program program; // program
cl_kernel kernels_code[NR_KERNELS];
cl_mem bufferIN; // data IN
cl_mem bufferOUT; // data OUT
int* inputImage; // input image
int* outputImage; // output image
cl_uint *ptr; // data to be shown
size_t global_work_size; // work size
// get platform, get device, create context
clGetPlatformIDs(1,&platform,NULL);
clGetDeviceIDs(platform,COMPUTE,1,&device,NULL);
context=clCreateContext(NULL,1,&device,NULL,NULL,NULL);
queue=clCreateCommandQueue(context,device,0,NULL);
// read data from file, allocate memory & copy
inputImage=input_image(INPUT_IMAGE,&width,&height);
bufferIN=clCreateBuffer(context,CL_MEM_READ_WRITE,width*height *sizeof(int),NULL,NULL);
clEnqueueWriteBuffer(queue,bufferIN,1,0,width * height * sizeof(int),inputImage,0,0,0);
bufferOUT= clCreateBuffer(context,CL_MEM_READ_WRITE,width*height *sizeof(int),NULL,NULL);
// read code from file / check code
source=readCode(FILENAME);
if(source==NULL)
{
printf("Error reading code. Exiting");
return 1;
}
// init OPENCL program
program=clCreateProgramWithSource(context,1,&source,NULL,&err);
check(err,program,device);
check(clBuildProgram(program,1,&device,NULL,NULL,NULL));
// kernels
for(int i=0;i<NR_KERNELS;i++)
{
kernels_code[i]=clCreateKernel(program,kernels[i],&err);
check(err,program,device);
}
// pass args, execute OPENCL kernels
global_work_size=width*height;
for(int i=0;i<NR_KERNELS;i++)
{
if((i+1)%2==0){
check(clSetKernelArg(kernels_code[i],0,sizeof(bufferOUT),(void*)&bufferOUT));
check(clSetKernelArg(kernels_code[i],1,sizeof(bufferIN),(void*)&bufferIN));
check(clSetKernelArg(kernels_code[i],2,sizeof(int),(void*)&width));
check(clSetKernelArg(kernels_code[i],3,sizeof(int),(void*)&height));
}
else {
check(clSetKernelArg(kernels_code[i],0,sizeof(bufferIN),(void*)&bufferIN));
check(clSetKernelArg(kernels_code[i],1,sizeof(bufferOUT),(void*)&bufferOUT));
check(clSetKernelArg(kernels_code[i],2,sizeof(int),(void*)&width));
check(clSetKernelArg(kernels_code[i],3,sizeof(int),(void*)&height));
}
}
size_t global_work[2];
global_work[0]=width;
global_work[1]=height;
// enqueue / run all kernels (pipeline in this case)
// start first kernel
check(clEnqueueNDRangeKernel(queue,kernels_code[0],2,NULL,global_work,NULL,0,NULL,&event[0]));
// run all the rest (n-2)
for(int i=1;i<NR_KERNELS-1;i++)
check(clEnqueueNDRangeKernel(queue,kernels_code[i],2,NULL,global_work,NULL,i,event,&event[i]));
// start last kernel
if(NR_KERNELS>2)
check(clEnqueueNDRangeKernel(queue,kernels_code[NR_KERNELS-1],2,NULL,global_work,NULL,NR_KERNELS-1,event,NULL));
// finish queue
check(clFinish(queue));
if(NR_KERNELS%2==1)
outputImage= (int*)clEnqueueMapBuffer(queue,bufferOUT,CL_TRUE,CL_MAP_READ,0,width*height*sizeof(int),0,NULL,NULL,NULL);
else
outputImage= (int*)clEnqueueMapBuffer(queue,bufferIN,CL_TRUE,CL_MAP_READ,0,width*height*sizeof(int),0,NULL,NULL,NULL);
output_image(OUTPUT_IMAGE,outputImage,width,height);
return 0;
}
// Processes the point cloud with OpenCV using the PCL cluster indices
void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
// const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud )
{
// -------------------------------------------------------------------------------------------------------
// Convert image
ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");
try
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
// pcl::toROSMsg (*pointcloud_msg, *image_msg);
pcl::toROSMsg (*cloud_transformed, *image_msg);
cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
full_input_image = input_bridge->image;
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
return;
}
// -------------------------------------------------------------------------------------------------------
// Process Image
// Convert image to gray
cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
//cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);
// Blur image - reduce noise with a 3x3 kernel
cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );
ROS_INFO_STREAM_NAMED("perception","Finished coverting");
// -------------------------------------------------------------------------------------------------------
// Check OpenCV and PCL image height for errors
int image_width = cloud.width;
int image_height = cloud.height;
ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
int image_width_cv = full_input_image.size.p[1];
int image_height_cv = full_input_image.size.p[0];
ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");
if( image_width != image_width_cv || image_height != image_height_cv )
{
ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
return;
}
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// First window
const char* opencv_window = "Source";
/*
cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
cv::imshow( opencv_window, full_input_image_gray );
*/
// while(true) // use this when we want to tweak the image
{
output_image = full_input_image.clone();
// -------------------------------------------------------------------------------------------------------
// Start processing clusters
ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");
int top_image_overlay_x = 0; // tracks were to copyTo the mini images
// for each cluster, see if it is a block
for(size_t c = 0; c < cluster_indices.size(); ++c)
{
ROS_INFO_STREAM_NAMED("perception","\n\n");
ROS_INFO_STREAM_NAMED("perception","On cluster " << c);
// find the outer dimensions of the cluster
double xmin = 0; double xmax = 0;
double ymin = 0; double ymax = 0;
// also remember each min & max's correponding other coordinate (not needed for z)
double xminy = 0; double xmaxy = 0;
double yminx = 0; double ymaxx = 0;
// also remember their corresponding indice
int xmini = 0; int xmaxi = 0;
int ymini = 0; int ymaxi = 0;
// loop through and find all min/max of x/y
for(size_t i = 0; i < cluster_indices[c].indices.size(); i++)
{
int j = cluster_indices[c].indices[i];
// Get RGB from point cloud
pcl::PointXYZRGB p = cloud_transformed->points[j];
double x = p.x;
double y = p.y;
if(i == 0) // initial values
{
xmin = xmax = x;
ymin = ymax = y;
xminy = xmaxy = y;
yminx = ymaxx = x;
xmini = xmaxi = ymini = ymaxi = j; // record the indice corresponding to the min/max
}
else
{
if( x < xmin )
{
xmin = x;
xminy = y;
xmini = j;
}
if( x > xmax )
{
xmax = x;
xmaxy = y;
xmaxi = j;
}
if( y < ymin )
{
ymin = y;
yminx = x;
ymini = j;
}
if( y > ymax )
{
ymax = y;
ymaxx = x;
ymaxi = j;
}
}
}
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmin: " << xmin << " xmax: " << xmax << " ymin: " << ymin << " ymax: " << ymax);
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmini: " << xmini << " xmaxi: " << xmaxi << " ymini: " << ymini << " ymaxi: " << ymaxi);
// ---------------------------------------------------------------------------------------------
// Check if these dimensions make sense for the block size specified
double xside = xmax-xmin;
double yside = ymax-ymin;
const double tol = 0.01; // 1 cm error tolerance
// In order to be part of the block, xside and yside must be between
// blocksize and blocksize*sqrt(2)
if(xside > block_size-tol &&
xside < block_size*sqrt(2)+tol &&
yside > block_size-tol &&
yside < block_size*sqrt(2)+tol )
{
// -------------------------------------------------------------------------------------------------------
// Get the four farthest corners of the block - use OpenCV only on the region identified by PCL
// Get the pixel coordinates of the xmax and ymax indicies
int px_xmax = 0; int py_xmax = 0;
int px_ymax = 0; int py_ymax = 0;
getXYCoordinates( xmaxi, image_height, image_width, px_xmax, py_xmax);
getXYCoordinates( ymaxi, image_height, image_width, px_ymax, py_ymax);
// Get the pixel coordinates of the xmin and ymin indicies
int px_xmin = 0; int py_xmin = 0;
int px_ymin = 0; int py_ymin = 0;
getXYCoordinates( xmini, image_height, image_width, px_xmin, py_xmin);
getXYCoordinates( ymini, image_height, image_width, px_ymin, py_ymin);
ROS_DEBUG_STREAM_NAMED("perception","px_xmin " << px_xmin << " px_xmax: " << px_xmax << " py_ymin: " << py_ymin << " py_ymax: " << py_ymax );
// -------------------------------------------------------------------------------------------------------
// Change the frame of reference from the robot to the camera
// Create an array of all the x value options
const int x_values_a[] = {px_xmax, px_ymax, px_xmin, px_ymin};
const int y_values_a[] = {py_xmax, py_ymax, py_xmin, py_ymin};
// Turn it into a vector
std::vector<int> x_values (x_values_a, x_values_a + sizeof(x_values_a) / sizeof(x_values_a[0]));
std::vector<int> y_values (y_values_a, y_values_a + sizeof(y_values_a) / sizeof(y_values_a[0]));
// Find the min
int x1 = *std::min_element(x_values.begin(), x_values.end());
int y1 = *std::min_element(y_values.begin(), y_values.end());
// Find the max
int x2 = *std::max_element(x_values.begin(), x_values.end());
int y2 = *std::max_element(y_values.begin(), y_values.end());
ROS_DEBUG_STREAM_NAMED("perception","x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Expand the ROI by a fudge factor, if possible
const int FUDGE_FACTOR = 5; // pixels
if( x1 > FUDGE_FACTOR)
x1 -= FUDGE_FACTOR;
if( y1 > FUDGE_FACTOR )
y1 -= FUDGE_FACTOR;
if( x2 < image_width - FUDGE_FACTOR )
x2 += FUDGE_FACTOR;
if( y2 < image_height - FUDGE_FACTOR )
y2 += FUDGE_FACTOR;
ROS_DEBUG_STREAM_NAMED("perception","After Fudge Factor - x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Create ROI parameters
// (x1,y1)----------------------
// | |
// | ROI |
// | |
// |_____________________(x2,y2)|
// Create Region of Interest
int roi_width = x2 - x1;
int roi_height = y2 - y1;
cv::Rect region_of_interest = cv::Rect( x1, y1, roi_width, roi_height );
ROS_DEBUG_STREAM_NAMED("perception","ROI: x " << x1 << " -- y " << y1 << " -- height " << roi_height << " -- width " << roi_width );
// -------------------------------------------------------------------------------------------------------
// Find paramters of the block in pixel coordiantes
int block_center_x = x1 + 0.5*roi_width;
int block_center_y = y1 + 0.5*roi_height;
int block_center_z = block_size / 2; // TODO: make this better
const cv::Point block_center = cv::Point( block_center_x, block_center_y );
// -------------------------------------------------------------------------------------------------------
// Create a sub image of just the block
cv::Point a1 = cv::Point(x1, y1);
cv::Point a2 = cv::Point(x2, y2);
cv::rectangle( output_image, a1, a2, cv::Scalar(0, 255, 255), 1, 8);
// Crop image (doesn't actually copy the data)
cropped_image = full_input_image_gray(region_of_interest);
// -------------------------------------------------------------------------------------------------------
// Detect edges using canny
ROS_INFO_STREAM_NAMED("perception","Detecting edges using canny");
// Find edges
cv::Mat canny_output;
cv::Canny( cropped_image, canny_output, canny_threshold, canny_threshold*2, 3 );
// Get mini window stats
const int mini_width = canny_output.size.p[1];
const int mini_height = canny_output.size.p[0];
const cv::Size mini_size = canny_output.size();
const cv::Point mini_center = cv::Point( mini_width/2, mini_height/2 );
// Find contours
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
ROS_INFO_STREAM_NAMED("perception","Contours");
// Draw contours
cv::Mat drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
ROS_INFO_STREAM_NAMED("perception","Drawing contours");
// Find the largest contour for getting the angle
double max_contour_length = 0;
int max_contour_length_i;
for( size_t i = 0; i< contours.size(); i++ )
{
double contour_length = cv::arcLength( contours[i], false );
if( contour_length > max_contour_length )
{
max_contour_length = contour_length;
max_contour_length_i = i;
}
//ROS_DEBUG_STREAM_NAMED("perception","Contour length = " << contour_length << " of index " << max_contour_length_i);
cv::Scalar color = cv::Scalar( (30 + i*10) % 255, (30 + i*10) % 255, (30 + i*10) % 255);
cv::drawContours( drawing, contours, (int)i, color, 1, 8, hierarchy, 0, cv::Point() );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
}
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to main image
cv::Scalar color = cv::Scalar( 0, 255, 0 );
cv::drawContours( output_image, contours, (int)max_contour_length_i, color, 1, 8, hierarchy, 0, a1 );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to seperate image
cv::Mat hough_input = cv::Mat::zeros( mini_size, CV_8UC1 );
cv::Mat hough_input_color;
cv::Scalar hough_color = cv::Scalar( 200 );
cv::drawContours( hough_input, contours, (int)max_contour_length_i, hough_color, 1, 8, hierarchy, 0 );
cv::cvtColor(hough_input, hough_input_color, CV_GRAY2BGR);
// -------------------------------------------------------------------------------------------------------
// Hough Transform
cv::Mat hough_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
std::vector<cv::Vec4i> lines;
ROS_DEBUG_STREAM_NAMED("perception","hough_rho " << hough_rho << " hough_theta " << hough_theta <<
" theta_converted " << (1/hough_theta)*CV_PI/180 << " hough_threshold " <<
hough_threshold << " hough_minLineLength " << hough_minLineLength <<
" hough_maxLineGap " << hough_maxLineGap );
cv::HoughLinesP(hough_input, lines, hough_rho, (1/hough_theta)*CV_PI/180, hough_threshold, hough_minLineLength, hough_maxLineGap);
ROS_WARN_STREAM_NAMED("perception","Found " << lines.size() << " lines");
std::vector<double> line_angles;
// Copy detected lines to the drawing image
for( size_t i = 0; i < lines.size(); i++ )
{
cv::Vec4i line = lines[i];
cv::line( hough_drawing, cv::Point(line[0], line[1]), cv::Point(line[2], line[3]),
cv::Scalar(255,255,255), 1, CV_AA);
// Error check
if(line[3] - line[1] == 0 && line[2] - line[0] == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Line is actually two points at the origin, unable to calculate. TODO: handle better?");
continue;
}
// Find angle
double line_angle = atan2(line[3] - line[1], line[2] - line[0]); //in radian, degrees: * 180.0 / CV_PI;
// Reverse angle direction if negative
if( line_angle < 0 )
{
line_angle += CV_PI;
}
line_angles.push_back(line_angle);
ROS_DEBUG_STREAM_NAMED("perception","Hough Line angle: " << line_angle * 180.0 / CV_PI;);
}
double block_angle = 0; // the overall result of the block's angle
// Everything is based on the first angle
if( line_angles.size() == 0 ) // make sure we have at least 1 angle
{
ROS_ERROR_STREAM_NAMED("perception","No lines were found for this cluster, unable to calculate block angle");
}
else
{
calculateBlockAngle( line_angles, block_angle );
}
// -------------------------------------------------------------------------------------------------------
// Draw chosen angle
ROS_INFO_STREAM_NAMED("perception","Using block angle " << block_angle*180.0/CV_PI);
// Draw chosen angle on mini image
cv::Mat angle_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
int line_length = 0.5*double(mini_width); // have the line go 1/4 across the screen
int new_x = mini_center.x + line_length*cos( block_angle );
int new_y = mini_center.y + line_length*sin( block_angle );
ROS_INFO_STREAM("Origin (" << mini_center.x << "," << mini_center.y << ") New (" << new_x << "," << new_y <<
") length " << line_length << " angle " << block_angle <<
" mini width " << mini_width << " mini height " << mini_height);
cv::Point angle_point = cv::Point(new_x, new_y);
cv::line( angle_drawing, mini_center, angle_point, cv::Scalar(255,255,255), 1, CV_AA);
// Draw chosen angle on contours image
cv::line( hough_drawing, mini_center, angle_point, cv::Scalar(255,0, 255), 1, CV_AA);
// Draw chosen angle on main image
line_length = 0.75 * double(mini_width); // have the line go 1/2 across the box
new_x = block_center_x + line_length*cos( block_angle );
new_y = block_center_y + line_length*sin( block_angle );
ROS_INFO_STREAM_NAMED("perception",block_center_x << ", " << block_center_y << ", " << new_x << ", " << new_y);
angle_point = cv::Point(new_x, new_y);
cv::line( output_image, block_center, angle_point, cv::Scalar(255,0,255), 2, CV_AA);
// -------------------------------------------------------------------------------------------------------
// Get world coordinates
// Find the block's center point
double world_x1 = xmin+(xside)/2.0;
double world_y1 = ymin+(yside)/2.0;
double world_z1 = table_height + block_size / 2;
// Convert pixel coordiantes back to world coordinates
double world_x2 = cloud_transformed->at(new_x, new_y).x;
double world_y2 = cloud_transformed->at(new_x, new_y).y;
double world_z2 = world_z1; // is this even necessary?
// Get angle from two world coordinates...
double world_theta = abs( atan2(world_y2 - world_y1, world_x2 - world_x1) );
// Attempt to make all angles point in same direction
makeAnglesUniform( world_theta );
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// Copy the cluster image to the main image in the top left corner
if( top_image_overlay_x + mini_width < image_width )
{
const int common_height = 42;
cv::Rect small_roi_row0 = cv::Rect(top_image_overlay_x, common_height*0, mini_width, mini_height);
cv::Rect small_roi_row1 = cv::Rect(top_image_overlay_x, common_height*1, mini_width, mini_height);
cv::Rect small_roi_row2 = cv::Rect(top_image_overlay_x, common_height*2, mini_width, mini_height);
cv::Rect small_roi_row3 = cv::Rect(top_image_overlay_x, common_height*3, mini_width, mini_height);
drawing.copyTo( output_image(small_roi_row0) );
hough_input_color.copyTo( output_image(small_roi_row1) );
hough_drawing.copyTo( output_image(small_roi_row2) );
angle_drawing.copyTo( output_image(small_roi_row3) );
top_image_overlay_x += mini_width;
}
// figure out the position and the orientation of the block
//double angle = atan(block_size/((xside+yside)/2));
//double angle = atan( (xmaxy - xminy) / (xmax - xmin ) );
// Then add it to our set
//addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
//ROS_INFO_STREAM_NAMED("perception","FOUND -> xside: " << xside << " yside: " << yside << " angle: " << block_angle);
addBlock( world_x1, world_y1, world_z1, world_theta );
//addBlock( block_center_x, block_center_y, block_center_z, block_angle);
}
else
{
