void execute()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_points (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::ConvexHull<pcl::PointXYZRGB> hull;
hull.setInputCloud(input_cloud_);
hull.setDimension(3);
hull.reconstruct (*output_cloud_);
}
// find contours of segmented hand and count fingers
int Kinect::KinectZoom::DetectContour( )
{
int numFingers = 0;
cv::Mat drawing = cv::Mat::zeros( mask.size(), CV_8UC3 );
cv::vector<cv::vector<cv::Point> > contours;
cv::vector<cv::Vec4i> hierarchy;
findContours( mask,contours, hierarchy, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE, cv::Point() );
if ( contours.size()>0 ) {
//hull MUSI byt typu int !!!
cv::vector<std::vector<int> >hull( contours.size() );
cv::vector<cv::vector<cv::Vec4i> > convDef( contours.size() );
cv::vector<cv::vector<cv::Point> > hull_points( contours.size() );
cv::vector<cv::vector<cv::Point> > defect_points( contours.size() );
for ( size_t i = 0; i < contours.size(); i++ ) {
if ( cv::contourArea( contours[i] )>500 ) {
cv::convexHull( contours[i], hull[i], false );
cv::convexityDefects( contours[i],hull[i], convDef[i] );
for ( size_t k=0; k<hull[i].size(); k++ ) {
auto ind=hull[i][k];
hull_points[i].push_back( contours[i][ind] );
}
for ( size_t k=0; k<convDef[i].size(); k++ ) {
if ( convDef[i][k][3]>20*256 ) { // filter defects by depth
numFingers++;
size_t ind_0=static_cast<size_t>( convDef[i][k][0] );
size_t ind_1=static_cast<size_t>( convDef[i][k][1] );
size_t ind_2=static_cast<size_t>( convDef[i][k][2] );
defect_points[i].push_back( contours[i][ind_2] );
cv::circle( drawing,contours[i][ind_0],5,cv::Scalar( 0,255,0 ),-1 );
cv::circle( drawing,contours[i][ind_1],5,cv::Scalar( 0,255,0 ),-1 );
cv::circle( drawing,contours[i][ind_2],5,cv::Scalar( 0,0,255 ),-1 );
cv::line( drawing,contours[i][ind_2],contours[i][ind_0],cv::Scalar( 0,0,255 ),1 );// filter defects by depth
cv::line( drawing,contours[i][ind_2],contours[i][ind_1],cv::Scalar( 0,0,255 ),1 );
}
}
// draw results
cv::drawContours( drawing, contours, static_cast<int>( i ), cv::Scalar( 0,255,0 ), 1, 8, cv::vector<cv::Vec4i>(), 0, cv::Point() );
drawContours( drawing, hull_points, static_cast<int>( i ), cv::Scalar( 255,0,0 ), 1, 8, cv::vector<cv::Vec4i>(), 0, cv::Point() );
}
}
}
#ifdef QT_DEBUG
// imshow( "Hull", drawing );
#endif
return numFingers;
}
ConvexHull2D MassTrace::getConvexhull() const
{
ConvexHull2D::PointArrayType hull_points(trace_peaks_.size());
Size i = 0;
for (MassTrace::const_iterator l_it = trace_peaks_.begin(); l_it != trace_peaks_.end(); ++l_it)
{
hull_points[i][0] = (*l_it).getRT();
hull_points[i][1] = (*l_it).getMZ();
++i;
}
ConvexHull2D hull;
hull.addPoints(hull_points);
return hull;
}
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);
}
}
