void ObservationGenerator::generateGroundTruthObservations(){
obs_object_->reset();
for (int i = 0; i <= WO_OPPONENT_LAST; i++){
OpponentModel
&cmodel = opponent_mem_->locModels[i - WO_OPPONENT_FIRST],
&pmodel = opponent_mem_->locModels[i - 1];
if (team_ == TEAM_BLUE){
obs_object_->objects_[i].loc = gt_object_->objects_[i].loc;
obs_object_->objects_[i].orientation = gt_object_->objects_[i].orientation;
obs_object_->objects_[i].absVel = gt_object_->objects_[i].absVel;
if (i >= WO_OPPONENT_FIRST){
// fill in opponent mem
cmodel.alpha = 1;
cmodel.X00 = gt_object_->objects_[i].loc.x / 10.0;
cmodel.X10 = gt_object_->objects_[i].loc.y / 10.0;
cmodel.P00 = cmodel.P11 = 10.0;
}
} else {
// have to swap players and opponent indices for red team
if (i == 0){
obs_object_->objects_[i].loc = -gt_object_->objects_[i].loc;
obs_object_->objects_[i].orientation = normalizeAngle(gt_object_->objects_[i].orientation + M_PI);
obs_object_->objects_[i].absVel = -gt_object_->objects_[i].absVel;
} else if (i <= WO_TEAM_LAST){
obs_object_->objects_[i+WO_TEAM_LAST].loc = -gt_object_->objects_[i].loc;
obs_object_->objects_[i+WO_TEAM_LAST].orientation = normalizeAngle(gt_object_->objects_[i].orientation + M_PI);
obs_object_->objects_[i+WO_TEAM_LAST].absVel = -gt_object_->objects_[i].absVel;
// fill in opponent mem
pmodel.alpha = 1.0;
pmodel.X00 = -gt_object_->objects_[i].loc.x / 10.0;
pmodel.X10 = -gt_object_->objects_[i].loc.y / 10.0;
pmodel.P00 = pmodel.P11 = 10.0;
} else {
obs_object_->objects_[i-WO_TEAM_LAST].loc = -gt_object_->objects_[i].loc;
obs_object_->objects_[i-WO_TEAM_LAST].orientation = normalizeAngle(gt_object_->objects_[i].orientation + M_PI);
obs_object_->objects_[i-WO_TEAM_LAST].absVel = -gt_object_->objects_[i].absVel;
}
}
} // copy all players and ball into our memory
opponent_mem_->syncModels();
WorldObject* oball = &(obs_object_->objects_[WO_BALL]);
WorldObject* orobot = &(obs_object_->objects_[player_]);
auto gtball = gt_object_->objects_[WO_BALL];
auto gtrobot = gt_object_->objects_[player_];
if(team_ == TEAM_RED) {
gtrobot.loc *= -1;
gtrobot.orientation = normalizeAngle(gtrobot.orientation + M_PI);
gtball.loc *= -1;
gtball.orientation = normalizeAngle(gtball.orientation + M_PI);
gtball.absVel *= -1;
}
orobot->sdOrientation = 25.0*DEG_T_RAD;
// update ball relative velocity from absolute
// have to make a copy first because rotate rotates the actual point
oball->relVel = gtball.absVel;
oball->relVel.rotate(-gtrobot.orientation);
oball->relPos = gtball.loc;
oball->relPos = oball->relPos.globalToRelative(gtrobot.loc, gtrobot.orientation);
oball->relOrientation = gtrobot.loc.getBearingTo(gtball.loc, gtrobot.orientation);
oball->sd.x = oball->sd.y = 0;
for (int i = 0; i < NUM_WORLD_OBJS; i++){
if (i == WO_ROBOT_CLUSTER)
continue;
WorldObject* wo = &(obs_object_->objects_[i]);
WorldObject* gto = >_object_->objects_[i];
// calculate distance and bearing to each object
wo->distance = gtrobot.loc.getDistanceTo(obs_object_->objects_[i].loc);
wo->bearing = gtrobot.loc.getBearingTo(obs_object_->objects_[i].loc,
gtrobot.orientation);
// decide if seen depending on pan
if (fabs(joint_->values_[HeadPan] - wo->bearing) < FOVx/2.0){
if(wo->isUnknown()) continue;
gto->seen = wo->seen = true;
gto->frameLastSeen = wo->frameLastSeen = frame_info_->frame_id;
float diff = joint_->values_[HeadPan] - wo->bearing;
wo->imageCenterX = iparams_.width/2.0 + (diff / (FOVx/2.0) * iparams_.width/2.0);
wo->imageCenterY = iparams_.height/2.0;
gto->visionDistance = wo->visionDistance = wo->distance;
gto->visionBearing = wo->visionBearing = wo->bearing;
} else gto->seen = wo->seen = false;
}
fillObservationObjects();
}
