bool UManArc::getSMRCLcmd(char * buf, int bufCnt, double maxDist)
{
int i, n, m;
double d, dd, da;
char * p1;
bool result = true;
double v;
double dist;
//
// turn is not allowed at zero speed
if (fabs(vel) < 0.2)
v = 0.25;
else
v = vel;
//
if (radius < 3.0)
{
dist = mind(maxDist, getDistance());
snprintf(buf, bufCnt, "turnr %.3f %.3f @v%.2f @a%.3f :($drivendist > %.2f)",
maxd(radius, 0.15), // use at least 15cm turn radius
angle * 180.0 / M_PI, v,
maxd(getMinAcc(), fabs(acc)), dist);
n = strlen(buf);
}
else
{ // it is too unsafe to turn and finish on the implicit angle criteria
// divide into 30 cm steps
m = maxi(1, roundi(fabs(angle) / 0.05)); // about 3 deg steps
d = radius * angle;
dd = d / double(m);
da = angle / double(m);
n = 0;
p1 = buf;
dist = mind(maxDist, getDistance());
for (i = 0; i < m; i++)
{ // one command takes a little less than 60 characters.
// so there need to be at least 60 characters left in buffer
result = ((bufCnt - n) > 60);
if (not result)
{ // no more space for next command
printf("*** UManArc::getSMRCLcmd: no space left (%d left) in buffer (size %d) for next drive command\n", bufCnt, n);
break;
}
// space for next command line
snprintf(p1, bufCnt - n, "turnr %.3f %.3f \"rad\" @v%.2f @a%.3f :($drivendist > %.2f)\n",
radius, da, v, maxd(getMinAcc(), fabs(acc)), dd);
n += strlen(p1);
p1 = &buf[n];
dist -= dd;
if (dist < 0.0)
break;
}
// remove laset '\n'
if (n > 0)
buf[n-1] = '\0';
}
return (n < bufCnt);
}
void sreScissors::UpdateWithProjectedPoint(float x, float y, double z) {
if (z >= - 1.001d) {
// Beyond the near plane.
z = maxd(- 1.0d, z);
double depth = 0.5d * z + 0.5d;
near = mind(depth, near);
far = maxd(depth, far);
left = minf(x, left);
right = maxf(x, right);
bottom = minf(y, bottom);
top = maxf(y, top);
return;
}
sreMessage(SRE_MESSAGE_WARNING,
"Unexpected vertex in front of the near plane in UpdateWorldSpaceBoundingHull "
"z = %lf", z);
// In front of the near plane.
near = 0;
far = 1.0;
// We know that the light volume intersects the frustum, so it must extend to
// both sides of the near plane.
// Assume it fills the whole viewport (not optimal).
left = - 1.0f;
right = 1.0f;
bottom = - 1.0f;
top = 1.0f;
}
void Blend(double *s, double *d, double *c, int modes, int moded) {
double fs[4];
double fd[4];
BlendScaleFactors(s, d, c, modes,fs);
BlendScaleFactors(s, d, c, moded,fd);
for(int i=0; i<4; i++) { s[i]=mind(1, s[i]*fs[i]+d[i]*fd[i]); }
}
inline double range( double a, double b ) {
long r = random();
double rate = (double)r / (double)(0x7fffffff);
double _a = mind(a,b);
double _b = maxd(a,b);
return _a + (_b-_a)*rate;
}
void gline(float x1, float y1, float x2, float y2, float width, uint8_t r, uint8_t g, uint8_t b, uint8_t a) {
#ifdef OPENGL
glColor4f(((float) r) / 255.0, ((float) g) / 255.0, ((float) b) / 255.0, ((float) a) / 255.0);
// c1x,c1y
// 0,0......
// c4x,c4y ........ c2x,c2y
// ........ px,py
// c3x,c3y
float c1x, c1y, c1l, c2x, c2y, c2l, c3x, c3y, c3l, c4x, c4y, c4l;
float px = x2 - x1;
float py = y2 - y1;
c1x = -py;
c1y = px;
c1l = hypotf(c1x, c1y);
c1x = (c1x * width / c1l / 2.0) + x1;
c1y = (c1y * width / c1l / 2.0) + y1;
c2x = -py;
c2y = px;
c2l = hypotf(c2x, c2y);
c2x = (c2x * width / c2l / 2.0) + px + x1;
c2y = (c2y * width / c2l / 2.0) + py + y1;
c3x = py;
c3y = -px;
c3l = hypotf(c3x, c3y);
c3x = (c3x * width / c3l / 2.0) + px + x1;
c3y = (c3y * width / c3l / 2.0) + py + y1;
c4x = py;
c4y = -px;
c4l = hypotf(c4x, c4y);
c4x = (c4x * width / c4l / 2.0) + x1;
c4y = (c4y * width / c4l / 2.0) + y1;
if (width == 4.0)
printf("LINE: [%3.0f,%3.0f]->[%3.0f,%3.0f]->[%3.0f,%3.0f]->[%3.0f,%3.0f]\n", c1x, c1y, c2x, c2y, c3x, c3y, c4x, c4y);
glVertex2f(c1x, c1y);
glVertex2f(c2x, c2y);
glVertex2f(c3x, c3y);
glVertex2f(c4x, c4y);
#else
thickLineRGBA(Surf_Display,
(x1 - viewPortL) * zoomFactor,
(viewPortB - y1) * zoomFactor,
(x2 - viewPortL) * zoomFactor,
(viewPortB - y2) * zoomFactor,
mind(maxd(width * zoomFactor, 1), 2),
r, g, b, a
);
#endif
}
void do_experiment(swp_t&swp, sind_t&sind, rng_t&rng)
{
constant_indicator mind(
moran_probability(swp.mutantFitness()/swp.residentFitness(),
swp.n_individuals()));
relative_indicator_t<sind_t, constant_indicator> rind(sind,mind);
if (parameters.experiment() == "SWEEP")
{ IndicatorsDisplayController<swp_t,ParamsClass>
ind_display;
ind_display.inheritParametersFrom(swp);
ind_display.setrecordEvery(0);
ind_display.setdisplayEvery(0);
ind_display.installIndicator(rind,"fixation/Moran prob");
// // cout << "fixation / Moran probability is " << rind(swp) << endl;
// for (double rsr = 0.001; rsr <= 1000; rsr *= 1.1/*1.01*/)
// // for (swp.room_switch_rate = 1000; swp.room_switch_rate >= 0.001;
// // swp.room_switch_rate /= 1.01)
// { double p_switch = rsr / (1 + rsr);
// swp.setroom_switch_rate(rsr);
// ind_display.update(p_switch,swp);
// }
for (double mp = 0.01; mp < 1; mp += 0.01)
{ swp.setlattice_move_probability(mp);
ind_display.update(mp,swp);
}
}
else if (parameters.experiment() == "SAMPLE")
{ cout << "swarming pattern:\n" << canonical(swp);
double pfix = sind(swp);
cout << "fixation probability: " << pfix << '\n';
cout << "Moran probability: " << mind(swp) << '\n';
}
else if (parameters.experiment() == "OPTIMIZE")
{ cout << "Moran probability: " << mind(swp) << '\n';
do_optimize(swp,sind,swp,rng);
}
}
/* compute HOG features given gradient histograms */
void hog( double *H, double *HG, int h, int w, int d, int sBin, int oBin ) {
double *N, *N1, *H1, *HG1, n; int o, x, y, x1, y1, hb, wb, nb, hb1, wb1, nb1;
double eps = 1e-4/4.0/sBin/sBin/sBin/sBin; /* precise backward equality */
hb=h/sBin; wb=w/sBin; nb=wb*hb; hb1=hb-2; wb1=wb-2; nb1=hb1*wb1;
if(hb1<=0 || wb1<=0) return; N = (double*) mxCalloc(nb,sizeof(double));
for(o=0; o<oBin; o++) for(x=0; x<nb; x++) N[x]+=H[x+o*nb]*H[x+o*nb];
for( x=0; x<wb1; x++ ) for( y=0; y<hb1; y++ ) {
HG1 = HG + x*hb1 + y; /* perform 4 normalizations per spatial block */
for(x1=1; x1>=0; x1--) for(y1=1; y1>=0; y1--) {
N1 = N + (x+x1)*hb + (y+y1); H1 = H + (x+1)*hb + (y+1);
n = 1.0/sqrt(*N1 + *(N1+1) + *(N1+hb) + *(N1+hb+1) + eps);
for(o=0; o<oBin; o++) { *HG1=mind(*H1*n, 0.2); HG1+=nb1; H1+=nb; }
}
} mxFree(N);
}
int minDistance(string word1, string word2) {
if (word1.size() == 0)
return word2.size();
if (word2.size() == 0)
return word1.size();
vector<vector<int>> mind(word1.size() + 1, vector<int>(word2.size() + 1));
for (int i = 0;i <= word1.size();i++)
mind[i][0] = i;
for (int i = 0;i <= word2.size();i++)
mind[0][i] = i;
for (int i = 1;i <= word1.size();i++)
for (int j = 1;j <= word2.size();j++) {
mind[i][j] = min(mind[i - 1][j] + 1, mind[i][j - 1] + 1);
if (word1[i - 1] == word2[j - 1])
mind[i][j] = min(mind[i - 1][j - 1], mind[i][j]);
else mind[i][j] = min(mind[i - 1][j - 1] + 1, mind[i][j]);
}
return mind[word1.size()][word2.size()];
}
void BlendScaleFactors(double *s, double *d, double *c, int mode, double *f) {
double i;
switch(mode){
case 0: f[0]=0; f[1]=0; f[2]=0; f[3]=0; break; // 0:ZERO
case 1: f[0]=1; f[1]=1; f[2]=1; f[3]=1; break; //1:ONE
case 2: f[0]=s[0]; f[1]=s[1]; f[2]=s[2]; f[3]=s[3]; break; // 2:SRC_COLOR
case 3: f[0]=1-s[0]; f[1]=1-s[1]; f[2]=1-s[2]; f[3]=1-s[3]; break;// 3:ONE_MINUS_SRC_COLOR
case 4: f[0]=d[0]; f[1]=d[1]; f[2]=d[2]; f[3]=d[3]; break; // 4:DST_COLOR
case 5: f[0]=1-d[0]; f[1]=1-d[1]; f[2]=1-d[2]; f[3]=1-d[3]; break; // 5:ONE_MINUS_DST_COLOR
case 6: f[0]=s[3]; f[1]=s[3]; f[2]=s[3]; f[3]=s[3]; break; // 6:SRC_ALPHA
case 7: f[0]=1-s[3]; f[1]=1-s[3]; f[2]=1-s[3]; f[3]=1-s[3]; break; // 7:ONE_MINUS_SRC_ALPHA
case 8: f[0]=d[3]; f[1]=d[3]; f[2]=d[3]; f[3]=d[3]; break; // 8:DST_ALPHA
case 9: f[0]=1-d[3]; f[1]=1-d[3]; f[2]=1-d[3]; f[3]=1-d[3]; break; // 9:ONE_MINUS_DST_ALPHA
case 10: i=mind(s[3], 1-d[3]); f[0]=i; f[1]=i; f[2]=i; f[3]=1; break; // 10:SRC_ALPHA_SATURATE
case 11: f[0]=c[0]; f[1]=c[1]; f[2]=c[2]; f[3]=c[3]; break; // 11:CONSTANT_COLOR
case 12: f[0]=1-c[0]; f[1]=1-c[1]; f[2]=1-c[2]; f[3]=1-c[3]; break; // 12:ONE_MINUS_CONSTANT_COLOR
case 13: f[0]=c[3]; f[1]=c[3]; f[2]=c[3]; f[3]=c[3]; break; // 13:CONSTANT_ALPHA
case 14: f[0]=1-c[3]; f[1]=1-c[3]; f[2]=1-c[3]; f[3]=1-c[3]; break; // 14:ONE_MINUS_CONSTANT_ALPHA
};
}
void tree::train(const vector<vector<float>>& x, const vector<float>& y, const size_t mtry, const function<double()>& u01, vector<float>& incPurity, vector<float>& incMSE, vector<float>& impSD, vector<float>& oobPreds, vector<size_t>& oobTimes)
{
const size_t num_samples = x.size();
const size_t num_variables = x.front().size();
/* float sum = 0;
for (size_t i = 0; i < num_samples; ++i)
{
sum += y[i];
}
const float sum_inv = 1 / sum;
vector<float> p(num_samples);
for (size_t i = 0; i < num_samples; ++i)
{
p[i] = y[i] * sum_inv;
}
for (size_t i = 1; i < num_samples; ++i)
{
p[i] += p[i - 1];
}*/
// Create bootstrap samples with replacement
reserve((num_samples << 1) - 1);
emplace_back();
node& root = front();
root.samples.resize(num_samples);
for (size_t& s : root.samples)
{
const auto u = u01();
s = static_cast<size_t>(u * num_samples);
// for (s = 0; s < num_samples - 1 && !(u < p[s]); ++s);
}
// Populate nodes
for (size_t k = 0; k < size(); ++k)
{
node& n = (*this)[k];
// Evaluate node y and purity
float sum = 0;
for (const size_t s : n.samples) sum += y[s];
n.y = sum / n.samples.size();
n.p = sum * n.y; // = n.y * n.y * n.samples.size() = sum * sum / n.samples.size()
// Do not split the node if it contains too few samples
if (n.samples.size() <= 5) continue;
// Find the best split that has the highest increase in node purity
float bestChildNodePurity = n.p;
vector<size_t> mind(num_variables);
iota(mind.begin(), mind.end(), 0);
for (size_t i = 0; i < mtry; ++i)
{
// Randomly select a variable without replacement
const size_t j = static_cast<size_t>(u01() * (num_variables - i));
const size_t v = mind[j];
mind[j] = mind[num_variables - i - 1];
// Sort the samples in ascending order of the selected variable
vector<size_t> ncase(n.samples.size());
iota(ncase.begin(), ncase.end(), 0);
sort(ncase.begin(), ncase.end(), [&x, &n, v](const size_t val1, const size_t val2)
{
return x[n.samples[val1]][v] < x[n.samples[val2]][v];
});
// Search through the gaps in the selected variable
float suml = 0;
float sumr = sum;
size_t popl = 0;
size_t popr = n.samples.size();
for (size_t j = 0; j < n.samples.size() - 1; ++j)
{
const float d = y[n.samples[ncase[j]]];
suml += d;
sumr -= d;
++popl;
--popr;
if (x[n.samples[ncase[j]]][v] == x[n.samples[ncase[j+1]]][v]) continue;
const float curChildNodePurity = (suml * suml / popl) + (sumr * sumr / popr);
if (curChildNodePurity > bestChildNodePurity)
{
bestChildNodePurity = curChildNodePurity;
n.var = v;
n.val = (x[n.samples[ncase[j]]][v] + x[n.samples[ncase[j+1]]][v]) * .5f;
}
}
}
// Do not split the node if purity does not increase
if (bestChildNodePurity == n.p) continue;
// Create two child nodes and distribute samples
n.children[0] = size();
emplace_back();
n.children[1] = size();
emplace_back();
for (const size_t s : n.samples)
{
(*this)[n.children[x[s][n.var] > n.val]].samples.push_back(s);
}
}
// Aggregate NodeIncPurity
for (const auto& n : *this)
{
if (!n.children[0]) continue;
incPurity[n.var] += (*this)[n.children[0]].p + (*this)[n.children[1]].p - n.p;
}
// Find the samples used in bootstrap
vector<size_t> in(num_samples, 0);
for (const auto s : front().samples)
{
++in[s];
}
// Find the out-of-bag samples and calculate the OOB error without permutation
vector<size_t> oob;
oob.reserve(num_samples);
float ooberr = 0;
for (size_t s = 0; s < num_samples; ++s)
{
if (in[s]) continue;
oob.push_back(s);
const auto p = (*this)(x[s]);
ooberr += (p - y[s]) * (p - y[s]);
oobPreds[s] += p;
++oobTimes[s];
}
// Generate a permutation of OOB samples for use in all variables
vector<size_t> perm(oob);
for (size_t i = 1; i < perm.size(); ++i)
{
const auto j = static_cast<size_t>(u01() * (perm.size() - i + 1));
const auto tmp = perm[perm.size() - i];
perm[perm.size() - i] = perm[j];
perm[j] = tmp;
}
// Aggregate incMSE and impSD
for (size_t v = 0; v < num_variables; ++v)
{
float ooberrperm = 0;
for (size_t i = 0; i < oob.size(); ++i)
{
auto permx = x[oob[i]];
permx[v] = x[perm[i]][v];
const auto p = (*this)(permx);
ooberrperm += (p - y[oob[i]]) * (p - y[oob[i]]);
}
const auto delta = (ooberrperm - ooberr) / oob.size();
incMSE[v] += delta;
impSD[v] += delta * delta;
}
}
/*
* if baseline is true then we just leave out the subset totally
*/
static void cluster (char *outfn, int baseline)
{
// ranking[n] is the index of the nth testcase we want a user to
// look at, or else -1
int *ranking = (int *) malloc (numvecs * sizeof (int));
assert (ranking);
// ranked[0] is true iff point 0 has been placed in the list
int *ranked = (int *) malloc (numvecs * sizeof (int));
assert (ranked);
// number of entries in ranking[] that are filled in now
int cur_ranking = 0;
int x;
for (x=0; x<numvecs; x++) {
ranking[x] = -1;
ranked[x] = 0;
}
printf ("output file: '%s'\n", outfn);
FILE *outf = fopen (outfn, "w+");
assert (outf);
if (baseline == 0) {
// give a place to each member (if any) of the already-found subset
for (x=0; x<numvecs; x++) {
if (in_subset[x]) {
ranked[x] = 1;
ranking[cur_ranking] = x;
cur_ranking++;
}
}
}
// need to start with at least one ranked point, so start
// with the one farthest from everyone else
if (cur_ranking==0) {
int dpos;
int i = -1;
for (dpos=0; dpos<numvecs*numvecs; dpos++) {
i = dlist[dpos].a;
int j = dlist[dpos].b;
if (baseline==2) {
if (!in_all_subset[i] &&
!in_all_subset[j]) break;
}
if (!in_subset[i] &&
!in_subset[j]) break;
}
assert (!in_subset[i]);
ranking[cur_ranking] = i;
ranked[i] = 1;
cur_ranking++;
fprintf (outf, "%d\n", i);
}
while (1) {
// find the point that maximizes the minimum distance from
// any ranked point
int y;
double max_dist = -HUGE_VALF;
int i = -1;
for (y=0; y<numvecs; y++) {
if (ranked[y]) continue;
if ((baseline==1) && in_subset[y]) continue;
if ((baseline==2) && in_all_subset[y]) continue;
int z;
double min_dist = HUGE_VALF;
for (z=0; z<cur_ranking; z++) {
int new_z = ranking[z];
if ((baseline==1) && in_subset[new_z]) continue;
if ((baseline==2) && in_all_subset[new_z]) continue;
min_dist = mind (min_dist, distances[y][new_z]);
}
// alternate, simpler implementation
int zz;
double min_dist2 = HUGE_VALF;
for (zz=0; zz<numvecs; zz++) {
if ((baseline==1) && in_subset[zz]) continue;
if ((baseline==2) && in_all_subset[zz]) continue;
if (ranked[zz])
min_dist2 = mind (min_dist2, distances[y][zz]);
}
assert ((min_dist == min_dist2) && "not the same min_dist!");
if (min_dist > max_dist) {
max_dist = min_dist;
i = y;
}
}
if (i == -1) {
printf ("cur_ranking = %d, subset_size = %d, all_subset_size = %d, numvecs = %d\n",
cur_ranking, subset_size, all_subset_size, numvecs);
switch (baseline) {
case 0:
assert (cur_ranking == numvecs);
break;
case 1:
assert ((subset_size + cur_ranking) == numvecs);
break;
case 2:
// assert ((all_subset_size + cur_ranking) == numvecs);
break;
default:
assert (0);
}
break;
}
ranking[cur_ranking] = i;
ranked[i] = 1;
cur_ranking++;
fprintf (outf, "%d\n", i);
}
fclose (outf);
free (ranking);
free (ranked);
}
bool UResPassable::combineNearIntervals(double obstSize, double lowVarLimit)
{
ULaserPi *pp1, *pp2;
int i, k;
const int PI_NEAR_LIMIT = MIN_MEASUREMENTS_PER_PI;
bool result = false;
bool combined;
double avgX, distLimit;
double xl, xr, xl2, xr2;
double v1, v2;
double vm1, vm2;
double a1, a2, ad;
double sg1l, sg2l;
// double sgLen;
// ULaserPi * ppis[MAX_PASSABLE_INTERVALS_PR_SCAN];
// const double LOW_VAR_LIMIT = 0.00005;
// /** max difference in variance for two intervals */
// const double varVarianceFactor = 1.0;
// const double varVarLengthFactor = 0.5;
// double vf;
// sort intervals from right to left
/* for (i = 0; i < pisCnt; i++)
ppis[i] = &pis[i];*/
qsort(pis, pisCnt, sizeof(ULaserPi), sortPis);
// combine near intervals
pp1 = pis;
v1 = pp1->getFitVariance();
vm1 = pp1->getVarMin();
a1 = atan2(pp1->getSegment()->vec.y, pp1->getSegment()->vec.x);
sg1l = pp1->getSegment()->length;
pp2 = pp1 + 1;
i = 0;
while (pp2 < &pis[pisCnt])
{ // test right of pp1 and left of pp2
combined = false;
v2 = pp2->getFitVariance();
vm2 = pp2->getVarMin();
a2 = atan2(pp2->getSegment()->vec.y, pp2->getSegment()->vec.x);
sg2l = pp2->getSegment()->length;
if (abs(pp2->getRight() - pp1->getLeft()) < PI_NEAR_LIMIT)
{ // combine possible to the left of pp1
// get average x-value
xl = scan->getPos(pp1->getLeft()).x;
xr = scan->getPos(pp2->getRight()).x;
avgX = (xl + xr)/2.0;
distLimit = obstSize * avgX / sensorPose->getZ(); //scannerHeight;
// dist limit is for small obstacles
// level steps must be smaller
combined = fabs(xl - xr) < distLimit;
if (combined)
{ // test also the ends of the fitted line
xl2 = pp1->getSegment()->getOtherEnd().x;
xr2 = pp2->getSegment()->pos.x;;
combined = fabs(xl2 - xr2) < distLimit;
}
// test min half-robot-width variance difference
if (combined)
{ // test if step in variance is less than a factor 2
// if (fmax(vm1, vm2) > fmax(fmin(vm1, vm2), lowVarLimit/9.0) * 2.0)
if (fmax(vm1, vm2) > fmax(fmin(vm1, vm2), lowVarLimit) * 1.8)
combined = false;
}
if (combined)
{ // test angle and angle difference of segments
ad = limitToPi(a1 - a2);
if ((ad > (60 * M_PI / 180.0)) or
(ad < (-20 * M_PI / 180.0)) or
(fabs(a1) < 30 * M_PI / 180.0) or
(fabs(a1) > 150 * M_PI / 180.0) or
(fabs(a2) < 30 * M_PI / 180.0) or
(fabs(a2) > 150 * M_PI / 180.0))
combined = false;
}
if (combined)
// test measurements inbetween
for (k = pp1->getLeft() + 1; k < pp2->getRight(); k++)
{
combined = (avgX - scan->getPos(k).x) < distLimit;
if (not combined)
break;
}
if (combined)
pp1->setInterval(pp1->getRight(), pp2->getLeft(),
scan,
mind(pp1->getVarMin(), pp2->getVarMin()),
mind(pp1->getVarMin2(), pp2->getVarMin2()));
}
// use data from left-most interval as new base (combined or not)
v1 = v2;
vm1 = vm2;
a1 = a2;
sg1l = sg2l;
if (combined)
{ // combined, so move the remaining intervals down
for (k = i + 2; k < pisCnt; k++)
pis[k - 1] = pis[k];
pisCnt--;
}
else
{ // not combined - so move on to next
pp1++;
pp2++;
i++;
}
}
return result;
}
int UResPassable::makePassableIntervals2(const int rightLim, const int leftLim)
{
int result = 0;
int i, j;
ULaserPoint *pr;
ULaserPoint *pl; // pointer to left test point
ULaserPoint *pll; // pointer to last left test point
ULaserPoint *par;
int piLeft, piRight; // left and right index
int paRight, paLeft; // actual interval limits
bool passable;
double w, d;
double varMin = 1.0;
double varMin2 = 1.0;
double varTest = 0.0;
const double LASER_MEARUREMENT_VARIANCE = sqr(0.01); // in meter^2
double varMinLim = LASER_MEARUREMENT_VARIANCE;
double varStop = 1.0;
UPosition p;
// max allowed separation of measurements
const double MAX_MEASUREMENT_INTERVAL_DIST = 0.8;
// const double VAR_MIN2_LIMIT = 2; // minimum measurements for valid varince type 2 (no min limit)
double d2p; // distance to previous measurement
bool limHeight, limMaxVar, limEndDist, limInside, limMeasureDist;
bool doContinue;
//
//
// set default as not passable
scan->setQ(rightLim, leftLim - 1, PQ_NOT);
//
pr = scan->getData(rightLim);
if (lineSmoothSettings)
varMinLim = LASER_MEARUREMENT_VARIANCE;
else
varMinLim = LASER_MEARUREMENT_VARIANCE * 2.0;
pl = pr;
i = rightLim;
passable = false;
piRight = 0;
piLeft = -1; // just to avoid warnings
while (i < leftLim)
{ // test for terrain structure
if (not passable)
{ // look for start of passable using max limit
passable = (pr->varL < lineFitVarLimit) and // within variance limit ...
(absd(pr->tilt) < lineFitXTiltLimit) and // and tilt OK
(pr->pos.z < MAX_ALLOWED_HEIGHT) and // and not too high ...
(pr->pos.z > MIN_ALLOWED_HEIGHT); // or too low
if (passable)
{ // right edge of a potential area is found
piRight = i;
varMinLim = LASER_MEARUREMENT_VARIANCE;
varMin = maxd(pr->varL, varMinLim);
varMin2 = varMin; // 1.0;
// initialize looking for left side of interval
piLeft = i;
pl = scan->getData(i);
}
}
//
if (passable)
{
pll = pl;
pl = scan->getData(i);
// look for left side of interval using adaptive limit
varTest = mind(varMin, lineFitVarLimit) * lineFitEndpointDev;
// save index for (adaptive) left limit
paLeft = scan->getData(piLeft)->varToL;
// get compensated distance for last measurement to the left of interval
d = pr->distLeft - lineFitConvexOffset;
// get distance to previous measurement
if (pll->isValid() and pl->isValid())
d2p = hypot(pl->pos.x - pll->pos.x, pl->pos.y - pll->pos.y);
else
d2p = 0.0;
// debug
//if (pr->pos.z >= MAX_ALLOWED_HEIGHT)
// pr->pos.z = pr->pos.z + 0.01;
// debug end
limHeight = (pr->pos.z < MAX_ALLOWED_HEIGHT) and (pr->pos.z > MIN_ALLOWED_HEIGHT);
limMaxVar = pr->varL < lineFitVarLimit;
limEndDist = sqr(d) < varTest;
limInside = paLeft <= leftLim;
limMeasureDist = d2p < MAX_MEASUREMENT_INTERVAL_DIST;
// debug
doContinue = (limMeasureDist and limInside and limEndDist and limMaxVar and limHeight);
if (not doContinue)
{
if (not limMeasureDist)
iMeasureDist++;
if (not limInside)
iInside++;
if (limEndDist)
iEndDist++;
if (limMaxVar)
iMaxVar++;
if (limHeight)
iHeight++;
}
// debug end
// test for end of passable interval
if ((pr->varL < lineFitVarLimit) and // max line-fit variance
(sqr(d) < varTest) and // fit of left-most point adapted
(paLeft <= leftLim) and // inside test interval
(absd(pr->tilt) < lineFitXTiltLimit) and // line tilt OK
(pr->pos.z < MAX_ALLOWED_HEIGHT) and // item height not too high ...
(pr->pos.z > MIN_ALLOWED_HEIGHT) and
(d2p < MAX_MEASUREMENT_INTERVAL_DIST)) // or too low
{ // still passable
if (pr->isValid())
piLeft = i; // left-most place to search for a right edge
// adapt limit of line-fit
if (pr->varL < varMin)
varMin = maxd(pr->varL, varMinLim);
if ((pr->varL < varMin2)) // and ((pr->varToL - i) > VAR_MIN2_LIMIT))
varMin2 = pr->varL;
}
else
{ // (adaptive) left end of interval is found
// look from here to the right for adaptive right limit
paRight = piLeft;
// get position of left side of interval
p = scan->getData(paLeft)->pos;
par = scan->getData(paRight); // first guess of adapted right edge
w = 0.0; // reset width
for (j = paRight; j >= piRight; j--)
{ // look for right edge with this variance.
// calculate passable width
w = p.dist(par->pos);
// get distance relative to line compensated
// to favor convex shaped road
d = par->distRight - lineFitConvexOffset;
varStop = par->varL;
//
limMaxVar = pr->varL < lineFitVarLimit;
limEndDist = sqr(d) < varTest;
doContinue = (limMeasureDist and limInside and limEndDist and limMaxVar and limHeight);
if (not doContinue)
{
if (limEndDist)
iEndDist++;
if (limMaxVar)
iMaxVar++;
}
//
if ((par->varL <= lineFitVarLimit) and // max line-fit variance limit
(sqr(d) < varTest))// and // last right-most point (adaptive)
paRight = j;
else
// right edge is found
break;
// go further right
par--;
}
// left is left side of last good interval
// missing test for big (in-line) jumps in untested interval
// from liLeft to paLeft
pl = scan->getData(piLeft);
for (j = piLeft + 1; j <= paLeft; j++)
{
pll = pl;
pl = scan->getData(j);
if (pll->isValid() and pl->isValid())
{ // test only if measurements are valid
d2p = hypot(pl->pos.x - pll->pos.x, pl->pos.y - pll->pos.y);
if (d2p >= MAX_MEASUREMENT_INTERVAL_DIST)
{ // stop before the jump
paLeft = j - 1;
break;
}
}
}
addPassableInterval(paRight, mini(paLeft, leftLim), varMin, varMin2);
//
result++;
// look for new start
i = paLeft;
pr = scan->getData(i);
passable = false;
// an interval is found (or failed), but there may be another
// to the right of the tested interval
if ((paRight - piRight) > MIN_MEASUREMENTS_PER_PI)
{ // test this interval
result += makePassableIntervals2(piRight, paRight - 1);
}
// continue with next interval
}
}
i++;
// prOld = pr;
pr++;
}
return result;
}
// calculation of the threshold in quiet in FFT-resolution
static void
Ruhehoerschwelle ( PsyModel* m,
unsigned int EarModelFlag,
int Ltq_offset,
int Ltq_max )
{
int n;
int k;
float f;
float erg;
double tmp;
float absLtq [512];
for ( n = 0; n < 512; n++ ) {
f = (float) ( (n+1) * (float)(m->SampleFreq / 2000.) / 512 ); // Frequency in kHz
switch ( EarModelFlag / 100 ) {
case 0: // ISO-threshold in quiet
tmp = 3.64*pow (f,-0.8) - 6.5*exp (-0.6*(f-3.3)*(f-3.3)) + 0.001*pow (f, 4.0);
break;
default:
case 1: // measured threshold in quiet (Nick Berglmeir, Andree Buschmann, Kopfh�er)
tmp = 3.00*pow (f,-0.8) - 5.0*exp (-0.1*(f-3.0)*(f-3.0)) + 0.0000015022693846297*pow (f, 6.0) + 10.*exp (-(f-0.1)*(f-0.1));
break;
case 2: // measured threshold in quiet (Filburt, Kopfh�er)
tmp = 9.00*pow (f,-0.5) - 15.0*exp (-0.1*(f-4.0)*(f-4.0)) + 0.0341796875*pow (f, 2.5) + 15.*exp (-(f-0.1)*(f-0.1)) - 18;
tmp = mind ( tmp, Ltq_max - 18 );
break;
case 3:
tmp = ATHformula_Frank ( 1.e3 * f );
break;
case 4:
tmp = ATHformula_Frank ( 1.e3 * f );
if ( f > 4.8 ) {
tmp += 3.00*pow (f,-0.8) - 5.0*exp (-0.1*(f-3.0)*(f-3.0)) + 0.0000015022693846297*pow (f, 6.0) + 10.*exp (-(f-0.1)*(f-0.1));
tmp *= 0.5 ;
}
break;
case 5:
tmp = ATHformula_Frank ( 1.e3 * f );
if ( f > 4.8 ) {
tmp = 3.00*pow (f,-0.8) - 5.0*exp (-0.1*(f-3.0)*(f-3.0)) + 0.0000015022693846297*pow (f, 6.0) + 10.*exp (-(f-0.1)*(f-0.1));
}
break;
}
tmp -= f * f * (int)(EarModelFlag % 100 - 50) * 0.0015; // 00: +30 dB, 100: -30 dB @20 kHz
tmp = mind ( tmp, Ltq_max ); // Limit ATH
tmp += Ltq_offset - 23; // Add chosen Offset
m->tables.fftLtq[n] = absLtq[n] = POW10 ( 0.1 * tmp); // conversion into power
}
// threshold in quiet in partitions (long)
for ( n = 0; n < PART_LONG; n++ ) {
erg = 1.e20f;
for ( k = MPC_WL[n]; k <= MPC_WH[n]; k++ )
erg = minf (erg, absLtq[k]);
m->tables.partLtq[n] = erg; // threshold in quiet
m->tables.invLtq [n] = 1.f / m->tables.partLtq[n]; // Inverse
}
}
