vector<Point> Detect::findFingerTips(vector<Point> defectEnds, std::pair<Point, double> maxCircle, Mat& raw) { vector<Point> tips; for(int k = 0; k < defectEnds.size(); k++) { for(int l = 0; l < defectEnds.size(); l++) { if( k != l) { float distance = euclideanDist(defectEnds[k], defectEnds[l]); if (distance < maxCircle.second / 2 ){ Point tip = (defectEnds[k] + defectEnds[l])*.5; float palmDist = euclideanDist(tip, maxCircle.first); if(palmDist > 2 * maxCircle.second) { tips.push_back(tip); circle(raw, tip, 10, Scalar(255, 255, 255), 2); break; } } } } } return tips; }
double Detect::getAngle(const Point& a, const Point& b, const Point& c) { float sideA = euclideanDist(a, c); float sideB = euclideanDist(b, c); float sideC = euclideanDist(a, b); double angle = acos((sideA*sideA + sideB*sideB - sideC*sideC) / (2* sideA * sideB)) * 180.0/3.14159; return angle; }
// get min distance to cluster using Euclidean distance AUD_Double calc_nearestdist( AUD_Matrix *pFeatMatrix, AUD_Int32s rowIndex, AUD_Matrix *pCluster ) { AUD_ASSERT( pFeatMatrix != NULL ); AUD_ASSERT( pCluster != NULL ); AUD_ASSERT( pFeatMatrix->dataType == AUD_DATATYPE_INT32S ); AUD_ASSERT( pCluster->dataType == AUD_DATATYPE_INT32S ); AUD_ASSERT( rowIndex >= 0 && rowIndex < pFeatMatrix->rows ); AUD_ASSERT( pFeatMatrix->cols == pCluster->cols && pCluster->cols > 0 ); AUD_Int32s i; AUD_Double minDistance = -1.; AUD_Double curDistance; AUD_Int32s *pVec = pFeatMatrix->pInt32s + rowIndex * pFeatMatrix->cols; for ( i = 0; i < pCluster->rows; i++ ) { curDistance = euclideanDist( pVec, pCluster->pInt32s + i * pCluster->cols, pCluster->cols ); if ( curDistance < minDistance || minDistance < 0 ) { minDistance = curDistance; } } return minDistance; }
bool BlitzleApp::getClosestTarget(const std::vector<cv::Point>& playersIn, const cv::Point& originIn, cv::Point& targetOut) { if (playersIn.empty()) { return false; } targetOut = playersIn[0]; float closestDist = euclideanDist(originIn, targetOut); for (const auto& player : playersIn) { float playerDist = euclideanDist(originIn, player); if (playerDist < closestDist) { closestDist = playerDist; targetOut = player; } } return true; }
float MusicShape::collisionDetector(glm::vec3 cent1, glm::vec3 cent2, float rad1, float rad2) { // float inner = (powf((cent1.x - cent2.x), 2.0f) + powf((cent1.y - cent2.y), 2.0f) + powf((cent1.z - cent2.z), 2.0f)); float eucDist = euclideanDist(cent1, cent2); if (eucDist == 0.0f) { return (rad1 + rad2); } if (eucDist < (rad1 + rad2)) { float radDiff1 = (rad2 - rad1); float radDiff2 = (rad1 - rad2); return (eucDist - radDiff1 - radDiff2); } else { return 0.0f; } }
CQuaternion CCD(CVector3f root,CVector3f curEnd,CVector3f desireEnd) { //Local Variables FLOAT cosAngle,turnAngle; CVector3f curVector,targetVector; CQuaternion quat(0,0,0,1); FLOAT IK_POS_THRESH = 0.0000029403; // SEE IF I AM ALREADY CLOSE ENOUGH if (euclideanDist(curEnd, desireEnd) > IK_POS_THRESH){ // CREATE THE VECTOR TO THE CURRENT EFFECTOR POS curVector = curEnd - root; // CREATE THE DESIRED EFFECTOR POSITION VECTOR targetVector= desireEnd - root; // NORMALIZE THE VECTORS (EXPENSIVE, REQUIRES A SQRT) curVector.normalize(); targetVector.normalize(); // THE DOT PRODUCT GIVES ME THE COSINE OF THE DESIRED ANGLE cosAngle = dotpdut(targetVector,curVector); assert(! (fabs(cosAngle)-0.0000001 > 1));//fabscosAngle = [0,1] // IF THE DOT PRODUCT RETURNS 1.0, I DON'T NEED TO ROTATE AS IT IS 0 DEGREES if(cosAngle < 0.99999999999) { // USE THE CROSS PRODUCT TO CHECK WHICH WAY TO ROTATE CVector3f crossResult = cropdut(curVector,targetVector); crossResult.normalize(); //normalize the vector turnAngle = acos((FLOAT)cosAngle); // GET THE ANGLE //calculate quaternion CQuaternion quat(\ crossResult.x*sin(turnAngle/2),\ crossResult.y*sin(turnAngle/2),\ crossResult.z*sin(turnAngle/2),\ cos(turnAngle/2)); quat.normalize(); return quat; } } return quat; }
void Detect::getRegionOfInterest( vector<vector<Point>>& goodPolyCurves, const vector<vector<Point>>& polyCurves, const std::pair<Point, double>& maxCircle) { for (int i = 0; i < polyCurves.size(); i++) { vector<Point> goodContour; for(int j = 0; j < polyCurves[i].size(); j++) { Point current = polyCurves[i][j]; if (euclideanDist(current, maxCircle.first) < 3.5 * maxCircle.second) { goodContour.push_back(current); } } // DO NOT ADD POLY CURVE IF FEWER THAN 3 POINTS SINCE CANNOT HAVE HULL INDEX if(goodContour.size() > 3){ goodPolyCurves.push_back(goodContour); } } }
// Assign visibility to a dummy vertex representing all the possible pins // for this pinClassId. void ConnEnd::assignPinVisibilityTo(VertInf *dummyConnectionVert, VertInf *targetVert) { unsigned int validPinCount = 0; COLA_ASSERT(m_anchor_obj); COLA_ASSERT(m_connection_pin_class_id != CONNECTIONPIN_UNSET); Router *router = m_anchor_obj->router(); for (ShapeConnectionPinSet::iterator curr = m_anchor_obj->m_connection_pins.begin(); curr != m_anchor_obj->m_connection_pins.end(); ++curr) { ShapeConnectionPin *currPin = *curr; if ((currPin->m_class_id == m_connection_pin_class_id) && (!currPin->m_exclusive || currPin->m_connend_users.empty())) { double routingCost = currPin->m_connection_cost; Point adjTargetPt = targetVert->point - currPin->m_vertex->point; double angle = rotationalAngle(adjTargetPt); bool inVisibilityRange = false; if (angle <= 45 || angle >= 315) { if (currPin->directions() & ConnDirRight) { inVisibilityRange = true; } } if (angle >= 45 && angle <= 135) { if (currPin->directions() & ConnDirDown) { inVisibilityRange = true; } } if (angle >= 135 && angle <= 225) { if (currPin->directions() & ConnDirLeft) { inVisibilityRange = true; } } if (angle >= 225 && angle <= 315) { if (currPin->directions() & ConnDirUp) { inVisibilityRange = true; } } if (!inVisibilityRange) { routingCost += router->routingParameter(portDirectionPenalty); } if (router->m_allows_orthogonal_routing) { // This has same ID and is either unconnected or not // exclusive, so give it visibility. EdgeInf *edge = new EdgeInf(dummyConnectionVert, currPin->m_vertex, true); // XXX Can't use a zero cost due to assumptions // elsewhere in code. edge->setDist(manhattanDist(dummyConnectionVert->point, currPin->m_vertex->point) + std::max(0.001, routingCost)); } if (router->m_allows_orthogonal_routing) { // This has same ID and is either unconnected or not // exclusive, so give it visibility. EdgeInf *edge = new EdgeInf(dummyConnectionVert, currPin->m_vertex, false); // XXX Can't use a zero cost due to assumptions // elsewhere in code. edge->setDist(euclideanDist(dummyConnectionVert->point, currPin->m_vertex->point) + std::max(0.001, routingCost)); } // Increment the number of valid pins for this ConnEnd connection. validPinCount++; } } if (validPinCount == 0) { // There should be at least one pin, otherwise we will have // problems finding connector routes. err_printf("Warning: In ConnEnd::assignPinVisibilityTo():\n" " ConnEnd for connector %d can't connect to shape %d\n" " since it has no pins with class id of %u.\n", (int) m_conn_ref->id(), (int) m_anchor_obj->id(), m_connection_pin_class_id); } }
// name : detect function // input : image and dataset // output : classification result and detect picture CDetectionResult CRForest::detection(CTestDataset &testSet) const { int classNum = classDatabase.vNode.size();//contain class number std::vector<CTestPatch> testPatch; std::vector<const LeafNode*> result; //std::vector<const LeafNode*> storedLN(0); //std::vector<std::vector<CParamset> > cluster(0); //std::vector<CParamset> clusterMean(0); cv::vector<cv::Mat> outputImage(classNum); cv::vector<cv::Mat> voteImage(classNum);//voteImage(classNum); //cv::vector<cv::Mat_<std::vector<CParamset> > > voteParam(classNum); std::vector<int> totalVote(classNum,0); //boost::timer t; //boost::timer::auto_cpu_timer t; //boost::timer::nanosecond_type time; std::vector<paramHist**> voteParam2(classNum); //timer.start(); testSet.loadImage(conf); testSet.extractFeatures(conf); //std::cout << "extracted feature " << t.elapsed() << " sec" << std::endl; //testSet.releaseImage(); //t.restart() // image row and col int imgRow = testSet.img.at(0)->rows; int imgCol = testSet.img.at(0)->cols; #pragma omp parallel { #pragma omp for for(int i = 0; i < classNum; ++i) { outputImage.at(i) = testSet.img.at(0)->clone(); voteImage.at(i) = cv::Mat::zeros(imgRow,imgCol,CV_32FC1); voteParam2.at(i) = new paramHist*[imgRow]; for(int j = 0; j < imgRow; ++j) voteParam2.at(i)[j] = new paramHist[imgCol]; } } //paramHist voteParam[testSet.img.at(0)->rows][testSet.img.at(0)->cols][classNum]; // extract feature from test image //features.clear(); //extractFeatureChannels(image.at(0), features); // add depth image to features //features.push_back(image.at(1)); // extract patches from features extractTestPatches(testSet,testPatch,this->conf); //std::cout << "extracted feature " << t.elapsed() << " sec" << std::endl; std::cout << "patch num: " << testPatch.size() << std::endl; std::cout << "detecting..." << std::endl; // regression and vote for every patch std::cout << "class num = " << classNum << std::endl; for(int j = 0; j < testPatch.size(); ++j) { // regression current patch result.clear(); this->regression(result, testPatch.at(j)); // for each tree leaf for(int m = 0; m < result.size(); ++m) { #pragma omp parallel { #pragma omp for for(int l = 0; l < result.at(m)->pfg.size(); ++l) { if(result.at(m)->pfg.at(l) > 0.9) { int cl = classDatabase.search(result.at(m)->param.at(l).at(0).getClassName()); for(int n = 0; n < result.at(m)->param.at(cl).size(); ++n) { cv::Point patchSize(conf.p_height/2,conf.p_width/2); cv::Point rPoint = result.at(m)->param.at(cl).at(n).getCenterPoint(); if(conf.learningMode != 2) { cv::Mat tempDepth = *testPatch[j].getDepth(); cv::Rect tempRect = testPatch[j].getRoi(); cv::Mat realDepth = tempDepth(tempRect); double centerDepth = realDepth.at<ushort>(tempRect.height / 2 + 1, tempRect.width / 2 + 1) + conf.mindist; rPoint *= centerDepth; rPoint.x /= 1000; rPoint.y /= 1000; } cv::Point pos(testPatch.at(j).getRoi().x + patchSize.x + rPoint.x, testPatch.at(j).getRoi().y + patchSize.y + rPoint.y); // vote to result image if(pos.x > 0 && pos.y > 0 && pos.x < voteImage.at(cl).cols && pos.y < voteImage.at(cl).rows) { double v = result.at(m)->pfg.at(cl) / ( result.size() * result.at(m)->param.at(l).size()); voteImage.at(cl).at<float>(pos.y,pos.x) += v;//(result.at(m)->pfg.at(c) - 0.9);// * 100;//weight * 500; voteParam2.at(cl)[pos.y][pos.x].roll.at<double>(0,result.at(m)->param.at(l).at(n).getAngle()[0]) += v * 10000; voteParam2.at(cl)[pos.y][pos.x].pitch.at<double>(0,result.at(m)->param.at(l).at(n).getAngle()[1]) += v * 10000; voteParam2.at(cl)[pos.y][pos.x].yaw.at<double>(0,result.at(m)->param.at(l).at(n).getAngle()[2]) += v * 10000; //std::cout << result.at(m)->param.at(l).at(n).getAngle() << std::endl; //std::cout << v << std::endl; totalVote.at(cl) += 1; } } //for(int n = 0; n < result.at(m)->param.at(cl).size(); ++n){ } //if(result.at(m)->pfg.at(l) > 0.9){ } //for(int l = 0; l < result.at(m)->pfg.size(); ++l){ }//pragma omp parallel } // for every leaf } // for every patch // vote end #pragma omp parallel { #pragma omp for // find balance by mean shift for(int i = 0; i < classNum; ++i) { cv::GaussianBlur(voteImage.at(i),voteImage.at(i), cv::Size(21,21),0); } } // measure time // double time = t.elapsed(); // std::cout << time << "sec" << std::endl; // std::cout << 1 / (time) << "Hz" << std::endl; // output image to file std::string opath; // if(!conf.demoMode){ // //create result directory // opath = testSet.getRgbImagePath(); // opath.erase(opath.find_last_of(PATH_SEP)); // std::string imageFilename = testSet.getRgbImagePath(); // imageFilename.erase(imageFilename.find_last_of(".")); // //imageFilename.erase(imageFilename.begin(),imageFilename.find_last_of(PATH_SEP)); // imageFilename = imageFilename.substr(imageFilename.rfind(PATH_SEP),imageFilename.length()); // //opath += PATH_SEP; // opath += imageFilename; // std::string execstr = "mkdir -p "; // execstr += opath; // system( execstr.c_str() ); // for(int c = 0; c < classNum; ++c){ // std::stringstream cToString; // cToString << c; // std::string outputName = "output" + cToString.str() + ".png"; // std::string outputName2 = opath + PATH_SEP + "vote_" + classDatabase.vNode.at(c).name + ".png"; // //cv::imwrite(outputName.c_str(),outputImage.at(c)); // //cv::cvtColor(voteImage) // cv::Mat writeImage; // //hist.convertTo(hist, hist.type(), 200 * 1.0/second_val,0); // voteImage.at(c).convertTo(writeImage, CV_8UC1, 254); // cv::imwrite(outputName2.c_str(),writeImage); // } // } // create detection result CDetectionResult detectResult; detectResult.voteImage = voteImage; // show ground truth std::cout << "show ground truth" << std::endl; // std::cout << dataSet.className.size() << std::endl; // std::cout << dataSet.centerPoint.size() << std::endl; for(int i = 0; i < testSet.param.size(); ++i) { testSet.param.at(i).showParam(); } // show detection reslut std::cout << "show result" << std::endl; // for every class for(int c = 0; c < classNum; ++c) { double min,max; cv::Point minLoc,maxLoc; cv::minMaxLoc(voteImage.at(c),&min,&max,&minLoc,&maxLoc); double min_pose_value[3], max_pose_value[3]; cv::Point min_pose[3], max_pose[3]; paramHist hist; for(int x = 0; x < conf.paramRadius; ++x) { for(int y = 0; y < conf.paramRadius; ++y) { if( maxLoc.x + x < imgCol && maxLoc.y + y < imgRow) hist += voteParam2.at(c)[maxLoc.y + y][maxLoc.x + x]; if(maxLoc.x - x > 0 && maxLoc.y - y > 0) hist += voteParam2.at(c)[maxLoc.y - y][maxLoc.x - x]; } } //hist.showHist(); // for(int p = 0; p < 360; ++p){ // std::cout << p << " " << voteParam2.at(c)[maxLoc.y][maxLoc.x].yaw.at<double>(0,p) << std::endl; // } //voteParam2.at(c)[maxLoc.y][maxLoc.x].showHist(); cv::minMaxLoc(hist.roll, &min_pose_value[0], &max_pose_value[0], &min_pose[0], &max_pose[0]); cv::minMaxLoc(hist.pitch, &min_pose_value[1], &max_pose_value[1], &min_pose[1], &max_pose[1]); cv::minMaxLoc(hist.yaw, &min_pose_value[2], &max_pose_value[2], &min_pose[2], &max_pose[2]); // draw detected class bounding box to result image // if you whant add condition of detection threshold, add here cv::Size tempSize = classDatabase.vNode.at(c).classSize; cv::Rect_<int> outRect(maxLoc.x - tempSize.width / 2,maxLoc.y - tempSize.height / 2 , tempSize.width,tempSize.height); cv::rectangle(outputImage.at(c),outRect,cv::Scalar(0,0,200),3); cv::putText(outputImage.at(c),classDatabase.vNode.at(c).name,cv::Point(outRect.x,outRect.y),cv::FONT_HERSHEY_SIMPLEX,1.2, cv::Scalar(0,0,200), 2, CV_AA); // draw grand truth to result image if(!conf.demoMode) { for(int i = 0; i < testSet.param.size(); ++i) { int tempClassNum = classDatabase.search(testSet.param.at(i).getClassName()); if(tempClassNum != -1) { cv::Size tempSize = classDatabase.vNode.at(tempClassNum).classSize; cv::Rect_<int> outRect(testSet.param.at(i).getCenterPoint().x - tempSize.width / 2,testSet.param.at(i).getCenterPoint().y - tempSize.height / 2 , tempSize.width,tempSize.height); cv::rectangle(outputImage.at(tempClassNum),outRect,cv::Scalar(200,0,0),3); cv::putText(outputImage.at(tempClassNum),classDatabase.vNode.at(c).name,cv::Point(testSet.param.at(i).getCenterPoint().x, testSet.param.at(i).getCenterPoint().y),cv::FONT_HERSHEY_SIMPLEX,1.2, cv::Scalar(200,0,0), 2, CV_AA); } } } // show result std::cout << c << " Name : " << classDatabase.vNode.at(c).name << "\tvote : " << totalVote.at(c) << " Score : " << voteImage.at(c).at<float>(maxLoc.y, maxLoc.x) << " CenterPoint : " << maxLoc << std::endl << " Pose : roll " << max_pose[0].x << " pitch : " << max_pose[1].x << " yaw : " << max_pose[2].x << std::endl; // if not in demo mode, output image to file if(!conf.demoMode) { std::string outputName = opath + PATH_SEP + "detectionResult" + "_" + classDatabase.vNode.at(c).name + ".png"; cv::imwrite(outputName.c_str(),outputImage.at(c)); } CDetectedClass detectedClass; detectedClass.name = classDatabase.vNode.at(c).name; detectedClass.angle[0] = max_pose[0].x; // calc euclidean dist to nearest object double minError = DBL_MAX; std::string nearestObject; for(int d = 0; d < testSet.param.size(); ++d) { double tempError = euclideanDist(maxLoc,testSet.param.at(d).getCenterPoint());//= std::sqrt(std::pow((double)(maxLoc.x - testSet.param.at(0).getCenterPoint().x), 2) + std::pow((double)(maxLoc.y - testSet.param.at(0).getCenterPoint().y), 2)); //std::cout << tempError << std::endl; if(tempError < minError) { minError = tempError; nearestObject = testSet.param.at(d).getClassName(); } } // calc and output result detectedClass.error = minError; detectedClass.nearestClass = nearestObject; detectedClass.score = voteImage.at(c).at<float>(maxLoc.y, maxLoc.x); detectResult.detectedClass.push_back(detectedClass); } // for every class for(int k = 0; k < classNum; ++k) { for(int i = 0; i < imgRow; ++i) { delete[] voteParam2.at(k)[i]; } } return detectResult; }
std::pair<vector<Point>, std::pair<Point,double>> Detect::operator() (Mat& frame, Mat& raw, int count) { vector<Point> tips; // first find the curves in the image vector<vector<Point>> polyCurves = getPolyCurves(frame); //std::cout << polyCurves.size() << std::endl; // Find max inscribed circle for the 0th polycurve and draw it. std::pair<Point, double> maxCircle = findMaxInscribedCircle(polyCurves, raw); circle(raw, maxCircle.first, maxCircle.second, cv::Scalar(220,75,20), 1, CV_AA); // Good PolyCurve is with 3.5 * max inscribed radius vector<vector<Point>> goodPolyCurves; getRegionOfInterest(goodPolyCurves, polyCurves, maxCircle); // draw good poly curve drawContours(raw,goodPolyCurves, -1 , cv::Scalar(0,255,0), 2); // Find min enclosing circle on goodPolyCurve and draw it std::pair<Point2f, double> minCircle = findMinEnclosingCircle(goodPolyCurves); circle(raw, minCircle.first, minCircle.second, cv::Scalar(220,75,20), 1, CV_AA); // now find the convex hull for each polyCurve if (goodPolyCurves.size() < 1) { return std::pair<vector<Point>, std::pair<Point, double>>(tips, maxCircle); } // Get convex hulls vector<vector<int>> hullIndices = getConvexHulls(goodPolyCurves); vector<vector<Point>> hullPoints; for(int i = 0; i < goodPolyCurves.size(); i++) { if (goodPolyCurves[i].size() > 0) { vector<Point> hullPoint; convexHull(goodPolyCurves[i], hullPoint); hullPoints.push_back(hullPoint); } } // Draw the convex hulls drawContours(raw, hullPoints, -1, cv::Scalar(255, 0, 0), 2); for (int i = 0; i < 1; ++i) { vector<Vec4i> defects; // find convexity defects for each poly curve and draw them convexityDefects(goodPolyCurves[i], hullIndices[i], defects); defects = filterDefects(defects); vector<Point> defectEnds; for (int j = 0; j < defects.size(); ++j) { Vec4i& defect = defects[j]; int startIdx = defect[0]; int endIdx = defect[1]; int farIdx = defect[2]; Point start = goodPolyCurves[i][startIdx]; Point end = goodPolyCurves[i][endIdx]; Point far = goodPolyCurves[i][farIdx]; if(euclideanDist(far, start) > maxCircle.second) { defectEnds.push_back(start); defectEnds.push_back(end); } } tips = findFingerTips(defectEnds, maxCircle, raw); } flip(raw, raw, 1); imshow("hand", raw); // movie code (we should return the frame to HandMade and put movie code there with the others.) if(makeMoviesD) { char buffer[30]; sprintf(buffer, "detected/detected_%03d.jpg", count++); imwrite(buffer, raw); } return std::pair<vector<Point>, std::pair<Point, double>>(tips, maxCircle); }