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