add hue filter to «filtre» node
This commit is contained in:
parent
fd0bda6af7
commit
2897943e42
2 changed files with 50 additions and 7 deletions
|
@ -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})
|
||||
|
|
|
@ -1,6 +1,8 @@
|
|||
#include <ros/ros.h>
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/tracking/impl/hsv_color_coherence.hpp>
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
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<PointCloud>("output", 1);
|
||||
Callback callback(publisher, (float) zmax);
|
||||
Callback callback(publisher, (float) zmax, (float) hue, (float) delta_hue);
|
||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
|
||||
|
||||
// démarrage
|
||||
|
|
Loading…
Reference in a new issue