int main (int argc, char const* argv[]) { if (argc != 2) { cout << "Usage : obb_test filename.pcd" << endl; return 1; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud) == -1) { PCL_ERROR ("Couldn't read file test_pcd.pcd \n"); return 1; } cloud_viewer.addPointCloud (cloud, "single_cloud"); OrientedBoundingBox obb; Eigen::Quaternionf q; Eigen::Vector3f t, dims; obb.compute_obb_pca (cloud, q, t, dims); cloud_viewer.addCube(t, q, dims.x(), dims.y(), dims.z()); cout << dims.x() << " " << dims.y() << " " << dims.z() << endl; while (!cloud_viewer.wasStopped()) { cloud_viewer.spinOnce(1); } return 0; }
void Inspector::updateDepth(const openni_wrapper::Image& image, const openni_wrapper::DepthImage& depth) { frame_.depth_->setZero(); ushort data[depth.getHeight() * depth.getWidth()]; depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data); int i = 0; for(size_t y = 0; y < depth.getHeight(); ++y) { for(size_t x = 0; x < depth.getWidth(); ++x, ++i) { if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue()) continue; frame_.depth_->coeffRef(y, x) = data[i]; } } if(dddm_ && use_intrinsics_) dddm_->undistort(frame_.depth_.get()); frame_.img_ = oniToCV(image); Cloud::Ptr cloud(new Cloud); FrameProjector proj; proj.width_ = 640; proj.height_ = 480; proj.cx_ = proj.width_ / 2; proj.cy_ = proj.height_ / 2; proj.fx_ = 525; proj.fy_ = 525; proj.frameToCloud(frame_, cloud.get()); pcd_vis_.updatePointCloud(cloud, "cloud"); pcd_vis_.spinOnce(5); }
void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame) { _viewer.removeAllPointClouds(0); _viewer.addPointCloud(mapFrame.makeShared(), "map"); _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map"); _viewer.spinOnce(); }
/* \brief Graphic loop for the viewer * */ void run() { while (!viz.wasStopped()) { //main loop of the visualizer viz.spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } }
void run () { // initial processing process (); while (!viewer.wasStopped ()) viewer.spinOnce (100); }
int main(int argc, char* argv[]) { if(argc < 3) { PCL_ERROR("run as ./project /path/to/cloud1/ /path/to/cloud2/ [pcd format]"); return -1; } string fCloud1 = ""; string fCloud2 = ""; string models = ""; string training = ""; int NN; pcl::console::parse_argument(argc, argv, "-cloud1", fCloud1); pcl::console::parse_argument(argc, argv, "-cloud2", fCloud2); pcl::console::parse_argument(argc, argv, "-models", models); pcl::console::parse_argument(argc, argv, "-training", training); pcl::console::parse_argument(argc, argv, "-nn", NN); moi.setVerbose(true); classificator.setModelsDir(models); classificator.setTrainingDir(training); classificator.setNN(NN); classificator.setup(); viewer.registerKeyboardCallback(&keyboard_cb, NULL); viewer.setBackgroundColor(0, 0, 0); viewer.initCameraParameters(); if(fCloud1 != "" && fCloud2 != "") { if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud1, *cloud1) == -1) { PCL_ERROR("Cloud1 reading failed\n"); return(-1); } if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud2, *cloud2) == -1) { PCL_ERROR("Cloud2 reading failed\n"); return(-1); } findObjectsAndClassify(); } else { keyboardCbLock = false; printInstructions(); } while(!viewer.wasStopped()) { viewer.spinOnce(100); boost::this_thread::sleep (boost::posix_time::milliseconds(100)); } return 0; }
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 compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float th_dd, int max_search) { CloudPtr cloud (new Cloud); fromROSMsg (*input, *cloud); pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>); pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setNormalSmoothingSize (10.0f); ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR); ne.setInputCloud (cloud); ne.compute (*normal); TicToc tt; tt.tic (); //OrganizedEdgeBase<PointXYZRGBA, Label> oed; //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed; //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed; OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed; oed.setInputNormals (normal); oed.setInputCloud (cloud); oed.setDepthDisconThreshold (th_dd); oed.setMaxSearchNeighbors (max_search); oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY); PointCloud<Label> labels; std::vector<PointIndices> label_indices; oed.compute (labels, label_indices); print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n"); // Make gray point clouds for (int idx = 0; idx < (int)cloud->points.size (); idx++) { uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3; cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray; } // Display edges in PCLVisualizer viewer.setSize (640, 480); viewer.addCoordinateSystem (0.2f); viewer.addPointCloud (cloud, "original point cloud"); viewer.registerKeyboardCallback(&keyboard_callback); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges); pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges); pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges); pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges); pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges); const int point_size = 2; viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges"); while (!viewer.wasStopped ()) { viewer.spinOnce (); pcl_sleep(0.1); } // Combine point clouds and edge labels sensor_msgs::PointCloud2 output_edges; toROSMsg (labels, output_edges); concatenateFields (*input, output_edges, output); }
int main (int argc, char** argv) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile (argv[1], *cloud); pcl::copyPointCloud( *cloud,*cloud_filtered); float i ; float j; float k; cv::namedWindow( "picture"); cvCreateTrackbar("X_limit", "picture", &a, 30, NULL); cvCreateTrackbar("Y_limit", "picture", &b, 30, NULL); cvCreateTrackbar("Z_limit", "picture", &c, 30, NULL); char last_c = 0; // pcl::visualization::PCLVisualizer viewer ("picture"); viewer.setBackgroundColor (0.0, 0.0, 0.5);//set backgroung according to the color of points pcl::PassThrough<pcl::PointXYZ> pass; while (!viewer.wasStopped ()) { pcl::copyPointCloud(*cloud_filtered, *cloud); i = 0.1*((float)a); j = 0.1*((float)b); k = 0.1*((float)c); // cout << "i = " << i << " j = " << j << " k = " << k << endl; pass.setInputCloud (cloud); pass.setFilterFieldName ("y"); pass.setFilterLimits (-j, j); pass.filter (*cloud); pass.setInputCloud (cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (-i, i); pass.filter (*cloud); pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (-k,k); pass.filter (*cloud); viewer.addPointCloud (cloud, "scene_cloud"); viewer.spinOnce (); viewer.removePointCloud("scene_cloud"); waitKey(10); } return 0; }
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; }
int main(int argc,char** argv){ if (argc < 2){ std::cout<<"Please enter <input.pcd> file"<<std::endl; return 0; } if (pcl::io::loadPCDFile (argv[1], *model) < 0) { std::cout << "Error loading model cloud." << std::endl; return (-1); } model_name = argv[1]; model_name = model_name.substr(0,model_name.size()-4); if(pcl::console::find_switch(argc,argv,"-vfh")){ vfh = true; } else if(pcl::console::find_switch(argc,argv,"-rv")){ std::cout<<"performing Resultant vector feature calculation"<<std::endl; rv = true; }else{ std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl; vfh = true; } if (pcl::console::find_switch (argc, argv, "-ds")) { _downsample = true; std::cout<<"performing downsampling on the input file"<<std::endl; } if (pcl::console::find_switch (argc, argv, "-sn")) { show_normals = true; std::cout<<"showing calclated normals"<<std::endl; } if(_downsample){ rec.setInputCloud(model,_leaf_size); std::cout<<"saving downsampled file to model_down.pcd"<<std::endl; pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud())); } else{ rec.setInputCloud(model); std::cout<<"setting input model without further downsampling"<<std::endl; } if(pcl::console::find_switch(argc,argv,"--showaxis")){ _show_axis = true; } if(vfh){ std::cout<<"estimating VFH features"<<std::endl; rec.Estimate_VFH_Features(); } else if(rv){ std::cout<<"estimating features using the resultant vector"<<std::endl; rec.Estimate_RV_Features(); PointNormalCloudT::Ptr cloud (new PointNormalCloudT); pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>); cloud = rec.getPointNormalCloud(); norm_cloud = rec.getNormalCloud(); pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*cloud,*plaincloud); //pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z); pcl::PointXYZ mass_center(0,0,0); pcl::PointXYZ kinectZ(0,0,-1); pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z); //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud); //viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0)); viewer.addPointCloud(cloud,single_color,"sample cloud"); if(_show_axis){ viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector"); viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ"); } std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl; if(show_normals){ std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl; viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud"); } while(!viewer.wasStopped()) viewer.spinOnce(); } std::cout<<"feature calculation complete"<<std::endl; ofstream myfile; if (vfh){ std::stringstream ss; ss<<"vfh_"<<model_name<<".txt"; myfile.open(ss.str().c_str()); for(size_t k =0 ;k<308;k++){ if(k != 307) myfile << rec._vfh_features->points[0].histogram[k]<<","; else myfile << rec._vfh_features->points[0].histogram[k]; } std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl; myfile.close(); }else if(rv){ std::stringstream ss; ss<<"rv_"<<model_name<<".txt"; myfile.open(ss.str().c_str()); for(int k = 0;k<181;k++){ if(k != 180) myfile << rec._rv_features_181[k]<<","; else myfile << rec._rv_features_181[k]; } std::cout<<"wrote the histogram to file: "<< ss.str()<<std::endl; //myfile.open(ss.c_str()); } }
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 (); }
int main (int argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); reader.read ("../bottle.pcd", *cloud); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* parseCommandLine(argc, argv); std::cout << "argc:" << argc << " argv:" << *argv << std::endl; std::cout << "x_min:" << x_pass_min_ << " x_max:" << x_pass_max_ << " y_min:" << y_pass_min_ << " y_max:"<<y_pass_max_<<std::endl; /*apply pass through filter*/ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); // Create the filtering object pcl::PassThrough<pcl::PointXYZ> pass; //pass.setFilterLimitsNegative (true); pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (z_pass_min_, z_pass_max_); pass.filter (*cloud_filtered); pass.setInputCloud (cloud_filtered); pass.setFilterFieldName ("y"); pass.setFilterLimits (y_pass_min_, y_pass_max_); pass.filter (*cloud_filtered); pass.setInputCloud (cloud_filtered); pass.setFilterFieldName ("x"); pass.setFilterLimits (x_pass_min_, x_pass_max_); pass.filter (*cloud_filtered); viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud"); #if 0 // while (!viewer.wasStopped ()) { // viewer.spinOnce (); } return(0); #endif viewer.removeAllPointClouds(); cloud = cloud_filtered; #if 0 #if 0 // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* #endif // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud"); while (!viewer.wasStopped ()) { viewer.spinOnce (); } #endif #if 1 pcl::PCDWriter writer; // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; cout << "ss:" << j << std::endl; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; cout << "ss:" << j << std::endl; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++; } return (0); #endif }