/* noise.h -- noisy data
* Copyright (C) 2013 Galois, Inc.
*
* This library is free software: you can redistribute it and/or modify it under
* the terms of the GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option) any later
* version.
*
* This library is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
* details.
*
* You should have received a copy of the GNU General Public License along with
* this library. If not, see .
*
* To contact Galois, complete the Web form at
* or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
* Oregon, 97204-1622. */
#ifdef HAVE_CONFIG_H
# include
#endif
#ifdef HAVE_CXX11_CLOSURES
# include
#endif
#include "measurement.h"
#include "noise.h"
Pose addNoise(const Pose &pose,
GaussianNoiseSource &positionNoise,
GaussianNoiseSource &angleNoise) {
Pose result = pose;
result.x += positionNoise.get();
result.y += positionNoise.get();
result.theta += angleNoise.get();
return result;
}
ControlSignals addNoise(const ControlSignals &signals,
GaussianNoiseSource &speedNoise,
GaussianNoiseSource &steeringAngleNoise) {
ControlSignals result = signals;
result.speed += speedNoise.get();
result.steeringAngle += steeringAngleNoise.get();
return result;
}
LidarDatum addNoise(const LidarDatum &datum,
GaussianNoiseSource &distanceNoise,
GaussianNoiseSource &intensityNoise) {
LidarDatum result = datum;
# ifdef HAVE_CXX11_CLOSURES
std::for_each(result.distance.begin(), result.distance.end(),
[&distanceNoise](float &d) {
d += distanceNoise.get();
});
std::for_each(result.intensity.begin(), result.intensity.end(),
[&intensityNoise](float &i) {
i += intensityNoise.get();
});
# else
for (float &d : result.distance) {
d += distanceNoise.get();
}
for (float &i : result.intensity) {
i += intensityNoise.get();
}
# endif
return result;
}