//======================================================================
// rnorm_trunc( mu, sigma, lower, upper)
//
// generates one random normal RVs with mean 'mu' and standard
// deviation 'sigma', truncated to the interval (lower,upper), where
// lower can be -Inf and upper can be Inf.
//======================================================================
double
rnorm_trunc (double mu, double sigma, double lower, double upper)
{
int change;
double a, b;
double logt1 = log(0.150), logt2 = log(2.18), t3 = 0.725, t4 = 0.45;
double z, tmp, lograt;
change = 0;
a = (lower - mu)/sigma;
b = (upper - mu)/sigma;
// First scenario
if( (a == R_NegInf) || (b == R_PosInf))
{
if(a == R_NegInf)
{
change = 1;
a = -b;
b = R_PosInf;
}
// The two possibilities for this scenario
if(a <= 0.45) z = norm_rs(a, b);
else z = exp_rs(a, b);
if(change) z = -z;
}
// Second scenario
else if((a * b) <= 0.0)
{
// The two possibilities for this scenario
if((dnorm(a, 0.0, 1.0, 1) <= logt1) || (dnorm(b, 0.0, 1.0, 1) <= logt1))
{
z = norm_rs(a, b);
}
else z = unif_rs(a,b);
}
// Third scenario
else
{
if(b < 0)
{
tmp = b; b = -a; a = -tmp; change = 1;
}
lograt = dnorm(a, 0.0, 1.0, 1) - dnorm(b, 0.0, 1.0, 1);
if(lograt <= logt2) z = unif_rs(a,b);
else if((lograt > logt1) && (a < t3)) z = half_norm_rs(a,b);
else z = exp_rs(a,b);
if(change) z = -z;
}
return (sigma*z + mu);
}
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;
}
}
void
rnorm_truncated (double *sample, int *n, double *mu,
double *sigma, double *lower, double *upper)
{
int k;
int change;
double a, b;
double logt1 = log(0.150), logt2 = log(2.18), t3 = 0.725, t4 = 0.45;
double z, tmp, lograt;
GetRNGstate();
for (k=0; k<(*n); k++)
{
change = 0;
a = (lower[k] - mu[k])/sigma[k];
b = (upper[k] - mu[k])/sigma[k];
// First scenario
if( (a == R_NegInf) || (b == R_PosInf))
{
if(a == R_NegInf)
{
change = 1;
a = -b;
b = R_PosInf;
}
// The two possibilities for this scenario
if(a <= 0.45) z = norm_rs(a, b);
else z = exp_rs(a, b);
if(change) z = -z;
}
// Second scenario
else if((a * b) <= 0.0)
{
// The two possibilities for this scenario
if((dnorm(a, 0.0, 1.0, 1) <= logt1) || (dnorm(b, 0.0, 1.0, 1) <= logt1))
{
z = norm_rs(a, b);
}
else z = unif_rs(a,b);
}
// Third scenario
else
{
if(b < 0)
{
tmp = b; b = -a; a = -tmp; change = 1;
}
lograt = dnorm(a, 0.0, 1.0, 1) - dnorm(b, 0.0, 1.0, 1);
if(lograt <= logt2) z = unif_rs(a,b);
else if((lograt > logt1) && (a < t3)) z = half_norm_rs(a,b);
else z = exp_rs(a,b);
if(change) z = -z;
}
sample[k] = sigma[k]*z + mu[k];
}
PutRNGstate();
}
void rnorm_truncated (double *sample, int *n, double *mu,
double *sigma, double *lower, double *upper)
{
int k;
int change = 0;
double a, b;
double logt1 = log(0.150), logt2 = log(2.18), t3 = 0.725, t4 = 0.45;
double z, tmp, lograt;
a = (*lower - *mu)/(*sigma);
b = (*upper - *mu)/(*sigma);
if(a==b) Rprintf("Warning!! a=%f, b=%f\n",a,b);
if(a>b) Rprintf("Warning!! a = %f > b = %f\n",a,b);
for (k=0; k<(*n); k++)
{
change=0;
// First scenario
if( (a == R_NegInf) || (b == R_PosInf))
{
if(a == R_NegInf)
{
change = 1;
a = -b;
b = R_PosInf;
}
// The two possibilities for this scenario
if(a <= t4) z = norm_rs(a, b);
else z = exp_rs(a, b);
if(change) z = -z;
}
// Second scenario
else if((a * b) <= 0.0)
{
// The two possibilities for this scenario
if((dnorm(a, 0.0, 1.0, 1) <= logt1) || (dnorm(b, 0.0, 1.0, 1) <= logt1))
{
z = norm_rs(a, b);
}
else z = unif_rs(a,b);
}
// Third scenario
else
{
if(b < 0)
{
tmp = b; b = -a; a = -tmp; change = 1;
}
lograt = dnorm(a, 0.0, 1.0, 1) - dnorm(b, 0.0, 1.0, 1);
if(lograt <= logt2) z = unif_rs(a,b);
else if((lograt > logt1) && (a < t3)) z = half_norm_rs(a,b);
else z = exp_rs(a,b);
if(change) z = -z;
}
sample[k] = *sigma*z + *mu;
}
}
