From 8c3e0a82cb5fd77c1ee1196fc2b645913ecc676f Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 25 Jul 2012 23:24:08 +0000 Subject: [PATCH] Dinast grabber example added, still not linking entry so commented on CMakeLists git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@6538 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- apps/CMakeLists.txt | 3 + apps/src/dinast_grabber_example.cpp | 244 ++++++++++++++++++++++++++++ 2 files changed, 247 insertions(+) create mode 100644 apps/src/dinast_grabber_example.cpp diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 2b845ee3b..55dd3196b 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -41,6 +41,9 @@ if(build) PCL_ADD_EXECUTABLE(pcl_statistical_multiscale_interest_region_extraction_example ${SUBSYS_NAME} src/statistical_multiscale_interest_region_extraction_example.cpp) target_link_libraries(pcl_statistical_multiscale_interest_region_extraction_example pcl_common pcl_io pcl_features pcl_filters) +# PCL_ADD_EXECUTABLE(pcl_dinast_grabber ${SUBSYS_NAME} src/dinast_grabber_example.cpp) +# target_link_libraries(pcl_dinast_grabber pcl_common pcl_visualization pcl_io) + if (VTK_FOUND) PCL_ADD_EXECUTABLE(pcl_ppf_object_recognition ${SUBSYS_NAME} src/ppf_object_recognition.cpp) target_link_libraries(pcl_ppf_object_recognition pcl_common pcl_io pcl_filters pcl_features pcl_registration pcl_visualization pcl_sample_consensus pcl_segmentation) diff --git a/apps/src/dinast_grabber_example.cpp b/apps/src/dinast_grabber_example.cpp new file mode 100644 index 000000000..a9aaa6afd --- /dev/null +++ b/apps/src/dinast_grabber_example.cpp @@ -0,0 +1,244 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#define FPS_CALC(_WHAT_) \ +do \ +{ \ + static unsigned count = 0;\ + static double last = pcl::getTime ();\ + double now = pcl::getTime (); \ + ++count; \ + if (now - last >= 1.0) \ + { \ + std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ + } \ +}while(false) + +void +savePGM (const unsigned char *image, const std::string& filename) +{ + FILE *file = fopen (filename.c_str (), "w"); + if (!file) + { + std::cerr << "Unable to open file '" << filename << "' for writing." << std::endl; + return; + } + + // PGM header + fprintf (file, "P2\n%d %d\n255\n", IMAGE_WIDTH, IMAGE_HEIGHT); + + // Write data as ASCII + for (int i = 0; i < IMAGE_HEIGHT; ++i) + { + for (int j = 0; j < IMAGE_WIDTH; ++j) + { + fprintf (file, "%3d ", (int)*image++); + } + fprintf (file, "\n"); + } + + fclose (file); +} + +void +keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void* data) +{ + static int NUM_IMAGES = 0; + if (event.getKeySym () == "s" && event.keyDown ()) + { + char filename[16]; + snprintf (filename, sizeof(filename), "image%.2d.pgm", NUM_IMAGES++); + unsigned char *image = reinterpret_cast (data); + savePGM (image, filename); + printf ("Wrote %s\n", filename); + } +} + +void +convertImageToCloud (const unsigned char *image, pcl::PointCloud &cloud) +{ + cloud.points.resize (IMAGE_WIDTH * IMAGE_HEIGHT); + cloud.width = IMAGE_WIDTH; + cloud.height = IMAGE_HEIGHT; + cloud.is_dense = false; + + int depth_idx = 0; + int pxl[9]; + + for (int x = 0; x < cloud.width; ++x) + { + for (int y = 0; y < cloud.height; ++y, ++depth_idx) + { + float xc = (float)(x - 160); + float yc = (float)(y - 120); + double r1 = sqrt (xc * xc + yc * yc); + double r2 = r1 * r1; + double r3 = r1 * r2; + double A = -2e-5 * r3 + 0.004 * r2 + 0.1719 * r1 + 350.03; + double B = -2e-9 * r3 + 3e-7 * r2 - 1e-5 * r1 - 0.01; + + // Low pass filtering + /// @todo Try a bilateral filter to avoid blurring over depth boundaries + int measure = 0; + if ((y > 0) && (y < (IMAGE_HEIGHT - 1)) && (x > 0) && (x < (IMAGE_WIDTH - 1))) + { + int ipx = x + IMAGE_WIDTH * y; +#if 1 + pxl[0] = image[ipx]; + pxl[1] = image[ipx-1]; + pxl[2] = image[ipx+1]; + pxl[3] = image[ipx - IMAGE_WIDTH]; + pxl[4] = image[ipx - IMAGE_WIDTH - 1]; + pxl[5] = image[ipx - IMAGE_WIDTH + 1]; + pxl[6] = image[ipx + IMAGE_WIDTH]; + pxl[7] = image[ipx + IMAGE_WIDTH - 1]; + pxl[8] = image[ipx + IMAGE_WIDTH + 1]; + + for (int ii = 0; ii < 9; ii++) + measure += pxl[ii]; + measure /= 9; +#else + // No blurring + measure = image[ipx]; +#endif + } + if (measure > 255) + measure = 255; // saturation for display + + unsigned char pixel = measure;//image[depth_idx]; + if (pixel < 1) + { + cloud.points[depth_idx].x = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].y = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].z = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].intensity = pixel; + continue; + } + + if (pixel > A) + pixel = A; + + float dy = y*0.1; + double dist = (log((double)pixel/A)/B-dy)*(7E-07*r3 - 0.0001*r2 + 0.004*r1 + 0.9985)*1.5; + double dist_2d = r1; + + static const double dist_max_2d = 1 / 160.0; /// @todo Why not 200? + //static const double dist_max_2d = 1 / 200.0; + static const double FOV = 64.0 * M_PI / 180.0; // diagonal FOV? + + double theta_colati = FOV * r1 * dist_max_2d; + double c_theta = cos (theta_colati); + double s_theta = sin (theta_colati); + double c_ksai = ((double)(x - 160.)) / r1; + double s_ksai = ((double)(y - 120.)) / r1; + + cloud.points[depth_idx].x = (dist * s_theta * c_ksai) / 500.0 + 0.5; //cartesian x + cloud.points[depth_idx].y = (dist * s_theta * s_ksai) / 500.0 + 0.5; //cartesian y + cloud.points[depth_idx].z = (dist * c_theta); //cartesian z + /// @todo This looks weird, can it cause artifacts? + if (cloud.points[depth_idx].z < 0.01) +#if 1 + cloud.points[depth_idx].z = 0.01; +#else + cloud.points[depth_idx].x = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].y = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].z = std::numeric_limits::quiet_NaN (); + cloud.points[depth_idx].intensity = pixel; + continue; +#endif + + cloud.points[depth_idx].z /= 500.0; + cloud.points[depth_idx].intensity = pixel; + } + } +} + +/* --[ */ + +int +main (int argc, char** argv) +{ + + pcl::DinastGrabber grabber; + + libusb_context *ctx = NULL; + struct libusb_device_handle* device = grabber.findDevice (ctx); + grabber.setDevice(device); + + if (device == NULL) + { + std::cerr << "Couldn't find any Dinast devices attached!" << std::endl; + return (-1); + } + + device = grabber.openDevice (ctx, 0); + if (device == NULL) + { + std::cerr << "Could not open or claim the USB device!" << std::endl; + return (-1); + } + + std::cerr << "Device version/revision number: " << grabber.getDeviceVersion (device) << std::endl; + + grabber.start (); + //std::cerr << "Could not start device!" << std::endl; + +// if (USBRxSyncSearch (device)) +// std::cerr << "Synchronization succesful." << std::endl; + + pcl::visualization::ImageViewer vis_img ("Dinast Image Viewer"); + pcl::visualization::PCLVisualizer vis_cld (argc, argv, "Dinast Cloud Viewer"); + + pcl::visualization::ImageViewer vis2 ("Dinast Viewer"); + unsigned char *img1 = (unsigned char*)malloc (IMAGE_SIZE); + unsigned char *img2 = (unsigned char*)malloc (IMAGE_SIZE); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + vis_img.registerKeyboardCallback (keyboardEventOccurred, (void*)img1); + + while (true) + { + //if (readImage (device, img1) == 0) + if (grabber.readImage (device, img1, img2) == 0) + continue; + + convertImageToCloud (img1, *cloud); + + FPS_CALC ("grabber + visualization"); + vis_img.showMonoImage (img1, IMAGE_WIDTH, IMAGE_HEIGHT); + vis2.showMonoImage (img2, IMAGE_WIDTH, IMAGE_HEIGHT); + // + + pcl::visualization::PointCloudColorHandlerGenericField handler (cloud, "intensity"); + if (!vis_cld.updatePointCloud (cloud, handler, "DinastCloud")) + { + vis_cld.addPointCloud (cloud, handler, "DinastCloud"); + vis_cld.resetCameraViewpoint ("DinastCloud"); + } + + vis_img.spinOnce (); + vis_cld.spinOnce (); + vis2.spinOnce (); + } + + grabber.stop (); + grabber.closeDevice (device); + libusb_exit (ctx); + + //free (img); + +} +/* ]-- */ -- GitLab