void menu(int item) { switch (item) { case ADD_PLANE: add_plane(); break; case REMOVE_PLANE: remove_plane(); break; case MOTION_ON: moving = GL_TRUE; glutChangeToMenuEntry(3, "Motion off", MOTION_OFF); glutIdleFunc(animate); break; case MOTION_OFF: moving = GL_FALSE; glutChangeToMenuEntry(3, "Motion", MOTION_ON); glutIdleFunc(NULL); break; case QUIT: exit(0); break; } }
static int tolua_plane_erase(lua_State *L) { plane *self = (plane *)tolua_tousertype(L, 1, 0); remove_plane(self); return 0; }
int main (int argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PCDWriter writer; pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>); std::vector<int> filenames; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); reader.read (argv[filenames[0]], *cloud); std::string pcd_filename; std::string png_filename = argv[filenames[0]]; // Take the origional png image out png::image<png::rgb_pixel> origin_image(cloud->width, cloud->height); int origin_index = 0; for (size_t y = 0; y < origin_image.get_height (); ++y) { for (size_t x = 0; x < origin_image.get_width (); ++x) { const PointType & p = cloud->points[origin_index++]; origin_image[y][x] = png::rgb_pixel(p.r, p.g, p.b); } } png_filename.replace(png_filename.length () - 4, 4, ".png"); origin_image.write(png_filename); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* float min_depth = 0.1; pcl::console::parse_argument (argc, argv, "-min_depth", min_depth); float max_depth = 3.0; pcl::console::parse_argument (argc, argv, "-max_depth", max_depth); float max_x = 1.0; pcl::console::parse_argument (argc, argv, "-max_x", max_x); float min_x = -1.0; pcl::console::parse_argument (argc, argv, "-min_x", min_x); float max_y = 1.0; pcl::console::parse_argument (argc, argv, "-max_y", max_y); float min_y = -1.0; pcl::console::parse_argument (argc, argv, "-min_y", min_y); int plane_seg_times = 1; pcl::console::parse_argument (argc, argv, "-plane_seg_times", plane_seg_times); for (int i = 0; i < plane_seg_times; i++) { remove_plane(cloud, min_depth, max_depth, min_x, max_x, min_y, max_y); } pcd_filename = argv[filenames[0]]; pcd_filename.replace(pcd_filename.length () - 4, 8, "plane.pcd"); pcl::io::savePCDFile(pcd_filename, *cloud); // std::cout << "PointCloud after removing the plane has: " << cloud->points.size () << " data points." << std::endl; //* uint32_t xmin = 1000, xmax = 0, ymin = 1000, ymax = 0; pcl::PointCloud<PointXYZRGBUV>::Ptr cloud_uv (new pcl::PointCloud<PointXYZRGBUV>); for (size_t index = 0; index < cloud->points.size(); index++) { const pcl::PointXYZRGB & p = cloud->points[index]; if (p.x != p.x || p.y != p.y || p.z != p.z) { // if current point is invalid continue; } PointXYZRGBUV cp = PointXYZRGBUV(p, index % cloud-> width, index / cloud->width); cloud_uv->points.push_back (cp); } cloud_uv->width = cloud_uv->points.size (); cloud_uv->height = 1; // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<PointXYZRGBUV>::Ptr tree (new pcl::search::KdTree<PointXYZRGBUV>); tree->setInputCloud (cloud_uv); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointXYZRGBUV> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (500); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_uv); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); xmin = 1000; xmax = 0; ymin = 1000; ymax = 0; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) { PointXYZRGBUV& p = cloud_uv->points[*pit]; pcl::PointXYZRGB cp_rgb; cp_rgb.x = p.x; cp_rgb.y = p.y; cp_rgb.z = p.z; cp_rgb.rgb = p.rgb; cloud_cluster->points.push_back(cp_rgb); xmin = std::min(xmin, p.u); xmax = std::max(xmax, p.u); ymin = std::min(ymin, p.v); ymax = std::max(ymax, p.v); } cloud_cluster->is_dense = true; cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; pcd_filename = argv[filenames[0]]; std::stringstream ss; ss << "cluster_" << j++ << ".pcd"; pcd_filename.replace(pcd_filename.length () - 4, ss.str().length(), ss.str()); pcl::io::savePCDFile(pcd_filename, *cloud_cluster); png::image<png::rgb_pixel> image(cloud_cluster->width, cloud_cluster->height); int i = 0; for (size_t y = 0; y < image.get_height (); ++y) { for (size_t x = 0; x < image.get_width (); ++x) { const PointType & p = cloud_cluster->points[i++]; image[y][x] = png::rgb_pixel(p.r, p.g, p.b); } } pcd_filename.replace(pcd_filename.length () - 4, 4, ".png"); image.write(pcd_filename); //crop out image patch png::image<png::rgb_pixel> image_patch(xmax - xmin + 1, ymax - ymin + 1); for (size_t y = 0; y < image_patch.get_height (); ++y) { for (size_t x = 0; x < image_patch.get_width (); ++x) { image_patch[y][x] = origin_image[y+ymin][x+xmin]; } } pcd_filename.replace(pcd_filename.length () - 4, 7, "box.png"); image_patch.write(pcd_filename); // writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //* } return (0); }