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 viewer->addPointCloud < pcl::PointXYZ > (cloud, "sample cloud"); 00044 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 00045 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.4, "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 } 00062 00063 void SamplingVisualizer::createViewerRGB(PointCloudRGB::ConstPtr cloud, std::vector<CylindricalShell> shells, 00064 Eigen::MatrixXd samples, double target_radius) 00065 { 00066 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 00067 viewer->setBackgroundColor(0, 0, 0); 00068 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud); 00069 viewer->addPointCloud < pcl::PointXYZRGB > (cloud, rgb, "sample cloud"); 00070 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); 00071 00072 addCylinders(shells, (void*)&viewer, "", 0, 1.0, 1.0); 00073 00074 for (std::size_t i = 0; i < samples.cols(); i++) 00075 { 00076 pcl::PointXYZ center; 00077 center.x = samples(0, i); 00078 center.y = samples(1, i); 00079 center.z = samples(2, i); 00080 std::string id = "sphere" + boost::lexical_cast < std::string > (i); 00081 viewer->addSphere(center, target_radius * 0.4, id); 00082 viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, id); 00083 } 00084 00085 this->viewer = viewer; 00086 } 00087