void showGT(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, pcl::visualization::PCLVisualizer::Ptr viewer) { std::vector<pcl::PointCloud<myPointXYZ>::Ptr> rec; for( std::vector<ModelT>::const_iterator it = model_set.begin() ; it < model_set.end() ; it++ ) { pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::fromPCLPointCloud2(it->model_mesh->cloud, *cur_cloud); rec.push_back(cur_cloud); } int count = 0; Eigen::Quaternionf calibrate_rot(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f (1, 0, 0))); for(std::vector<poseT>::const_iterator it = poses.begin() ; it < poses.end() ; it++, count++ ) { for( int i = 0 ; i < model_set.size() ; i++ ) { if(model_set[i].model_label == it->model_name ) { pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>()); //pcl::transformPointCloud(*rec[i], *cur_cloud, Eigen::Vector3f (0, 0, 0), calibrate_rot); pcl::transformPointCloud(*rec[i], *cur_cloud, it->shift, it->rotation*calibrate_rot); std::stringstream ss; ss << count; viewer->addPolygonMesh<myPointXYZ>(cur_cloud, model_set[i].model_mesh->polygons, it->model_name+"_"+ss.str()); if( it->model_name == "link" ) viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.55, 0.05, it->model_name+"_"+ss.str()); else if( it->model_name == "node" ) viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.05, 0.55, 1.0, it->model_name+"_"+ss.str()); break; } } } }
void callback(const sensor_msgs::PointCloud2 &pc) { double t1, t2, avg_t; ROS_INFO("Received point cloud! Applying method %d",method_id); pcl::PointCloud<PointT>::Ptr scene_pc(new pcl::PointCloud<PointT>()); pcl::PCLPointCloud2 pcl_pc; pcl_conversions::toPCL(pc, pcl_pc); pcl::fromPCLPointCloud2(pcl_pc, *scene_pc); if( scene_pc->empty() == true ) { ROS_ERROR("Recieved empty point cloud message!"); return; } pcl::PointCloud<myPointXYZ>::Ptr scene_xyz(new pcl::PointCloud<myPointXYZ>()); pcl::copyPointCloud(*scene_pc, *scene_xyz); if( view_flag == true ) { viewer->removeAllPointClouds(); viewer->addPointCloud(scene_xyz, "whole_scene"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0.5, 0.5, "whole_scene"); } std::vector<poseT> all_poses; std::vector<poseT> link_poses, node_poses; std::cout << " ... processing:" << std::endl; switch(method_id) { case 0: t1 = get_wall_time(); objrec->StandardRecognize(scene_xyz, all_poses); t2 = get_wall_time(); break; case 1: { t1 = get_wall_time(); int pose_num = 0; int iter = 0; pcl::PointCloud<myPointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<myPointXYZ>()); filtered_cloud = scene_xyz; while(true) { //std::cerr<< "Recognizing Attempt --- " << iter << std::endl; objrec->StandardRecognize(filtered_cloud, all_poses); if( all_poses.size() <= pose_num ) break; else pose_num = all_poses.size(); pcl::PointCloud<myPointXYZ>::Ptr model_cloud = objrec->FillModelCloud(all_poses); filtered_cloud = FilterCloud(filtered_cloud, model_cloud); iter++; } t2 = get_wall_time(); //std::cerr<< "Recognizing Done!!!" << std::endl; break; } case 2: { //std::vector<poseT> tmp_poses; t1 = get_wall_time(); objrec->GreedyRecognize(scene_xyz, all_poses); t2 = get_wall_time(); //all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } case 3: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); std::cout << " ... starting at " << t1 << std::endl; //std::vector<poseT> link_poses, node_poses; linkrec->StandardRecognize(link_cloud, link_poses); noderec->StandardRecognize(node_cloud, node_poses); t2 = get_wall_time(); std::cout << " ... done at " << t2 << std::endl; all_poses.insert(all_poses.end(), link_poses.begin(), link_poses.end()); all_poses.insert(all_poses.end(), node_poses.begin(), node_poses.end()); break; } case 4: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); std::cout << " ... starting at " << t1 << std::endl; int pose_num = 0; std::vector<poseT> tmp_poses; int iter = 0; while(true) { //std::cerr<< "Recognizing Attempt --- " << iter << std::endl; list<AcceptedHypothesis> acc_hypotheses; std::cout << " ... generating" << std::endl; linkrec->genHypotheses(link_cloud, acc_hypotheses); noderec->genHypotheses(node_cloud, acc_hypotheses); std::cout << " ... merging" << std::endl; linkrec->mergeHypotheses(scene_xyz, acc_hypotheses, tmp_poses); if( tmp_poses.size() <= pose_num ) { break; } else { pose_num = tmp_poses.size(); std::cout << "Number of merged hypotheses: " << tmp_poses.size() << std::endl; } pcl::PointCloud<myPointXYZ>::Ptr link_model = linkrec->FillModelCloud(tmp_poses); pcl::PointCloud<myPointXYZ>::Ptr node_model = noderec->FillModelCloud(tmp_poses); std::cout << " ... filtering" << std::endl; if( link_model->empty() == false ) { link_cloud = FilterCloud(link_cloud, link_model); } if( node_model->empty() == false) { node_cloud = FilterCloud(node_cloud, node_model); } iter++; } t2 = get_wall_time(); std::cout << " ... done at " << t2 << std::endl; //std::cerr<< "Recognizing Done!!!" << std::endl; all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } case 5: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); linkrec->GreedyRecognize(link_cloud, link_poses); noderec->GreedyRecognize(node_cloud, node_poses); t2 = get_wall_time(); std::vector<poseT> tmp_poses; tmp_poses.insert(tmp_poses.end(), link_poses.begin(), link_poses.end()); tmp_poses.insert(tmp_poses.end(), node_poses.begin(), node_poses.end()); //all_poses = tmp_poses; all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } default: ROS_ERROR("Code %d not recognized!",method_id); return; } if( view_flag == true ) { switch(method_id) { case 0: case 1: case 2: objrec->visualize(viewer, all_poses); break; case 3: case 4: case 5: linkrec->visualize(viewer, all_poses); noderec->visualize(viewer, all_poses); break; } viewer->spin(); } geometry_msgs::PoseArray links; geometry_msgs::PoseArray nodes; links.header.frame_id = pc.header.frame_id; nodes.header.frame_id = pc.header.frame_id; // publish poses as TF if (method_id < 3) { for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); links.poses.push_back(pose); } pub_link.publish(links); } else { for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); links.poses.push_back(pose); } for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); nodes.poses.push_back(pose); } pub_link.publish(links); pub_link.publish(nodes); } std::cout << "Time 1 = " << t1 << std::endl; std::cout << "Time 2 = " << t2 << std::endl; ROS_INFO("... done."); }
int main (int argc, char** argv) { srand (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); } // 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); double 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); double pc_scale = PC_SCALE; pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale); // 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"); if (p_file_indices.size () != 0) { y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size ())))); x_s = y_s + static_cast<int>(ceil ((p_file_indices.size () / static_cast<double>(y_s)) - y_s)); print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ()); } else if (vtk_file_indices.size () != 0) { y_s = static_cast<int>(floor (sqrt (static_cast<float>(vtk_file_indices.size ())))); x_s = y_s + static_cast<int>(ceil ((vtk_file_indices.size () / static_cast<double>(y_s)) - y_s)); print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ()); } 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_info (" files ("); 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 PCLHistogramVisualizer object pcl::visualization::PCLHistogramVisualizer::Ptr 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 ()); } pcl::PCLPointCloud2::Ptr cloud; // Go through PCD files for (size_t i = 0; i < p_file_indices.size (); ++i) { cloud.reset (new pcl::PCLPointCloud2); 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::PCLHistogramVisualizer); 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, (void*)&cloud); 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 ("%d", (int)cloud->width * cloud->height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ()); // 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<pcl::PCLPointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i])); else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud)); } else color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud)); // Add the dataset with a XYZ and a random handler geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (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 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::fromPCLPointCloud2 (*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::fromPCLPointCloud2 (*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::fromPCLPointCloud2 (*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::fromPCLPointCloud2 (*cloud, *cloud_normals); pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>); pcl::fromPCLPointCloud2 (*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<pcl::PointXYZ, pcl::Normal> (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<pcl::PCLPointCloud2> (cloud)); else { if (!isValidFieldName (cloud->fields[f].name)) continue; color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (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<pcl::PCLPointCloud2> (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 ()); } //////////////////////////////////////////////////////////////// // Key binding for saving simulated point cloud: if (p) p->registerKeyboardCallback(simulate_callback, (void*)&p); int width = 640; int height = 480; window_width_ = width * 2; window_height_ = height * 2; print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]); for (int i=0; i<2048; i++) { float v = i/2048.0; v = powf(v, 3)* 6; t_gamma[i] = v*6*256; } GLenum err = glewInit (); if (GLEW_OK != err) { std::cerr << "Error: " << glewGetErrorString (err) << std::endl; exit (-1); } std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl; if (glewIsSupported ("GL_VERSION_2_0")) std::cout << "OpenGL 2.0 supported" << std::endl; else { std::cerr << "Error: OpenGL 2.0 not supported" << std::endl; exit(1); } camera_ = Camera::Ptr (new Camera ()); scene_ = Scene::Ptr (new Scene ()); range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood(1, 1, height, width, scene_)); // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_)); // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_)); // Actually corresponds to default parameters: range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860, 576.09757860, 321.06398107, 242.97676897); range_likelihood_->setComputeOnCPU (false); range_likelihood_->setSumOnCPU (true); range_likelihood_->setUseColor (true); initialize (argc, argv); 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) { double 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, "reference"); } // 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 (); }