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