/* \brief Visual update. Create visualizations and add them to the viewer
*
*/
void update()
{
//remove existing shapes from visualizer
clearView();
//prevent the display of too many cubes
bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
showLegend(displayCubeLegend);
if (displayCubeLegend)
{
//show octree as cubes
showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
if (showPointsWithCubes)
{
//add original cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
viz.addPointCloud(cloud, color_handler, "cloud");
}
}
else
{
//add current cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
viz.addPointCloud(displayCloud, color_handler, "cloud");
}
}
/**
* Main (what else?)
*/
int main()
{
init();
while(1)
{
color_handler();
}
}
void
pcl::modeler::CloudMesh::getColorScalarsFromField(vtkSmartPointer<vtkDataArray> &scalars, const std::string& field) const
{
if (field == "rgb" || field == "rgba")
{
pcl::visualization::PointCloudColorHandlerRGBField<PointT> color_handler(cloud_);
color_handler.getColor(scalars);
return;
}
if (field == "random")
{
pcl::visualization::PointCloudColorHandlerRandom<PointT> color_handler(cloud_);
color_handler.getColor(scalars);
return;
}
pcl::visualization::PointCloudColorHandlerGenericField<PointT> color_handler(cloud_, field);
color_handler.getColor(scalars);
return;
}
template <typename PointT> void
FrameCloudView<PointT>::show ()
{
if (cloud_ && cld_mutex_.try_lock())
{
pcl::visualization::PointCloudColorHandlerRGBField<PointT> color_handler (cloud_);
if (!visualizer_.updatePointCloud<PointT> (cloud_, color_handler, cloud_id_))
{
visualizer_.addPointCloud<PointT> (cloud_, color_handler, cloud_id_);
visualizer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloud_id_);
}
cld_mutex_.unlock();
}
}
template <typename PointT> bool
PCLVisualizer::addPointCloud (
const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
const PointCloudGeometryHandler<PointT> &geometry_handler,
const std::string &id, int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
if (am_it != cloud_actor_map_->end ())
{
PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
//PointCloudColorHandlerRandom<PointT> color_handler (cloud);
PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
}
template <typename PointT> bool
PCLVisualizer::addPointCloud (
const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
const GeometryHandlerConstPtr &geometry_handler,
const std::string &id, int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
if (am_it != cloud_actor_map_->end ())
{
// Here we're just pushing the handlers onto the queue. If needed, something fancier could
// be done such as checking if a specific handler already exists, etc.
am_it->second.geometry_handlers.push_back (geometry_handler);
return (true);
}
//PointCloudColorHandlerRandom<PointT> color_handler (cloud);
PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
}
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");
}
}
guint32
gnc_table_get_color (Table *table, VirtualLocation virt_loc,
gboolean *hatching)
{
TableGetCellColorHandler color_handler;
const char *handler_name;
if (hatching)
*hatching = FALSE;
if (!table || !table->model)
return COLOR_UNDEFINED;
handler_name = gnc_table_get_cell_name (table, virt_loc);
color_handler = gnc_table_model_get_cell_color_handler (table->model,
handler_name);
if (!color_handler)
return COLOR_UNDEFINED;
return color_handler (virt_loc, hatching,
table->model->handler_user_data);
}
void Run()
{
int win_w = 800;
int win_h = 600;
o3d3xx::FrameGrabber::Ptr fg =
std::make_shared<o3d3xx::FrameGrabber>(this->cam_);
o3d3xx::ImageBuffer::Ptr buff =
std::make_shared<o3d3xx::ImageBuffer>();
//
// Setup for point cloud
//
std::shared_ptr<pcl::visualization::PCLVisualizer> pclvis_ =
std::make_shared<pcl::visualization::PCLVisualizer>(this->description_);
pclvis_->setBackgroundColor(0, 0, 0);
pclvis_->setSize(win_w, win_h);
pclvis_->setCameraPosition(-3.0, // x-position
0, // y-position
0, // z-position
0, // x-axis "up" (0 = false)
0, // y-axis "up" (0 = false)
1); // z-axis "up" (1 = true)
// use "A" and "a" to toggle axes indicators
pclvis_->registerKeyboardCallback(
[&](const pcl::visualization::KeyboardEvent& ev)
{
if (ev.getKeySym() == "A" && ev.keyDown())
{
pclvis_->addCoordinateSystem();
}
else if (ev.getKeySym() == "a" && ev.keyDown())
{
pclvis_->removeCoordinateSystem();
}
});
//
// Setup for amplitude, depth, and confidence images
//
// used for all 2d images
cv::namedWindow(this->description_, cv::WINDOW_NORMAL|cv::WINDOW_OPENGL);
cv::resizeWindow(this->description_, win_w, win_h);
int retval;
double min, max;
cv::Rect roi;
// depth
cv::Mat display_img;
cv::Mat depth_colormap_img;
// confidence
cv::Mat conf_img;
cv::Mat conf_colormap_img;
// amplitude
cv::Mat amp_colormap_img;
cv::Mat raw_amp_colormap_img;
bool is_first = true;
while (! pclvis_->wasStopped())
{
pclvis_->spinOnce(100);
if (! fg->WaitForFrame(buff.get(), 500))
{
continue;
}
//------------
// Point cloud
//------------
pcl::visualization::PointCloudColorHandlerGenericField<o3d3xx::PointT>
color_handler(buff->Cloud(), "intensity");
if (is_first)
{
is_first = false;
pclvis_->addPointCloud(buff->Cloud(), color_handler, "cloud");
}
else
{
pclvis_->updatePointCloud(buff->Cloud(), color_handler, "cloud");
}
//------------
// 2D images
//------------
// depth image
cv::minMaxIdx(buff->DepthImage(), &min, &max);
cv::convertScaleAbs(buff->DepthImage(),
depth_colormap_img, 255 / max);
cv::applyColorMap(depth_colormap_img, depth_colormap_img,
cv::COLORMAP_JET);
// confidence image: show as binary image of good pixel vs. bad pixel.
conf_img = buff->ConfidenceImage();
conf_colormap_img = cv::Mat::ones(conf_img.rows,
conf_img.cols,
CV_8UC1);
cv::bitwise_and(conf_img, conf_colormap_img,
conf_colormap_img);
cv::convertScaleAbs(conf_colormap_img,
conf_colormap_img, 255);
cv::applyColorMap(conf_colormap_img, conf_colormap_img,
cv::COLORMAP_SUMMER);
// amplitude
cv::minMaxIdx(buff->AmplitudeImage(), &min, &max);
cv::convertScaleAbs(buff->AmplitudeImage(),
amp_colormap_img, 255 / max);
cv::applyColorMap(amp_colormap_img, amp_colormap_img,
cv::COLORMAP_BONE);
cv::minMaxIdx(buff->RawAmplitudeImage(), &min, &max);
cv::convertScaleAbs(buff->RawAmplitudeImage(),
raw_amp_colormap_img, 255 / max);
cv::applyColorMap(raw_amp_colormap_img, raw_amp_colormap_img,
cv::COLORMAP_BONE);
// stich 2d images together and display
display_img.create(depth_colormap_img.rows*2,
depth_colormap_img.cols*2,
depth_colormap_img.type());
roi = cv::Rect(0, 0,
depth_colormap_img.cols,
depth_colormap_img.rows);
depth_colormap_img.copyTo(display_img(roi));
roi = cv::Rect(depth_colormap_img.cols, 0,
conf_colormap_img.cols,
conf_colormap_img.rows);
conf_colormap_img.copyTo(display_img(roi));
roi = cv::Rect(0, depth_colormap_img.rows,
amp_colormap_img.cols,
amp_colormap_img.rows);
amp_colormap_img.copyTo(display_img(roi));
roi = cv::Rect(depth_colormap_img.cols,
depth_colormap_img.rows,
raw_amp_colormap_img.cols,
raw_amp_colormap_img.rows);
raw_amp_colormap_img.copyTo(display_img(roi));
cv::imshow(this->description_, display_img);
// `ESC', `q', or `Q' to exit
retval = cv::waitKey(33);
if ((retval == 27) || (retval == 113) || (retval == 81))
{
break;
}
} // end: while (...)
}
