void MotionSystem::noiseShiftWithOdo(Particle* particle, float dX, float dY, float dH) {
// How x,y,h factors when deciding ranges
float xF = 5.f;
float yF = 5.f;
float hF = 5.f;
float xL, xU, yL, yU, hL, hU;
if ((std::fabs(dX * xF) - .2f) < 0.001f) {
xL = -.2f;
xU = .2f;
}
else if (dX >0) {
xL = -1.f * dX * xF;
xU = dX * xF;
}
else {//dX <0
xL = dX * xF;
xU = -1.f * dX * xF;
}
if ((std::fabs(dY * yF) - .2f) < 0.001f) {
yL = -.2f;
yU = .2f;
}
else if (dY >0) {
yL = -1.f * dY * yF;
yU = dY * yF;
}
else { //dY <0
yL = dY * yF;
yU = -1.f * dY * yF;
}
if ((std::fabs(dH * hF) - .025f) < 0.001f) {
hL = -.025f;
hU = .025f;
}
else if (dH >0) {
hL = -1.f * dH * hF;
hU = dH * hF;
}
else { //dH <0
hL = dH * hF;
hU = -1.f * dH * hF;
}
boost::uniform_real<float> xRange(xL, xU);
boost::uniform_real<float> yRange(yL, yU);
boost::uniform_real<float> hRange(hL, hU);
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > xShift(rng, xRange);
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > yShift(rng, yRange);
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > hShift(rng, hRange);
particle->shift(xShift(), yShift(), hShift());
}
void
ILAC_Median_CC::classify ()
{
/*
* 1 CREATE RANGE ARRAY
* Color mapping: hRange[0] < RED < hRange[1] |
* hRange[1] < YELLOW < hRange[2] |
* ... |
* hRange[6] < RED < hRange[7]
* Ranges are sorted in increasing order.
*/
vector<int> hRange(8,0);
/*
* Fill the range with colors and means. Order based on hue. Make sure
* smallest and largets values in the endpoints.
*/
for ( int i = 0 ; i < 6 ; i++ )
{
hRange[i] = this->calcHueMedian(samples[i]);
for ( int j = i ; j > 0 && hRange[j] < hRange[j-1] ; j-- )
std::swap( hRange[j], hRange[j-1] );
}
/*
* Calculate the max ranges. The midpoints between two medians are calculated
* by adding half (/2) the distance (subtraction) -between to medians- to the
* smallest median; and making sure it does not exceed 256 (%256)
*/
hRange[6] = 256+hRange[0];
for ( int i = 6 ; i > 0 ; i-- )
hRange[i] = ( hRange[i-1]
+ ((hRange[i] - hRange[i-1]) / 2) )
% 256;
/*
* There is a possibility that things are not in order. We do this because the
* hue 257 is equal to 0 (and so on)
*/
for ( int i = 1 ; i < 7 ; i++ )
for ( int j = i ; j > 1 && hRange[j] < hRange[j-1] ; j-- )
std::swap( hRange[j], hRange[j-1] );
/* We polish the ends. */
hRange[0] = 0;
hRange[7] = 256;
/*
* 2.CALCULATE CLASS
* Each accumulator offset will represent a color.
* c_accum[0] -> red, c_accum[1] -> yellow, c_accum[2] -> green,
* c_accum[3] -> cyan, c_accum[4] -> Blue, c_accum[5] -> magenta
* These values are related to the range argument.
* The color with more hits is the one that is chosen.
*/
Mat hImg;
uchar *data_ptr;
int coffset = 0;
for ( vector<ILAC_Square>::iterator _data = data.begin();
_data != data.end() ; ++_data, coffset++ )
{
/* Transform from BGR to HSV */
{
Mat hsvImg;
cvtColor ( (*_data).getImg(), hsvImg, CV_BGR2HSV_FULL );
vector<Mat> tmp_dim;
split( hsvImg, tmp_dim );
hImg = tmp_dim[0];
}
unsigned long c_accum[6] = {0};
for ( unsigned int row = 0 ; row <= hImg.rows ; row++ )
for ( unsigned int col = 0 ; col <= hImg.cols ; col++ )
{
data_ptr = hImg.data + (row*hImg.cols) + col;
for ( int j = 0 ; j < 8 ; j++ )
if ( hRange[j] > *data_ptr )
{
c_accum[(j-1)%6]++;
break;
}
}
/* find where the maximum offset is*/
int max_offset = 0;
for ( int i = 0 ; i < 6 ; i++ )
if ( c_accum[max_offset] < c_accum[i] )
max_offset = i;
this->classes[coffset] = max_offset;
}
}
