コード例 #1
0
// 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();
}
コード例 #2
0
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();
   }
}
コード例 #3
0
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());
}