void PTrackerROS::addArtificialNoise(vector<ObjectSensorReading::Observation>& obs)
{
/// Adding Gaussian noise to observations.
if (isGaussianNoise)
{
for (vector<ObjectSensorReading::Observation>::iterator it = obs.begin(); it != obs.end(); ++it)
{
it->observation.x += gaussianNoise(gaussianNoiseX,0.3);
it->observation.y += gaussianNoise(gaussianNoiseY,0.3);
}
}
/// Adding false positive observations for a burst period of time equals to falsePositiveBurstTime.
if ((falsePositiveBurstTime > 0.0) && (falsePositiveObservations > 0))
{
static const int TIME_TO_WAIT_BEFORE_NEXT_BURST = 10000 - falsePositiveBurstTime;
static vector<ObjectSensorReading::Observation> falseObservations;
static Timestamp initialTimestamp, initialTimestampBurst;
static bool isBurstTime = false, isGenerationTime = true;
if (isBurstTime)
{
if ((Timestamp() - initialTimestampBurst).getMs() < falsePositiveBurstTime)
{
if (isGenerationTime)
{
falseObservations.clear();
isGenerationTime = false;
for (int i = 1; i <= falsePositiveObservations; ++i)
{
ObjectSensorReading::Observation falseObservation;
falseObservation.observation.x = Utils::randr(worldSizeX);
falseObservation.observation.y = Utils::randr(worldSizeY);
falseObservation.head.x = falseObservation.observation.x;
falseObservation.head.y = falseObservation.observation.y;
falseObservation.model.barycenter = falseObservation.observation.x;
falseObservations.push_back(falseObservation);
}
}
for (vector<ObjectSensorReading::Observation>::const_iterator it = falseObservations.begin(); it != falseObservations.end(); ++it)
{
ObjectSensorReading::Observation falseObservation;
falseObservation.observation.x = it->observation.x + gaussianNoise(gaussianNoiseX,0.3);
falseObservation.observation.y = it->observation.y + gaussianNoise(gaussianNoiseY,0.3);
obs.push_back(falseObservation);
}
}
else
{
initialTimestamp.setToNow();
isBurstTime = false;
}
}
else
{
if ((TIME_TO_WAIT_BEFORE_NEXT_BURST - (Timestamp() - initialTimestamp).getMs()) < 0.0)
{
initialTimestampBurst.setToNow();
isBurstTime = true;
isGenerationTime = true;
}
}
}
if (strcasecmp(distortion.c_str(),"none") != 0)
{
Point2of currentRobotPose;
mutex.lock();
currentRobotPose = robotPose;
mutex.unlock();
/// Adding sensor's distortion. Set this parameter to 'none' for having an ideal sensor.
for (vector<ObjectSensorReading::Observation>::iterator it = obs.begin(); it != obs.end(); ++it)
{
if (strcasecmp(distortion.c_str(),"linear") == 0)
{
float displacementX, displacementY;
displacementX = currentRobotPose.x - it->observation.x;
displacementY = currentRobotPose.y - it->observation.y;
if (sqrt(pow(displacementX,2) + pow(displacementY,2)) > startingDistortionDistance)
{
it->observation.x += (displacementX * 0.1);
it->observation.y += (displacementY * 0.1);
}
}
}
}
}
