void PeopleDetector::addNewCloudToViewer(PointCloudT::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr viewer_obj) { viewer_obj->removeAllPointClouds(); viewer_obj->removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer_obj->addPointCloud<PointT> (cloud, rgb, "input_cloud"); }
void setCam( /* in: */ Eigen::Vector3d &pos, Eigen::Vector3d &up, Eigen::Vector3d &dir, ::pcl::visualization::PCLVisualizer::Ptr pViewerPtr ) { vtkSmartPointer<vtkRendererCollection> rens = pViewerPtr->getRendererCollection(); rens->InitTraversal (); vtkRenderer* renderer = NULL; int it = 0; while ((renderer = rens->GetNextItem ()) != NULL && (it < 1) ) { vtkCamera& camera = *renderer->GetActiveCamera(); camera.SetPosition ( pos[0], pos[1], pos[2] ); camera.SetViewUp ( up[0], up[1], up[2] ); camera.SetFocalPoint( dir[0], dir[1], dir[2] ); ++it; } pViewerPtr->spinOnce(); }
void Visualizer::saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr &visualizer) { lock.lock(); std::ostringstream oss, oss_cloud; oss_cloud << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".pcd"; oss << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".png"; outInfo("saving cloud: " << oss_cloud.str()); // pcl::io::savePCDFileASCII(oss.str(), *cloud); outInfo("saving screenshot: " << oss.str()); visualizer->saveScreenshot(oss.str()); ++saveFrameCloud; lock.unlock(); }
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 visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, pcl::visualization::PCLVisualizer::Ptr pVisualizer) { //init visualizer pVisualizer->setSize(640, 480); pVisualizer->setPosition(640, 0); pVisualizer->setBackgroundColor(0x00, 0x00, 0x00); pVisualizer->initCameraParameters(); pVisualizer->addPointCloud(pCloud, "cloud"); //reload visualizer content pVisualizer->spinOnce(1000000); }
void fetchViewerZBuffer( /* out: */ cv::Mat & zBufMat, /* in: */ ::pcl::visualization::PCLVisualizer::Ptr const& viewer, double zNear, double zFar ) { std::cout << "saving vtkZBuffer..."; vtkSmartPointer<vtkWindowToImageFilter> filter = vtkSmartPointer<vtkWindowToImageFilter>::New(); vtkSmartPointer<vtkImageShiftScale> scale = vtkSmartPointer<vtkImageShiftScale>::New(); vtkSmartPointer<vtkWindow> renWin = viewer->getRenderWindow(); // Create Depth Map filter->SetInput( renWin.GetPointer() ); filter->SetMagnification(1); filter->SetInputBufferTypeToZBuffer(); // scale scale->SetOutputScalarTypeToFloat(); scale->SetInputConnection(filter->GetOutputPort()); scale->SetShift(0.f); scale->SetScale(1.f); scale->Update(); // fetch data vtkSmartPointer<vtkImageData> imageData = scale->GetOutput(); int* dims = imageData->GetDimensions(); if ( dims[2] > 1 ) { std::cerr << "am::util::pcl::fetchZBuffer(): ZDim != 1 !!!!" << std::endl; } // output zBufMat.create( dims[1], dims[0], CV_32FC1 ); for ( int y = 0; y < dims[1]; ++y ) { for ( int x = 0; x < dims[0]; ++x ) { float* pixel = static_cast<float*>( imageData->GetScalarPointer(x,y,0) ); float d = round(2.0 * zNear * zFar / (zFar + zNear - pixel[0] * (zFar - zNear)) * 1000.f); zBufMat.at<float>( dims[1] - y - 1, x ) = (d > 10000.f) ? 0.f : d; //data[ z * dims[1] * dims[0] + (dims[1] - y - 1) * dims[0] + x ] = pixel[0];//(pixel[0] == 10001) ? 0 : pixel[0]; } //std::cout << std::endl; } //std::cout << std::endl; }
virtual void processBufferElement(CloudConstPtr cloud) override { cloud_viewer_->spinOnce(); if (!cloud_viewer_->updatePointCloud(cloud, "OpenNICloud")) { cloud_viewer_->setPosition(0, 0); cloud_viewer_->setSize(cloud->width, cloud->height); cloud_viewer_->addPointCloud(cloud, "OpenNICloud"); cloud_viewer_->resetCameraViewpoint("OpenNICloud"); cloud_viewer_->setCameraPosition( 0, 0, 0, // Position 0, 0, 1, // Viewpoint 0, -1, 0); // Up } // TODO; Figure out how to achieve this // if(cloud_viewer_->wasStopped()) { // this->cloud_buffer_.reset(new Buffer<CloudConstPtr>()); // this->stop(); // cloud_viewer_->close(); // } }
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) { ros::init(argc,argv,"greedy_objrec_ransac_node"); ros::NodeHandle nh; pcl::console::parse_argument(argc, argv, "--m", method_id); pcl::console::parse_argument(argc, argv, "--p", path); pcl::console::parse_argument(argc, argv, "--t", trial_id); pcl::console::parse_argument(argc, argv, "--link", link_mesh_name); pcl::console::parse_argument(argc, argv, "--node", node_mesh_name); pcl::console::parse_argument(argc, argv, "--link-width", linkWidth); pcl::console::parse_argument(argc, argv, "--node-width", nodeWidth); pcl::console::parse_argument(argc, argv, "--voxel-size", voxelSize); linkrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(linkWidth, voxelSize)); noderec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize)); objrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize)); bool view_flag = false; if( pcl::console::find_switch(argc, argv, "-v") == true ) view_flag = true; if( view_flag == true ) { viewer = pcl::visualization::PCLVisualizer::Ptr (new pcl::visualization::PCLVisualizer()); viewer->initCameraParameters(); viewer->addCoordinateSystem(0.1); viewer->setSize(640, 480); viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0); } std::stringstream mm; mm << method_id; if( method_id >= 3 ) { ROS_INFO("Loading link mesh from \"%s.obj\"...",link_mesh_name.c_str()); link_mesh = LoadMesh(link_mesh_name+".obj", "link"); ROS_INFO("Loading node mesh from \"%s.obj\"...",node_mesh_name.c_str()); node_mesh = LoadMesh(node_mesh_name+".obj", "node"); mesh_set.push_back(link_mesh); mesh_set.push_back(node_mesh); } switch(method_id) { case 0: case 1: case 2: objrec->AddModel(link_mesh_name, "link"); objrec->AddModel(node_mesh_name, "node"); break; case 3: case 4: case 5: linkrec->AddModel(link_mesh_name, "link"); noderec->AddModel(node_mesh_name, "node"); break; default:return 0; } if (method_id < 3) { pub_link = nh.advertise<geometry_msgs::PoseArray>("objects",1000); } else { pub_link = nh.advertise<geometry_msgs::PoseArray>("links",1000); pub_node = nh.advertise<geometry_msgs::PoseArray>("nodes",1000); } sub = nh.subscribe("points_in",1,callback); // send and recieve messages while (ros::ok()) { ros::spin(); } return 0; }
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 (); }
virtual void initializeThreadResources() override { cloud_viewer_.reset(new pcl::visualization::PCLVisualizer("PCL OpenNI2 Cloud")); cloud_viewer_->registerMouseCallback(&CloudViewer::mouseCallback, *this); cloud_viewer_->registerKeyboardCallback(&CloudViewer::keyboardCallback, *this); }