void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun) { const std::string centroidname = this->name + "_centroids"; const std::string coloredvoxelname = this->name + "_voxels_colored"; const std::string normalsname = this->name + "_supervoxel_normals"; if(!firstRun) { visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, centroidname); } visualizer.removeAllPointClouds(); visualizer.removeAllShapes(); addCentroids(visualizer, centroidname); switch(dispMode) { case ALL: addVoxels(visualizer, coloredvoxelname); addAdjacency(visualizer); addNormals(visualizer, normalsname); break; case W_VOXELS: addVoxels(visualizer, coloredvoxelname); break; case W_VA: addVoxels(visualizer, coloredvoxelname); addAdjacency(visualizer); break; case W_VN: addVoxels(visualizer, coloredvoxelname); addNormals(visualizer, normalsname); break; case W_AN: addAdjacency(visualizer); addNormals(visualizer, normalsname); break; case W_ADJACENCY: addAdjacency(visualizer); break; case W_NORMALS: addNormals(visualizer, normalsname); break; case ADJACENCY: visualizer.removeAllPointClouds(); addAdjacency(visualizer); break; case NONE: break; case TEST: filterAdjacency(visualizer); break; } }
void Inspector::pointPickingCallback(const pcl::visualization::PointPickingEvent& event, void* cookie) { if(event.getPointIndex() == -1) return; Point pt; event.getPoint(pt.x, pt.y, pt.z); cout << "Selected point: " << pt.x << ", " << pt.y << ", " << pt.z << endl; pcd_vis_.removeAllShapes(); Point origin(0, 0, 0); pcd_vis_.addArrow<Point, Point>(origin, pt, 0.0, 0.0, 0.8, 0.9, 0.9, 0.9, "line"); }
void ObstacleVisualizer<PointT>::drawShapes( std::vector<ObjectModelPtr> obstacles, pcl::visualization::PCLVisualizer& viewer) { // Remove all old shapes... viewer.removeAllShapes(); // ...and draw all the new ones. ModelDrawer drawer(viewer); size_t const sz = obstacles.size(); for (size_t i = 0; i < sz; ++i) { obstacles[i]->accept(drawer); } }
void filterAdjacency(pcl::visualization::PCLVisualizer &visualizer) { visualizer.removeAllPointClouds(); visualizer.removeAllShapes(); std::multimap<uint32_t, uint32_t> supervoxel_adjacency; supervoxels->getSupervoxelAdjacency(supervoxel_adjacency); pcl::PointCloud< pcl::PointXYZRGBA>::Ptr filtered_supervoxel(new pcl::PointCloud< pcl::PointXYZRGBA>); for(std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); label_itr != supervoxel_adjacency.end();) { //First get the label uint32_t supervoxel_label = label_itr->first; //Now get the supervoxel corresponding to the label pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label); int threshold = 7 - supervoxel->centroid_.z * 0.375; int count = supervoxel_adjacency.count(supervoxel_label); if(count >= threshold) { //Now we need to iterate through the adjacent supervoxels and make a point cloud of them pcl::PointCloud< pcl::PointXYZRGBA>::Ptr adjacent_supervoxel_centers(new pcl::PointCloud< pcl::PointXYZRGBA>); std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first; for(; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr) { uint32_t snd_label = adjacent_itr->second; int snd_count = supervoxel_adjacency.count(snd_label); pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr neighbor_supervoxel = supervoxel_clusters.at(snd_label); int snd_threshold = 7 - neighbor_supervoxel->centroid_.z * 0.375; if(snd_count >= snd_threshold) { adjacent_supervoxel_centers->push_back(neighbor_supervoxel->centroid_); filtered_supervoxel->push_back(neighbor_supervoxel->centroid_); } } //Now we make a name for this polygon std::stringstream ss; ss << "supervoxel_" << supervoxel_label; //This function generates a "star" polygon mesh from the points given addSupervoxelConnectionsToViewer(supervoxel->centroid_, *adjacent_supervoxel_centers, ss.str(), visualizer); } label_itr = supervoxel_adjacency.upper_bound(supervoxel_label); } const std::string foo = "fassdfasdf"; visualizer.addPointCloud(filtered_supervoxel, foo); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, foo); }
void Inspector::keyboardCallback(const pcl::visualization::KeyboardEvent& event, void* cookie) { if(event.keyDown()) { if(event.getKeyCode() == 'm') { toggleModel(); } else if(event.getKeyCode() == 'c') { pcd_vis_.removeAllShapes(); } else if(event.getKeyCode() == 27) { quitting_ = true; } } }
int main (int argc, char** argv) { if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h")) return print_help(); // Algorithm parameters: std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml"; float min_confidence = -1.5; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; Eigen::Matrix3f rgb_intrinsics_matrix; rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics // Read if some parameters are passed from command line: pcl::console::parse_argument (argc, argv, "--svm", svm_filename); pcl::console::parse_argument (argc, argv, "--conf", min_confidence); pcl::console::parse_argument (argc, argv, "--min_h", min_height); pcl::console::parse_argument (argc, argv, "--max_h", max_height); // Read Kinect live stream: PointCloudT::Ptr cloud (new PointCloudT); bool new_cloud_available_flag = false; pcl::Grabber* interface = new pcl::OpenNIGrabber(); boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag); interface->registerCallback (f); interface->start (); // Wait for the first frame: while(!new_cloud_available_flag) boost::this_thread::sleep(boost::posix_time::milliseconds(1)); new_cloud_available_flag = false; cloud_mutex.lock (); // for not overwriting the point cloud // Display pointcloud: pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args cb_args; PointCloudT::Ptr clicked_points_3d (new PointCloudT); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer); viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args); std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl; // Spin until 'Q' is pressed: viewer.spin(); std::cout << "done." << std::endl; cloud_mutex.unlock (); // Ground plane estimation: Eigen::VectorXf ground_coeffs; ground_coeffs.resize(4); std::vector<int> clicked_points_indices; for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++) clicked_points_indices.push_back(i); pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d); model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs); std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl; // Initialize new viewer: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Create classifier for people detection: pcl::people::PersonClassifier<pcl::RGB> person_classifier; person_classifier.loadSVMFromFile(svm_filename); // load trained SVM // People detection app initialization: pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier people_detector.setHeightLimits(min_height, max_height); // set person classifier // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical // For timing: static unsigned count = 0; static double last = pcl::getTime (); // Main loop: while (!viewer.wasStopped()) { if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available { new_cloud_available_flag = false; // Perform people detection on the new cloud: std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters people_detector.setInputCloud(cloud); people_detector.setGround(ground_coeffs); // set floor coefficients people_detector.compute(clusters); // perform people detection ground_coeffs = people_detector.getGround(); // get updated floor coefficients // Draw cloud and people bounding boxes in the viewer: viewer.removeAllPointClouds(); viewer.removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); unsigned int k = 0; for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) { if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold { // draw theoretical person bounding box in the PCL viewer: it->drawTBoundingBox(viewer, k); k++; } } std::cout << k << " people found" << std::endl; viewer.spinOnce(); // Display average framerate: if (++count == 30) { double now = pcl::getTime (); std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl; count = 0; last = now; } cloud_mutex.unlock (); } } return 0; }
void PbMapMaker::viz_cb (pcl::visualization::PCLVisualizer& viz) { if (mPbMap.globalMapPtr->empty()) { mrpt::system::sleep(10); return; } { mrpt::synch::CCriticalSectionLocker csl(&CS_visualize); // Render the data { viz.removeAllShapes(); viz.removeAllPointClouds(); char name[1024]; if(graphRepresentation) { for(size_t i=0; i<mPbMap.vPlanes.size(); i++) { pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0], 2*mPbMap.vPlanes[i].v3center[1], 2*mPbMap.vPlanes[i].v3center[2]); double radius = 0.1 * sqrt(mPbMap.vPlanes[i].areaVoxels); sprintf (name, "sphere%u", static_cast<unsigned>(i)); viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name); if( !mPbMap.vPlanes[i].label.empty() ) viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], mPbMap.vPlanes[i].label); else { sprintf (name, "P%u", static_cast<unsigned>(i)); viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name); } // Draw edges if(!configPbMap.graph_mode) // Nearby neighbors for(set<unsigned>::iterator it = mPbMap.vPlanes[i].nearbyPlanes.begin(); it != mPbMap.vPlanes[i].nearbyPlanes.end(); it++) { if(*it > mPbMap.vPlanes[i].id) break; sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it)); pcl::PointXYZ center_it(2*mPbMap.vPlanes[*it].v3center[0], 2*mPbMap.vPlanes[*it].v3center[1], 2*mPbMap.vPlanes[*it].v3center[2]); viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name); } else for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[i].neighborPlanes.begin(); it != mPbMap.vPlanes[i].neighborPlanes.end(); it++) { if(it->first > mPbMap.vPlanes[i].id) break; sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first)); pcl::PointXYZ center_it(2*mPbMap.vPlanes[it->first].v3center[0], 2*mPbMap.vPlanes[it->first].v3center[1], 2*mPbMap.vPlanes[it->first].v3center[2]); viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name); sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first)); char commonObs[8]; sprintf (commonObs, "%u", it->second); pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 ); viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name); } } } else { // Regular representation if (!viz.updatePointCloud (mPbMap.globalMapPtr, "cloud")) viz.addPointCloud (mPbMap.globalMapPtr, "cloud"); if(mpPbMapLocaliser != NULL) if(mpPbMapLocaliser->alignedModelPtr){ if (!viz.updatePointCloud (mpPbMapLocaliser->alignedModelPtr, "model")) viz.addPointCloud (mpPbMapLocaliser->alignedModelPtr, "model");} sprintf (name, "PointCloud size %u", static_cast<unsigned>( mPbMap.globalMapPtr->size() ) ); viz.addText(name, 10, 20); for(size_t i=0; i<mPbMap.vPlanes.size(); i++) { Plane &plane_i = mPbMap.vPlanes[i]; sprintf (name, "normal_%u", static_cast<unsigned>(i)); pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]); pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]), plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]), plane_i.v3center[2] + (0.5f * plane_i.v3normal[2])); viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name); // Draw Ppal diretion // if( plane_i.elongation > 1.3 ) // { // sprintf (name, "ppalComp_%u", static_cast<unsigned>(i)); // pcl::PointXYZ pt3 = pcl::PointXYZ ( plane_i.v3center[0] + (0.2f * plane_i.v3PpalDir[0]), // plane_i.v3center[1] + (0.2f * plane_i.v3PpalDir[1]), // plane_i.v3center[2] + (0.2f * plane_i.v3PpalDir[2])); // viz.addArrow (pt3, plane_i.pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name); // } // if( !plane_i.label.empty() ) // viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label); // else { sprintf (name, "n%u", static_cast<unsigned>(i)); // sprintf (name, "n%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(plane_i.semanticGroup)); viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name); } // sprintf (name, "planeRaw_%02u", static_cast<unsigned>(i)); // viz.addPointCloud (plane_i.planeRawPointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr // if(!configPbMap.makeClusters) // { // sprintf (name, "plane_%02u", static_cast<unsigned>(i)); // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[i%10], grn[i%10], blu[i%10]); //// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]); // viz.addPointCloud (plane_i.planePointCloudPtr, color, name); // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name); // } // else // { // sprintf (name, "plane_%02u", static_cast<unsigned>(i)); // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]); // viz.addPointCloud (plane_i.planePointCloudPtr, color, name); // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name); // } // sprintf (name, "planeBorder_%02u", static_cast<unsigned>(i)); // pcl::visualization::PointCloudColorHandlerCustom <PointT> color2 (plane_i.contourPtr, 255, 255, 255); // viz.addPointCloud (plane_i.contourPtr, color2, name);// contourPtr, planePointCloudPtr, polygonContourPtr // //Edges // if(mPbMap.edgeCloudPtr->size() > 0) // { // sprintf (name, "planeEdge_%02u", static_cast<unsigned>(i)); // pcl::visualization::PointCloudColorHandlerCustom <PointT> color4 (mPbMap.edgeCloudPtr, 255, 255, 0); // viz.addPointCloud (mPbMap.edgeCloudPtr, color4, name);// contourPtr, planePointCloudPtr, polygonContourPtr // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name); // // sprintf (name, "edge%u", static_cast<unsigned>(i)); // viz.addLine (mPbMap.edgeCloudPtr->points.front(), mPbMap.edgeCloudPtr->points.back(), ared[3], agrn[3], ablu[3], name); // } sprintf (name, "approx_plane_%02d", int (i)); viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name); } //if(configPbMap.makeClusters) // for(map<unsigned, std::vector<unsigned> >::iterator it=clusterize->groups.begin(); it != clusterize->groups.end(); it++) // for(size_t i=0; i < it->second.size(); i++) // { // unsigned planeID = it->second[i]; // Plane &plane_i = mPbMap.vPlanes[planeID]; // sprintf (name, "plane_%02u", static_cast<unsigned>(planeID)); // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[planeID%10], grn[planeID%10], blu[planeID%10]); // viz.addPointCloud (plane_i.planePointCloudPtr, color, name);// contourPtr, planePointCloudPtr, polygonContourPtr // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, name); // } // Draw recognized plane labels if(mpPbMapLocaliser != NULL) for(map<string, pcl::PointXYZ>::iterator it = mpPbMapLocaliser->foundPlaces.begin(); it != mpPbMapLocaliser->foundPlaces.end(); it++) viz.addText3D (it->first, it->second, 0.3, 0.9, 0.9, 0.9, it->first); } } } }
void process () { std::cout << "threshold: " << threshold_ << std::endl; std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n"); unsigned char red [6] = {255, 0, 0, 255, 255, 0}; unsigned char grn [6] = { 0, 255, 0, 255, 0, 255}; unsigned char blu [6] = { 0, 0, 255, 0, 255, 255}; pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); refinement_compare->setDistanceThreshold (threshold_, depth_dependent_); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (5000); mps.setAngularThreshold (0.017453 * 3.0); //3 degrees mps.setDistanceThreshold (0.03); //2cm mps.setRefinementComparator (refinement_compare); std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>); typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>); char name[1024]; typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); double normal_start = pcl::getTime (); ne.setInputCloud (cloud); ne.compute (*normal_cloud); double normal_end = pcl::getTime (); std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl; double plane_extract_start = pcl::getTime (); mps.setInputNormals (normal_cloud); mps.setInputCloud (cloud); if (refine_) mps.segmentAndRefine (regions); else mps.segment (regions); double plane_extract_end = pcl::getTime (); std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl; std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl; typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>); viewer.removeAllPointClouds (0); viewer.removeAllShapes (0); pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0); viewer.addPointCloud<PointT> (cloud, single_color, "cloud"); pcl::PlanarPolygon<PointT> approx_polygon; //Draw Visualization for (size_t i = 0; i < regions.size (); i++) { Eigen::Vector3f centroid = regions[i].getCentroid (); Eigen::Vector4f model = regions[i].getCoefficients (); pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]); pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]), centroid[1] + (0.5f * model[1]), centroid[2] + (0.5f * model[2])); sprintf (name, "normal_%d", unsigned (i)); viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name)); contour->points = regions[i].getContour (); sprintf (name, "plane_%02d", int (i)); pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]); viewer.addPointCloud (contour, color, name); pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_); approx_contour->points = approx_polygon.getContour (); std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl; typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour; // sprintf (name, "approx_plane_%02d", int (i)); // viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name); for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx) { sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx)); viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name); } } }
int main (int argc, char** argv) { //ROS Initialization ros::init(argc, argv, "detecting_people"); ros::NodeHandle nh; ros::Rate rate(13); ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback); ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5); frmsg::people pub_people_; CloudConverter* cc_ = new CloudConverter(); while (!cc_->ready_xyzrgb_) { ros::spinOnce(); rate.sleep(); if (!ros::ok()) { printf("Terminated by Control-c.\n"); return -1; } } // Input parameter from the .yaml std::string package_path_ = ros::package::getPath("detecting_people") + "/"; cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ); // Algorithm parameters: std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml"; std::cout << svm_filename << std::endl; float min_confidence = -1.5; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; Eigen::Matrix3f rgb_intrinsics_matrix; rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics // Read if some parameters are passed from command line: pcl::console::parse_argument (argc, argv, "--svm", svm_filename); pcl::console::parse_argument (argc, argv, "--conf", min_confidence); pcl::console::parse_argument (argc, argv, "--min_h", min_height); pcl::console::parse_argument (argc, argv, "--max_h", max_height); // Read Kinect live stream: PointCloudT::Ptr cloud_people (new PointCloudT); cc_->ready_xyzrgb_ = false; while ( !cc_->ready_xyzrgb_ ) { ros::spinOnce(); rate.sleep(); } pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_; // Display pointcloud: pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args cb_args; PointCloudT::Ptr clicked_points_3d (new PointCloudT); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer); viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args); std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl; // Spin until 'Q' is pressed: viewer.spin(); std::cout << "done." << std::endl; //cloud_mutex.unlock (); // Ground plane estimation: Eigen::VectorXf ground_coeffs; ground_coeffs.resize(4); std::vector<int> clicked_points_indices; for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++) clicked_points_indices.push_back(i); pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d); model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs); std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl; // Initialize new viewer: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Create classifier for people detection: pcl::people::PersonClassifier<pcl::RGB> person_classifier; person_classifier.loadSVMFromFile(svm_filename); // load trained SVM // People detection app initialization: pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier people_detector.setHeightLimits(min_height, max_height); // set person classifier // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical // For timing: static unsigned count = 0; static double last = pcl::getTime (); int people_count = 0; histogram* first_hist; int max_people_num = (int)fs_->getFirstTopLevelNode()["max_people_num"]; // Main loop: while (!viewer.wasStopped() && ros::ok() ) { if ( current_state == 1 ) { if ( cc_->ready_xyzrgb_ ) // if a new cloud is available { // std::cout << "In state 1!!!!!!!!!!" << std::endl; std::vector<float> x; std::vector<float> y; std::vector<float> depth; cloud = cc_->msg_xyzrgb_; PointCloudT::Ptr cloud_new(new PointCloudT(*cloud)); cc_->ready_xyzrgb_ = false; // Perform people detection on the new cloud: std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters people_detector.setInputCloud(cloud_new); people_detector.setGround(ground_coeffs); // set floor coefficients people_detector.compute(clusters); // perform people detection ground_coeffs = people_detector.getGround(); // get updated floor coefficients // Draw cloud and people bounding boxes in the viewer: viewer.removeAllPointClouds(); viewer.removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); unsigned int k = 0; std::vector<pcl::people::PersonCluster<PointT> >::iterator it; std::vector<pcl::people::PersonCluster<PointT> >::iterator it_min; float min_z = 10.0; float histo_dist_min = 2.0; int id = -1; for(it = clusters.begin(); it != clusters.end(); ++it) { if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold { x.push_back((it->getTCenter())[0]); y.push_back((it->getTCenter())[1]); depth.push_back(it->getDistance()); // draw theoretical person bounding box in the PCL viewer: /*pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people); if ( people_count == 0 ) { first_hist = calc_histogram_a( cloud_people ); people_count++; it->drawTBoundingBox(viewer, k); } else if ( people_count <= 10 ) { histogram* hist_tmp = calc_histogram_a( cloud_people ); add_hist( first_hist, hist_tmp ); it->drawTBoundingBox(viewer, k); people_count++; }*/ pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people); if ( people_count < 11 ) { if ( it->getDistance() < min_z ) { it_min = it; min_z = it->getDistance(); } } else if ( people_count == 11 ) { normalize_histogram( first_hist ); people_count++; histogram* hist_tmp = calc_histogram( cloud_people ); float tmp = histo_dist_sq( first_hist, hist_tmp ); std::cout << "The histogram distance is " << tmp << std::endl; histo_dist_min = tmp; it_min = it; id = k; } else { histogram* hist_tmp = calc_histogram( cloud_people ); float tmp = histo_dist_sq( first_hist, hist_tmp ); std::cout << "The histogram distance is " << tmp << std::endl; if ( tmp < histo_dist_min ) { histo_dist_min = tmp; it_min = it; id = k; } } k++; //std::cout << "The data of the center is " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].x << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].y << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << std::endl; //std::cout << "The size of the people cloud is " << cloud_people->points.size() << std::endl; std::cout << "The " << k << " person's distance is " << it->getDistance() << std::endl; } } pub_people_.x = x; pub_people_.y = y; pub_people_.depth = depth; if ( k > 0 ) { if ( people_count <= 11 ) { pcl::copyPointCloud( *cloud, it_min->getIndices(), *cloud_people); if ( people_count == 0) { first_hist = calc_histogram_a( cloud_people ); people_count++; it_min->drawTBoundingBox(viewer, 1); } else if ( people_count < 11 ) { histogram* hist_tmp = calc_histogram_a( cloud_people ); add_hist( first_hist, hist_tmp ); it_min->drawTBoundingBox(viewer, 1); people_count++; } } else { pub_people_.id = k-1; if ( histo_dist_min < 1.3 ) { it_min->drawTBoundingBox(viewer, 1); std::cout << "The minimum distance of the histogram is " << histo_dist_min << std::endl; std::cout << "The vector is " << it_min->getTCenter() << std::endl << "while the elements are " << (it_min->getTCenter())[0] << " " << (it_min->getTCenter())[1] << std::endl; } else { pub_people_.id = -1; } } } else { pub_people_.id = -1; } pub_people_.header.stamp = ros::Time::now(); people_pub.publish(pub_people_); std::cout << k << " people found" << std::endl; viewer.spinOnce(); ros::spinOnce(); // Display average framerate: if (++count == 30) { double now = pcl::getTime (); std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl; count = 0; last = now; } //cloud_mutex.unlock (); } } else { viewer.spinOnce(); ros::spinOnce(); // std::cout << "In state 0!!!!!!!!!" << std::endl; } } return 0; }
void PbMapVisualizer::viz_cb (pcl::visualization::PCLVisualizer& viz) { if (pbmap.globalMapPtr->empty()) { mrpt::system::sleep(10); return; } // Render the data { viz.removeAllShapes(); viz.removeAllPointClouds(); char name[1024]; if(graphRepresentation) { // cout << "show graphRepresentation\n"; for(size_t i=0; i<pbmap.vPlanes.size(); i++) { pcl::PointXYZ center(2*pbmap.vPlanes[i].v3center[0], 2*pbmap.vPlanes[i].v3center[1], 2*pbmap.vPlanes[i].v3center[2]); double radius = 0.1 * sqrt(pbmap.vPlanes[i].areaVoxels); // cout << "radius " << radius << endl; sprintf (name, "sphere%u", static_cast<unsigned>(i)); viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name); if( !pbmap.vPlanes[i].label.empty() ) viz.addText3D (pbmap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], pbmap.vPlanes[i].label); else { sprintf (name, "P%u", static_cast<unsigned>(i)); viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name); } // Draw edges // for(set<unsigned>::iterator it = pbmap.vPlanes[i].nearbyPlanes.begin(); it != pbmap.vPlanes[i].nearbyPlanes.end(); it++) // { // if(*it > pbmap.vPlanes[i].id) // break; // // sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it)); // pcl::PointXYZ center_it(2*pbmap.vPlanes[*it].v3center[0], 2*pbmap.vPlanes[*it].v3center[1], 2*pbmap.vPlanes[*it].v3center[2]); // viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name); // } for(map<unsigned,unsigned>::iterator it = pbmap.vPlanes[i].neighborPlanes.begin(); it != pbmap.vPlanes[i].neighborPlanes.end(); it++) { if(it->first > pbmap.vPlanes[i].id) break; sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first)); pcl::PointXYZ center_it(2*pbmap.vPlanes[it->first].v3center[0], 2*pbmap.vPlanes[it->first].v3center[1], 2*pbmap.vPlanes[it->first].v3center[2]); viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name); sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first)); char commonObs[8]; sprintf (commonObs, "%u", it->second); pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 ); viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name); } } } else { // Regular representation if (!viz.updatePointCloud (pbmap.globalMapPtr, "cloud")) viz.addPointCloud (pbmap.globalMapPtr, "cloud"); sprintf (name, "PointCloud size %u", static_cast<unsigned>( pbmap.globalMapPtr->size() ) ); viz.addText(name, 10, 20); for(size_t i=0; i<pbmap.vPlanes.size(); i++) { Plane &plane_i = pbmap.vPlanes[i]; // sprintf (name, "normal_%u", static_cast<unsigned>(i)); name[0] = *(mrpt::format("normal_%u", static_cast<unsigned>(i)).c_str()); pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]); pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]), plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]), plane_i.v3center[2] + (0.5f * plane_i.v3normal[2])); viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name); if( !plane_i.label.empty() ) viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label); else { sprintf (name, "n%u", static_cast<unsigned>(i)); viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name); } sprintf (name, "approx_plane_%02d", int (i)); viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name); } } } }
void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map, bool show_votes, const std::string & filename) { pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud (*scene_vis, *scene); fdrf.setInputCloud (scene); if (heat_map) { pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>); fdrf.setFaceHeatMapCloud (intensity_cloud); } fdrf.detectFaces (); typedef typename pcl::traits::fieldList<PointInT>::type FieldListM; double rgb_m; bool exists_m; pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, double> (scene_vis->points[0], "rgb", exists_m, rgb_m)); std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl; if (exists_m) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::copyPointCloud (*scene_vis, *to_visualize); pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize); vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud"); } else { vis.addPointCloud (scene_vis, "scene_cloud"); } if (heat_map) { pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>); fdrf.getFaceHeatMap (intensity_cloud); pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity"); vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map"); } if (show_votes) { //display votes_ /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>()); fdrf.getVotes(votes_cloud); pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0); vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/ pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ()); fdrf.getVotes2 (votes_cloud); pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity"); vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud"); vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); } vis.addCoordinateSystem (0.1, "global"); std::vector<Eigen::VectorXd> heads; fdrf.getDetectedFaces (heads); face_detection_apps_utils::displayHeads (heads, vis); if (SHOW_GT) { //check if there is ground truth data std::string pose_file (filename); boost::replace_all (pose_file, ".pcd", "_pose.txt"); Eigen::Matrix4d pose_mat; pose_mat.setIdentity (4, 4); bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat); if (result) { Eigen::Vector3d ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2); Eigen::Vector3d trans_vector = Eigen::Vector3d (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3)); std::cout << ea << std::endl; std::cout << trans_vector << std::endl; pcl::PointXYZ center_point; center_point.x = trans_vector[0]; center_point.y = trans_vector[1]; center_point.z = trans_vector[2]; vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere"); pcl::ModelCoefficients cylinder_coeff; cylinder_coeff.values.resize (7); // We need 7 values cylinder_coeff.values[0] = center_point.x; cylinder_coeff.values[1] = center_point.y; cylinder_coeff.values[2] = center_point.z; cylinder_coeff.values[3] = ea[0]; cylinder_coeff.values[4] = ea[1]; cylinder_coeff.values[5] = ea[2]; Eigen::Vector3d vec = Eigen::Vector3d::UnitZ () * -1.; Eigen::Matrix3d matrixxx; matrixxx = Eigen::AngleAxisd (ea[0], Eigen::Vector3d::UnitX ()) * Eigen::AngleAxisd (ea[1], Eigen::Vector3d::UnitY ()) * Eigen::AngleAxisd (ea[2], Eigen::Vector3d::UnitZ ()); //matrixxx = pose_mat.block<3,3>(0,0); vec = matrixxx * vec; cylinder_coeff.values[3] = vec[0]; cylinder_coeff.values[4] = vec[1]; cylinder_coeff.values[5] = vec[2]; cylinder_coeff.values[6] = 0.01; vis.addCylinder (cylinder_coeff, "cylinder"); } } vis.setRepresentationToSurfaceForAllActors (); if (VIDEO) { vis.spinOnce (50, true); } else { vis.spin (); } vis.removeAllPointClouds (); vis.removeAllShapes (); }
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun) { const std::string &cloudname = this->name; if(firstRun) { visualizer.addPointCloud(cloud, cloudname); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); } else { visualizer.updatePointCloud(cloud, cloudname); visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); visualizer.removeAllShapes(); } visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X"); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y"); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z"); tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0); tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0); tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0); tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2); pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z()); pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z()); pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z()); pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z()); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line"); visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX"); visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY"); visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ"); for(int i = 0; i < regions.size(); ++i) { const Region ®ion = regions[i]; tf::Transform transform = worldToCam * region.transform; std::ostringstream oss; oss << "region_" << i; tf::Vector3 originB = transform * tf::Vector3(0, 0, 0); tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0); tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0); tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2); pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z()); pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z()); pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z()); pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z()); visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str()); visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str()); visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str()); visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str()); Eigen::Vector3d translation; Eigen::Quaterniond rotation; tf::vectorTFToEigen(transform.getOrigin(), translation); tf::quaternionTFToEigen(transform.getRotation(), rotation); visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str()); } }
int main(int argc, char** argv) { int total_frame = 0; ros::init(argc, argv, "pub_pcl"); ros::NodeHandle nh; ros::Publisher pub = nh.advertise<PointCloudT>("/openni/points2", 1); // Read PointCloudT::Ptr cloud(new PointCloudT); bool new_cloud_avaliable_flag = false; pcl::Grabber* interface = new pcl::OpenNIGrabber(); boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_avaliable_flag); interface->registerCallback(f); interface->start(); // Wait for the first frame while (!new_cloud_avaliable_flag) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } cloud_mutex.lock(); // for not overwriting the point cloud // Display printf("frame %d\n", ++total_frame); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud"); viewer.setCameraPosition(0, 0, -2, 0, -1, 0, 0); cloud_mutex.unlock(); ros::Rate loop_rate(4); while (nh.ok()) { ros::Time scan_time = ros::Time::now(); // Get & publish new cloud if (new_cloud_avaliable_flag && cloud_mutex.try_lock()) { new_cloud_avaliable_flag = false; pub.publish(cloud); printf("frame %d\n", ++total_frame); viewer.removeAllPointClouds(); viewer.removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud"); cloud_mutex.unlock(); } else { printf("no %d\n", total_frame); } ros::spinOnce(); loop_rate.sleep(); } return 0; }