void gestureTracker::detectGesture(Mat hand_roi, string rl)
{
Mat thresh;
Mat img = Mat::zeros( hand_roi.size(), CV_8UC3 );
img = hand_roi.clone();
Size max = hand_roi.size();
int max_x = max.width;
int max_y = max.height;
double palm_dist = 0.0;
Rect thresh_roi = Rect(3*max_x/7, 3*max_y/7, max_x/7, max_y/7);
Scalar avg = cv::mean(img(thresh_roi));
//cerr << avg[0] << endl;
int max_dist = avg[0] + 1400;
int min_dist = 0;
if(avg[0] >= 1400)
min_dist = avg[0] - 1400;
for (int i = 0; i < max_y; i++)
{
for (int j = 0; j < max_x; j++)
{
int val = (int)img.at<short>(i,j);
if(val <= min_dist)
img.at<short>(i,j) = 0;
else if(val >= max_dist)
img.at<short>(i,j) = 0;
}
}
img.convertTo(img, CV_8UC3);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours( img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
/// Draw contours
double max_area = 0.0;
int idx = -1;
Mat drawing = Mat::zeros( img.size(), CV_8UC3 );
//cerr << contours.size() << endl;
for( int i = 0; i < contours.size(); i++ )
{
double area = contourArea(contours[i]);
if(area > max_area )
{
max_area = area;
idx = i;
}
}
Scalar white = Scalar( 255, 255, 255 );
Scalar yellow = Scalar( 0, 255, 255 );
Scalar purple = Scalar( 255, 0, 255 );
Scalar blue = Scalar( 255, 0, 0 );
Scalar red = Scalar( 0, 0, 255 );
Scalar green = Scalar( 0, 255, 0 );
if(idx != -1)
{
//drawContours( drawing, contours, idx, white, 1, 8, vector<Vec4i>(), 0, Point() );
//imshow(rl, drawing);
//cv::waitKey(20);
}
if(idx != -1)
{
if (contours[idx].size() <= 5)
return;
//cerr << idx << endl;
// convex hull for drawing (Points)
vector<vector<Point> > hullp(contours[idx].size());
// convex Hull for defects (ints)
vector<vector<int> > hulli(contours[idx].size());
vector<vector<Point> > hull_points(contours[idx].size());
vector<vector<Point> > defect_points(contours[idx].size());
convexHull( Mat(contours[idx]), hulli[0], false );
convexHull( Mat(contours[idx]), hullp[0], false );
/*
for(int i = 0; i < hullp[0].size(); i++)
circle( drawing, hullp[0][i], 5, blue, -1 );
*/
vector<vector<Vec4i> > defects(contours[idx].size());
if (hulli[0].size() > 2 && contours[idx].size() > 3)
{
convexityDefects(contours[idx], hulli[0], defects[0]);
Point palm;
double x_sum = 0.0;
double y_sum = 0.0;
for(int i = 0; i < defects[0].size(); i++)
{
for(int j = 0; j < 3; j++)
{
x_sum += contours[idx][defects[0][i][j]].x;
y_sum += contours[idx][defects[0][i][j]].y;
}
}
palm.x = (x_sum / defects[0].size()) / 3.0;
palm.y = (y_sum / defects[0].size()) / 3.0;
if(rl == "right")
{
r_hand_palm_x_filter.add(palm.x);
r_hand_palm_y_filter.add(palm.y);
palm.x = r_hand_palm_x_filter.average();
palm.y = r_hand_palm_y_filter.average();
}
else
{
l_hand_palm_x_filter.add(palm.x);
l_hand_palm_y_filter.add(palm.y);
palm.x = l_hand_palm_x_filter.average();
palm.y = l_hand_palm_y_filter.average();
}
// max = 20 ish, min = 7 ish
palm_dist = (hand_roi.at<short>(palm.y, palm.x)/65536.0)*100.0;
cerr << "Palm: " << palm_dist << endl;
circle(drawing, palm, 5, green, -1);
double avg_dist = 0.0;
for(int i = 0; i < defects[0].size(); i++)
{
int idx_0 = defects[0][i][0];
int idx_1 = defects[0][i][1];
int idx_2 = defects[0][i][2];
defect_points[i].push_back(contours[idx][idx_2]);
/*
circle(drawing, contours[idx][idx_0], 5, yellow, -1);
circle(drawing, contours[idx][idx_1], 5, blue, -1);
circle(drawing, contours[idx][idx_2], 5, red, -1);
line(drawing, contours[idx][idx_2], contours[idx][idx_0], purple, 1);
line(drawing, contours[idx][idx_2], contours[idx][idx_1], purple, 1);
*/
int tip_x = contours[idx][idx_0].x;
int tip_y = contours[idx][idx_0].y;
int defect_x = contours[idx][idx_2].x;
int defect_y = contours[idx][idx_2].y;
double dist = 0.0;
dist = sqrt(pow((tip_x - defect_x), 2) + pow((tip_y - defect_y), 2));
avg_dist += dist;
}
if(defects[0].size() > 0)
{
avg_dist = avg_dist / defects[0].size();
// normalize for fingers
avg_dist = avg_dist / 20.0;
cerr << rl << " " << defects[0].size()/3 - 2 << endl;
std_msgs::Float64 state_msg;
if(rl == "right")
{
r_hand_state_filter.add(avg_dist - 0.3);
state_msg.data = (r_hand_state_filter.average());
r_hand_state_pub.publish(state_msg);
//cerr << "distance R: " << r_hand_state_filter.average() << endl;
}
else
{
l_hand_state_filter.add(avg_dist - 0.3);
state_msg.data = (l_hand_state_filter.average());
l_hand_state_pub.publish(state_msg);
//cerr << "distance L: " << l_hand_state_filter.average() << endl;
}
}
}
/*
try{
if(rl == "right")
imshow(rl + "hand", drawing);
else
imshow(rl + "hand", drawing);
}
catch(...){return;}
*/
cv::waitKey(20);
//threshold(img, thresh, 30, 255, CV_THRESH_BINARY_INV);
}
}
void HandDetector::FindConvexHull(Mat& LiveFrame, Mat SkinThreshold)
{
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
Point2f COM; //Center of Mass
int TiltAngle = 0;
int LargestArea = 0;
int LargestAreaIndex = -1;
cv::findContours(SkinThreshold,contours,hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<vector<Point>> hull(contours.size());
vector<vector<int> > inthull(contours.size());
vector<vector<Vec4i> > defects(contours.size());
for(int i=0;i<contours.size();++i)
{
convexHull(Mat(contours[i]),hull[i],false);
convexHull(Mat(contours[i]), inthull[i], false);
if (inthull[i].size()>3)
convexityDefects(contours[i], inthull[i], defects[i]);
}
for(int i=0;i<contours.size();++i)
{
double area = cv::contourArea(contours[i],false);
if(area > LargestArea)
{
LargestArea = area;
LargestAreaIndex = i;
}
}
if(contours.size() > 0 && LargestArea > 10000 && LargestArea < 90000 && !mObjectTrackFlag)
{
//cv::drawContours(frame,contours,id,CV_RGB(0,255,0),2,8,hierarchy);
drawContours(LiveFrame, hull ,LargestAreaIndex, CV_RGB(0, 255, 0), 2, 8, hierarchy);
drawContours(LiveFrame, contours, LargestAreaIndex, CV_RGB(0, 0, 255), 2, 8, hierarchy);
//kCurvature(contours,hull,inthull,LiveFrame);
//cout<<"\nlargest Area : "<<largest_area;
Moments moment;
moment = moments( contours[LargestAreaIndex], false );
/// Get the mass centers:
COM = Point2f( moment.m10/moment.m00 , moment.m01/moment.m00 );
circle(LiveFrame,COM,4,Scalar(255,0,0),2);
//TiltAngle = CalculateHandTilt(moment.m11,moment.m20,moment.m02);
//cout<<"\n Tilt Angle : "<<TiltAngle;
//cout<<GetOrientation(contours[LargestAreaIndex],LiveFrame)<<"\n";
//cout<<"\n"<<COM.x<<" "<<COM.y;
FindConvexityDefects(defects[LargestAreaIndex], contours[LargestAreaIndex], LiveFrame,COM, TiltAngle);
}
else if(contours.size() > 0 && LargestArea > 10 && LargestArea < 30000 && mObjectTrackFlag)
{
drawContours(LiveFrame, hull ,LargestAreaIndex, CV_RGB(0, 255, 0), 2, 8, hierarchy);
drawContours(LiveFrame, contours, LargestAreaIndex, CV_RGB(0, 0, 255), 2, 8, hierarchy);
Moments moment;
moment = moments( contours[LargestAreaIndex], false );
/// Get the mass centers:
COM = Point2f( moment.m10/moment.m00 , moment.m01/moment.m00 );
circle(LiveFrame,COM,4,Scalar(255,0,0),2);
Draw(COM.x,COM.y);
}
else if(mSwipeON)
{
mSwipeGesture.Reset();
}
}
