void ArLaser::laserProcessReadings(void)
{
// if we have no readings... don't do anything
if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end())
return;
std::list<ArSensorReading *>::iterator sensIt;
ArSensorReading *sReading;
double x, y;
double lastX = 0.0, lastY = 0.0;
//unsigned int i = 0;
ArTime len;
len.setToNow();
bool clean;
if (myCumulativeCleanInterval <= 0 ||
(myCumulativeLastClean.mSecSince() >
myCumulativeCleanInterval))
{
myCumulativeLastClean.setToNow();
clean = true;
}
else
{
clean = false;
}
myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myRawReadings->front()->getEncoderPoseTaken());
myCurrentBuffer.beginRedoBuffer();
// walk the buffer of all the readings and see if we want to add them
for (sensIt = myRawReadings->begin();
sensIt != myRawReadings->end();
++sensIt)
{
sReading = (*sensIt);
// if we have ignore readings then check them here
if (!myIgnoreReadings.empty() &&
(myIgnoreReadings.find(
(int) ceil(sReading->getSensorTh())) !=
myIgnoreReadings.end()) ||
myIgnoreReadings.find(
(int) floor(sReading->getSensorTh())) !=
myIgnoreReadings.end())
sReading->setIgnoreThisReading(true);
// see if the reading is valid
if (sReading->getIgnoreThisReading())
continue;
// if we have a max range then check it here...
if (myMaxRange != 0 &&
sReading->getRange() > myMaxRange)
{
sReading->setIgnoreThisReading(true);
}
// see if the reading is valid... this is set up this way so that
// max range readings can cancel out other readings, but will
// still be ignored other than that... ones ignored for other
// reasons were skipped above
if (sReading->getIgnoreThisReading())
{
internalProcessReading(sReading->getX(), sReading->getY(),
sReading->getRange(), clean, true);
continue;
}
// get our coords
x = sReading->getX();
y = sReading->getY();
// see if we're checking on the filter near dist... if we are
// and the reading is a good one we'll check the cumulative
// buffer
if (myMinDistBetweenCurrentSquared > 0.0000001)
{
// see where the last reading was
//squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY);
// see if the reading is far enough from the last reading
if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) >
myMinDistBetweenCurrentSquared)
{
lastX = x;
lastY = y;
// since it was a good reading, see if we should toss it in
// the cumulative buffer...
internalProcessReading(x, y, sReading->getRange(), clean, false);
/* we don't do this part anymore since it wound up leaving
// too many things not really tehre... if its outside of our
// sensor angle to use to filter then don't let this one
// clean (ArMath::fabs(sReading->getSensorTh()) > 50)
// filterAddAndCleanCumulative(x, y, false); else*/
}
// it wasn't far enough, skip this one and go to the next one
else
{
continue;
}
}
// we weren't filtering the readings, but see if it goes in the
// cumulative buffer anyways
else
{
internalProcessReading(x, y, sReading->getRange(), clean, false);
}
// now drop the reading into the current buffer
myCurrentBuffer.redoReading(x, y);
//i++;
}
myCurrentBuffer.endRedoBuffer();
/* Put this in to see how long the cumulative filtering is taking
if (clean)
printf("### %ld %d\n", len.mSecSince(), myCumulativeBuffer.getBuffer()->size());
*/
internalGotReading();
}
void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
char buf[1024];
int i;
int j;
ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
reading->setIgnoreThisReading(true);
continue;
}
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
if (!goodAll || !goodAny)
reading->setIgnoreThisReading(true);
else
lastAddedReading = reading;
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
good ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
selfUnlockDevice();
myLaser->unlockDevice();
}
void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
//file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
file = ArUtil::fopen("/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
// if we're not doing any filtering, just short circuit out now
if (myAllFactor <= 0 && myAnyFactor <= 0 && myAnyMinRange <= 0)
{
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
return;
}
char buf[1024];
int i;
int j;
//ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
/* Taking this check out since the base class does it now and if
* it gets marked ignore now it won't get used for clearing
* cumulative readings
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f beyond max range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
*/
if (myAnyMinRange >= 0 && reading->getRange() < myAnyMinRange &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle))
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f within min range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
/*
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
*/
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
bool goodMinRange = true;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
/* You can't skip these, or you get onesided filtering
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
// you can't ignore these or you get one sided filtering
/*
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
if (!goodAll || !goodAny || !goodMinRange)
reading->setIgnoreThisReading(true);
/*
else
lastAddedReading = reading;
*/
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
goodAll && goodAny && goodMinRange ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
}
