void MotionRequest::draw() const { DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector if(motion == walk) { switch(walkRequest.mode) { case WalkRequest::targetMode: { LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x(), walkRequest.target.translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0)); CROSS("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), 50, 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0)); Vector2f rotation(500.f, 0.f); rotation.rotate(walkRequest.target.rotation); ARROW("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), walkRequest.target.translation.x() + rotation.x(), walkRequest.target.translation.y() + rotation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127)); break; } case WalkRequest::speedMode: case WalkRequest::percentageSpeedMode: { Vector2f translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f; ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0)); if(walkRequest.target.rotation != 0.0f) { translation.x() = translation.norm(); translation.y() = 0; translation.rotate(walkRequest.speed.rotation); ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127)); } break; } } } }
bool GridStateSpace::stateValid(const Vector2f &pt) const { return pt.x() >= 0 && pt.y() >= 0 && pt.x() < width() && pt.y() < height() && !obstacleAt(gridSquareForState(pt)); }
void RRTWidget::drawTerminalState(QPainter &painter, const Vector2f &pos, const Vector2f &vel, const QColor &color) { // draw point painter.setPen(QPen(color, 6)); QPointF rootLoc(pos.x(), pos.y()); painter.drawEllipse(rootLoc, 2, 2); Vector2f tipOffset = vel * VelocityDrawingMultiplier; Vector2f tipLocVec = pos + tipOffset; QPointF tipLoc(tipLocVec.x(), tipLocVec.y()); // draw arrow shaft painter.setPen(QPen(color, 3)); painter.drawLine(rootLoc, tipLoc); // draw arrow head Vector2f headBase = tipLocVec - tipOffset.normalized()*4; Vector2f perp = Vector2f(-tipOffset.y(), tipOffset.x()).normalized(); Vector2f tipLeftVec = headBase + perp*4; Vector2f tipRightVec = headBase - perp*4; QPointF trianglePts[] = { tipLoc, QPointF(tipLeftVec.x(), tipLeftVec.y()), QPointF(tipRightVec.x(), tipRightVec.y()) }; painter.drawPolygon(trianglePts, 3); }
Vector4f::Vector4f( const Vector2f& xy, const Vector2f& zw ) { m_elements[0] = xy.x(); m_elements[1] = xy.y(); m_elements[2] = zw.x(); m_elements[3] = zw.y(); }
bool GoalFramePerceptor::calcGoalFrame(const Pose2f& prePose, const float yTranslation, GoalFrame& goalFrame) const { static const float borderToGroundLineDistance = theFieldDimensions.xPosOpponentFieldBorder - theFieldDimensions.xPosOpponentGroundline; const Pose2f prePoseInverse(prePose.inverse()); int positive = 0, negative = 0; for(const Vector2f& p : theFieldBoundary.boundaryOnField) { Vector2f pInField = prePoseInverse * p; if(std::abs(std::abs(pInField.x()) - borderToGroundLineDistance) < allowedFieldBoundaryDivergence) { if(pInField.x() > 0) positive++; else negative++; } } if(neededConvexBoundaryPoints > positive && neededConvexBoundaryPoints > negative) return false; const float sign = (positive < negative) ? -1.f : 1.f; goalFrame = Pose2f(prePose).rotate(-pi_2 + sign * pi_2); goalFrame.translation = goalFrame * Vector2f(0.f, (goalFrame.inverse().translation.y() > 0.f ? 1.f : -1.f)).normalized(yTranslation); return true; }
/** Render a string of text at the specified starting poisition. The string encoding * is assumed to be Latin-1 (ISO 8859-1). * * Note that this method will not draw anything on systems with OpenGL ES 2.0 (typically mobile * devices.) For compatibility with all systems, use renderStringToBuffer instead. */ Vector2f TextureFont::render(const string& text, const Vector2f& startPosition) const { Vector2f currentPosition = startPosition; #ifndef VESTA_NO_IMMEDIATE_MODE_3D glBegin(GL_QUADS); for (unsigned int i = 0; i < text.length(); ++i) { // The cast to unsigned char is critical for glyph lookup to work correctly; // otherwise, extended characters will generate negative indices. const Glyph* glyph = lookupGlyph((unsigned char) text[i]); if (glyph) { Vector2f p = currentPosition + glyph->offset; glTexCoord2fv(glyph->textureCoords[0].data()); glVertex2f(p.x(), p.y()); glTexCoord2fv(glyph->textureCoords[1].data()); glVertex2f(p.x() + glyph->size.x(), p.y()); glTexCoord2fv(glyph->textureCoords[2].data()); glVertex2f(p.x() + glyph->size.x(), p.y() + glyph->size.y()); glTexCoord2fv(glyph->textureCoords[3].data()); glVertex2f(p.x(), p.y() + glyph->size.y()); currentPosition.x() += glyph->advance; } } glEnd(); #endif return currentPosition; }
void MandelbrotProcessorCPU::process(Vector2f from, Vector2f to, Image<unsigned short>& iterations) { assert (MAX_ITERATIONS < std::numeric_limits<unsigned short>::max()); assert (iterations.cn == 1); size_t width = iterations.width; size_t height = iterations.height; float x_step = (to.x() - from.x()) / width; float y_step = (to.y() - from.y()) / height; #pragma omp parallel for for (size_t py = 0; py < height; py++) { float y0 = from.y() + y_step * py; for (size_t px = 0; px < width; px++) { float x0 = from.x() + x_step * px; unsigned short iteration; float x = 0.0f; float y = 0.0f; for (iteration = 0; iteration < MAX_ITERATIONS; iteration++) { float xn = x * x - y * y + x0; y = 2 * x * y + y0; x = xn; if (x * x + y * y > INFINITY) { break; } } iterations(py, px) = iteration; } } }
void TeamData::draw() const { DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField"); for(auto const& teammate : teammates) { ColorRGBA posCol; if(teammate.status == Teammate::PLAYING) posCol = ColorRGBA::green; else if(teammate.status == Teammate::FALLEN) posCol = ColorRGBA::yellow; else posCol = ColorRGBA::red; const Vector2f& rPos = teammate.theRobotPose.translation; const float radius = std::max(50.f, teammate.theRobotPose.deviation); Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f); // Circle around Player CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen, posCol, Drawings::noBrush, ColorRGBA::white); // Direction of the Robot LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20, Drawings::solidPen, posCol); // Player number DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number); // Ball position const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position; CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow); } }
void TextEditComponent::onCursorChanged() { if(isMultiline()) { Vector2f textSize = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor); if(mScrollOffset.y() + getTextAreaSize().y() < textSize.y() + mFont->getHeight()) //need to scroll down? { mScrollOffset[1] = textSize.y() - getTextAreaSize().y() + mFont->getHeight(); }else if(mScrollOffset.y() > textSize.y()) //need to scroll up? { mScrollOffset[1] = textSize.y(); } }else{ Vector2f cursorPos = mFont->sizeText(mText.substr(0, mCursor)); if(mScrollOffset.x() + getTextAreaSize().x() < cursorPos.x()) { mScrollOffset[0] = cursorPos.x() - getTextAreaSize().x(); }else if(mScrollOffset.x() > cursorPos.x()) { mScrollOffset[0] = cursorPos.x(); } } }
/** * @brief Draw all the nodes generated in the RRT algorithm. * @param painter */ void RenderArea::drawNodes(QPainter &painter) { painter.save(); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::black); painter.setBrush(QBrush(Qt::black)); Vector2f pos; for(int i = 0; i < (int)rrt->nodes.size(); i++) { for(int j = 0; j < (int)rrt->nodes[i]->children.size(); j++) { pos = rrt->nodes[i]->children[j]->position; painter.drawEllipse(pos.x()-1.5, pos.y()-1.5, 3, 3); } pos = rrt->nodes[i]->position; painter.drawEllipse(pos.x() - NODE_RADIUS, pos.y() - NODE_RADIUS, 2 * NODE_RADIUS, 2 * NODE_RADIUS); } painter.setPen(Qt::red); painter.setBrush(QBrush(Qt::red)); // if a path exists, draw it. for(int i = 0; i < (int)rrt->path.size() - 1; i++) { QPointF p1(rrt->path[i]->position.x(), rrt->path[i]->position.y()); QPointF p2(rrt->path[i+1]->position.x(), rrt->path[i+1]->position.y()); painter.drawLine(p1, p2); } painter.restore(); }
// static Vector3f Vector2f::cross( const Vector2f& v0, const Vector2f& v1 ) { return Vector3f ( 0, 0, v0.x() * v1.y() - v0.y() * v1.x() ); }
void test_autodiff_scalar() { Vector2f p = Vector2f::Random(); typedef AutoDiffScalar<Vector2f> AD; AD ax(p.x(),Vector2f::UnitX()); AD ay(p.y(),Vector2f::UnitY()); AD res = foo<AD>(ax,ay); VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); }
void KickEngineData::calcLegJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const DamageConfigurationBody& theDamageConfigurationBody) { const int sign = joint == Joints::lHipYawPitch ? 1 : -1; const Vector3f& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra]; const Vector3f& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot]; const RotationMatrix rotateBodyTilt = RotationMatrix::aroundX(bodyAngle.x()).rotateY(bodyAngle.y()); Vector3f anklePos = rotateBodyTilt * (footPos - Vector3f(0.f, 0, -theRobotDimensions.footHeight)); anklePos -= Vector3f(0.f, sign * (theRobotDimensions.yHipOffset), 0); //const RotationMatrix zRot = RotationMatrix::aroundZ(footRotAng.z()).rotateX(sign * pi_4); //anklePos = zRot * anklePos; const float leg0 = 0;//std::atan2(-anklePos.x(), anklePos.y()); const float diagonal = anklePos.norm(); // upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength - theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) / (2 * theRobotDimensions.upperLegLength * diagonal); //if(std::abs(a1) > 1.f) OUTPUT_TEXT("clipped a1"); a1 = std::abs(a1) > 1.f ? 0.f : std::acos(a1); float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength + theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) / (2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength); //if(std::abs(a2) > 1.f) OUTPUT_TEXT("clipped a2"); a2 = std::abs(a2) > 1.f ? pi : std::acos(a2); const float leg2 = -a1 - std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm() * -sgn(anklePos.z())); const float leg1 = anklePos.z() == 0.0f ? 0.0f : (std::atan(anklePos.y() / -anklePos.z())); const float leg3 = pi - a2; const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(0).rotateX(bodyAngle.x()).rotateY(bodyAngle.y()); //calculate inverse foot rotation so that they are flat to the ground RotationMatrix footRot = RotationMatrix::aroundX(leg1).rotateY(leg2 + leg3); footRot = footRot.inverse() /* * zRot*/ * rotateBecauseOfHip; //and add additonal foot rotation (which is probably not flat to the ground) const float leg4 = std::atan2(footRot(0, 2), footRot(2, 2)) + footRotAng.y(); const float leg5 = std::asin(-footRot(1, 2)) + footRotAng.x() ; jointRequest.angles[joint] = leg0; jointRequest.angles[joint + 1] = (/*-pi_4 * sign + */leg1); jointRequest.angles[joint + 2] = leg2; jointRequest.angles[joint + 3] = leg3; jointRequest.angles[joint + 4] = leg4; jointRequest.angles[joint + 5] = leg5; //quickhack which allows calibration, but works only if the left foot is the support foot in .kmc file Vector2f tiltCalibration = currentKickRequest.mirror ? theDamageConfigurationBody.startTiltRight : theDamageConfigurationBody.startTiltLeft; if(currentKickRequest.mirror) tiltCalibration.x() *= -1.f; jointRequest.angles[Joints::lAnkleRoll] += tiltCalibration.x(); jointRequest.angles[Joints::lAnklePitch] += tiltCalibration.y(); }
bool BoxFilterFilm::AddSample(const rose::Spectrum &s, const Vector2ui &pixel, const Vector2f &sample) { // Check we are inside pixel boundaries if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; } // Add sample raster[pixel.y() * width + pixel.x()] += s; // Increase number of samples of the pixel num_samples[pixel.y() * width + pixel.x()]++; return true; }
SwingFoot::SwingFoot(bool isLeft, const Vector2f& p, float footHeight, AngDeg ang, AngDeg rotateFoot, float bodyHeight, float duration, Task* primary) :LowerLimbsMotion(duration, primary), mIsLeft(isLeft) { if ( mIsLeft ){ // left foot mDesiredFootL.rotationZ(ang); mDesiredFootL.p() = Vector3f(p.x(),p.y(),footHeight); // right foot mDesiredFootR.identity(); } else{ // right foot mDesiredFootR.rotationZ(ang); mDesiredFootR.p() = Vector3f(p.x(),p.y(),footHeight); // left foot mDesiredFootL.identity(); } mDesiredFootL.p().x() -= HUMANOID.getHalfFeetWidth(); mDesiredFootR.p().x() += HUMANOID.getHalfFeetWidth(); mDesiredFootL.p().z() += HUMANOID.getMinFootHeight(); mDesiredFootR.p().z() += HUMANOID.getMinFootHeight(); // body AngDeg dirR = mDesiredFootR.rotatedAngZ(); AngDeg dirL = mDesiredFootL.rotatedAngZ(); if ( dirR > dirL ){ std::swap(dirR, dirL); } AngDeg dirBody = calBisectorTwoAngles(dirR,dirL); mDesiredBody.rotationX(-10); mDesiredBody.rotateLocalZ( dirBody ); mDesiredBody.pos() = (mDesiredFootL.p()+mDesiredFootR.p())*0.5f; // mDesiredBody.pos().x() = 0; mDesiredBody.pos().z() = bodyHeight; //dynamic step //mDesiredBody.p().y()-=addStepY; if ( p.y()>0 ){ if(mIsLeft){ mDesiredFootL.rotateLocalX(rotateFoot); } else{ mDesiredFootR.rotateLocalX(rotateFoot); } } }
void TeamPlayersLocator::update(TeamPlayersModel& teamPlayersModel) { teamPlayersModel.obstacles.clear(); std::vector<Obstacle> ownTeam; teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST); teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST); teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST); teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST); if(theRobotInfo.penalty == PENALTY_NONE && theGroundContactState.contact) { ownTeam.emplace_back(theRobotPose.covariance.topLeftCorner(2, 2), theRobotPose.translation); // pose and covariance of the robot itself if(theFallDownState.state == theFallDownState.upright) //why is this? for(const auto& obstacle : theObstacleModel.obstacles) { if(obstacle.type == GOALPOST) continue; // if seen robots are of the opponent team and are not detected outside the field const Vector2f p = theRobotPose * obstacle.center; if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder) teamPlayersModel.obstacles.emplace_back(obstacle.covariance, p, obstacle.type); } } for(auto const& teammate : theTeammateData.teammates) { if(teammate.status == Teammate::FULLY_ACTIVE) { if(teammate.pose.deviation < 50.f) //todo, magic number will be replaced by parameter. 50.f might be too low ownTeam.emplace_back(teammate.pose.covariance.topLeftCorner(2, 2), teammate.pose.translation); //position of teammates for(const auto& obstacle : teammate.obstacleModel.obstacles) { if(obstacle.type == GOALPOST) continue; // if seen robots are of the opponent team and are not detected outside the field const Vector2f p = teammate.pose * obstacle.center; if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder) { Matrix2f covariance = (Matrix2f() << obstacle.covXX, obstacle.covXY, obstacle.covXY, obstacle.covYY).finished(); Obstacle converted = Obstacle(rotateCovariance(covariance, teammate.pose.rotation), p, obstacle.type); merge(converted, teamPlayersModel.obstacles); } } } } for(auto& teammate : ownTeam) { removeAround(teammate, teamPlayersModel.obstacles); } }
bool BoxFilterFilmBlock::AddSample(const Spectrum &s, const Vector2ui &global_pixel, const Vector2f &sample) { // Check we are inside pixel boundaries if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; } // Compute local pixel coordinates Vector2ui local_pixel = global_pixel - Vector2ui(width_offset, height_offset); // Add sample raster[local_pixel.y() * width + local_pixel.x()] += s; // Increase number of samples of the pixel num_samples[local_pixel.y() * width + local_pixel.x()]++; return true; }
int PxController::bound(Vector2f &n) { n.x() = -n.x(); if(n.dot(v_) >= 0) { return 1; } v_ += -2*(n.dot(v_))*n; bound_locked_ = true; std::cout << "----bound----- "<< std::endl; std::cout << " n:" << n.x() << "," << n.y() << std::endl; std::cout<< " v:" << v_.x() << "," << v_.y() << std::endl; return 0; }
void GlobalFieldCoverageProvider::setCoverageAtFieldPosition(GlobalFieldCoverage& globalFieldCoverage, const Vector2f& positionOnField, const int coverage) const { if(theFieldDimensions.isInsideField(positionOnField)) { ASSERT(std::isfinite(positionOnField.x())); ASSERT(std::isfinite(positionOnField.y())); const int x = static_cast<int>((positionOnField.x() - theFieldDimensions.xPosOwnGroundline) / cellLengthX); const int y = static_cast<int>((positionOnField.y() - theFieldDimensions.yPosRightSideline) / cellLengthY); globalFieldCoverage.grid[y * numOfCellsX + x].timestamp = theFrameInfo.time; globalFieldCoverage.grid[y * numOfCellsX + x].coverage = coverage; } }
LIPStateEstimator::SupportFoot LIPStateEstimator::guessSupportFoot(const Vector2f& leftOrigin) const { const Pose3f leftOriginToTorso = theRobotModel.soleLeft + Vector3f(leftOrigin.x(), leftOrigin.y(), 0.f); const Pose3f rightOriginToTorso = theRobotModel.soleRight + Vector3f(leftOrigin.x(), -leftOrigin.y(), 0.f); const Pose3f torsoToWorld(theInertialData.orientation2D); const Pose3f leftOriginToWorld = torsoToWorld * leftOriginToTorso; const Pose3f rightOriginToWorld = torsoToWorld * rightOriginToTorso; if(leftOriginToWorld.translation.z() < rightOriginToWorld.translation.z()) return SupportFoot::left; else return SupportFoot::right; }
void TeamData::draw() const { DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField"); for(auto const& teammate : teammates) { ColorRGBA posCol; if(teammate.status == Teammate::PLAYING) posCol = ColorRGBA::green; else if(teammate.status == Teammate::FALLEN) posCol = ColorRGBA::yellow; else posCol = ColorRGBA::red; const Vector2f& rPos = teammate.theRobotPose.translation; const float radius = std::max(50.f, teammate.theRobotPose.deviation); Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f); // Circle around Player CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen, posCol, Drawings::noBrush, ColorRGBA::white); // Direction of the Robot LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20, Drawings::solidPen, posCol); // Player number DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number); // Role DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 150, 100, ColorRGBA::black, Role::getName(teammate.theBehaviorStatus.role)); // Time to reach ball int ttrb = teammate.theBehaviorStatus.role == Role::striker ? static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBallStriker) : static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBall); if(Blackboard::getInstance().exists("FrameInfo")) { const FrameInfo& theFrameInfo = (const FrameInfo&) Blackboard::getInstance()["FrameInfo"]; ttrb = theFrameInfo.getTimeSince(ttrb); } DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 300, 100, ColorRGBA::black, "TTRB: " << ttrb); //Line from Robot to WalkTarget LINE("representation:TeamData", rPos.x(), rPos.y(), teammate.theSPLStandardBehaviorStatus.walkingTo.x(), teammate.theSPLStandardBehaviorStatus.walkingTo.y(), 10, Drawings::dashedPen, ColorRGBA::magenta); // Ball position const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position; CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow); } }
void UKFPose2D::lineSensorUpdate(bool vertical, const Vector2f& reading, const Matrix2f& readingCov) { generateSigmaPoints(); // computeLineReadings Vector2f lineReadings[7]; if(vertical) for(int i = 0; i < 7; ++i) lineReadings[i] = Vector2f(sigmaPoints[i].y(), sigmaPoints[i].z()); else for(int i = 0; i < 7; ++i) lineReadings[i] = Vector2f(sigmaPoints[i].x(), sigmaPoints[i].z()); // computeMeanOfLineReadings Vector2f lineReadingMean = lineReadings[0]; for(int i = 1; i < 7; ++i) lineReadingMean += lineReadings[i]; lineReadingMean *= 1.f / 7.f; // computeCovOfLineReadingsAndSigmaPoints Matrix2x3f lineReadingAndMeanCov = Matrix2x3f::Zero(); for(int i = 0; i < 3; ++i) { const Vector2f d1 = lineReadings[i * 2 + 1] - lineReadingMean; lineReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished(); const Vector2f d2 = lineReadings[i * 2 + 2] - lineReadingMean; lineReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished(); } lineReadingAndMeanCov *= 0.5f; // computeCovOfLineReadingsReadings const Vector2f d = lineReadings[0] - lineReadingMean; Matrix2f lineReadingCov = (Matrix2f() << d * d.x(), d * d.y()).finished(); for(int i = 1; i < 7; ++i) { const Vector2f d = lineReadings[i] - lineReadingMean; lineReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished(); } lineReadingCov *= 0.5f; lineReadingMean.y() = Angle::normalize(lineReadingMean.y()); const Matrix3x2f kalmanGain = lineReadingAndMeanCov.transpose() * (lineReadingCov + readingCov).inverse(); Vector2f innovation = reading - lineReadingMean; innovation.y() = Angle::normalize(innovation.y()); const Vector3f correction = kalmanGain * innovation; mean += correction; mean.z() = Angle::normalize(mean.z()); cov -= kalmanGain * lineReadingAndMeanCov; Covariance::fixCovariance(cov); }
inline Vector2f VectorLocalization2D::observationFunction(line2f l, Vector2f p) const { static const bool debug = false; Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1())); float location = (p-p0).dot(dir); attraction = (p-p0) - dir*location ; if(debug){ debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) ); const float crossSize = 0.002; debugLines.push_back( line2f( vector2f(p.x()+crossSize,p.y()) , vector2f(p.x()-crossSize,p.y())) ); debugLines.push_back( line2f( vector2f(p.x(),p.y()+crossSize) , vector2f(p.x(),p.y()-crossSize)) ); } return attraction; }
void UKFPose2D::landmarkSensorUpdate(const Vector2f& landmarkPosition, const Vector2f& reading, const Matrix2f& readingCov) { generateSigmaPoints(); // computeLandmarkReadings Vector2f landmarkReadings[7]; for(int i = 0; i < 7; ++i) { Pose2f pose(sigmaPoints[i].z(), sigmaPoints[i].head<2>()); Vector2f landmarkPosRel = pose.invert() * landmarkPosition; // TODO: optimize this landmarkReadings[i] = Vector2f(landmarkPosRel.x(), landmarkPosRel.y()); } // computeMeanOfLandmarkReadings Vector2f landmarkReadingMean = landmarkReadings[0]; for(int i = 1; i < 7; ++i) landmarkReadingMean += landmarkReadings[i]; landmarkReadingMean *= 1.f / 7.f; // computeCovOfLandmarkReadingsAndSigmaPoints Matrix2x3f landmarkReadingAndMeanCov = Matrix2x3f::Zero(); for(int i = 0; i < 3; ++i) { const Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean; landmarkReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished(); const Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean; landmarkReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished(); } landmarkReadingAndMeanCov *= 0.5f; // computeCovOfLandmarkReadingsReadings const Vector2f d = landmarkReadings[0] - landmarkReadingMean; Matrix2f landmarkReadingCov = Matrix2f::Zero(); landmarkReadingCov << d * d.x(), d * d.y(); for(int i = 1; i < 7; ++i) { const Vector2f d = landmarkReadings[i] - landmarkReadingMean; landmarkReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished(); } landmarkReadingCov *= 0.5f; const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).inverse(); Vector2f innovation = reading - landmarkReadingMean; const Vector3f correction = kalmanGain * innovation; mean += correction; mean.z() = Angle::normalize(mean.z()); cov -= kalmanGain * landmarkReadingAndMeanCov; Covariance::fixCovariance(cov); }
void Button::autosize() { myLabel.autosize(); myImage.autosize(); // HACK: we add some default margin to the label. Vector2f size = Vector2f::Zero(); if(myTextEnabled) { size = myLabel.getSize(); } size[0] += myImage.getSize()[0]; size[1] = max(size[1], myImage.getSize()[1]); myLabel.setHeight(size[1]); // Commented: avoid stretching the image. //myImage.setHeight(size[1]); if(myCheckable) { size += Vector2f(size[1] + 10, 4); } else { size += Vector2f(10, 4); } if(isImageEnabled()) { size.x() += myImage.getSize().x(); } setSize(size); }
Vector4f::Vector4f( float x, float y, const Vector2f& zw ) { m_elements[0] = x; m_elements[1] = y; m_elements[2] = zw.x(); m_elements[3] = zw.y(); }
Vector4f::Vector4f( float x, const Vector2f& yz, float w ) { m_elements[0] = x; m_elements[1] = yz.x(); m_elements[2] = yz.y(); m_elements[3] = w; }
Vector4f::Vector4f( const Vector2f& xy, float z, float w ) { m_elements[0] = xy.x(); m_elements[1] = xy.y(); m_elements[2] = z; m_elements[3] = w; }
void ManualHeadMotionProvider::update(HeadMotionRequest& headMotionRequest) { bool parametersChanged = xImg != currentX || yImg != currentY; if(parametersChanged && camera == theCameraInfo.camera) { currentX = xImg; currentY = yImg; Vector2f targetOnField; if(Transformation::imageToRobot(currentX, currentY, theCameraMatrix, theCameraInfo, targetOnField)) { headMotionRequest.target.x() = targetOnField.x(); headMotionRequest.target.y() = targetOnField.y(); headMotionRequest.target.z() = 0; headMotionRequest.mode = HeadMotionRequest::targetOnGroundMode; headMotionRequest.watchField = false; //Use the camera that the user is seeing right now. switch(camera) { case CameraInfo::lower: headMotionRequest.cameraControlMode = HeadMotionRequest::lowerCamera; break; case CameraInfo::upper: headMotionRequest.cameraControlMode = HeadMotionRequest::upperCamera; break; default: ASSERT(false); } headMotionRequest.speed = 150_deg; } } }
void SideConfidenceProvider::findAgreemates() { // If I have not seen the ball recently, no matches can be found anyway ... if(!ballModelCanBeUsed(theBallModel, theWorldModelPrediction.robotPose)) return; const Vector2f myGlobalBallPosition = theWorldModelPrediction.robotPose * theBallModel.estimate.position; const Vector2f myGlobalBallPositionMirrored(-myGlobalBallPosition.x(), -myGlobalBallPosition.y()); // Check all teammates: for(const auto& teammate : theTeamData.teammates) { // Robots that are not playing are of no interest if(teammate.status != Teammate::PLAYING) continue; // Only trust B-Human robots if(teammate.mateType != Teammate::TeamOrigin::BHumanRobot) continue; if(ballModelCanBeUsed(teammate.theBallModel, teammate.theRobotPose)) { const Vector2f teammateGlobalBallPosition = teammate.theRobotPose * teammate.theBallModel.estimate.position; const TeammateBallCompatibility bc = compareBallWithTeammateBall(myGlobalBallPosition, myGlobalBallPositionMirrored, teammateGlobalBallPosition); addToAgreemateList(teammate, bc); } } }