void initViewer(pcl::visualization::PCLVisualizer &viewer) { viewer.setBackgroundColor(0, 0, 0); viewer.addCoordinateSystem(1.0, "reference"); viewer.initCameraParameters(); viewer.setRepresentationToPointsForAllActors(); viewer.setCameraPosition(0, 0, -1, 0, 0, 0, 0, -1, 0); viewer.registerKeyboardCallback(keyboardCallback); }
void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) { Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(10, 10, 10); Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 0); Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0); viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2], look_at_vector[0], look_at_vector[1], look_at_vector[2], up_vector[0], up_vector[1], up_vector[2]); }
int main (int argc, char** argv) { //pcl::PointCloud<pcl::PointXYZ> cloud; // Read Kinect live stream: PointCloudT cloud_obj; PointCloudT::Ptr cloud (new PointCloudT); bool new_cloud_available_flag = false; pcl::Grabber* interface = new pcl::OpenNIGrabber(); boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind (&cloud_cb_, _1, &cloud_obj, &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)); pcl::copyPointCloud<PointT, PointT>(cloud_obj, *cloud); new_cloud_available_flag = false; // 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 (to allow ground manual initialization): viewer.spin(); std::cout << "done." << std::endl; pcl::io::savePCDFileASCII ("/home/igor/pcds/pcd_grabber_out_1.pcd", *cloud); std::cerr << "Saved " << (*cloud).points.size () << " data points to pcd_grabber_out_1.pcd." << std::endl; /* for (size_t i = 0; i < cloud.points.size (); ++i) std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl; */ while (!viewer.wasStopped ()) { boost::this_thread::sleep (boost::posix_time::microseconds (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; }
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) { 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; }