RobotPose MotionModelScanMatcher::sampleMotion(RobotPose pose) {
RobotPose sampled_pose = pose;
sampled_pose.x += getGaussianRand(0.0, cov[0]);
sampled_pose.y += getGaussianRand(0.0, cov[4]);
double unnormalized_theta += getGaussianRand(0.0, cov[8]);
sampled_pose.theta = fmod(unnormalized_theta, 2.0*M_PI); // keep within range
if (sampled_pose.theta == unnormalized_theta) {
ROS_INFO("unnormalized_theta=\t%f, sampled_pose.theta=\t%f", unnormalized_theta, sampled_pose.theta);
}
return sampled_pose;
}
void StrongCausalityExperimentAgentObserver::mutateWithBouncingBounds()
{
_wm->_genome.clear();
for ( int i = 0 ; i != 18 ; i++ )
{
double value = _storedGenome[i] + getGaussianRand(0,_sigma);
// bouncing upper/lower bounds
if ( value < _minValue )
{
double range = _maxValue - _minValue;
double overflow = - ( (double)value - _minValue );
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _minValue + overflow;
else // overflow btw range and range*2
value = _minValue + range - (overflow-range);
}
else
if ( value > _maxValue )
{
double range = _maxValue - _minValue;
double overflow = (double)value - _maxValue;
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _maxValue - overflow;
else // overflow btw range and range*2
value = _maxValue - range + (overflow-range);
}
_wm->_genome.push_back(value);
}
_wm->setNewGenomeStatus(true);
}
void MedeaSpAgentObserver::mutateWithBouncingBounds( float sigma)
{
_wm->_genome.clear();
if ( MedeaSpSharedData::gDynamicSigma == true)
{
_wm->_currentSigma = sigma;
}
else
{
float randValue = float(rand() %100) / 100.0;
if ( ( randValue >= 0.0 ) && ( randValue < MedeaSpSharedData::gProbRef) )
{
_wm->_currentSigma = MedeaSpSharedData::gSigmaRef;
}
else if ( ( randValue >= MedeaSpSharedData::gProbRef ) && ( randValue < MedeaSpSharedData::gProbMax+MedeaSpSharedData::gProbRef) )
{
_wm->_currentSigma = MedeaSpSharedData::gSigmaMax;
}
else
{
std::cerr << "Error : In SwarmOnlineObserver, the rand value is out of bound. The sum of MedeaSpSharedData::gProbRef and MedeaSpSharedData::gProbMax is probably not equal to one" << std::endl;
exit(1);
}
}
for (unsigned int i = 0 ; i != _wm->_currentGenome.size() ; i++ )
{
double value = _wm->_currentGenome[i] + getGaussianRand(0,_wm->_currentSigma);
// bouncing upper/lower bounds
if ( value < _wm->_minValue )
{
double range = _wm->_maxValue - _wm->_minValue;
double overflow = - ( (double)value - _wm->_minValue );
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _wm->_minValue + overflow;
else // overflow btw range and range*2
value = _wm->_minValue + range - (overflow-range);
}
else
if ( value > _wm->_maxValue )
{
double range = _wm->_maxValue - _wm->_minValue;
double overflow = (double)value - _wm->_maxValue;
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _wm->_maxValue - overflow;
else // overflow btw range and range*2
value = _wm->_maxValue - range + (overflow-range);
}
_wm->_genome.push_back(value);
}
_wm->_currentGenome = _wm->_genome;
}
void BasicProjectMedeaAgentObserver::mutateWithBouncingBounds( float sigma)
{
_wm->_genome.clear();
if ( BasicProjectMedeaSharedData::gDynamicSigma == true)
{
_wm->_currentSigma = sigma;
}
else
{
float randValue = float(rand() % 100) / 100.0;
if ( ( randValue >= 0.0 ) && ( randValue < BasicProjectMedeaSharedData::gProbRef) )
{
_wm->_currentSigma = BasicProjectMedeaSharedData::gSigmaRef;
}
else if ( ( randValue >= BasicProjectMedeaSharedData::gProbRef ) && ( randValue < BasicProjectMedeaSharedData::gProbMax+BasicProjectMedeaSharedData::gProbRef) )
{
_wm->_currentSigma = BasicProjectMedeaSharedData::gSigmaMax;
}
else
{
std::cerr << "Error : In SwarmOnlineObserver, the rand value is out of bound. The sum of BasicProjectMedeaSharedData::gProbRef and BasicProjectMedeaSharedData::gProbMax is probably not equal to one" << std::endl;
exit(1);
}
}
for (unsigned int i = 0 ; i != _wm->_currentGenome.size() ; i++ )
{
double value = _wm->_currentGenome[i] + getGaussianRand(0,_wm->_currentSigma);
// bouncing upper/lower bounds
if ( value < _wm->_minValue )
{
double range = _wm->_maxValue - _wm->_minValue;
double overflow = - ( (double)value - _wm->_minValue );
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _wm->_minValue + overflow;
else // overflow btw range and range*2
value = _wm->_minValue + range - (overflow-range);
}
else if ( value > _wm->_maxValue )
{
double range = _wm->_maxValue - _wm->_minValue;
double overflow = (double)value - _wm->_maxValue;
overflow = overflow - 2*range * (int)( overflow / (2*range) );
if ( overflow < range )
value = _wm->_maxValue - overflow;
else // overflow btw range and range*2
value = _wm->_maxValue - range + (overflow-range);
}
_wm->_genome.push_back(value);
}
_wm->_currentGenome = _wm->_genome;
gLogFile << std::fixed << std::showpoint << "Info(" << gWorld->getIterations() << ") : robot nb." << _wm->_agentId << " has mutate the genome with sigma : " << _wm->_currentSigma << std::endl;
}