00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #include <pcl/console/parse.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl/visualization/pcl_visualizer.h>
00051 #include <pcl/io/openni_grabber.h>
00052 #include <pcl/sample_consensus/sac_model_plane.h>
00053 #include <pcl/people/ground_based_people_detection_app.h>
00054
00055 typedef pcl::PointXYZRGBA PointT;
00056 typedef pcl::PointCloud<PointT> PointCloudT;
00057
00058
00059 pcl::visualization::PCLVisualizer viewer("PCL Viewer");
00060
00061
00062 boost::mutex cloud_mutex;
00063
00064 enum { COLS = 640, ROWS = 480 };
00065
00066 int print_help()
00067 {
00068 cout << "*******************************************************" << std::endl;
00069 cout << "Ground based people detection app options:" << std::endl;
00070 cout << " --help <show_this_help>" << std::endl;
00071 cout << " --svm <path_to_svm_file>" << std::endl;
00072 cout << " --conf <minimum_HOG_confidence (default = -1.5)>" << std::endl;
00073 cout << " --min_h <minimum_person_height (default = 1.3)>" << std::endl;
00074 cout << " --max_h <maximum_person_height (default = 2.3)>" << std::endl;
00075 cout << "*******************************************************" << std::endl;
00076 return 0;
00077 }
00078
00079 void cloud_cb_ (const PointCloudT::ConstPtr &callback_cloud, PointCloudT::Ptr& cloud,
00080 bool* new_cloud_available_flag)
00081 {
00082 cloud_mutex.lock ();
00083 *cloud = *callback_cloud;
00084 *new_cloud_available_flag = true;
00085 cloud_mutex.unlock ();
00086 }
00087
00088 struct callback_args{
00089
00090 PointCloudT::Ptr clicked_points_3d;
00091 pcl::visualization::PCLVisualizer::Ptr viewerPtr;
00092 };
00093
00094 void
00095 pp_callback (const pcl::visualization::PointPickingEvent& event, void* args)
00096 {
00097 struct callback_args* data = (struct callback_args *)args;
00098 if (event.getPointIndex () == -1)
00099 return;
00100 PointT current_point;
00101 event.getPoint(current_point.x, current_point.y, current_point.z);
00102 data->clicked_points_3d->points.push_back(current_point);
00103
00104 pcl::visualization::PointCloudColorHandlerCustom<PointT> red (data->clicked_points_3d, 255, 0, 0);
00105 data->viewerPtr->removePointCloud("clicked_points");
00106 data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
00107 data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
00108 std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
00109 }
00110
00111 int main (int argc, char** argv)
00112 {
00113 if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
00114 return print_help();
00115
00116
00117 std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
00118 float min_confidence = -1.5;
00119 float min_height = 1.3;
00120 float max_height = 2.3;
00121 float voxel_size = 0.06;
00122 Eigen::Matrix3f rgb_intrinsics_matrix;
00123 rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0;
00124
00125
00126 pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
00127 pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
00128 pcl::console::parse_argument (argc, argv, "--min_h", min_height);
00129 pcl::console::parse_argument (argc, argv, "--max_h", max_height);
00130
00131
00132 PointCloudT::Ptr cloud (new PointCloudT);
00133 bool new_cloud_available_flag = false;
00134 pcl::Grabber* interface = new pcl::OpenNIGrabber();
00135 boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
00136 boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);
00137 interface->registerCallback (f);
00138 interface->start ();
00139
00140
00141 while(!new_cloud_available_flag)
00142 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00143 new_cloud_available_flag = false;
00144
00145 cloud_mutex.lock ();
00146
00147
00148 pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
00149 viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
00150 viewer.setCameraPosition(0,0,-2,0,-1,0,0);
00151
00152
00153 struct callback_args cb_args;
00154 PointCloudT::Ptr clicked_points_3d (new PointCloudT);
00155 cb_args.clicked_points_3d = clicked_points_3d;
00156 cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
00157 viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
00158 std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
00159
00160
00161 viewer.spin();
00162 std::cout << "done." << std::endl;
00163
00164 cloud_mutex.unlock ();
00165
00166
00167 Eigen::VectorXf ground_coeffs;
00168 ground_coeffs.resize(4);
00169 std::vector<int> clicked_points_indices;
00170 for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
00171 clicked_points_indices.push_back(i);
00172 pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
00173 model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
00174 std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
00175
00176
00177 pcl::visualization::PCLVisualizer viewer("PCL Viewer");
00178 viewer.setCameraPosition(0,0,-2,0,-1,0,0);
00179
00180
00181 pcl::people::PersonClassifier<pcl::RGB> person_classifier;
00182 person_classifier.loadSVMFromFile(svm_filename);
00183
00184
00185 pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;
00186 people_detector.setVoxelSize(voxel_size);
00187 people_detector.setIntrinsics(rgb_intrinsics_matrix);
00188 people_detector.setClassifier(person_classifier);
00189 people_detector.setHeightLimits(min_height, max_height);
00190
00191
00192
00193 static unsigned count = 0;
00194 static double last = pcl::getTime ();
00195
00196
00197 while (!viewer.wasStopped())
00198 {
00199 if (new_cloud_available_flag && cloud_mutex.try_lock ())
00200 {
00201 new_cloud_available_flag = false;
00202
00203
00204 std::vector<pcl::people::PersonCluster<PointT> > clusters;
00205 people_detector.setInputCloud(cloud);
00206 people_detector.setGround(ground_coeffs);
00207 people_detector.compute(clusters);
00208
00209 ground_coeffs = people_detector.getGround();
00210
00211
00212 viewer.removeAllPointClouds();
00213 viewer.removeAllShapes();
00214 pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
00215 viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
00216 unsigned int k = 0;
00217 for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
00218 {
00219 if(it->getPersonConfidence() > min_confidence)
00220 {
00221
00222 it->drawTBoundingBox(viewer, k);
00223 k++;
00224 }
00225 }
00226 std::cout << k << " people found" << std::endl;
00227 viewer.spinOnce();
00228
00229
00230 if (++count == 30)
00231 {
00232 double now = pcl::getTime ();
00233 std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
00234 count = 0;
00235 last = now;
00236 }
00237 cloud_mutex.unlock ();
00238 }
00239 }
00240
00241 return 0;
00242 }
00243