AppState* TriggerRecognizer::recognize(QImage image_)
{
image = image_;
auto pixmap = getPixmap();
std::set<QPoint> visited;
std::vector<std::pair<QRgb, QPoint> > cells;
for (int i = 0; i < image.width(); i++)
for (int j = 0; j < image.height(); j++)
{
if ( (pixmap[i][j] == red || pixmap[i][j] == blue) && !visited.count(QPoint(i, j)))
{
std::set<QPoint> tmp;
exploreRegion(QPoint(i, j), pixmap, tmp, pixmap[i][j]);
if ((int)tmp.size() >= minRegionSize)
{
cells.push_back(std::make_pair(pixmap[i][j], meanPoint(tmp)));
visited.insert(tmp.begin(), tmp.end());
}
}
}
//std::cerr << "game consists of " << cells.size() << " cells" << std::endl;
auto preField = cluster(cells);
int width, height;
if (preField.empty())
width = height = 0;
else
{
width = preField[0].size();
height = preField.size();
}
TriggerInternalState* internalState = new TriggerInternalState(height, width);
TriggerExternalState* externalState = new TriggerExternalState(height, width);
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
internalState->setField(i, j, preField[i][j].first == red);
QPoint point = preField[i][j].second;
externalState->setCoordinate(i, j, point.x(), point.y());
}
}
return new AppState(internalState, externalState);
}
// correlate imagePoints to modelPoints
void PnPObj::correlatePoints(int callNo) {
// Assumes 5 LEDs numbered 1-5:
// Order Found LED ID # Description
// point1 = [0] or [4] First wingtip found
// point2 = [1] or [3] First horizontal found (same side as point1)
// point3 = [4] or [0] Opposite wing of point1
// point4 = [3] or [1] Horizontal opposite side of point1
// point5 = [2] Vertical tail
// create temporary variables to hold points while things get moved around
std::vector<cv::Point2f> bufferImagePoints = imagePoints, newImagePoints = imagePoints;
if(callNo > 3) std::cout << "ledFinder only supports a maximum of 3 calls!" << std::endl;
// Compute the mean pixel location of the LEDs
cv::Point2f zero(0.0f, 0.0f);
cv::Point2f sum = std::accumulate(bufferImagePoints.begin(), bufferImagePoints.end(), zero);
cv::Point2f meanPoint(sum.x / bufferImagePoints.size(), sum.y / bufferImagePoints.size());
// Compute distances from mean point and find index of farthest point
int point1 = 0;
double distFromMean, maxDistFromMean = 0;
for (int i=0; i<NO_LEDS; i++) {
distFromMean = cv::norm(cv::Mat(bufferImagePoints[i]) - cv::Mat(meanPoint));
point1 = (distFromMean > maxDistFromMean) ? i : point1;
maxDistFromMean = (distFromMean > maxDistFromMean) ? distFromMean : maxDistFromMean;
}
// Determine if this is a right or left wingtip
char firstWing;
if (bufferImagePoints[point1].x < meanPoint.x) {
// left wingtip
newImagePoints[0] = bufferImagePoints[point1];
firstWing = 'L';
} else {
// right wingtip
newImagePoints[4] = bufferImagePoints[point1];
firstWing = 'R';
}
// Find the closest and farthest points from first wingtip and make them the
// horizontal of the same side and opposite wingtip, respectively
int point2 = 0, point3;
double distFrom1, minDistFrom1 = INFINITY;
std::vector<std::pair<double,int> > point3Pairs;
for (int i=0; i<NO_LEDS; i++) {
if (i==point1) continue; // Can't reuse points
// find point2 (horizontal on same side as point1 wingtip)
distFrom1 = cv::norm(cv::Mat(bufferImagePoints[i]) - cv::Mat(bufferImagePoints[point1]));
point2 = (distFrom1 < minDistFrom1) ? i : point2;
minDistFrom1 = (distFrom1 < minDistFrom1) ? distFrom1 : minDistFrom1;
// find point3 (wingtip opposite of point1)
// create a vector of distances from point 1 that can be sorted later
point3Pairs.push_back(std::make_pair(i,distFrom1));
}
// sort by descending distance from point 1
std::sort(point3Pairs.begin(), point3Pairs.end(), PnPObj::pairComparator);
if (callNo==1) {
// farthest point
point3 = point3Pairs[0].first;
} else if (callNo==2) {
// second farthest point
point3 = point3Pairs[1].first;
} else if (callNo==3) {
// third farthest point
point3 = point3Pairs[2].first;
}
switch (firstWing) {
case 'L':
newImagePoints[1] = bufferImagePoints[point2];
newImagePoints[4] = bufferImagePoints[point3];
break;
case 'R':
newImagePoints[3] = bufferImagePoints[point2];
newImagePoints[0] = bufferImagePoints[point3];
break;
default:
std::cout << "Error evaluating if the first LED is left or right wingtip" << std::endl;
break;
}
// Compute the angle of the wings and find which point (point4), when matched with the
// known horizontal, most closely matches the wing angle.
double wingSlope, wingAngle;
wingSlope = (newImagePoints[4].y - newImagePoints[0].y)/(newImagePoints[4].x - newImagePoints[0].x);
wingAngle = atan(wingSlope);
int point4 = 0;
double slope, angle, absAngleDiff, minAbsAngleDiff = INFINITY;
for (int i=0; i<NO_LEDS; i++) {
if (i==point1 || i==point2 || i== point3) continue; // Can't reuse previous points
switch (firstWing) {
case 'L':
slope = (newImagePoints[1].y - bufferImagePoints[i].y) / (newImagePoints[1].x - bufferImagePoints[i].x);
break;
case 'R':
slope = (bufferImagePoints[i].y - newImagePoints[3].y) / (bufferImagePoints[i].x - newImagePoints[3].x);
break;
default:
std::cout << "Error evaluating if the first LED is left or right wingtip" << std::endl;
break;
}
angle = atan(slope);
absAngleDiff = std::abs(angle - wingAngle);
point4 = (absAngleDiff < minAbsAngleDiff) ? i : point4;
minAbsAngleDiff = (absAngleDiff < minAbsAngleDiff) ? absAngleDiff : minAbsAngleDiff;
}
switch (firstWing) {
case 'L':
newImagePoints[3] = bufferImagePoints[point4];
break;
case 'R':
newImagePoints[1] = bufferImagePoints[point4];
break;
default:
std::cout << "Error evaluating if the first LED is left or right wingtip" << std::endl;
break;
}
// The final LED (point5) must be the tail
int point5;
for (int i=0; i<NO_LEDS; i++) {
if (i==point1 || i ==point2 || i==point3 || i==point4) {
continue;
} else {
point5 = i;
break;
}
}
newImagePoints[2] = bufferImagePoints[point5];
imagePoints = newImagePoints;
}
void WorldSimulation::Advance(Real dt)
{
if(fakeSimulation) {
AdvanceFake(dt);
return;
}
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
Reset(i->second);
}
Timer timer;
Real timeLeft=dt;
Real accumTime=0;
int numSteps = 0;
//printf("Advance %g -> %g\n",time,time+dt);
while(timeLeft > 0.0) {
Real step = Min(timeLeft,simStep);
for(size_t i=0;i<controlSimulators.size();i++)
controlSimulators[i].Step(step);
for(size_t i=0;i<hooks.size();i++)
hooks[i]->Step(step);
//update viscous friction approximation as dry friction from current velocity
for(size_t i=0;i<controlSimulators.size();i++) {
Robot* robot=world->robots[i].robot;
for(size_t j=0;j<robot->drivers.size();j++) {
//setup viscous friction
if(robot->drivers[j].viscousFriction != 0) {
Real v=controlSimulators[i].oderobot->GetDriverVelocity(j);
for(size_t k=0;k<robot->drivers[j].linkIndices.size();k++) {
int l=robot->drivers[j].linkIndices[k];
controlSimulators[i].oderobot->SetLinkDryFriction(l,robot->drivers[j].dryFriction+robot->drivers[j].viscousFriction*Abs(v));
}
}
}
}
odesim.Step(step);
accumTime += step;
timeLeft -= step;
numSteps++;
//accumulate contact information
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum || i->second.accumFull) {
ODEContactList* list = odesim.GetContactFeedback(i->first.first,i->first.second);
if(!list) continue;
if(i->second.accum) {
if(list->forces.empty()) i->second.separationCount++;
else i->second.contactCount++;
i->second.inContact = !list->forces.empty();
Vector3 meanPoint(Zero),meanForce(Zero),meanTorque(Zero);
if(!list->forces.empty()) {
Real wsum = 0;
for(size_t k=0;k<list->forces.size();k++) {
Real w = list->forces[k].dot(list->points[k].n);
meanPoint += list->points[k].x*w;
wsum += w;
}
if(wsum == 0) {
meanPoint.setZero();
for(size_t k=0;k<list->forces.size();k++)
meanPoint += list->points[k].x;
meanPoint /= list->forces.size();
}
else
meanPoint /= wsum;
//update average;
i->second.meanPoint += 1.0/i->second.contactCount*(meanPoint - i->second.meanPoint);
}
for(size_t k=0;k<list->forces.size();k++) {
meanForce += list->forces[k];
meanTorque += cross((list->points[k].x-meanPoint),list->forces[k]);
}
//update average
i->second.meanForce += 1.0/numSteps*(meanForce - i->second.meanForce);
i->second.meanTorque += 1.0/numSteps*(meanTorque - i->second.meanTorque);
}
if(i->second.accumFull) {
i->second.times.push_back(time + accumTime);
i->second.contactLists.push_back(*list);
}
}
}
}
time += dt;
UpdateModel();
/*
//convert sums to means
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum) {
i->second.meanForce /= numSteps;
i->second.meanPoint /= numSteps;
i->second.meanTorque /= numSteps;
}
}
*/
//printf("WorldSimulation: Sim step %gs, real step %gs\n",dt,timer.ElapsedTime());
}
