void pcl::people::PersonCluster<PointT>::drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number)
{
// draw theoretical person bounding box in the PCL viewer:
pcl::ModelCoefficients coeffs;
// translation
coeffs.values.push_back (tcenter_[0]);
coeffs.values.push_back (tcenter_[1]);
coeffs.values.push_back (tcenter_[2]);
// rotation
coeffs.values.push_back (0.0);
coeffs.values.push_back (0.0);
coeffs.values.push_back (0.0);
coeffs.values.push_back (1.0);
// size
if (vertical_)
{
coeffs.values.push_back (height_);
coeffs.values.push_back (0.5);
coeffs.values.push_back (0.5);
}
else
{
coeffs.values.push_back (0.5);
coeffs.values.push_back (height_);
coeffs.values.push_back (0.5);
}
std::stringstream bbox_name;
bbox_name << "bbox_person_" << person_number;
viewer.removeShape (bbox_name.str());
viewer.addCube (coeffs, bbox_name.str());
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name.str());
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name.str());
// std::stringstream confid;
// confid << person_confidence_;
// PointT position;
// position.x = tcenter_[0]- 0.2;
// position.y = ttop_[1];
// position.z = tcenter_[2];
// viewer.addText3D(confid.str().substr(0, 4), position, 0.1);
}
void
tviewer::ArrowArrayObject::removeDataFromVisualizer (pcl::visualization::PCLVisualizer& v)
{
for (const auto& id : arrows_in_visualizer_)
v.removeShape (id);
arrows_in_visualizer_.clear ();
}
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.removeShape ("hull");
viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
}
new_cloud_ = false;
}
}
void updateVisualizer(pcl::visualization::PCLVisualizer& visualizer, PointCloudAggregator& aggregator)
{
boost::mutex::scoped_lock lock(m_);
std::string cube_id = name() + "_cube";
visualizer.removeShape(cube_id);
if(visibility_ != ShowCameraAndCloud)
{
//visualizer.removePointCloud(name());
aggregator.remove(name());
}
if(visibility_ != ShowNothing && cloud_)
{
if(visibility_ == ShowCameraAndCloud)
{
aggregator.add(name(), cloud_->build());
//if(!visualizer.updatePointCloud(cloud_->build(), name()))
//{
// visualizer.addPointCloud(cloud_->build(), name());
//}
//visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name());
}
//visualizer.addCube(cloud_->pose.translation().cast<float>(), Eigen::Quaternionf(cloud_->pose.rotation().cast<float>()), 0.05, 0.05, 0.05, cube_id);
//visualizer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color().r, color().g, color().b, cube_id);
//visualizer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cube_id);
}
}
void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
static unsigned count = 0;
std::stringstream ss;
ss << "Once per viewer loop: " << count++ << std::endl;
viewer.removeShape("text", 0);
viewer.addText(ss.str(), 200, 200, "text", 0);
user_data++;
}
void Inspector::disableModel()
{
use_intrinsics_ = false;
pcd_vis_.removeShape("text");
int x = 10;
int y = 10;
int fontsize = 16;
pcd_vis_.addText("raw", x, y, fontsize, 1, 0, 0, "text");
}
void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
static unsigned count = 0;
std::stringstream ss;
ss << "Once per viewer loop: " << count++;
viewer.removeShape ("text", 0);
viewer.addText (ss.str(), 200, 300, "text", 0);
//FIXME: possible race condition here:
user_data++;
}
/* \brief Helper function that draw info for the user on the viewer
*
*/
void showLegend(bool showCubes)
{
char dataDisplay[256];
sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS"));
viz.removeShape("disp_t");
viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t");
char level[256];
sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
viz.removeShape("level_t1");
viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1");
viz.removeShape("level_t2");
sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
displayCloud->points.size());
viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");
viz.removeShape("org_t");
if (showPointsWithCubes)
viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t");
}
void Inspector::enableModel()
{
use_intrinsics_ = true;
if(use_intrinsics_ && !dddm_) {
cout << "Cannot apply non-existent distortion model. Supply one at the command line." << endl;
use_intrinsics_ = false;
return;
}
pcd_vis_.removeShape("text");
int x = 10;
int y = 10;
int fontsize = 16;
pcd_vis_.addText("undistorted", x, y, fontsize, 0, 1, 0, "text");
}
void viewerUpdate(pcl::visualization::PCLVisualizer& viewer)
{
std::stringstream ss;
ss << "Rawlog entry: " << rawlogEntry;
viewer.removeShape ("text", 0);
viewer.addText (ss.str(), 10,50, "text", 0);
static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP;
{ // Mutex protected
std::lock_guard<std::mutex> lock(td_cs);
if (td.new_timestamp!=last_time)
{
last_time = td.new_timestamp;
viewer.removePointCloud("cloud", 0);
viewer.addPointCloud (td.new_cloud,"cloud",0);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0);
const size_t N = td.new_cloud->size();
std::cout << "Showing new point cloud of size=" << N << std::endl;
static bool first = true;
if (N && first)
{
first = false;
//viewer.resetCameraViewpoint("cloud");
}
#if 0
std::cout << mrpt::format(
"camera: clip = %f %f\n"
" focal = %f %f %f\n"
" pos = %f %f %f\n"
" view = %f %f %f\n",
viewer.camera_.clip[0],viewer.camera_.clip[1],
viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2],
viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2],
viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]);
#endif
}
}
}
void updateVisualizer(pcl::visualization::PCLVisualizer& visualizer)
{
if(pairs_.empty()) return;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for(list::iterator it = pairs_.begin(); it != pairs_.end(); ++it)
{
cloud->points.push_back(it->second);
}
for(list::reverse_iterator it = pairs_.rbegin(); it != pairs_.rend(); ++it)
{
cloud->points.push_back(it->second);
}
visualizer.removeShape(name());
visualizer.addPolygon<pcl::PointXYZ>(cloud, name());
visualizer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color().r, color().g, color().b, name());
visualizer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 4, name());
}
void
visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4);
for (std::size_t i = 0; i < curve_cloud->size () - 1; i++)
{
pcl::PointXYZRGB &p1 = curve_cloud->at (i);
pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1);
std::ostringstream os;
os << "line" << i;
viewer.removeShape (os.str ());
viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ());
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>);
for (int i = 0; i < curve.CVCount (); i++)
{
ON_3dPoint p1;
curve.GetCV (i, p1);
double pnt[3];
surface.Evaluate (p1.x, p1.y, 0, 3, pnt);
pcl::PointXYZRGB p2;
p2.x = float (pnt[0]);
p2.y = float (pnt[1]);
p2.z = float (pnt[2]);
p2.r = 255;
p2.g = 0;
p2.b = 0;
curve_cps->push_back (p2);
}
viewer.removePointCloud ("cloud_cps");
viewer.addPointCloud (curve_cps, "cloud_cps");
}
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;
}
void drawArrow(pcl::visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor (0.0, 0.0, 0.0);
viewer.removeShape("line", 0);
viewer.addArrow(o1, o2, 1.0, 0.0, 0.0, "line", 0);
}
