Go to the documentation of this file.00001 #include "handle_detector/sampling_visualizer.h"
00002
00003 pcl::ModelCoefficients SamplingVisualizer::createCylinder(Eigen::Vector3d pt_on_axis, Eigen::Vector3d axis_direction,
00004 double radius, double extent)
00005 {
00006 pcl::ModelCoefficients cylinder_coeff;
00007 cylinder_coeff.values.resize(7);
00008 cylinder_coeff.values[0] = pt_on_axis.x();
00009 cylinder_coeff.values[1] = pt_on_axis.y();
00010 cylinder_coeff.values[2] = pt_on_axis.z();
00011 cylinder_coeff.values[3] = axis_direction.x() * extent;
00012 cylinder_coeff.values[4] = axis_direction.y() * extent;
00013 cylinder_coeff.values[5] = axis_direction.z() * extent;
00014 cylinder_coeff.values[6] = radius;
00015 return cylinder_coeff;
00016 }
00017
00018 void SamplingVisualizer::addCylinders(const std::vector<CylindricalShell> &shells, void* viewer_void,
00019 std::string handle_index, double r, double g, double b)
00020 {
00021 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<
00022 pcl::visualization::PCLVisualizer> *>(viewer_void);
00023
00024 for (std::size_t i = 0; i < shells.size(); i++)
00025 {
00026 std::string id = "cylinder" + handle_index + boost::lexical_cast < std::string > (i);
00027 viewer->addCylinder(
00028 createCylinder(shells[i].getCentroid(), shells[i].getCurvatureAxis(), shells[i].getRadius(),
00029 shells[i].getExtent()),
00030 id);
00031 viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, id);
00032 viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
00033 pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, id);
00034 viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.4, id);
00035 }
00036 }
00037
00038 void SamplingVisualizer::createViewer(PointCloud::ConstPtr cloud, std::vector<CylindricalShell> shells,
00039 Eigen::MatrixXd samples, double target_radius)
00040 {
00041 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
00042 viewer->setBackgroundColor(0, 0, 0);
00043 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(cloud);
00044 viewer->addPointCloud < pcl::PointXYZRGBA > (cloud, rgb, "sample cloud");
00045 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
00046
00047 addCylinders(shells, (void*)&viewer, "", 0, 1.0, 1.0);
00048
00049 for (std::size_t i = 0; i < samples.cols(); i++)
00050 {
00051 pcl::PointXYZ center;
00052 center.x = samples(0, i);
00053 center.y = samples(1, i);
00054 center.z = samples(2, i);
00055 std::string id = "sphere" + boost::lexical_cast < std::string > (i);
00056 viewer->addSphere(center, target_radius * 0.6, id);
00057 viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, id);
00058 }
00059
00060 this->viewer = viewer;
00061 }