TEST(Tree, ASC) {
// test adaptive stepsize control
Tree<Vector2f>* tree =
TreeFor2dPlane(make_shared<GridStateSpace>(50, 50, 50, 50),
Vector2f(40, 40), // goal point
5); // step size
// give it plenty of iterations so it's not likely to fail
const int maxIterations = 10000;
tree->setMaxIterations(maxIterations);
tree->setGoalMaxDist(5);
tree->setMaxStepSize(10);
tree->setASCEnabled(true);
tree->setStartState(Vector2f(10, 10));
bool success = tree->run(); // run with the given starting point
ASSERT_TRUE(success);
vector<Vector2f> path;
tree->getPath(path, tree->lastNode(), true);
// Check to see if the nodes in the tree have uniform stepsize or varied.
// Stepsizes should vary
bool varied = false;
for (int i = 1; !varied && i < path.size() - 2; i++) {
Vector2f pathA = path[i] - path[i - 1];
Vector2f pathB = path[i] - path[i + 1];
float n = pathA.norm() / pathB.norm();
if (n < 0.99 || n > 1.01) varied = true;
}
ASSERT_TRUE(varied);
}
SideConfidenceProvider::SideConfidenceProvider()
: lost(false),
timeOfLastFall(0),
lastTimeWithoutArmContact(0),
timeOfLastBallObservation(0),
timeOfLastTeamBallObservation(0)
{
const Vector2f maxPos(theFieldDimensions.xPosOpponentFieldBorder, theFieldDimensions.yPosLeftFieldBorder);
maxDistanceToFieldCenterForArmConsideration = maxPos.norm() / 2.f;
maxDistanceToFieldCenterForFallDownConsideration = maxPos.norm() / 3.f;
}
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;
}
}
}
}
Vector2f PlaneStateSpace::intermediateState(const Vector2f &source, const Vector2f &target, float stepSize) const {
Vector2f delta = target - source;
delta = delta / delta.norm(); // unit vector
Vector2f val = source + delta * stepSize;
return val;
}
void AutomaticCameraCalibrator::accumulate()
{
if(theCameraInfo.camera != currentCamera)
return;
auto isInsideImage = [&](const Vector2i & point)
{
return point.x() >= 0 && point.x() < theCameraInfo.width
&& point.y() >= 0 && point.y() < theCameraInfo.height;
};
std::vector<Sample> pointsOnALine;
for(const LinesPercept::Line& line : theLinesPercept.lines)
{
for(const Vector2i& point : line.spotsInImg)
{
// store all necessary information in the sample
Vector2f pointOnField;
if(!isInsideImage(point))
continue;
else if(!Transformation::imageToRobot(point.x(), point.y(), theCameraMatrix, theCameraInfo, pointOnField))
{
OUTPUT_TEXT("MEEEK! Point not on field!" << (theCameraInfo.camera == CameraInfo::upper ? " Upper " : " Lower "));
continue;
}
Sample sample;
sample.pointInImage = point;
sample.pointOnField = pointOnField;
sample.torsoMatrix = theTorsoMatrix;
sample.headYaw = theJointAngles.angles[Joints::headYaw];
sample.headPitch = theJointAngles.angles[Joints::headPitch];
sample.cameraInfo = theCameraInfo;
bool sufficientDistanceToOtherSamples = true;
for(const Sample& testSample : pointsOnALine)
{
Vector2f difference = sample.pointOnField - testSample.pointOnField;
if(testSample.cameraInfo.camera != sample.cameraInfo.camera)
continue;
if(difference.norm() < minimumSampleDistance)
sufficientDistanceToOtherSamples = false;
}
if(sufficientDistanceToOtherSamples)
{
samples.push_back(sample);
pointsOnALine.push_back(sample);
}
}
}
accumulationTimestamp = Time::getCurrentSystemTime();
state = WaitForUser;
}
bool SideConfidenceProvider::ballModelCanBeUsed(const BallModel& ballModel, const Pose2f& robotPose) const
{
// Is the ball model still "fresh"?
if(theFrameInfo.getTimeSince(ballModel.timeWhenLastSeen) > maxBallAge)
return false;
// We do not want to consider fast rolling balls for the SideConfidence:
if(ballModel.estimate.velocity.norm() > maxBallVelocity)
return false;
// Close to the field center, the ball information does not help:
const Vector2f globalPos = robotPose * ballModel.estimate.position;
if(globalPos.norm() < centerBanZoneRadius)
return false;
// As the perception of the new ball might be unreliable, we only consider stuff away from lines:
if(ballIsCloseToPenaltyMarkOrLine(globalPos))
return false;
return true;
}
void BoundaryField::setImage(QImage const & image)
{
m_image = image;
m_sumField.clear();
if (m_image.isNull())
{
return;
}
QPainter painter;
painter.begin(&m_image);
QPen pen;
pen.setColor(QColor(100, 100, 255));
painter.setPen(pen);
border::Regions regions;
border::findRegions(regions, image);
foreach (border::Boundary const & boundary, regions)
{
foreach (border::BoundarySegment const & segment, boundary)
{
QPoint a = segment[0];
Vector2f af = m_image.toFieldCoords(a);
foreach (QPoint b, segment)
{
Vector2f bf = m_image.toFieldCoords(b);
Vector2f d = bf - af;
if (d.norm() > 0.02)
{
m_sumField += new BasisField(af, 1.0f, d);
painter.drawLine(a,b);
a = b;
af = m_image.toFieldCoords(a);
}
}
// static
bool GeometryUtils::lineLineSegmentIntersection( const Vector2f& p, const Vector2f& dir,
const Vector2f& p1, const Vector2f& p2, Vector2f &outIntersection)
{
Vector2f segDir = (p2 - p1);
float segDirLen = segDir.norm();
if(segDirLen < EPSILON)
return false;
segDir = segDir / segDirLen;
Vector2f dir90 = { -dir[1], dir[0] };
float dirCross = Vector2f::dot(segDir, dir90);
if(abs(dirCross) < EPSILON)
return false;
float param = Vector2f::dot(p - p1, dir90) / dirCross;
if(param < 0.f || param > segDirLen)
return false;
outIntersection = p1 + param * segDir;
return true;
}
Vector2f GridStateSpace::intermediateState(const Vector2f &source, const Vector2f &target, float minStepSize, float maxStepSize) const {
bool debug;
Vector2f delta = target - source;
delta = delta / delta.norm(); // unit vector
float dist = _obstacleGrid.nearestObstacleDist(source, maxStepSize * 2);
float stepSize = (dist / maxStepSize) * minStepSize; // scale based on how far we are from obstacles
if (stepSize > maxStepSize) stepSize = maxStepSize;
if (stepSize < minStepSize) stepSize = minStepSize;
if (debug) {
cout << "ASC intermediateState" << endl;
cout << " stepsize: " << minStepSize << endl;
cout << " nearest obs dist: " << dist << endl;
cout << " maximum stepsize: " << maxStepSize << endl;
cout << " new step: " << stepSize << endl;
}
Vector2f val = source + delta * stepSize;
return val;
}
void MeshRenderer::draw2D(
cl::BufferGL PBO,
uint width,
uint height,
Eigen::Transform<float, 3, Eigen::Affine> pixelToViewportTransform,
float PBOspacing,
Vector2f translation
) {
boost::lock_guard<boost::mutex> lock(mMutex);
OpenCLDevice::pointer device = getMainDevice();
cl::CommandQueue queue = device->getCommandQueue();
std::vector<cl::Memory> v;
v.push_back(PBO);
queue.enqueueAcquireGLObjects(&v);
// Map would probably be better here, but doesn't work on NVIDIA, segfault surprise!
//float* pixels = (float*)queue.enqueueMapBuffer(PBO, CL_TRUE, CL_MAP_WRITE, 0, width*height*sizeof(float)*4);
boost::shared_array<float> pixels(new float[width*height*sizeof(float)*4]);
queue.enqueueReadBuffer(PBO, CL_TRUE, 0, width*height*4*sizeof(float), pixels.get());
boost::unordered_map<uint, Mesh::pointer>::iterator it;
for(it = mMeshToRender.begin(); it != mMeshToRender.end(); it++) {
Mesh::pointer mesh = it->second;
if(mesh->getDimensions() != 2) // Mesh must be 2D
continue;
Color color = mDefaultColor;
ProcessObjectPort port = getInputPort(it->first);
if(mInputColors.count(port) > 0) {
color = mInputColors[port];
}
MeshAccess::pointer access = mesh->getMeshAccess(ACCESS_READ);
std::vector<VectorXui> lines = access->getLines();
std::vector<MeshVertex> vertices = access->getVertices();
// Draw each line
for(int i = 0; i < lines.size(); ++i) {
Vector2ui line = lines[i];
Vector2f a = vertices[line.x()].getPosition();
Vector2f b = vertices[line.y()].getPosition();
Vector2f direction = b - a;
float lengthInPixels = ceil(direction.norm() / PBOspacing);
// Draw the line
for(int j = 0; j <= lengthInPixels; ++j) {
Vector2f positionInMM = a + direction*((float)j/lengthInPixels);
Vector2f positionInPixels = positionInMM / PBOspacing;
int x = round(positionInPixels.x());
int y = round(positionInPixels.y());
y = height - 1 - y;
if(x < 0 || y < 0 || x >= width || y >= height)
continue;
pixels[4*(x + y*width)] = color.getRedValue();
pixels[4*(x + y*width) + 1] = color.getGreenValue();
pixels[4*(x + y*width) + 2] = color.getBlueValue();
}
}
}
//queue.enqueueUnmapMemObject(PBO, pixels);
queue.enqueueWriteBuffer(PBO, CL_TRUE, 0, width*height*4*sizeof(float), pixels.get());
queue.enqueueReleaseGLObjects(&v);
}
bool PenaltyMarkPerceptor::isPenaltyMark(PenaltyMarkPercept& pp)
{
const int height = theImage.height - 3;
int horizon = std::max(2, static_cast<int>(theImageCoordinateSystem.origin.y()));
horizon = std::min(horizon, height);
Vector2f relativePosition;
if(!Transformation::imageToRobot(pp.position.x(), pp.position.y(), theCameraMatrix, theCameraInfo, relativePosition))
return false;
int maxWidth = static_cast<int>(Geometry::getSizeByDistance(theCameraInfo, theFieldDimensions.penaltyMarkSize, relativePosition.norm()));
//s DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", 60, 60, 7, ColorRGBA::black, std::to_string(maxWidth));
const int leftLimit = 2;
const int whiteSkipping = 5;
const int rightLimit = theImage.width - 3;
int skipped = 0;
int lower, upper;
int left = 0;
int right = 0;
left = right = pp.position.x();
skipped = 0;
while(left >= leftLimit && skipped < whiteSkipping)
{
if(std::abs(right - (left + skipped + 1)) > maxWidth * scaledToleratedSizeDeviation)
{
return false;
}
if(theColorTable[theImage[pp.position.y()][left]].is(ColorClasses::white))
skipped = 0;
else
skipped++;
left--;
}
left += skipped + 1;
skipped = 0;
while(right <= rightLimit && skipped < whiteSkipping)
{
if(std::abs(left - (right - skipped)) > maxWidth * scaledToleratedSizeDeviation)
{
return false;
}
if(theColorTable[theImage[pp.position.y()][right]].is(ColorClasses::white))
skipped = 0;
else
skipped++;
right++;
}
right -= skipped;
pp.position.x() = (left + right) / 2;
// find upper/lower => middle vertical
lower = upper = pp.position.y();
skipped = 0;
while(lower <= height && skipped < whiteSkipping)
{
if(std::abs(upper - (lower - skipped)) > maxWidth * scaledToleratedSizeDeviation)
{
return false;
}
if(theColorTable[theImage[lower][pp.position.x()]].is(ColorClasses::white))
{
skipped = 0;
}
else
{
skipped++;
}
lower++;
}
lower -= skipped;
skipped = 0;
while(upper >= horizon && skipped < whiteSkipping)
{
if(std::abs(lower - (upper + skipped + 1)) > maxWidth * scaledToleratedSizeDeviation)
{
return false;
}
if(theColorTable[theImage[upper][pp.position.x()]].is(ColorClasses::white))
skipped = 0;
else
skipped++;
upper--;
}
upper += skipped + 1;
pp.position.y() = (lower + upper) / 2;
LINE("module:PenaltyMarkPerceptor:penaltyPointScanLines",
pp.position.x(), lower,
pp.position.x(), upper,
1, Drawings::solidPen, ColorRGBA::blue);
// find left/right => middle horizontal
LINE("module:PenaltyMarkPerceptor:penaltyPointScanLines",
left, pp.position.y(),
right, pp.position.y(),
1, Drawings::solidPen, ColorRGBA::blue);
const int minBallSpotRadius = maxWidth / scaledToleratedSizeDeviation;
bool minRadiusReached = (right - left) >= minBallSpotRadius &&
(lower - upper) >= minBallSpotRadius &&
(right - left) >= minNumPixelVertical &&
(lower - upper) >= minNumPixelHorizontal;
if(!minRadiusReached)
{
DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y(), 7, ColorRGBA::black, "minRadius");
}
else
{
//check if region is sourounded by green which is always the case for the
//ball spot and not for line segments
//go with a rectangle around the region
int yUpper = upper - scaledWhiteSpacing;
int xRight = right + scaledWhiteSpacing;
int yLower = lower + scaledWhiteSpacing;
int xLeft = left - scaledWhiteSpacing;
if(yUpper <= 2 || yLower <= 2 || xRight <= 2 || xLeft <= 2 || yUpper >= height - 2 || yLower >= height - 2 || xRight >= rightLimit - 2 || xLeft >= rightLimit - 2)
return false;
std::vector<unsigned char> yValues;
for(int i = xLeft; i <= xRight; i++)
{
if(!checkIsNoise(i, yUpper, yValues))
{
return false;
}
if(!checkIsNoise(i, yLower, yValues))
{
return false;
}
}
for(int i = yUpper; i <= yLower; i++)
{
if(!checkIsNoise(xRight, i, yValues))
{
return false;
}
if(!checkIsNoise(xLeft, i, yValues))
{
return false;
}
}
RECTANGLE("module:PenaltyMarkPerceptor:penaltyPointScanLines",
xLeft, yUpper,
xRight, yLower,
2, Drawings::solidPen, ColorRGBA::red);
DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y(), 7, ColorRGBA::black, std::to_string(minBallSpotRadius) + ", " + std::to_string(right - left) + ", " + std::to_string(lower - upper));
const float variance = calcVariance(yValues);
DRAWTEXT("module:PenaltyMarkPerceptor:penaltyPointScanLines", pp.position.x(), pp.position.y() + 20, 7, ColorRGBA::black, "Variance: " << variance);
if(variance >= maxVariance)
{
ANNOTATION("PenaltyMarkPerceptor", "Ignored Spot (variance: " << variance);
}
return variance < maxVariance;
}
return false;
}
