// draw absolute position covariance ellipse
void FieldPainter::drawAbsCovEllipse(const AbsCoord &pos, QPen variancePen) {
variancePen.setWidth(20);
Eigen::VectorXf eigenvalues = pos.var.block<2, 2>(0,0).marked<Eigen::SelfAdjoint>().eigenvalues();
int r_major_axis = sqrt(MAX(eigenvalues[0], eigenvalues[1]));
int r_minor_axis = sqrt(MIN(eigenvalues[0], eigenvalues[1]));
float theta = atan2(2*pos.var(0,1), pos.var(0,0)-pos.var(1,1))/2.0;
save();
translate(pos.x(), pos.y());
rotate(RAD2DEG(theta));
QRect varRect(-r_major_axis, -r_minor_axis, r_major_axis*2, r_minor_axis*2);
setPen(variancePen);
setBrush(QBrush(Qt::NoBrush));
drawEllipse(varRect);
restore();
}
void FieldPainter::drawRobotAbs (const AbsCoord &pos, QColor colour, bool ellipse, QColor varColour)
{
static const int robot_radius = 120;
QRect roboRect(-robot_radius,-robot_radius,robot_radius*2,robot_radius*2);
QPen variancePen(varColour);
variancePen.setWidth(20);
// draw pacman
save();
translate(pos.x(), pos.y());
setPen("black");
int fov;
if (pos.var(2,2) > 0) {
fov = MIN(160, RAD2DEG(sqrt(pos.var(2,2))));
} else {
fov = 0;
}
setBrush(QBrush(colour));
rotate((RAD2DEG(pos.theta())-(fov/2)));
drawPie(roboRect, 0, (360-fov)*16);
restore();
/* Converting covariance matrix to ellipse representation
* Eigenvalues of the matrix are the major and minor axis lengths
* Eigenvectors are the vectors from the center along the axes (should be perpendicular)
*/
if (ellipse) {
Eigen::VectorXf eigenvalues = pos.var.block<2, 2>(0,0).marked<Eigen::SelfAdjoint>().eigenvalues();
int r_major_axis = MAX(sqrt(MAX(eigenvalues[0], eigenvalues[1])), robot_radius);
int r_minor_axis = MAX(sqrt(MIN(eigenvalues[0], eigenvalues[1])), robot_radius);
float theta = atan2(2*pos.var(0,1), pos.var(0,0)-pos.var(1,1))/2.0;
// draw position covariance ellipse
save();
translate(pos.x(), pos.y());
rotate(RAD2DEG(theta));
QRect varRect(-r_major_axis, -r_minor_axis, r_major_axis*2, r_minor_axis*2);
setPen(variancePen);
setBrush(QBrush(Qt::NoBrush));
drawEllipse(varRect);
restore();
}
}
void VariableView::redraw(NaoData *naoData) {
Frame frame = naoData->getCurrentFrame();
Blackboard *blackboard = (frame.blackboard);
if(!blackboard) return;
// struct RobotPos wrapperPos[4];
// readArray(localisation, robot, wrapperPos);
AbsCoord pos = readFrom(localisation, robotPos);
stringstream spos;
uint32_t ballLostCount = readFrom(localisation, ballLostCount);
AbsCoord ballPos = readFrom(localisation, ballPos);
AbsCoord ballVelRRC = readFrom(localisation, ballVelRRC);
spos << " Robot @ (" << pos.x() << ", " << pos.y() << ")" << endl;
spos << "Heading: " << pos.theta() << " (" << RAD2DEG(pos.theta()) << " deg)" << endl;
spos << "Variance: " << pos.var(0,0) << ", " << pos.var(1,1) << ", " << pos.var(2,2) << endl;
spos << "ballLostCount: " << ballLostCount << endl;
spos << "ball pos; x:" << ballPos.x() << ", y:" << ballPos.y() << endl;
spos << "ball var; xx:" << ballPos.var(0,0) << ", xy:" << ballPos.var(0,1) << endl;
spos << " yx:" << ballPos.var(1,0) << ", yy:" << ballPos.var(1,1) << endl;
spos << "ball vel rrc; x':" << ballVelRRC.x() << ", y':" << ballVelRRC.y() << endl;
spos << "ball vel rrc var; x'x':" << ballVelRRC.var(0,0) << ", x'y':" << ballVelRRC.var(0,1) << endl;
spos << " y'x':" << ballVelRRC.var(1,0) << ", y'y':" << ballVelRRC.var(1,1) << endl;
localisationRobotPos->setText(0, spos.str().c_str());
updateVision(naoData);
updateBehaviour(naoData);
stringstream steam;
steam << "gamecontroller.team_red = " << readFrom(gameController, team_red) << endl;
gameControllerTeam->setText(0, steam.str().c_str());
}
