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
}