LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

better hsv computing code in filtre.cpp

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-26 16:03:42 +02:00
parent 31638974d8
commit ed7e054c15
1 changed files with 17 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

42
src/filtre.cpp
View file

@ -19,7 +19,9 @@ class Callback {
copy_info(msg, pcl); copy_info(msg, pcl);
BOOST_FOREACH (const Point& pt, msg->points) BOOST_FOREACH (const Point& pt, msg->points)
{ {
if (pt.z < z_max and hue_dist(pt) < delta_hue and sat(pt) < sat_max and sat(pt) > sat_min and val(pt) < val_max and val(pt) > val_min) float hue_dist, sat, val;
hdist_s_v(pt, hue_dist, sat, val);
if (pt.z < z_max and hue_dist < delta_hue and sat < sat_max and sat > sat_min and val < val_max and val > val_min)
pcl->push_back(pt); pcl->push_back(pt);
} }
pcl->height = 1; pcl->height = 1;
@ -41,7 +43,7 @@ class Callback {
} }
void void
reconfigure(const hand_control::FiltreConfig& c, const uint32_t level) { reconfigure(const hand_control::FiltreConfig& c, const uint32_t& level) {
z_max = c.z_max; z_max = c.z_max;
hue = c.hue; hue = c.hue;
delta_hue = c.delta_hue; delta_hue = c.delta_hue;
@ -49,6 +51,14 @@ class Callback {
val_max = c.val_max; val_max = c.val_max;
sat_min = c.sat_min; sat_min = c.sat_min;
sat_max = c.sat_max; sat_max = c.sat_max;
assert(delta_hue > 0);
assert(z_max > 0);
assert(hue >= 0);
assert(hue <= 360.);
assert(sat_min >= 0);
assert(sat_max <= 1.);
assert(val_min >= 0);
assert(val_max <= 1.);
} }
private: private:
@ -58,7 +68,7 @@ class Callback {
inline inline
void void
copy_info(const PointCloud::ConstPtr& a, copy_info(const PointCloud::ConstPtr& a,
PointCloud::Ptr b) PointCloud::Ptr& b)
{ {
b->header = a->header; b->header = a->header;
b->sensor_origin_ = a->sensor_origin_; b->sensor_origin_ = a->sensor_origin_;
@ -67,10 +77,10 @@ class Callback {
} }
inline inline
float void
hue_dist(const Point& pt) hdist_s_v(const Point& pt, float& h_dist, float& s, float& v)
{ {
float h, s, v, diff1, diff2; float h, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
h *= 360.0f ; h *= 360.0f ;
diff1 = std::fabs(h - hue); diff1 = std::fabs(h - hue);
@ -78,25 +88,7 @@ class Callback {
diff2 = std::fabs(360.0f + h - hue); diff2 = std::fabs(360.0f + h - hue);
else else
diff2 = std::fabs(360.0f + hue - h); diff2 = std::fabs(360.0f + hue - h);
return std::min(diff1, diff2); h_dist = std::min(diff1, diff2);
}
inline
float
sat(const Point& pt)
{
float h, s, v, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
return s;
}
inline
float
val(const Point& pt)
{
float h, s, v, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
return v;
} }
}; };