void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
mtx_.lock ();
if (!cloud_ || !normals_)
{
mtx_.unlock ();
return;
}
CloudConstPtr temp_cloud;
pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
temp_normals.swap (normals_);
mtx_.unlock ();
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
new_cloud_ = false;
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_)
{
boost::this_thread::sleep(boost::posix_time::seconds(1));
return;
}
CloudConstPtr temp_cloud;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && normals_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, normals_, 200, 0.05, "normalcloud");
new_cloud_ = false;
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (!cloud_ || !new_cloud_)
{
boost::this_thread::sleep (boost::posix_time::milliseconds (1));
return;
}
{
boost::mutex::scoped_lock lock (mtx_);
FPS_CALC ("visualization");
CloudPtr temp_cloud;
temp_cloud.swap (cloud_pass_);
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && cloud_hull_)
{
viz.removePointCloud ("hull");
viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
}
new_cloud_ = false;
}
}
//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
std::this_thread::sleep_for(1s);
return;
}
//Draw downsampled point cloud from sensor
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
cloud_pass = cloud_pass_downsampled_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
bool ret = drawParticles (viz);
if (ret)
drawResult (viz);
}
new_cloud_ = false;
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!keypoints_ && !cloud_)
{
boost::this_thread::sleep(boost::posix_time::seconds(1));
return;
}
FPS_CALC ("visualization");
viz.removePointCloud ("raw");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud_);
viz.addPointCloud<pcl::PointXYZRGBA> (cloud_, color_handler, "raw");
if (!viz.updatePointCloud<pcl::PointXYZ> (keypoints_, "keypoints"))
{
viz.addPointCloud<pcl::PointXYZ> (keypoints_, "keypoints");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints");
viz.resetCameraViewpoint ("keypoints");
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
return;
}
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
if (!visualize_non_downsample_)
cloud_pass = cloud_pass_downsampled_;
else
cloud_pass = cloud_pass_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
}
if (new_cloud_ && reference_)
{
bool ret = drawParticles (viz);
if (ret)
{
drawResult (viz);
// draw some texts
viz.removeShape ("N");
viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
10, 20, 20, 1.0, 1.0, 1.0, "N");
viz.removeShape ("M");
viz.addText ((boost::format ("number of Measured PointClouds: %d") % cloud_pass_downsampled_->points.size ()).str (),
10, 40, 20, 1.0, 1.0, 1.0, "M");
viz.removeShape ("tracking");
viz.addText ((boost::format ("tracking: %f fps") % (1.0 / tracking_time_)).str (),
10, 60, 20, 1.0, 1.0, 1.0, "tracking");
viz.removeShape ("downsampling");
viz.addText ((boost::format ("downsampling: %f fps") % (1.0 / downsampling_time_)).str (),
10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
viz.removeShape ("computation");
viz.addText ((boost::format ("computation: %f fps") % (1.0 / computation_time_)).str (),
10, 100, 20, 1.0, 1.0, 1.0, "computation");
viz.removeShape ("particles");
viz.addText ((boost::format ("particles: %d") % tracker_->getParticles ()->points.size ()).str (),
10, 120, 20, 1.0, 1.0, 1.0, "particles");
}
}
new_cloud_ = false;
}
