void SelfLocator::initSamplesAtGivenPositions(const vector<Pose2D>& poses,
const vector< Pose2D >& standardDeviations)
{
for(int i = 0; i < samples->size(); ++i)
{
Sample& sample(samples->at(i));
if(poses.size())
{
// Select one of the poses from the list:
unsigned int index = random((short)poses.size());
Pose2D pose = poses[index];
Pose2D stdDev = standardDeviations[index];
// Create sample:
sample.translation.x = static_cast<int>(pose.translation.x);
sample.translation.x += sampleTriangularDistribution(static_cast<int>(stdDev.translation.x));
sample.translation.y = static_cast<int>(pose.translation.y);
sample.translation.y += sampleTriangularDistribution(static_cast<int>(stdDev.translation.y));
float rotation = normalize(pose.rotation + sampleTriangularDistribution(stdDev.rotation));
sample.angle = rotation;
sample.rotation = Vector2<int>(int(cos(rotation) * 1024), int(sin(rotation) * 1024));
}
else //No given positions, spread uniformly:
{
Pose2D pose(bb->theFieldDimensions.randomPoseOnField());
sample.translation = Vector2<int>(int(pose.translation.x), int(pose.translation.y));
sample.angle = pose.rotation;
sample.rotation = Vector2<int>(int(cos(pose.rotation) * 1024), int(sin(pose.rotation) * 1024));
}
}
lastOdometry = bb->theOdometryData;
poseCalculator->init();
}
SampleTemplate SampleTemplateGenerator::generateTemplateFromPosition(
const Vector2<double>& posSeen, const Vector2<double>& posReal,
const Pose2D& postOdometry) const
{
SampleTemplate newTemplate;
double r = posSeen.abs() + theFieldDimensions.goalPostRadius;
double distUncertainty = sampleTriangularDistribution(standardDeviationGoalpostSampleBearingDistance);
if(r+distUncertainty > standardDeviationGoalpostSampleBearingDistance)
r += distUncertainty;
Vector2<double> realPosition = posReal;
double minY = std::max<double>(posReal.y - r, static_cast<double>(theFieldDimensions.yPosRightFieldBorder));
double maxY = std::min<double>(posReal.y + r, static_cast<double>(theFieldDimensions.yPosLeftFieldBorder));
Vector2<double> p;
p.y = minY + randomDouble()*(maxY - minY);
double xOffset(sqrt(sqr(r) - sqr(p.y - posReal.y)));
p.x = posReal.x;
p.x += (p.x > 0) ? -xOffset : xOffset;
if(theFieldDimensions.isInsideCarpet(p) && checkTemplateClipping(p))
{
double origAngle = (realPosition-p).angle();
double observedAngle = posSeen.angle();
Pose2D templatePose(origAngle-observedAngle, p);
Pose2D odometryOffset = theOdometryData - postOdometry;
templatePose += odometryOffset;
newTemplate = templatePose;
newTemplate.timestamp = theFrameInfo.time;
}
return newTemplate;
}
SampleTemplate SampleTemplateGenerator::generateTemplateFromFullGoal(const FullGoal& goal) const
{
SampleTemplate newTemplate;
Pose2D odometryOffset = theOdometryData - goal.odometry;
double leftPostDist = goal.seenLeftPosition.abs();
double leftDistUncertainty = sampleTriangularDistribution(standardDeviationGoalpostSampleBearingDistance);
if(leftPostDist+leftDistUncertainty > standardDeviationGoalpostSampleBearingDistance)
leftPostDist += leftDistUncertainty;
double rightPostDist = goal.seenRightPosition.abs();
double rightDistUncertainty = sampleTriangularDistribution(standardDeviationGoalpostSampleBearingDistance);
if(rightPostDist+rightDistUncertainty > standardDeviationGoalpostSampleBearingDistance)
rightPostDist += rightDistUncertainty;
Geometry::Circle c1(goal.realLeftPosition, leftPostDist + theFieldDimensions.goalPostRadius);
Geometry::Circle c2(goal.realRightPosition, rightPostDist + theFieldDimensions.goalPostRadius);
// If there are intersections, take the first one that is in the field:
Vector2<double> p1,p2;
int result = Geometry::getIntersectionOfCircles(c1, c2, p1, p2);
if(result)
{
if(theFieldDimensions.isInsideCarpet(p1) && checkTemplateClipping(p1))
{
double origAngle = (goal.realLeftPosition-p1).angle();
double observedAngle = goal.seenLeftPosition.angle();
Pose2D templatePose(origAngle-observedAngle, p1);
templatePose += odometryOffset;
newTemplate = templatePose;
newTemplate.timestamp = theFrameInfo.time;
}
else if(theFieldDimensions.isInsideCarpet(p2) && checkTemplateClipping(p2))
{
double origAngle = (goal.realLeftPosition-p2).angle();
double observedAngle = goal.seenLeftPosition.angle();
Pose2D templatePose(origAngle-observedAngle, p2);
templatePose += odometryOffset;
newTemplate = templatePose;
newTemplate.timestamp = theFrameInfo.time;
}
}
return newTemplate;
}
