diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b4f701..78bfffb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,8 +53,8 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR add_executable(filtreRGB src/filtreRGB.cpp) target_link_libraries(filtreRGB ${catkin_LIBRARIES}) -add_executable(filtreHue src/filtreHue.cpp) -target_link_libraries(filtreHue ${catkin_LIBRARIES}) +#add_executable(filtreHue src/filtreHue.cpp) +#target_link_libraries(filtreHue ${catkin_LIBRARIES}) add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) diff --git a/src/filtre.cpp b/src/filtre.cpp index 107456b..541ae03 100644 --- a/src/filtre.cpp +++ b/src/filtre.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include typedef pcl::PointXYZRGB Point; @@ -15,7 +17,7 @@ class Callback { copy_info(msg, pcl); BOOST_FOREACH (const Point& pt, msg->points) { - if (pt.z < zmax) + if (pt.z < zmax and hue_dist(pt) < delta_hue) pcl->push_back(pt); } pcl->height = 1; @@ -23,12 +25,18 @@ class Callback { publisher.publish(pcl); } - Callback(ros::Publisher& pub, float z) - : publisher(pub), zmax(z) {} + Callback(ros::Publisher& pub, float z, float h, float delta_h) + : publisher(pub), zmax(z), hue(h), delta_hue(delta_h) + { + assert(delta_hue > 0); + assert(zmax > 0); + assert(hue >= 0); + assert(hue <= 360.); + } private: ros::Publisher publisher; - float zmax; + float zmax, hue, delta_hue; inline void @@ -40,6 +48,21 @@ class Callback { b->sensor_orientation_ = a->sensor_orientation_; b->is_dense = a->is_dense; } + + inline + float + hue_dist(const Point& pt) + { + float h, s, v, diff1, diff2; + pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); + h *= 360.0f ; + diff1 = std::fabs(h - hue); + if (h < hue) + diff2 = std::fabs(360.0f + h - hue); + else + diff2 = std::fabs(360.0f + hue - h); + return std::max(diff1, diff2); + } }; int @@ -59,9 +82,29 @@ main(int argc, char** argv) ROS_INFO("zmax : %f (default value)", zmax); } + double hue(0); + if (node.getParam("hue", hue)) + { + ROS_INFO("hue : %f" , hue); + } else { + node.setParam("hue", 0.0); + node.getParam("hue", hue); + ROS_INFO("hue : %f (default value)", hue); + } + + double delta_hue(0); + if (node.getParam("delta_hue", delta_hue)) + { + ROS_INFO("delta_hue : %f" , delta_hue); + } else { + node.setParam("delta_hue", 10.0); + node.getParam("delta_hue", delta_hue); + ROS_INFO("delta_hue : %f (default value)", delta_hue); + } + // initialisatio ros::Publisher publisher = node.advertise("output", 1); - Callback callback(publisher, (float) zmax); + Callback callback(publisher, (float) zmax, (float) hue, (float) delta_hue); ros::Subscriber subscriber = node.subscribe("input", 1, callback); // démarrage