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()); }