/* ---[ */ int main (int argc, char** argv) { srand (static_cast<unsigned int> (time (0))); print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n"); if (argc < 2) { printHelp (argc, argv); return (-1); } bool debug = false; pcl::console::parse_argument (argc, argv, "-debug", debug); if (debug) pcl::console::setVerbosityLevel (pcl::console::L_DEBUG); bool cam = pcl::console::find_switch (argc, argv, "-cam"); // Parse the command line arguments for .pcd files std::vector<int> p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk"); int dir_position = pcl::console::find_argument(argc, argv, "-dir"); if (p_file_indices.size () == 0 && vtk_file_indices.size () == 0 && dir_position == -1) { print_error ("No .PCD or .VTK file given. Nothing to visualize.\n"); return (-1); } // Command line parsing std::string dir_name; pcl::console::parse_argument(argc, argv, "-dir", dir_name); printf("file index %s\n", dir_name.c_str()); boost::filesystem::path dir_path(dir_name); std::vector<std::string> dir_files; for(boost::filesystem::directory_iterator it = boost::filesystem::directory_iterator(dir_path); it != boost::filesystem::directory_iterator(); it++) { //printf("%s\n", it->path().c_str()); if (it->path().string().find(".pcd") != -1) { dir_files.push_back(it->path().string()); //printf("%s\n", it->path().c_str()); printf("%s\n", dir_files[dir_files.size() - 1].c_str()); } } double bcolor[3] = {0, 0, 0}; pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]); std::vector<double> fcolor_r, fcolor_b, fcolor_g; bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b); std::vector<int> psize; pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize); std::vector<double> opaque; pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque); std::vector<std::string> shadings; pcl::console::parse_multiple_arguments (argc, argv, "-shading", shadings); int mview = 0; pcl::console::parse_argument (argc, argv, "-multiview", mview); int normals = 0; pcl::console::parse_argument (argc, argv, "-normals", normals); float normals_scale = NORMALS_SCALE; pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale); int pc = 0; pcl::console::parse_argument (argc, argv, "-pc", pc); float pc_scale = PC_SCALE; pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale); bool use_vbos = false; pcl::console::parse_argument (argc, argv, "-vbo_rendering", use_vbos); if (use_vbos) print_highlight ("Vertex Buffer Object (VBO) visualization enabled.\n"); bool use_pp = pcl::console::find_switch (argc, argv, "-use_point_picking"); if (use_pp) print_highlight ("Point picking enabled.\n"); // If VBOs are not enabled, then try to use immediate rendering bool use_immediate_rendering = false; if (!use_vbos) { pcl::console::parse_argument (argc, argv, "-immediate_rendering", use_immediate_rendering); if (use_immediate_rendering) print_highlight ("Using immediate mode rendering.\n"); } // Multiview enabled? int y_s = 0, x_s = 0; double x_step = 0, y_step = 0; if (mview) { print_highlight ("Multi-viewport rendering enabled.\n"); y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size () + vtk_file_indices.size ())))); x_s = y_s + static_cast<int>(ceil (double (p_file_indices.size () + vtk_file_indices.size ()) / double (y_s) - y_s)); if (p_file_indices.size () != 0) { print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ()); print_info (" pcd files.\n"); } if (vtk_file_indices.size () != 0) { print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ()); print_info (" vtk files.\n"); } x_step = static_cast<double>(1.0 / static_cast<double>(x_s)); y_step = static_cast<double>(1.0 / static_cast<double>(y_s)); print_value ("%d", x_s); print_info ("x"); print_value ("%d", y_s); print_info (" / "); print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step); print_info (")\n"); } // Fix invalid multiple arguments if (psize.size () != p_file_indices.size () && psize.size () > 0) for (size_t i = psize.size (); i < p_file_indices.size (); ++i) psize.push_back (1); if (opaque.size () != p_file_indices.size () && opaque.size () > 0) for (size_t i = opaque.size (); i < p_file_indices.size (); ++i) opaque.push_back (1.0); if (shadings.size () != p_file_indices.size () && shadings.size () > 0) for (size_t i = shadings.size (); i < p_file_indices.size (); ++i) shadings.push_back ("flat"); // Create the PCLVisualizer object #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) boost::shared_ptr<pcl::visualization::PCLPlotter> ph; #endif // Using min_p, max_p to set the global Y min/max range for the histogram float min_p = FLT_MAX; float max_p = -FLT_MAX; int k = 0, l = 0, viewport = 0; // Load the data files pcl::PCDReader pcd; pcl::console::TicToc tt; ColorHandlerPtr color_handler; GeometryHandlerPtr geometry_handler; // Go through VTK files for (size_t i = 0; i < vtk_file_indices.size (); ++i) { // Load file tt.tic (); print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]); vtkPolyDataReader* reader = vtkPolyDataReader::New (); reader->SetFileName (argv[vtk_file_indices.at (i)]); reader->Update (); vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput (); if (!polydata) return (-1); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n"); // Create the PCLVisualizer object here on the first encountered XYZ file if (!p) p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer")); // Multiview enabled? if (mview) { p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport); k++; if (k >= x_s) { k = 0; l++; } } // Add as actor std::stringstream cloud_name ("vtk-"); cloud_name << argv[vtk_file_indices.at (i)] << "-" << i; p->addModelFromPolyData (polydata, cloud_name.str (), viewport); // Change the shape rendered color if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ()); // Change the shape rendered point size if (psize.size () > 0) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ()); // Change the shape rendered opacity if (opaque.size () > 0) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ()); // Change the shape rendered shading if (shadings.size () > 0) { if (shadings[i] == "flat") { print_highlight (stderr, "Setting shading property for %s to FLAT.\n", argv[vtk_file_indices.at (i)]); p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_FLAT, cloud_name.str ()); } else if (shadings[i] == "gouraud") { print_highlight (stderr, "Setting shading property for %s to GOURAUD.\n", argv[vtk_file_indices.at (i)]); p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_GOURAUD, cloud_name.str ()); } else if (shadings[i] == "phong") { print_highlight (stderr, "Setting shading property for %s to PHONG.\n", argv[vtk_file_indices.at (i)]); p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_PHONG, cloud_name.str ()); } } } sensor_msgs::PointCloud2::Ptr cloud; // Go through PCD files //for (size_t i = 0; i < p_file_indices.size (); ++i) for (size_t i = 0; i < 1; ++i) { tt.tic (); cloud.reset (new sensor_msgs::PointCloud2); Eigen::Vector4f origin; Eigen::Quaternionf orientation; int version; print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]); if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0) return (-1); std::stringstream cloud_name; // ---[ Special check for 1-point multi-dimension histograms if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0])) { cloud_name << argv[p_file_indices.at (i)]; #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) if (!ph) ph.reset (new pcl::visualization::PCLPlotter); #endif pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p); #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ()); #endif print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n"); continue; } // ---[ Special check for 2D images if (cloud->fields.size () == 1 && isOnly2DImage (cloud->fields[0])) { print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); std::stringstream name; name << "PCD Viewer :: " << argv[p_file_indices.at (i)]; pcl::visualization::ImageViewer::Ptr img (new pcl::visualization::ImageViewer (name.str ())); pcl::PointCloud<pcl::RGB> rgb_cloud; pcl::fromROSMsg (*cloud, rgb_cloud); img->addRGBImage (rgb_cloud); imgs.push_back (img); continue; } cloud_name << argv[p_file_indices.at (i)] << "-" << i; // Create the PCLVisualizer object here on the first encountered XYZ file if (!p) { p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer")); if (use_pp) // Only enable the point picking callback if the command line parameter is enabled p->registerPointPickingCallback (&pp_callback, static_cast<void*> (&cloud)); // Set whether or not we should be using the vtkVertexBufferObjectMapper p->setUseVbos (use_vbos); if (!cam) { Eigen::Matrix3f rotation; rotation = orientation; p->setCameraPosition (origin [0] , origin [1] , origin [2], origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2), rotation (0, 1), rotation (1, 1), rotation (2, 1)); } else { //print_info("CAM is HERE"); } } // Multiview enabled? if (mview) { p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport); k++; if (k >= x_s) { k = 0; l++; } } if (cloud->width * cloud->height == 0) { print_error ("[error: no points found!]\n"); return (-1); } // If no color was given, get random colors if (fcolorparam) { if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i) color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i])); else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); } else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); // Add the dataset with a XYZ and a random handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2> (cloud)); // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport); if (mview) // Add text with file name p->addText (argv[p_file_indices.at (i)], 5, 5, 10, 1.0, 1.0, 1.0, "text_" + std::string (argv[p_file_indices.at (i)]), viewport); // If normal lines are enabled if (normals != 0) { int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); std::stringstream cloud_name_normals; cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals"; p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport); } // If principal curvature lines are enabled if (pc != 0) { if (normals == 0) normals = pc; int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x"); if (pc_idx == -1) { print_error ("Principal Curvature information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>); pcl::fromROSMsg (*cloud, *cloud_pc); std::stringstream cloud_name_normals_pc; cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals"; int factor = (std::min)(normals, pc); p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ()); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); cloud_name_normals_pc << "-pc"; p->addPointCloudPrincipalCurvatures (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); } // Add every dimension as a possible color if (!fcolorparam) { for (size_t f = 0; f < cloud->fields.size (); ++f) { if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba") color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (cloud)); else { if (!isValidFieldName (cloud->fields[f].name)) continue; color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> (cloud, cloud->fields[f].name)); } // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport); } } // Additionally, add normals as a handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2> (cloud)); if (geometry_handler->isCapable ()) //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport); if (use_immediate_rendering) // Set immediate mode rendering ON p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ()); // Change the cloud rendered point size if (psize.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ()); // Change the cloud rendered opacity if (opaque.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ()); // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud if (i == 0 && !p->cameraParamsSet ()) p->resetCameraViewpoint (cloud_name.str ()); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); } if (!mview && p) { std::string str; if (!p_file_indices.empty ()) str = std::string (argv[p_file_indices.at (0)]); else if (!vtk_file_indices.empty ()) str = std::string (argv[vtk_file_indices.at (0)]); for (size_t i = 1; i < p_file_indices.size (); ++i) str += ", " + std::string (argv[p_file_indices.at (i)]); for (size_t i = 1; i < vtk_file_indices.size (); ++i) str += ", " + std::string (argv[vtk_file_indices.at (i)]); p->addText (str, 5, 5, 10, 1.0, 1.0, 1.0, "text_allnames"); } if (p) p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]); // Read axes settings double axes = 0.0; pcl::console::parse_argument (argc, argv, "-ax", axes); if (axes != 0.0 && p) { float ax_x = 0.0, ax_y = 0.0, ax_z = 0.0; pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false); // Draw XYZ axes if command-line enabled p->addCoordinateSystem (axes, ax_x, ax_y, ax_z); } // Clean up the memory used by the binary blob // Note: avoid resetting the cloud, otherwise the PointPicking callback will fail if (!use_pp) // Only enable the point picking callback if the command line parameter is enabled { cloud.reset (); xyzcloud.reset (); } // If we have been given images, create our own loop so that we can spin each individually if (!imgs.empty ()) { bool stopped = false; do { #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) if (ph) ph->spinOnce (); #endif for (int i = 0; i < int (imgs.size ()); ++i) { if (imgs[i]->wasStopped ()) { stopped = true; break; } imgs[i]->spinOnce (); } if (p) { if (p->wasStopped ()) { stopped = true; break; } p->spinOnce (); } printf("1\n"); boost::this_thread::sleep (boost::posix_time::microseconds (100)); } while (!stopped); } else { // If no images, continue #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) if (ph) { //printf("ph\n"); //print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p); //ph->setGlobalYRange (min_p, max_p); //ph->updateWindowPositions (); if (p) p->spin (); else ph->spin (); } else #endif if (p) { int keyState = 0; int i; p->registerKeyboardCallback(keyboardcallback, &keyState); //p->spin (); while ( !p->wasStopped()) { p->spinOnce(100); if(keyState) { printf("%d keystate\n", keyState); if(keyState == 1) i++; if(keyState == 2) i--; if(i >= dir_files.size()) i = dir_files.size() - 1; if(i < 0) i = 0; printf("i %d p_size %d %s\n", i, dir_files.size(), dir_files.at(i).c_str()); //cloud->data.clear(); p->removeAllPointClouds(); { tt.tic (); cloud.reset (new sensor_msgs::PointCloud2); Eigen::Vector4f origin; Eigen::Quaternionf orientation; int version; print_highlight (stderr, "Loading "); print_value (stderr, "%s ", dir_files[i].c_str()); if (pcd.read (dir_files[i], *cloud, origin, orientation, version) < 0) return (-1); std::stringstream cloud_name; // ---[ Special check for 1-point multi-dimension histograms if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0])) { cloud_name << dir_files[i]; #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) if (!ph) ph.reset (new pcl::visualization::PCLPlotter); #endif pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p); #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ()); #endif print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n"); continue; } // ---[ Special check for 2D images if (cloud->fields.size () == 1 && isOnly2DImage (cloud->fields[0])) { print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); std::stringstream name; name << "PCD Viewer :: " << dir_files[i]; pcl::visualization::ImageViewer::Ptr img (new pcl::visualization::ImageViewer (name.str ())); pcl::PointCloud<pcl::RGB> rgb_cloud; pcl::fromROSMsg (*cloud, rgb_cloud); img->addRGBImage (rgb_cloud); imgs.push_back (img); continue; } cloud_name << dir_files[i] << "-" << i; // Create the PCLVisualizer object here on the first encountered XYZ file if (!p) { p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer")); if (use_pp) // Only enable the point picking callback if the command line parameter is enabled p->registerPointPickingCallback (&pp_callback, static_cast<void*> (&cloud)); // Set whether or not we should be using the vtkVertexBufferObjectMapper p->setUseVbos (use_vbos); if (!cam) { Eigen::Matrix3f rotation; rotation = orientation; p->setCameraPosition (origin [0] , origin [1] , origin [2], origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2), rotation (0, 1), rotation (1, 1), rotation (2, 1)); } else { //print_info("CAM is HERE"); } } // Multiview enabled? if (mview) { p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport); k++; if (k >= x_s) { k = 0; l++; } } if (cloud->width * cloud->height == 0) { print_error ("[error: no points found!]\n"); return (-1); } // If no color was given, get random colors if (fcolorparam) { if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i) color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i])); else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); } else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); // Add the dataset with a XYZ and a random handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2> (cloud)); // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport); if (mview) // Add text with file name p->addText (dir_files[i], 5, 5, 10, 1.0, 1.0, 1.0, "text_" + std::string (dir_files[i]), viewport); // If normal lines are enabled if (normals != 0) { int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); std::stringstream cloud_name_normals; cloud_name_normals << dir_files[i] << "-" << i << "-normals"; p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport); } // If principal curvature lines are enabled if (pc != 0) { if (normals == 0) normals = pc; int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x"); if (pc_idx == -1) { print_error ("Principal Curvature information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>); pcl::fromROSMsg (*cloud, *cloud_pc); std::stringstream cloud_name_normals_pc; cloud_name_normals_pc << dir_files[i] << "-" << i << "-normals"; int factor = (std::min)(normals, pc); p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ()); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); cloud_name_normals_pc << "-pc"; p->addPointCloudPrincipalCurvatures (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); } // Add every dimension as a possible color if (!fcolorparam) { for (size_t f = 0; f < cloud->fields.size (); ++f) { if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba") color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (cloud)); else { if (!isValidFieldName (cloud->fields[f].name)) continue; color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> (cloud, cloud->fields[f].name)); } // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport); } } // Additionally, add normals as a handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2> (cloud)); if (geometry_handler->isCapable ()) //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport); if (use_immediate_rendering) // Set immediate mode rendering ON p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ()); // Change the cloud rendered point size if (psize.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ()); // Change the cloud rendered opacity if (opaque.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ()); // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud if (i == 0 && !p->cameraParamsSet ()) p->resetCameraViewpoint (cloud_name.str ()); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); } keyState = 0; } boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } } } }
/* ---[ */ int main (int argc, char** argv) { srand (static_cast<unsigned int> (time (0))); print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n"); if (argc < 2) { printHelp (argc, argv); return (-1); } bool cam = pcl::console::find_switch (argc, argv, "-cam"); // Command line parsing double bcolor[3] = {0, 0, 0}; pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]); std::vector<double> fcolor_r, fcolor_b, fcolor_g; bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b); std::vector<int> psize; pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize); std::vector<double> opaque; pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque); int mview = 0; pcl::console::parse_argument (argc, argv, "-multiview", mview); int normals = 0; pcl::console::parse_argument (argc, argv, "-normals", normals); float normals_scale = NORMALS_SCALE; pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale); int pc = 0; pcl::console::parse_argument (argc, argv, "-pc", pc); float pc_scale = PC_SCALE; pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale); bool use_vbos = pcl::console::find_switch (argc, argv, "-use_vbos"); // Parse the command line arguments for .pcd files std::vector<int> p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk"); if (p_file_indices.size () == 0 && vtk_file_indices.size () == 0) { print_error ("No .PCD or .VTK file given. Nothing to visualize.\n"); return (-1); } // Multiview enabled? int y_s = 0, x_s = 0; double x_step = 0, y_step = 0; if (mview) { print_highlight ("Multi-viewport rendering enabled.\n"); y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size () + vtk_file_indices.size ())))); x_s = y_s + static_cast<int>(ceil (double (p_file_indices.size () + vtk_file_indices.size ()) / double (y_s) - y_s)); if (p_file_indices.size () != 0) { print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ()); print_info (" pcd files.\n"); } if (vtk_file_indices.size () != 0) { print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ()); print_info (" vtk files.\n"); } x_step = static_cast<double>(1.0 / static_cast<double>(x_s)); y_step = static_cast<double>(1.0 / static_cast<double>(y_s)); print_value ("%d", x_s); print_info ("x"); print_value ("%d", y_s); print_info (" / "); print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step); print_info (")\n"); } // Fix invalid multiple arguments if (psize.size () != p_file_indices.size () && psize.size () > 0) for (size_t i = psize.size (); i < p_file_indices.size (); ++i) psize.push_back (1); if (opaque.size () != p_file_indices.size () && opaque.size () > 0) for (size_t i = opaque.size (); i < p_file_indices.size (); ++i) opaque.push_back (1.0); // Create the PCLVisualizer object boost::shared_ptr<pcl::visualization::PCLPlotter> ph; // Using min_p, max_p to set the global Y min/max range for the histogram float min_p = FLT_MAX; float max_p = -FLT_MAX; int k = 0, l = 0, viewport = 0; // Load the data files pcl::PCDReader pcd; pcl::console::TicToc tt; ColorHandlerPtr color_handler; GeometryHandlerPtr geometry_handler; // Go through VTK files for (size_t i = 0; i < vtk_file_indices.size (); ++i) { // Load file tt.tic (); print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]); vtkPolyDataReader* reader = vtkPolyDataReader::New (); reader->SetFileName (argv[vtk_file_indices.at (i)]); reader->Update (); vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput (); if (!polydata) return (-1); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n"); // Create the PCLVisualizer object here on the first encountered XYZ file if (!p) p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer")); // Multiview enabled? if (mview) { p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport); k++; if (k >= x_s) { k = 0; l++; } } // Add as actor std::stringstream cloud_name ("vtk-"); cloud_name << argv[vtk_file_indices.at (i)] << "-" << i; p->addModelFromPolyData (polydata, cloud_name.str (), viewport); // Change the shape rendered color if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ()); // Change the shape rendered point size if (psize.size () > 0) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ()); // Change the shape rendered opacity if (opaque.size () > 0) p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ()); } sensor_msgs::PointCloud2::Ptr cloud; // Go through PCD files for (size_t i = 0; i < p_file_indices.size (); ++i) { cloud.reset (new sensor_msgs::PointCloud2); Eigen::Vector4f origin; Eigen::Quaternionf orientation; int version; print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]); tt.tic (); if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0) return (-1); std::stringstream cloud_name; // ---[ Special check for 1-point multi-dimension histograms if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0])) { cloud_name << argv[p_file_indices.at (i)]; if (!ph) ph.reset (new pcl::visualization::PCLPlotter); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n"); pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p); ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ()); continue; } cloud_name << argv[p_file_indices.at (i)] << "-" << i; // Create the PCLVisualizer object here on the first encountered XYZ file if (!p) { p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer")); p->registerPointPickingCallback (&pp_callback, static_cast<void*> (&cloud)); if (!cam) { Eigen::Matrix3f rotation; rotation = orientation; p->setCameraPosition (origin [0] , origin [1] , origin [2], origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2), rotation (0, 1), rotation (1, 1), rotation (2, 1)); } } // Multiview enabled? if (mview) { p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport); k++; if (k >= x_s) { k = 0; l++; } } if (cloud->width * cloud->height == 0) { print_error ("[error: no points found!]\n"); return (-1); } print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); // Set whether or not we should be using the vtkVertexBufferObjectMapper p->setUseVbos(use_vbos); // If no color was given, get random colors if (fcolorparam) { if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i) color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i])); else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); } else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> (cloud)); // Add the dataset with a XYZ and a random handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2> (cloud)); // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport); if (mview) // Add text with file name p->addText (argv[p_file_indices.at (i)], 5, 5, 10, 1.0, 1.0, 1.0, "text_" + std::string (argv[p_file_indices.at (i)]), viewport); // If normal lines are enabled if (normals != 0) { int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); std::stringstream cloud_name_normals; cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals"; p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport); } // If principal curvature lines are enabled if (pc != 0) { if (normals == 0) normals = pc; int normal_idx = pcl::getFieldIndex (*cloud, "normal_x"); if (normal_idx == -1) { print_error ("Normal information requested but not available.\n"); continue; //return (-1); } int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x"); if (pc_idx == -1) { print_error ("Principal Curvature information requested but not available.\n"); continue; //return (-1); } // // Convert from blob to pcl::PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*cloud, *cloud_xyz); cloud_xyz->sensor_origin_ = origin; cloud_xyz->sensor_orientation_ = orientation; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg (*cloud, *cloud_normals); pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>); pcl::fromROSMsg (*cloud, *cloud_pc); std::stringstream cloud_name_normals_pc; cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals"; int factor = (std::min)(normals, pc); p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ()); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); cloud_name_normals_pc << "-pc"; p->addPointCloudPrincipalCurvatures (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport); p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ()); } // Add every dimension as a possible color if (!fcolorparam) { for (size_t f = 0; f < cloud->fields.size (); ++f) { if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba") color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (cloud)); else { if (!isValidFieldName (cloud->fields[f].name)) continue; color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> (cloud, cloud->fields[f].name)); } // Add the cloud to the renderer //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport); } } // Additionally, add normals as a handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2> (cloud)); if (geometry_handler->isCapable ()) //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport); p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport); // Set immediate mode rendering ON p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ()); // Change the cloud rendered point size if (psize.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ()); // Change the cloud rendered opacity if (opaque.size () > 0) p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ()); // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud if (i == 0 && !p->cameraParamsSet ()) p->resetCameraViewpoint (cloud_name.str ()); } if (!mview) { std::string str (argv[p_file_indices.at (0)]); for (size_t i = 01; i < p_file_indices.size (); ++i) str += ", " + std::string (argv[p_file_indices.at (i)]); p->addText (str, 5, 5, 10, 1.0, 1.0, 1.0, "text_allnames"); } if (p) p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]); // Read axes settings double axes = 0.0; pcl::console::parse_argument (argc, argv, "-ax", axes); if (axes != 0.0 && p) { float ax_x = 0.0, ax_y = 0.0, ax_z = 0.0; pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false); // Draw XYZ axes if command-line enabled p->addCoordinateSystem (axes, ax_x, ax_y, ax_z); } // Clean up the memory used by the binary blob // Note: avoid resetting the cloud, otherwise the PointPicking callback will fail //cloud.reset (); if (ph) { //print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p); //ph->setGlobalYRange (min_p, max_p); //ph->updateWindowPositions (); if (p) p->spin (); else { ph->spin (); } } else if (p) p->spin (); }