void setFormat(Image * I, int type_)
{
transfer(I,1);
if(I->type == type_) return;
else if(type_ == HSVTYPE) toHSV(I);
else if(type_ == RGBTYPE) toRGB(I);
I->type = type_;
}
void KColourProc::gammaCorrect( int& r, int& g, int& b ) const
{
double dr = (double)r;
double dg = (double)g;
double db = (double)b;
if( toHSV( dr, dg, db ) == false )
return;
db = _gammat [ (int) (db * MAX_GAMMA) ];
toRGB( dr, dg, db );
r = (int)dr;
g = (int)dg;
b = (int)db;
}
void colorSegment(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr GCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr BCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr YCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr OCloud)
{
float r_range[][2] = {{343.0, 10.0},
{0.10, 1.0},
{0.10, 0.75}};
float b_range[][2] = {{170.0, 243.0},
{0.50, 1.0},
{0.18, 0.70}};
float g_range[][2] = {{61.0, 169.0},
{0.0, 1.0},
{0.25, 0.70}};
float y_range[][2] = {{26.0, 60.0},
{0.5, 1.0},
{0.5, 1.0}};
float o_range[][2] = {{11.0, 25.0},
{0.5, 1.0},
{0.6, 1.0}};
/*
float r_range[][2] = { {350.0, 10.0},
{0.7377, 1.0},
{0.4431, 0.8353}};
float g_range[][2] = { {133.0, 157.0},
{0.3761, 1.0},
{0.2471, 0.9647}};
float b_range[][2] = { {220.0, 235.0},
{0.662, 1.0},
{0.2863, 0.6941}};
float y_range[][2] = { {21.0, 60.0},
{0.7917, 1.0},
{0.5686, 1.0}};
float o_range[][2] = { {13.0, 22.0},
{0.9228, 1.0},
{0.6706, 1.0}};
float l_range[][2] = { {68.0, 94.0},
{0.7124, 1.0},
{0.2667, 0.6784}};
*/
for (int i = 0; i < input->points.size(); i++)
{
float h, s, v;
toHSV(input->points[i].r, input->points[i].g, input->points[i].b, &h, &s, &v);
if ( (h > r_range[0][0]) || (h < r_range[0][1]) &&
(s > r_range[1][0]) && (s < r_range[1][1]) &&
(v > r_range[2][0]) && (v < r_range[2][1]))
{
RCloud->push_back(input->points[i]);
}
else if ( (h > g_range[0][0]) && (h < g_range[0][1]) &&
(s > g_range[1][0]) && (s < g_range[1][1]) &&
(v > g_range[2][0]) && (v < g_range[2][1]))
{
GCloud->push_back(input->points[i]);
}
else if ( (h > b_range[0][0]) && (h < b_range[0][1]) &&
(s > b_range[1][0]) && (s < b_range[1][1]) &&
(v > b_range[2][0]) && (v < b_range[2][1]))
{
BCloud->push_back(input->points[i]);
}
else if ( (h > y_range[0][0]) && (h < y_range[0][1]) &&
(s > y_range[1][0]) && (s < y_range[1][1]) &&
(v > y_range[2][0]) && (v < y_range[2][1]))
{
YCloud->push_back(input->points[i]);
}
else if ( (h > o_range[0][0]) && (h < o_range[0][1]) &&
(s > o_range[1][0]) && (s < o_range[1][1]) &&
(v > o_range[2][0]) && (v < o_range[2][1]))
{
OCloud->push_back(input->points[i]);
}
}
}
void normalSegment(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr GCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr BCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr YCloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr OCloud)
{
//color h s (mean, std)
//red
//green 116.964, 36.432 0.8780, 0.1616
//blue 229.644, 5.292 0.8527, 0.1730
//yellow 037.188, 6.948 0.9835, 0.0347
//orange 012.361, 7.1757 0.9822, 0.0813
double r1h[] = {0.00000, 15.000};
double r2h[] = {359.00000, 15.000};
//double gh[] = {116.964, 36.432};
double gh[] = {120.00, 25.000};
//double bh[] = {229.644, 5.2920};
double bh[] = {235.00, 25.2920};
double yh[] = {037.188, 6.9480};
//double yh[] = {60.00, 8.000};
double oh[] = {012.361, 7.1757};
double rs[] = {0.9547, 0.0778};
double gs[] = {0.8728, 0.1598};
double bs[] = {0.8527, 0.1730};
double ys[] = {0.9835, 0.0347};
double os[] = {0.9822, 0.0813};
for (int i = 0; i < input->points.size(); i++)
{
float h, s, v;
toHSV(input->points[i].r, input->points[i].g, input->points[i].b, &h, &s, &v);
if (h != h) // check if h is NaN
{
cout << "skipping ..." << endl;
continue;
}
/* calculate the probability that this point is red */
boost::math::normal_distribution<double> norm_r1h(r1h[0], r1h[1]);
double prob_r1h = boost::math::cdf(norm_r1h, h+0.5) - boost::math::cdf(norm_r1h, h-0.5);
boost::math::normal_distribution<double> norm_r2h(r2h[0], r2h[1]);
double prob_r2h = boost::math::cdf(norm_r2h, h+0.5) - boost::math::cdf(norm_r2h, h-0.5);
boost::math::normal_distribution<double> norm_rs(rs[0], rs[1]);
double prob_rs = boost::math::cdf(norm_rs, s+0.05) - boost::math::cdf(norm_rs, s-0.05);
double prob_r = (prob_r1h + prob_r2h) * prob_rs;
/* calculate the probability that this point is green */
boost::math::normal_distribution<double> norm_gh(gh[0], gh[1]);
double prob_gh = boost::math::cdf(norm_gh, h+0.5) - boost::math::cdf(norm_gh, h-0.5);
boost::math::normal_distribution<double> norm_gs(gs[0], gs[1]);
double prob_gs = boost::math::cdf(norm_gs, s+0.05) - boost::math::cdf(norm_gs, s-0.05);
double prob_g = prob_gh * prob_gs;
/* calculate the probability that this point is blue */
boost::math::normal_distribution<double> norm_bh(bh[0], bh[1]);
double prob_bh = boost::math::cdf(norm_bh, h+0.5) - boost::math::cdf(norm_bh, h-0.5);
boost::math::normal_distribution<double> norm_bs(bs[0], bs[1]);
double prob_bs = boost::math::cdf(norm_bs, s+0.05) - boost::math::cdf(norm_bs, s-0.05);
double prob_b = prob_bh * prob_bs;
/* calculate the probability that this point is yellow */
boost::math::normal_distribution<double> norm_yh(yh[0], yh[1]);
double prob_yh = boost::math::cdf(norm_yh, h+0.5) - boost::math::cdf(norm_yh, h-0.5);
boost::math::normal_distribution<double> norm_ys(ys[0], ys[1]);
double prob_ys = boost::math::cdf(norm_ys, s+0.05) - boost::math::cdf(norm_ys, s-0.05);
double prob_y = prob_yh * prob_ys;
/* calculate the probability that this point is yellow */
boost::math::normal_distribution<double> norm_oh(oh[0], oh[1]);
double prob_oh = boost::math::cdf(norm_oh, h+0.5) - boost::math::cdf(norm_oh, h-0.5);
boost::math::normal_distribution<double> norm_os(os[0], os[1]);
double prob_os = boost::math::cdf(norm_os, s+0.05) - boost::math::cdf(norm_os, s-0.05);
double prob_o = prob_oh * prob_os;
// now assign to buckets
if (prob_r > 0.001 && prob_r > prob_b && prob_r > prob_y && prob_r > prob_o && prob_r > prob_g)
RCloud->push_back(input->points[i]);
else if (prob_g > 0.001 && prob_g > prob_b && prob_g > prob_y && prob_g > prob_o && prob_g > prob_r)
GCloud->push_back(input->points[i]);
else if (prob_b > 0.001 && prob_b > prob_g && prob_b > prob_y && prob_b > prob_o && prob_b > prob_r)
BCloud->push_back(input->points[i]);
else if (prob_y > 0.001 && prob_y > prob_g && prob_y > prob_b && prob_y > prob_o && prob_y > prob_r)
YCloud->push_back(input->points[i]);
else if (prob_o > 0.001 && prob_o > prob_g && prob_o > prob_b && prob_o > prob_y && prob_o > prob_r)
OCloud->push_back(input->points[i]);
else
cout << i << " didn't classify" << endl;
}
}
bool operator==(const cocos2d::extension::HSV& a, const cocos2d::Color4F& b) { return (a == toHSV(b)); }
HSV toHSV(YCbCr ycbcr) {
RGB rgb = toRGB(ycbcr);
return toHSV(rgb);
}
