float _g(const Indiv &ind) {
float g = 0.0f;
assert(ind.size() == 30);
for (size_t i = 1; i < 30; ++i)
g += ind.data(i);
g = 9.0f * g / 29.0f;
g += 1.0f;
return g;
}
static void filter_indiv(statinfos_map &filter, Indiv &oInd,
TransformationType fTransf) {
DbValueMap oRes;
DbValueMap &src = const_cast<DbValueMap &>(oInd.data());
typedef std::pair<IntType, StatInfo> MyPair;
std::for_each(filter.begin(), filter.end(), [&](const MyPair &oPair) {
DbValue v;
IntType key = oPair.first;
auto it = src.find(key);
if (it != src.end()) {
DbValue vx = (*it).second;
if (vx.empty()) {
const StatInfo &info = oPair.second;
double vm, vv, vs;
info.get_mean_var_std(vm, vv, vs);
v = DbValue(vm);
}
else {
v = DbValue(vx.double_value());
}
}
else {
const StatInfo &info = oPair.second;
double vm, vv, vs;
info.get_mean_var_std(vm, vv, vs);
v = DbValue(vm);
}
switch (fTransf) {
case TransformationType::normalized:
{
const StatInfo &info = oPair.second;
double vm, vv, vs = 0;
info.get_mean_var_std(vm, vv, vs);
if (vs != 0) {
v = DbValue((v.double_value() - vm) / vs);
}
}
break;
case TransformationType::recoded:
{
const StatInfo &info = oPair.second;
double vmin, vmax;
info.get_min_max(vmin, vmax);
v = DbValue((v.double_value() - vmin) / (vmax - vmin));
}
break;
default:
break;
} // fTransf
oRes[key] = v;
});
oInd.set_data(oRes);
} //filter_indiv
void eval(Indiv& ind) {
this->_objs.resize(3);// resize for div
float f1 = ind.data(0);
float g = _g(ind);
float h = 1.0f - pow((f1 / g), 2.0);
float f2 = g * h;
this->_objs[0] = -f1;
this->_objs[1] = -f2;
this->_v = Eigen::VectorXf(ind.data().size());
for (size_t i = 0; i < this->_v.size(); ++i)
this->_v(i) = ind.data()[i];
}
float dist(const Indiv& ind)
{
cv::Mat tmp(ind.image());
assert(tmp.cols == _image.cols);
assert(tmp.rows == _image.rows);
float total_distance = 0;
// Change specific color of every pixel in the image
for (int x = 0; x < tmp.cols; ++x)
{
for (int y = 0; y < tmp.rows; ++y)
{
cv::Vec3b color_2 = _image.at<cv::Vec3b>(cv::Point(x,y));
//
cv::Vec3b color_1 = tmp.at<cv::Vec3b >(cv::Point(x,y));
// Three color values H, S, L
for (int c = 0; c < 3; c++)
{
float d = color_1[c] - color_2[c];
total_distance += d * d; // Add the square of this distance
}
}
}
return sqrtf(total_distance);
}
void eval(Indiv& ind) {
this->_objs.resize(2);
float f1 = ind.data(0);
float g = _g(ind);
float h = 1.0f - pow((f1 / g), 2.0);
float f2 = g * h;
this->_objs[0] = -f1;
this->_objs[1] = -f2;
}
void eval(const Indiv& ind)
{
if (Params::image::color)
{
// Convert image to BGR before evaluating
cv::Mat output;
// Convert HLS into BGR because imwrite uses BGR color space
cv::cvtColor(ind.image(), output, CV_HLS2BGR);
// Create an empty list to store get 1000 probabilities
_setProbabilityList(output);
}
else // Grayscale
{
// Create an empty list to store get 1000 probabilities
_setProbabilityList(ind.image());
}
}
void eval(Indiv& ind)
{
assert(this == _this);
this->_objs.resize(2);
float f1 = ind.data(0);
float g = _g(ind);
float h = 1.0f - pow((f1 / g), 2.0);
float f2 = g * h;
this->_objs[0] = -f1;
this->_objs[1] = -f2;
this->_value = -f1 -f2;
}
bool NumericIndivProvider::find_indiv(const IntType aIndex,
doubles_vector &data) {
assert(this->is_valid());
data.clear();
Indiv oInd;
VariableMode mode = VariableMode::modeNumeric;
if (!this->find_indiv(aIndex, oInd, mode)) {
return (false);
}
const DbValueMap &oMap = oInd.data();
const ints_vector &vv = this->m_ids;
const size_t n = vv.size();
data.resize(n);
for (size_t i = 0; i < n; ++i) {
const IntType key = vv[i];
auto it = oMap.find(key);
assert(it != oMap.end());
DbValue v = (*it).second;
data[i] = v.double_value();
} // i
return (true);
} //find_indiv
void eval(Indiv& ind)
{
ind.nn().simplify();
nb_coll=0;
speed=0;
lin_speed=0;
stop_eval=false;
#ifdef VERBOSE
std::cout<<"Eval ..."<<std::endl;
#endif
typedef simu::Fastsim<Params> simu_t;
simu_t simu;
assert(simu.map()!=NULL);
// init
init_simu(simu);
ind.nn().init();
#ifdef SAVETRAJ
std::ostringstream straj;
straj<<"# map size "<<simu.map()->get_real_w()<<" "<<simu.map()->get_real_h()<<std::endl;
straj<<"# "<<Params::simu::map_name()<<std::endl;
#endif
time=0;
int success=0;
size_t i;
// *** Main Loop ***
for (i = 0; i < Params::simu::nb_steps && !stop_eval;)
{
// Number of steps the robot is evaluated
time++;
// Update robot info & caracs
simu.refresh();
#ifdef VISU
if (1) {
#elif defined(NO_VISU)
if (0) {
#else
if (this->mode() == fit::mode::view) {
#endif
simu.refresh_view();
#ifdef SAVEBMP
// WARNING: use with caution as it will generate many BMP...
std::ostringstream os;
os<<"img_"<<std::setfill('0')<<std::setw(6)<<time<<".bmp";
std::cout<<"Saving image: "<<os.str()<<std::endl;
if (simu.display().save_BMP(os.str().c_str())!=0) {
std::cerr<<"ERROR, can't save file: "<<os.str()<<std::endl;
}
#endif
}
// Get inputs
get_inputs(simu);
// Step neural network -- outf is the output vector.
step_check(ind.nn());
// move the robot and check for collision and if is still
move_check(simu);
#ifdef SAVETRAJ
straj<<simu.robot().get_pos().get_x()<<" "<<simu.robot().get_pos().get_y()<<" "<<simu.robot().get_pos().theta()<<std::endl;
#endif
if ((simu.robot().get_pos().get_x()>simu.map()->get_real_w()*Params::fitness::min_x)
&&(simu.robot().get_pos().get_x()<simu.map()->get_real_w()*Params::fitness::max_x)
&&(simu.robot().get_pos().get_y()>simu.map()->get_real_h()*Params::fitness::min_y)
&&(simu.robot().get_pos().get_y()<simu.map()->get_real_h()*Params::fitness::max_y)) {
//std::cout<<"The robot has found the goal;"<<std::endl;
success=1;
if (this->mode() != fit::mode::view)
stop_eval=1;
}
#ifdef NOVELTY
if ((i>0)&&(i%(int)(Params::simu::nb_steps/Params::novelty::nb_pos)==0))
pos_bd.push_back(simu.robot().get_pos());
#endif
// loop forever if we are in the visualization mode
if (this->mode() != fit::mode::view)
i++;
}
#ifdef NOVELTY
for (unsigned int j=pos_bd.size();j<Params::novelty::nb_pos;j++)
pos_bd.push_back(simu.robot().get_pos());
#endif
end_pos=simu.robot().get_pos();
Posture goal(simu.map()->get_real_w()*(Params::fitness::min_x+Params::fitness::max_x)/2.0,
simu.map()->get_real_h()*(Params::fitness::min_y+Params::fitness::max_y)/2.0,
0);
// Compute the fitness value
#if defined(DIVERSITY) || defined(NOVELTY)
this->_objs.resize(2);
#endif
#if defined(FITDIST)
this->_objs[0] = -end_pos.dist_to(goal);
this->_value = this->_objs[0] ;
#else
this->_objs[0] = success;
this->_value = success;
#endif
//std::cout<<"End_pos | "<<end_pos.get_x()<<" "<<end_pos.get_y()<<" | "<<end_pos.get_x()/simu.map()->get_real_w()<<" "<<end_pos.get_y()/simu.map()->get_real_h()<<std::endl;
#ifdef VERBOSE
static int nbeval=0;
std::cout<<"fit="<<this->_objs[0]<<" nbeval="<<nbeval<<std::endl;
nbeval++;
#endif
#ifdef SAVETRAJ
traj=straj.str();
#endif
} // *** end of eval ***
float dist(const Indiv& ind) {
return (_v - ind.fit()._v).squaredNorm();
}
void eval(Indiv& ind) {
this->_value = -felli(ind.data());
}
void eval(Indiv& ind)
{
ind.nn().simplify();
nb_coll=0;
speed=0;
lin_speed=0;
stop_eval=false;
#ifdef VERBOSE
std::cout<<"Eval ..."<<std::endl;
#endif
typedef simu::Fastsim<Params> simu_t;
simu_t simu;
assert(simu.map()!=NULL);
// init
init_simu(simu);
ind.nn().init();
time=0;
// *** Main Loop ***
for (size_t i = 0; i < Params::simu::nb_steps && !stop_eval;)
{
// Number of steps the robot is evaluated
time++;
// Update robot info & caracs
simu.refresh();
#ifdef VISU
if (1) {
#elif defined(NO_VISU)
if (0) {
#else
if (this->mode() == fit::mode::view) {
#endif
simu.refresh_view();
#ifdef SAVEBMP
// WARNING: use with caution as it will generate many BMP...
std::ostringstream os;
os<<res_dir<<"/img_"<<std::setfill('0')<<std::setw(6)<<time<<".bmp";
std::cout<<"Saving image: "<<os.str()<<std::endl;
if (simu.display().save_BMP(os.str().c_str())!=0) {
std::cerr<<"ERROR, can't save file: "<<os.str()<<std::endl;
}
#endif
}
// Get inputs
get_inputs(simu);
// Step neural network -- outf is the output vector.
step_check(ind.nn());
// move the robot and check for collision and if is still
move_check(simu);
// loop forever if we are in the visualization mode
if (this->mode() != fit::mode::view)
i++;
}
#if defined(FIT1)
// Compute the fitness value
this->_objs[0] = 1.0/(float)(1+nb_coll);
this->_value = 1.0/(float)(1+nb_coll);
#elif defined(FIT2)
// Compute the fitness value
this->_objs[0] = (speed/(float)Params::simu::nb_steps)*1.0/(float)(1+nb_coll);
this->_value = (speed/(float)Params::simu::nb_steps)*1.0/(float)(1+nb_coll);
#elif defined(FIT3)
// Compute the fitness value
this->_objs[0] = (lin_speed/(float)Params::simu::nb_steps)*1.0/(float)(1+nb_coll);
this->_value = (lin_speed/(float)Params::simu::nb_steps)*1.0/(float)(1+nb_coll);
#endif
#ifdef VERBOSE
static int nbeval=0;
std::cout<<"fit="<<this->_objs[0]<<" nbeval="<<nbeval<<std::endl;
nbeval++;
#endif
} // *** end of eval ***
