void SSLVision::parse(SSL_DetectionFrame &pck)
{
// update camera fps
int cid = pck.camera_id();
if(cid == 0) _fpscam0.Pulse();
if(cid == 1) _fpscam1.Pulse();
switch(_camera)
{
case CAMERA_BOTH:
break;
case CAMERA_0:
if (cid==1) return;
break;
case CAMERA_1:
if (cid==0) return;
break;
case CAMERA_NONE:
default:
return;
}
// update vision frame
//_vframe[cid].frame_number = pck.frame_number();
vector<Position> pt;
// Team side Coefficient
float ourSide = (_side == SIDE_RIGHT)? -1.0f : 1.0f;
double time = _time.elapsed(); //pck.t_capture();
// insert balls
int max_balls=min(VOBJ_MAX_NUM, pck.balls_size());
for(int i=0; i<max_balls; ++i)
{
auto b = pck.balls(i);
if(b.has_confidence() && b.has_x() && b.has_y())
if(b.confidence() > MIN_CONF && fabs(b.x()) < FIELD_MAX_X && fabs(b.y()) < FIELD_MAX_Y)
{
Position tp;
tp.loc = Vector2D(b.x()*ourSide, b.y()*ourSide);
pt.push_back(tp);
}
}
_wm->ball.seenAt(pt, time, cid);
if(_color == COLOR_BLUE)
{
APPEND_ROBOTS(blue, our);
APPEND_ROBOTS(yellow, opp);
}
else // _color == COLOR_YELLOW
{
APPEND_ROBOTS(yellow, our);
APPEND_ROBOTS(blue, opp);
}
}
void SoccerView::UpdateRobots ( SSL_DetectionFrame &detection )
{
int robots_blue_n = detection.robots_blue_size();
int robots_yellow_n = detection.robots_yellow_size();
int i,j,yellowj=0,bluej=0;
int team=teamBlue;
SSL_DetectionRobot robot;
for ( i = 0; i < robots_blue_n+robots_yellow_n; i++ )
{
if ( i<robots_blue_n )
{
robot = detection.robots_blue ( i );
team = teamBlue;
j=bluej;
}
else
{
robot = detection.robots_yellow ( i-robots_blue_n );
team = teamYellow;
j=yellowj;
}
double x,y,orientation,conf =robot.confidence();
int id=NA;
if ( robot.has_robot_id() )
id = robot.robot_id();
else
id = NA;
x = robot.x();
y = robot.y();
if ( robot.has_orientation() )
orientation = robot.orientation() *180.0/M_PI;
else
orientation = NAOrientation;
//seek to the next robot of the same camera and team colour
while ( j<robots.size() && ( robots[j]->key!=detection.camera_id() || robots[j]->teamID!=team ) )
j++;
if ( j+1>robots.size() )
AddRobot ( new Robot ( x,y,orientation,team,id,detection.camera_id(),conf ) );
robots[j]->SetPose ( x,y,orientation,conf );
QString label;
if ( id!=NA )
label.setNum ( id,16 );
else
label = "?";
label = label.toUpper();
if ( label!=robots[j]->robotLabel )
robots[j]->robotLabel = label;
j++;
if ( i<robots_blue_n )
bluej=j;
else
yellowj=j;
}
for ( j=bluej; j<robots.size(); j++ )
{
if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamBlue )
robots[j]->conf=0.0;
}
for ( j=yellowj; j<robots.size(); j++ )
{
if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamYellow )
robots[j]->conf=0.0;
}
return;
}