1 #include "pcl/pcl_base.h" 2 #include "pcl/point_types.h" 3 #include "jsk_pcl_ros/color_converter.h" 5 #include "pcl/kdtree/tree_types.h" 6 #include "pcl/kdtree/kdtree.h" 7 #include "pcl/kdtree/kdtree_flann.h" 8 #include "pcl/kdtree/impl/kdtree_flann.hpp" 10 #include <boost/smart_ptr/make_shared.hpp> 12 int main (
int argc,
char** argv) {
13 pcl::PointCloud<pcl::PointXYZRGB> p_rgb, p_rgb2;
14 pcl::PointCloud<pcl::PointXYZHSV> p_hsv;
16 for(
int r=0;
r < 10;
r++) {
17 for(
int g=0;
g < 10;
g++) {
18 for(
int b=0;
b < 10;
b++) {
27 pt.rgb = v.float_value;
28 p_rgb.points.push_back(pt);
33 p_rgb.is_dense =
true;
37 pcl::RGB2HSVConverter<pcl::PointXYZRGB, pcl::PointXYZHSV> rgb2hsv;
38 pcl::HSV2RGBConverter<pcl::PointXYZHSV, pcl::PointXYZRGB> hsv2rgb;
40 pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree
41 = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZRGB> > ();
43 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr tree2
44 = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZHSV> > ();
46 rgb2hsv.setSearchMethod (tree);
47 rgb2hsv.setKSearch (50);
49 hsv2rgb.setSearchMethod (tree2);
50 hsv2rgb.setKSearch (50);
52 rgb2hsv.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> > (p_rgb));
53 rgb2hsv.compute(p_hsv);
55 hsv2rgb.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZHSV> > (p_hsv));
56 hsv2rgb.compute(p_rgb2);
58 for(
int i = 0 ; i < 1000; i++ )
60 pcl::PointXYZRGB rgb_p= p_rgb.points[i];
61 pcl::PointXYZRGB rgb2_p= p_rgb2.points[i];
65 v.float_value = rgb_p.rgb;
66 v2.float_value = rgb2_p.rgb;
68 std::cout <<
"[" << (int)v.Red <<
", " << (
int)v.Green <<
", " << (int)v.Blue <<
"] -> " 69 <<
"[" << hsv_p.hue <<
", " << hsv_p.saturation <<
", " 70 << hsv_p.value <<
"] -> " 71 <<
"[" << (
int)v2.Red <<
", " 72 << (int)v2.Green <<
", " 73 << (
int)v2.Blue <<
"]"
int main(int argc, char **argv)