work on progress : filtreHue

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-06 21:00:14 +02:00
parent 1a17b44c7d
commit fd0bda6af7
2 changed files with 101 additions and 91 deletions

View file

@ -13,6 +13,5 @@
<remap from="/estimator/input" to="/filtre/output"/> <remap from="/estimator/input" to="/filtre/output"/>
</node> </node>
<include file="$(find hand_control)/launch/commande.launch"/>
</launch> </launch>

View file

@ -2,6 +2,7 @@
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/filters/passthrough.h> #include <pcl/filters/passthrough.h>
#include <assert.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> PointCloud;
@ -15,31 +16,41 @@ class Callback {
copy_info(msg, pcl); copy_info(msg, pcl);
BOOST_FOREACH (const Point& pt, msg->points) BOOST_FOREACH (const Point& pt, msg->points)
{ {
int h(0); uint8_t min, max, c;
float rScaled(pt.r/255); if (pt.r >= pt.g) {
float gScaled(pt.g/255); if (pt.g >= pt.b) {
float bScaled(pt.b/255); max = pt.r
min = pt.b;
float cMax(std::max(std::max(rScaled, gScaled), bScaled)); } else if (pt.r >= pt.b) {
float cMin(std::min(std::min(rScaled, gScaled), bScaled)); max = pt.r;
float cDelta(cMin-cMax); min = pt.g;
} else {
if (cMax == rScaled){ max = pt.b;
h = (int) 60*(gScaled-bScaled)/cDelta; min = pt.g;
}else if (cMax = gScaled){ }
h = (int) 60*(2+(bScaled-rScaled)/cDelta); } else if (pt.r >= pt.b) {
}else if (cMax = rScaled){ max = pt.g;
h = (int) 60*(4+(rScaled-gScaled)/cDelta); min = pt.b;
} else if (pt.g >= pt.b) {
max = pt.g;
min = pt.r;
} else {
max = pt.b;
min = pt.r;
} }
if (h < 0) { c = max - min;
h += 360;
} assert(c > 0);
assert(max > pt.r);
assert(max > pt.g);
assert(max > pt.b);
assert(min < pt.r);
assert(min < pt.g);
assert(min < pt.b);
if (abs(h - hue) < delta) {
pcl->push_back(pt);
}
}
pcl->height = 1; pcl->height = 1;
pcl->width = pcl->points.size(); pcl->width = pcl->points.size();
publisher.publish(pcl); publisher.publish(pcl);
@ -64,11 +75,11 @@ class Callback {
b->sensor_orientation_ = a->sensor_orientation_; b->sensor_orientation_ = a->sensor_orientation_;
b->is_dense = a->is_dense; b->is_dense = a->is_dense;
} }
}; };
int int
main(int argc, char** argv) main(int argc, char** argv)
{ {
ros::init(argc, argv, "filtreHue"); ros::init(argc, argv, "filtreHue");
ros::NodeHandle node("filtreHue"); ros::NodeHandle node("filtreHue");
@ -105,4 +116,4 @@ main(int argc, char** argv)
ros::spin(); ros::spin();
ROS_INFO("exit"); ROS_INFO("exit");
return 0; return 0;
} }