void ScheinrieseApp::findBlobs() {
CBlobResult blobs;
int i;
CBlob *currentBlob;
IplImage *original, *originalThr;
// load an image and threshold it
original = cvLoadImage("pic1.png", 0);
cvThreshold( original, originalThr, 100, 0, 255, CV_THRESH_BINARY );
// find non-white blobs in thresholded image
blobs = CBlobResult( originalThr, NULL, 255 );
// exclude the ones smaller than param2 value
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, param2 );
// get mean gray color of biggest blob
CBlob biggestBlob;
CBlobGetMean getMeanColor( original );
double meanGray;
blobs.GetNth( CBlobGetArea(), 0, biggestBlob );
meanGray = getMeanColor( biggestBlob );
// display filtered blobs
cvMerge( originalThr, originalThr, originalThr, NULL, displayedImage );
for (i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( displayedImage, CV_RGB(255,0,0));
}
}
// threshold trackbar callback
void on_trackbar( int dummy )
{
if(!originalThr)
{
originalThr = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U,1);
}
if(!displayedImage)
{
displayedImage = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U,3);
}
// threshold input image
cvThreshold( original, originalThr, param1, 255, CV_THRESH_BINARY );
// get blobs and filter them using its area
CBlobResult blobs;
int i;
CBlob *currentBlob;
// find blobs in image
blobs = CBlobResult( originalThr, NULL, 255 );
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, param2 );
// display filtered blobs
cvMerge( originalThr, originalThr, originalThr, NULL, displayedImage );
for (i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( displayedImage, CV_RGB(255,0,0));
}
cvShowImage( wndname, displayedImage );
}
void TabletopSegmentor::pcCallback(sensor_msgs::PointCloud2::ConstPtr pc_msg) {
if(!pc_lock.try_lock())
return;
pcl::PointCloud<pcl::PointXYZRGB> pc_full, pc_full_frame;
pcl::fromROSMsg(*pc_msg, pc_full);
string base_frame("/base_link");
ros::Time now = ros::Time::now();
tf_listener.waitForTransform(pc_msg->header.frame_id, base_frame, now, ros::Duration(3.0));
pcl_ros::transformPointCloud(base_frame, pc_full, pc_full_frame, tf_listener);
// pc_full_frame is in torso lift frame
cv::Mat cur_height_img = cv::Mat::zeros(imgx, imgy, CV_8U);
BOOST_FOREACH(const pcl::PointXYZRGB& pt, pc_full_frame.points) {
if(pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
continue;
int32_t x, y, z;
x = (pt.x - minx)/(maxx-minx) * imgx;
y = (pt.y - miny)/(maxy-miny) * imgy;
z = (pt.z - minz)/(maxz-minz) * 256;
if(x < 0 || y < 0) continue;
if(x >= imgx || y >= imgy) continue;
if(z < 0 || z >= 256) continue;
if(cur_height_img.at<uint8_t>(x, y) == 0 || cur_height_img.at<uint8_t>(x, y) < (uint8_t) z)
cur_height_img.at<uint8_t>(x, y) = (uint8_t) z;
}
cv::max(height_img_max, cur_height_img, height_img_max);
cv::Mat cur_height_img_flt;
cur_height_img.convertTo(cur_height_img_flt, CV_32F);
height_img_sum += cur_height_img_flt;
cv::Mat cur_count(imgx, imgy, CV_8U);
cur_count = (cur_height_img > 0) / 255;
cv::Mat cur_count_flt(imgx, imgy, CV_32F);
cur_count.convertTo(cur_count_flt, CV_32F);
height_img_count += cur_count_flt;
cv::Mat height_img_avg_flt = height_img_sum / height_img_count;
cv::Mat height_img_avg(imgx, imgy, CV_8U);
height_img_avg_flt.convertTo(height_img_avg, CV_8U);
height_img_avg = height_img_max;
cv::Mat height_hist(256, 1, CV_32F, cv::Scalar(0));
for(uint32_t x=0;x<imgx;x++)
for(uint32_t y=0;y<imgy;y++) {
if(height_img_avg.at<uint8_t>(x,y) == 255)
height_img_avg.at<uint8_t>(x,y) = 0;
if(height_img_avg.at<uint8_t>(x,y) != 0) {
height_hist.at<float>(height_img_avg.at<uint8_t>(x,y), 0)++;
}
}
////////////////////// Finding best table height /////////////////////////
uint32_t gfiltlen = 25;
float stddev = 256/(maxz-minz) * 0.015;
cv::Mat gauss_filt(gfiltlen, 1, CV_32F, cv::Scalar(0));
for(uint32_t i=0;i<gfiltlen;i++)
gauss_filt.at<float>(i,0) = 0.39894 / stddev * std::exp(-(i-((float)gfiltlen)/2)*(i-((float)gfiltlen)/2)/(2*stddev*stddev));
//cout << gauss_filt;
uint32_t maxval = 0, maxidx = 0;
for(uint32_t i=0;i<256-gfiltlen;i++) {
uint32_t sum = 0;
for(uint32_t j=0;j<gfiltlen;j++)
sum += height_hist.at<float>(i+j,0) * gauss_filt.at<float>(j,0);
if(sum > maxval && i != 0) {
maxval = sum;
maxidx = i+gfiltlen/2;
}
}
int32_t table_height = ((int32_t)maxidx);
//printf("%d %d, ", maxval, maxidx);
/////////////////////////// Getting table binary /////////////////////
cv::Mat height_img_thresh(imgx, imgy, CV_8U);
height_img_thresh = height_img_avg.clone();
for(uint32_t x=0;x<imgx;x++)
for(uint32_t y=0;y<imgy;y++) {
if(std::fabs(table_height - ((int32_t)height_img_thresh.at<uint8_t>(x,y))) < stddev*2)
height_img_thresh.at<uint8_t>(x,y) = 255;
else
height_img_thresh.at<uint8_t>(x,y) = 0;
}
//////////////////////////////////////////////////////////////////
IplImage height_img_thresh_ipl = height_img_thresh;
IplConvKernel* element = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT);
cvMorphologyEx(&height_img_thresh_ipl, &height_img_thresh_ipl, NULL, element, CV_MOP_CLOSE, num_closes);
//cvMorphologyEx(&height_img_thresh, &height_img_thresh, NULL, element, CV_MOP_OPEN, 2);
cv::Mat height_img_thresh_blob = height_img_thresh.clone();
IplImage blob_img = height_img_thresh_blob;
CBlobResult blobs = CBlobResult(&blob_img, NULL, 0);
//blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 10);
CBlob biggestblob;
blobs.GetNthBlob(CBlobGetArea(), 0, biggestblob);
cv::Mat table_blob(imgx, imgy, CV_8U, cv::Scalar(0)); IplImage table_blob_img = table_blob;
biggestblob.FillBlob(&table_blob_img, cv::Scalar(150));
//drawCvBox2D(blob_img, table_roi, cv::Scalar(50), 1);
CvBox2D table_roi = biggestblob.GetEllipse(); table_roi.angle *= CV_PI/180;
cv::Mat table_hull(imgx, imgy, CV_8U, cv::Scalar(0));
IplImage hull_img = table_hull;
fillCvBox2D(hull_img, table_roi, cv::Scalar(255));
//printf("Cvbox: %f, %f, %f, %f, %f\n", table_roi.center.x, table_roi.center.y, table_roi.size.width, table_roi.size.height, table_roi.angle);
//cv::Mat height_morph(imgx, imgy, CV_8U, cv::Scalar(0));
//cv::Mat tmp_img(imgx, imgy, CV_8U, cv::Scalar(0));
//IplImage t1 = height_img_thresh; IplImage = height_morph;
cv::Mat above_table(imgx, imgy, CV_8U);
bitwise_and(height_img_max > table_height + stddev*2, table_hull, above_table);
IplImage above_table_img = above_table;
CBlobResult obj_blobs = CBlobResult(&above_table_img, NULL, 0);
obj_blobs.Filter(obj_blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, obj_min_area);
CBlob cur_obj_blob;
vector<float> obj_cents_x, obj_cents_y, obj_cents_r, obj_areas, obj_dists;
vector<uint32_t> obj_inds;
for(int i=0;i<obj_blobs.GetNumBlobs();i++) {
obj_blobs.GetNthBlob(CBlobGetArea(), i, cur_obj_blob);
CvBox2D obj_box = cur_obj_blob.GetEllipse();
obj_cents_x.push_back(obj_box.center.x);
obj_cents_y.push_back(obj_box.center.y);
obj_cents_r.push_back(obj_box.angle * CV_PI/180);
obj_areas.push_back(cur_obj_blob.Area());
obj_dists.push_back((obj_box.center.x-imgx/2)*(obj_box.center.x-imgx/2)+obj_box.center.y*obj_box.center.y);
obj_inds.push_back(i);
}
boost::function<bool(uint32_t, uint32_t)> sortind = boost::bind(&compind, _1, _2, obj_dists);
sort(obj_inds.begin(), obj_inds.end(), sortind);
obj_poses.poses.clear();
for(uint32_t i=0;i<obj_inds.size();i++) {
float posey = obj_cents_x[obj_inds[i]], posex = obj_cents_y[obj_inds[i]];
float poser = -obj_cents_r[obj_inds[i]] + 3*CV_PI/2;
geometry_msgs::Pose cpose;
cpose.position.x = posex/imgx*(maxx-minx) + minx;
cpose.position.y = posey/imgy*(maxy-miny) + miny;
cpose.position.z = table_height/256.0*(maxz-minz) + minz;
btMatrix3x3 quatmat; btQuaternion quat;
quatmat.setEulerZYX(poser, 0 , 0);
quatmat.getRotation(quat);
cpose.orientation.x = quat.x(); cpose.orientation.y = quat.y();
cpose.orientation.z = quat.z(); cpose.orientation.w = quat.w();
CvPoint centerpt; centerpt.y = posex; centerpt.x = posey;
printf("[%d](%f, %f, area: %f)\n", i, posex, posey, obj_areas[obj_inds[i]]);
IplImage height_img_max_ipl = height_img_max;
cvCircle(&height_img_max_ipl, centerpt, 3, cv::Scalar(200), 2);
obj_poses.poses.push_back(cpose);
}
obj_poses.header.stamp = ros::Time::now();
obj_poses.header.frame_id = base_frame;
obj_arr_pub.publish(obj_poses);
cv_bridge::CvImage cvb_height_img;
//cvb_height_img.image = height_img_avg;
//cvb_height_img.image = height_img_max;
//cvb_height_img.image = height_morph;
//cvb_height_img.image = obj_img;
//cvb_height_img.image = height_img_thresh_blob;
//cvb_height_img.image = table_blob;
//cvb_height_img.image = height_img_thresh;
cvb_height_img.image = above_table;
//cvb_height_img.image = table_edge;
cvb_height_img.header.stamp = ros::Time::now();
cvb_height_img.header.frame_id = base_frame;
cvb_height_img.encoding = enc::MONO8;
height_pub.publish(cvb_height_img.toImageMsg());
pc_full_frame.header.stamp = ros::Time::now();
pc_full_frame.header.frame_id = base_frame;
//pc_pub.publish(pc_full_frame);
if(obj_poses.poses.size() > 0)
obj_poses_found = true;
//delete element;
pc_lock.unlock();
}
void
Auvsi_Recognize::extractLetter( void )
{
typedef cv::Vec<unsigned char, 1> VT_binary;
#ifdef TWO_CHANNEL
typedef cv::Vec<T, 2> VT;
#else
typedef cv::Vec<T, 3> VT;
#endif
typedef cv::Vec<int, 1> IT;
// Erode input slightly
cv::Mat input;
cv::erode( _shape, input, cv::Mat() );
// Remove any small white blobs left over
CBlobResult blobs;
CBlob * currentBlob;
CBlob biggestBlob;
IplImage binaryIpl = input;
blobs = CBlobResult( &binaryIpl, NULL, 0 );
blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( &binaryIpl, cvScalar(0));
}
// Perform k-means on this region only
int areaLetter = (int)biggestBlob.Area();
cv::Mat kMeansInput = cv::Mat( areaLetter, 1, _image.type() );
// Discard if we couldn't extract a letter
if( areaLetter <= 0 )
{
_letter = cv::Mat( _shape );
_letter = cv::Scalar(0);
return;
}
cv::MatIterator_<VT_binary> binaryIterator = input.begin<VT_binary>();
cv::MatIterator_<VT_binary> binaryEnd = input.end<VT_binary>();
cv::MatIterator_<VT> kMeansIterator = kMeansInput.begin<VT>();
for( ; binaryIterator != binaryEnd; ++binaryIterator )
{
if( (*binaryIterator)[0] > 0 )
{
(*kMeansIterator) = _image.at<VT>( binaryIterator.pos() );
++kMeansIterator;
}
}
// Get k-means labels
cv::Mat labels = doClustering<T>( kMeansInput, 2, false );
int numZeros = areaLetter - cv::countNonZero( labels );
bool useZeros = numZeros < cv::countNonZero( labels );
// Reshape into original form
_letter = cv::Mat( _shape.size(), _shape.type() );
_letter = cv::Scalar(0);
binaryIterator = input.begin<VT_binary>();
binaryEnd = input.end<VT_binary>();
cv::MatIterator_<IT> labelsIterator = labels.begin<IT>();
for( int index = 0; binaryIterator != binaryEnd; ++binaryIterator )
{
if( (*binaryIterator)[0] > 0 )
{
// Whichever label was the minority, we make that value white and all other values black
unsigned char value = (*labelsIterator)[0];
if( useZeros )
if( value )
value = 0;
else
value = 255;
else
if( value )
value = 255;
else
value = 0;
_letter.at<VT_binary>( binaryIterator.pos() ) = VT_binary( value );
++labelsIterator;
}
}
}
void
Auvsi_Recognize::extractShape( void )
{
typedef cv::Vec<T, 1> VT;
// Reduce input to two colors
cv::Mat reducedColors = doClustering<T>( _image, 2 );
cv::Mat grayScaled, binary;
// Make output grayscale
grayScaled = convertToGray( reducedColors );
//cv::cvtColor( reducedColors, grayScaled, CV_RGB2GRAY );
// Make binary
double min, max;
cv::minMaxLoc( grayScaled, &min, &max );
cv::threshold( grayScaled, binary, min, 1.0, cv::THRESH_BINARY );
// ensure that background is black, image white
if( binary.at<VT>(0, 0)[0] > 0.0f )
cv::threshold( grayScaled, binary, min, 1.0, cv::THRESH_BINARY_INV );
binary.convertTo( binary, CV_8U, 255.0f );
// Fill in all black regions smaller than largest black region with white
CBlobResult blobs;
CBlob * currentBlob;
IplImage binaryIpl = binary;
blobs = CBlobResult( &binaryIpl, NULL, 255 );
// Get area of biggest blob
CBlob biggestBlob;
blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );
// Remove all blobs of smaller area
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( &binaryIpl, cvScalar(255));
}
// Fill in all small white regions black
blobs = CBlobResult( &binaryIpl, NULL, 0 );
blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( &binaryIpl, cvScalar(0));
}
binary = cv::Scalar(0);
biggestBlob.FillBlob( &binaryIpl, cvScalar(255));
_shape = binary;
}
void TabletopDetector::pcCallback(sensor_msgs::PointCloud2::ConstPtr pc_msg) {
if(!pc_lock.try_lock())
return;
pcl::PointCloud<pcl::PointXYZRGB> pc_full, pc_full_frame;
pcl::fromROSMsg(*pc_msg, pc_full);
string base_frame("/base_link");
ros::Time now = ros::Time::now();
tf_listener.waitForTransform(pc_msg->header.frame_id, base_frame, now, ros::Duration(3.0));
pcl_ros::transformPointCloud(base_frame, pc_full, pc_full_frame, tf_listener);
// pc_full_frame is in torso lift frame
cv::Mat cur_height_img = cv::Mat::zeros(imgx, imgy, CV_8U);
BOOST_FOREACH(const pcl::PointXYZRGB& pt, pc_full_frame.points) {
if(pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
continue;
int32_t x, y, z;
x = (pt.x - minx)/(maxx-minx) * imgx;
y = (pt.y - miny)/(maxy-miny) * imgy;
z = (pt.z - minz)/(maxz-minz) * 256;
if(x < 0 || y < 0) continue;
if(x >= imgx || y >= imgy) continue;
if(z < 0 || z >= 256) continue;
if(cur_height_img.at<uint8_t>(x, y) == 0 || cur_height_img.at<uint8_t>(x, y) < (uint8_t) z)
cur_height_img.at<uint8_t>(x, y) = (uint8_t) z;
}
cv::max(height_img_max, cur_height_img, height_img_max);
cv::Mat cur_height_img_flt;
cur_height_img.convertTo(cur_height_img_flt, CV_32F);
height_img_sum += cur_height_img_flt;
cv::Mat cur_count(imgx, imgy, CV_8U);
cur_count = (cur_height_img > 0) / 255;
cv::Mat cur_count_flt(imgx, imgy, CV_32F);
cur_count.convertTo(cur_count_flt, CV_32F);
height_img_count += cur_count_flt;
cv::Mat height_img_avg_flt = height_img_sum / height_img_count;
cv::Mat height_img_avg(imgx, imgy, CV_8U);
height_img_avg_flt.convertTo(height_img_avg, CV_8U);
height_img_avg = height_img_max;
cv::Mat height_hist(256, 1, CV_32F, cv::Scalar(0));
for(uint32_t x=0;x<imgx;x++)
for(uint32_t y=0;y<imgy;y++) {
if(height_img_avg.at<uint8_t>(x,y) == 255)
height_img_avg.at<uint8_t>(x,y) = 0;
if(height_img_avg.at<uint8_t>(x,y) != 0) {
height_hist.at<float>(height_img_avg.at<uint8_t>(x,y), 0)++;
}
}
////////////////////// Finding best table height /////////////////////////
uint32_t gfiltlen = 25;
float stddev = 256/(maxz-minz) * 0.015;
cv::Mat gauss_filt(gfiltlen, 1, CV_32F, cv::Scalar(0));
for(uint32_t i=0;i<gfiltlen;i++)
gauss_filt.at<float>(i,0) = 0.39894 / stddev * std::exp(-(i-((float)gfiltlen)/2)*(i-((float)gfiltlen)/2)/(2*stddev*stddev));
//cout << gauss_filt;
uint32_t maxval = 0, maxidx = 0;
for(uint32_t i=0;i<256-gfiltlen;i++) {
uint32_t sum = 0;
for(uint32_t j=0;j<gfiltlen;j++)
sum += height_hist.at<float>(i+j,0) * gauss_filt.at<float>(j,0);
if(sum > maxval && i != 0) {
maxval = sum;
maxidx = i+gfiltlen/2;
}
}
int32_t table_height = ((int32_t)maxidx);
//printf("%d %d, ", maxval, maxidx);
/////////////////////////// Getting table binary /////////////////////
cv::Mat height_img_thresh(imgx, imgy, CV_8U);
height_img_thresh = height_img_avg.clone();
for(uint32_t x=0;x<imgx;x++)
for(uint32_t y=0;y<imgy;y++) {
if(std::fabs(table_height - ((int32_t)height_img_thresh.at<uint8_t>(x,y))) < stddev*2)
height_img_thresh.at<uint8_t>(x,y) = 255;
else
height_img_thresh.at<uint8_t>(x,y) = 0;
}
//////////////////////////////////////////////////////////////////
IplImage height_img_thresh_ipl = height_img_thresh;
IplConvKernel* element = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT);
cvMorphologyEx(&height_img_thresh_ipl, &height_img_thresh_ipl, NULL, element, CV_MOP_CLOSE, num_closes);
//cvMorphologyEx(&height_img_thresh, &height_img_thresh, NULL, element, CV_MOP_OPEN, 2);
cv::Mat height_img_thresh_blob = height_img_thresh.clone();
IplImage blob_img = height_img_thresh_blob;
CBlobResult blobs = CBlobResult(&blob_img, NULL, 0);
//blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 10);
CBlob biggestblob;
blobs.GetNthBlob(CBlobGetArea(), 0, biggestblob);
cv::Mat table_blob(imgx, imgy, CV_8U, cv::Scalar(0)); IplImage table_blob_img = table_blob;
biggestblob.FillBlob(&table_blob_img, cv::Scalar(150));
//drawCvBox2D(blob_img, table_roi, cv::Scalar(50), 1);
CvBox2D table_roi = biggestblob.GetEllipse(); table_roi.angle *= CV_PI/180;
cv::Mat table_hull(imgx, imgy, CV_8U, cv::Scalar(0));
IplImage hull_img = table_hull;
fillCvBox2D(hull_img, table_roi, cv::Scalar(255));
//printf("Cvbox: %f, %f, %f, %f, %f\n", table_roi.center.x, table_roi.center.y, table_roi.size.width, table_roi.size.height, table_roi.angle);
//cv::Mat height_morph(imgx, imgy, CV_8U, cv::Scalar(0));
//cv::Mat tmp_img(imgx, imgy, CV_8U, cv::Scalar(0));
//IplImage t1 = height_img_thresh; IplImage = height_morph;
cv::Mat table_edge(imgx, imgy, CV_8U);
cv::Sobel(table_blob, table_edge, CV_8U, 0, 1, 1);
cv::Mat above_table(imgx, imgy, CV_8U);
bitwise_and(height_img_max > table_height + stddev*2, table_hull, above_table);
IplImage above_table_img = above_table;
CBlobResult obj_blobs = CBlobResult(&above_table_img, NULL, 0);
CBlob biggest_obj_blob;
double objcentx = 0, objcenty = 0;
if(obj_blobs.GetNumBlobs() > 0) {
obj_blobs.GetNthBlob(CBlobGetArea(), 0, biggest_obj_blob);
CvBox2D obj_box = biggest_obj_blob.GetEllipse();
objcenty = obj_box.center.x, objcentx = obj_box.center.y;
}
//double objcentx = 0, objcenty = 0;
//cv::Mat table_edge = height_morph.clone();
//cvMorphologyEx(&height_img, &height_morph, NULL, element, CV_MOP_CLOSE);
//cvFillPoly(&ipl_hull, rpts, npts, 1, cv::Scalar(255));
//cv::Mat obj_img(imgx, imgy, CV_8U, cv::Scalar(0));
//std::vector<int32_t> xfeats, yfeats, zfeats;
//double sumobjx = 0, sumobjy = 0, sumobja = 0;
//for(uint32_t y=0;y<imgx;y++)
// for(uint32_t x=0;x<imgx;x++)
// if(table_hull.at<uint8_t>(x,y) == 255 && height_morph.at<uint8_t>(x,y) == 0
// && height_img_avg.at<uint8_t>(x,y) > table_height + stddev*2) {
// obj_img.at<uint8_t>(x,y) = height_img_avg.at<uint8_t>(x,y);
// sumobjx += x; sumobjy += y; sumobja ++;
// //xfeats.push_back(x); yfeats.push_back(y);
// //zfeats.push_back(height_img.at<uint8_t>(x,y));
// }
//double objcentx = sumobjx/sumobja, objcenty = sumobjy/sumobja;
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq* lines = 0;
cv::Mat lines_img = height_img_max.clone();
IplImage lines_img_ipl = lines_img;
IplImage table_edge_ipl = table_edge;
cvMorphologyEx(&table_edge_ipl, &table_edge_ipl, NULL, element, CV_MOP_DILATE, num_edge_dilate);
lines = cvHoughLines2(&table_edge_ipl, storage, CV_HOUGH_STANDARD, 1, degree_bins*CV_PI/180, hough_thresh, 0, 0);
vector<float> theta_bins, rho_bins;
vector<uint32_t> count_bins;
for(uint32_t i=0; i < (uint32_t) lines->total; i++) {
float* line = (float*)cvGetSeqElem(lines, i);
float rho = line[0];
float theta = line[1];
bool found_same = false;
for(int32_t j=theta_bins.size()-1; j >= 0; j--) {
if(fabs(theta_bins[j]/count_bins[j] - theta) < theta_gran &&
fabs(rho_bins[j]/count_bins[j] - rho) < rho_gran) {
theta_bins[j] += theta;
rho_bins[j] += rho;
count_bins[j]++;
found_same = true;
break;
}
}
if(!found_same) {
theta_bins.push_back(theta);
rho_bins.push_back(rho);
count_bins.push_back(1);
}
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
CvPoint pt1, pt2;
a = cos(theta); b = sin(theta);
//a = cos(theta+CV_PI/2); b = sin(theta+CV_PI/2);
//x0 = objcenty; y0 = objcentx;
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
cvLine(&lines_img_ipl, pt1, pt2, cv::Scalar(100), 2, 8 );
}
//delete[] lines;
for(uint32_t i=0;i<theta_bins.size();i++) {
theta_bins[i] /= count_bins[i];
rho_bins[i] /= count_bins[i];
}
vector<float> posesx, posesy, poses_theta, dists_obj;
vector<uint32_t> pose_inds;
for(uint32_t i=0;i<theta_bins.size();i++) {
double theta = theta_bins[i];
double rho = rho_bins[i];
double a1 = cos(theta-CV_PI/2), b1 = sin(theta-CV_PI/2);
double a2 = cos(theta-CV_PI), b2 = sin(theta-CV_PI);
double vvcl = a2*b1-b2*a1, deltpx = cos(theta)*rho-objcenty, deltpy = sin(theta)*rho-objcentx;
double pvcr = deltpx*b1 - deltpy*a1;
double t = pvcr/vvcl;
double posey = objcenty + t*a2, posex = objcentx + t*b2;
printf("\naPose %d: (t: %f, %f, %f)[%f, %f](%f, %f)[%f, %f](1 %f, %f)(2 %f, %f)[theta %f, %f]\n", i, t, posex, posey, t*a2, t*b2, a1*rho, b1*rho, objcentx, objcenty, a1, b1, a2, b2, theta, rho);
if(posex == posex && posey == posey &&
posex >= 0 && posey >= 0 &&
posex < imgx && posey < imgy) {
posesx.push_back(posex); posesy.push_back(posey); poses_theta.push_back(theta);
pose_inds.push_back(posesx.size()-1);
float dist = (posex-objcentx)*(posex-objcentx)+(posey-objcenty)*(posey-objcenty);
dists_obj.push_back(dist);
}
//lines_img.at<uint8_t>(posex, posey)
}
boost::function<bool(uint32_t, uint32_t)> sortind = boost::bind(&compind, _1, _2, dists_obj);
sort(pose_inds.begin(), pose_inds.end(), sortind);
vector<float> retposesx, retposesy, retposesr;
grasp_points.poses.clear();
for(uint32_t i=0;i<pose_inds.size();i++) {
float posex = posesx[pose_inds[i]], posey = posesy[pose_inds[i]];
float poser = -poses_theta[pose_inds[i]] + 3*CV_PI/2;
bool same_found = false;
for(int32_t j=((int)retposesx.size())-1;j>=0;j--) {
if(fabs(posex - retposesx[j]) < xgran &&
fabs(posey - retposesy[j]) < ygran) {
same_found = true;
}
}
if(!same_found) {
retposesx.push_back(posex);
retposesy.push_back(posey);
retposesr.push_back(poser);
geometry_msgs::Pose cpose;
cpose.position.x = posex/imgx*(maxx-minx) + minx;
cpose.position.y = posey/imgy*(maxy-miny) + miny;
cpose.position.z = table_height/256.0*(maxz-minz) + minz;
btMatrix3x3 quatmat; btQuaternion quat;
quatmat.setEulerZYX(poser, 0 , 0);
quatmat.getRotation(quat);
cpose.orientation.x = quat.x(); cpose.orientation.y = quat.y();
cpose.orientation.z = quat.z(); cpose.orientation.w = quat.w();
grasp_points.poses.push_back(cpose);
}
}
grasp_points.header.stamp = ros::Time::now();
grasp_points.header.frame_id = base_frame;
pose_arr_pub.publish(grasp_points);
printf("\nCenter (%f, %f)\n", objcentx, objcenty);
for(uint32_t i=0;i<retposesx.size();i++) {
//for(uint32_t i=0;i<1;i++) {
printf("\nPose %d: (%f, %f, r: %f)\n", i, retposesx[i], retposesy[i], retposesr[i]);
//CvPoint centerpt; centerpt.x = objcenty; centerpt.y = objcentx;
CvPoint centerpt; centerpt.x = retposesy[i]; centerpt.y = retposesx[i];
cvCircle(&lines_img_ipl, centerpt, 3, cv::Scalar(200), 2);
}
//cv::Mat obj_feats(xfeats.size(), 1, CV_32S, cv::Scalar(0));
//for(uint32_t i=0;i<xfeats.size();i++) {
// obj_feats.at<uint32_t>(i,0) = xfeats[i]; obj_feats.at<uint32_t>(i,1) = yfeats[i]; obj_feats.at<uint32_t>(i,2) = zfeats[i];
//}
//cvflann::KMeansIndexParams kmips;
//kmips.branching = 32;
//kmips.iterations = 11;
//kmips.centers_init = cvflann::CENTERS_RANDOM;
//kmips.cb_index = 0.2;
//cv::Mat obj_centers;
//CvMat obj_feats_mat = obj_feats;
////cvflann::Matrix<uint32_t> obj_feats_mat;
////cvflann::Matrix<cvflann::EUCLIDEAN> obj_centers_mat;
//int num_clust = cvflann::hierarchicalClustering<CV_32S,CV_32S>(obj_feats_mat, obj_centers, kmips);
//printf("\nNum clust: %d \n", num_clust);
cv::Mat table_edge2 = table_edge.clone();
IplImage table_edge_ipl2 = table_edge2;
cvMorphologyEx(&table_edge_ipl2, &table_edge_ipl2, NULL, element, CV_MOP_DILATE, 3);
BOOST_FOREACH(const pcl::PointXYZRGB& pt, pc_full_frame.points) {
if(pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
continue;
int32_t x, y, z;
x = (pt.x - minx)/(maxx-minx) * imgx;
y = (pt.y - miny)/(maxy-miny) * imgy;
z = (pt.z - minz)/(maxz-minz) * 256;
if(x < 0 || y < 0) continue;
if(x >= imgx || y >= imgy) continue;
if(z < 0 || z >= 256) continue;
if(table_blob.at<uint8_t>(x,y) == 255 &&
std::fabs(table_height - z) < stddev*2) {
uint32_t red = 0xFFFF0000;
((uint32_t*) &pt.rgb)[0] = red;
}
if(table_edge2.at<uint8_t>(x,y) == 255 &&
std::fabs(table_height - z) < stddev*4) {
uint32_t blue = 0xFF0000FF;
((uint32_t*) &pt.rgb)[0] = blue;
}
}
cv_bridge::CvImage cvb_height_img;
//cvb_height_img.image = height_img_avg;
//cvb_height_img.image = height_img_max;
//cvb_height_img.image = height_morph;
//cvb_height_img.image = obj_img;
cvb_height_img.image = lines_img;
//cvb_height_img.image = height_img_thresh_blob;
//cvb_height_img.image = table_blob;
//cvb_height_img.image = height_img_thresh;
//cvb_height_img.image = above_table;
//cvb_height_img.image = table_edge;
cvb_height_img.header.stamp = ros::Time::now();
cvb_height_img.header.frame_id = base_frame;
cvb_height_img.encoding = enc::MONO8;
height_pub.publish(cvb_height_img.toImageMsg());
pc_full_frame.header.stamp = ros::Time::now();
pc_full_frame.header.frame_id = base_frame;
pc_pub.publish(pc_full_frame);
if(grasp_points.poses.size() > 0)
grasp_points_found = true;
//delete element;
pc_lock.unlock();
}
int main(int argc, char *argv[])
{
CvCapture* capture = cvCreateFileCapture( "recording_01.avi");
handOrientation rightOrientationLast = NONE, leftOrientationLast = NONE;
handOrientation rightOrientationCur = NONE, leftOrientationCur = NONE;
//cvNamedWindow("Input Image", CV_WINDOW_AUTOSIZE);
//cvNamedWindow("Skin Pixels", CV_WINDOW_AUTOSIZE);
cvNamedWindow("Skin Blobs", CV_WINDOW_AUTOSIZE);
while(1){
Mat imageBGR = cvQueryFrame(capture);
if(imageBGR.empty())break;
//imshow("Input Image", imageBGR);
// Convert the image to HSV colors.
Mat imageHSV = Mat(imageBGR.size(), CV_8UC3); // Full HSV color image.
cvtColor(imageBGR, imageHSV, CV_BGR2HSV); // Convert from a BGR to an HSV image.
std::vector<Mat> channels(3);
split(imageHSV, channels);
Mat planeH = channels[0];
Mat planeS = channels[1];
Mat planeV = channels[2];
// Detect which pixels in each of the H, S and V channels are probably skin pixels.
threshold(channels[0], channels[0], 150, UCHAR_MAX, CV_THRESH_BINARY_INV);//18
threshold(channels[1], channels[1], 60, UCHAR_MAX, CV_THRESH_BINARY);//50
threshold(channels[2], channels[2], 170, UCHAR_MAX, CV_THRESH_BINARY);//80
// Combine all 3 thresholded color components, so that an output pixel will only
// be white if the H, S and V pixels were also white.
Mat imageSkinPixels = Mat( imageBGR.size(), CV_8UC3); // Greyscale output image.
bitwise_and(channels[0], channels[1], imageSkinPixels); // imageSkin = H {BITWISE_AND} S.
bitwise_and(imageSkinPixels, channels[2], imageSkinPixels); // imageSkin = H {BITWISE_AND} S {BITWISE_AND} V.
// Show the output image on the screen.
//imshow("Skin Pixels", imageSkinPixels);
IplImage ipl_imageSkinPixels = imageSkinPixels;
// Find blobs in the image.
CBlobResult blobs;
blobs = CBlobResult(&ipl_imageSkinPixels, NULL, 0); // Use a black background color.
// Ignore the blobs whose area is less than minArea.
blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, minBlobArea);
srand (time(NULL));
// Show the large blobs.
IplImage* imageSkinBlobs = cvCreateImage(imageBGR.size(), 8, 3); //Colored Output//,1); Greyscale output image.
for (int i = 0; i < blobs.GetNumBlobs(); i++) {
CBlob *currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob(imageSkinBlobs, CV_RGB(rand()%255,rand()%255,rand()%255)); // Draw the large blobs as white.
cvDrawRect(imageSkinBlobs,
cvPoint(currentBlob->GetBoundingBox().x,currentBlob->GetBoundingBox().y),
cvPoint(currentBlob->GetBoundingBox().x + currentBlob->GetBoundingBox().width,currentBlob->GetBoundingBox().y + currentBlob->GetBoundingBox().height),
cvScalar(0,0,255),
2);//Draw Bounding Boxes
}
cvShowImage("Skin Blobs", imageSkinBlobs);
//Gestures
//std::cout << "Number of Blobs: "<< blobs.GetNumBlobs() <<endl;
if(blobs.GetNumBlobs() == 0){
//picture empty
}else if(blobs.GetNumBlobs() == 1) {
//head detected
}else if(blobs.GetNumBlobs() == 2 || blobs.GetNumBlobs() == 3){
//head + one hand || head + two hands
CvRect rect[3];
int indexHead = -1, indexHandLeft = -1, indexHandRight = -1;
//Get Bounding Boxes
for(int i = 0; i< blobs.GetNumBlobs(); i++){
rect[i] = blobs.GetBlob(i)->GetBoundingBox();
}
//Detect Head and Hand indexes
if(blobs.GetNumBlobs() == 2){
int indexHand = -1;
if(getCenterPoint(rect[0]).y < getCenterPoint(rect[1]).y){
indexHead = 0;
indexHand = 1;
}else{
indexHead = 1;
indexHand = 0;
}
if(getHandside(rect[indexHead], rect[indexHand]) == LEFT){
indexHandLeft = 1;
indexHandRight = -1;
}else{
// right hand
indexHandLeft = -1;
indexHandRight = 1;
}
}else{
//two hands
int indexHand1 = -1;
int indexHand2 = -1;
if(getCenterPoint(rect[0]).y < getCenterPoint(rect[1]).y && getCenterPoint(rect[0]).y < getCenterPoint(rect[2]).y){
indexHead = 0;
indexHand1 = 1;
indexHand2 = 2;
}else if(getCenterPoint(rect[1]).y < getCenterPoint(rect[0]).y && getCenterPoint(rect[1]).y < getCenterPoint(rect[2]).y){
indexHead = 1;
indexHand1 = 0;
indexHand2 = 2;
}else{
indexHead = 2;
indexHand1 = 0;
indexHand2 = 1;
}
if(getHandside(rect[indexHead], rect[indexHand1]) == LEFT){
indexHandLeft = indexHand1;
indexHandRight = indexHand2;
}else{
indexHandLeft = indexHand2;
indexHandRight = indexHand1;
}
}
// follow the right hand
if(indexHandRight > 0) {
//std::cout << "right hand deteced" <<endl;
if(isMoving(handRight)) {
std::cout << "hand moving" <<endl;
handRight.centerPrev = handRight.centerCurr;
handRight.centerCurr = getCenterPoint(rect[indexHandRight]);
} else {
std::cout << "hand not moving" <<endl;
if(handRight.centerInit.y != 0 && abs(handRight.centerInit.y - handRight.centerCurr.y) > 20) {
if(handRight.centerInit.y < handRight.centerCurr.y) {
// hand moved down
std::cout << " hand moved down" <<endl;
} else {
// hand moved up
std::cout << " hand moved up" <<endl;
}
}
handRight.centerInit = getCenterPoint(rect[indexHandRight]);
handRight.centerPrev = handRight.centerCurr;
handRight.centerCurr = getCenterPoint(rect[indexHandRight]);
}
}
//Get Orientations from Hand rects
leftOrientationCur = (indexHandLeft != -1)?getOrientationOfRect(rect[indexHandLeft]):NONE;
rightOrientationCur = (indexHandRight != -1)?getOrientationOfRect(rect[indexHandRight]):NONE;
//Check Change of Left hand
/*switch(detectHandStateChange(leftOrientationLast, leftOrientationCur)){
case PORTRAIT_TO_LANDSCAPE:
handleGestures(LEFT_FLIP_DOWN);
break;
case LANDSCAPE_TO_PORTRAIT:
handleGestures(LEFT_FLIP_UP);
break;
case NOCHANGE:
default:
break;
}
//Check Change of Right hand
switch(detectHandStateChange(rightOrientationLast, rightOrientationCur)){
case PORTRAIT_TO_LANDSCAPE:
handleGestures(RIGHT_FLIP_DOWN);
break;
case LANDSCAPE_TO_PORTRAIT:
handleGestures(RIGHT_FLIP_UP);
break;
case NOCHANGE:
default:
break;
}*/
}else if(blobs.GetNumBlobs() > 3){
//too much information
cout<<"too much information"<<endl;
}
leftOrientationLast = leftOrientationCur;
rightOrientationLast = rightOrientationCur;
// Free all the resources.
/*cvReleaseImage( &imageBGR );
cvReleaseImage( &imageHSV );
cvReleaseImage( &planeH );
cvReleaseImage( &planeS );
cvReleaseImage( &planeV );
cvReleaseImage( &imageSkinPixels );
cvReleaseImage( &imageSkinBlobs );*/
//if ESC is pressed then exit loop
cvWaitKey(33);
}
cvWaitKey(0);
return 0;
}
void MyWindow::on_updateImages()
{
if(video_being_processed)
{
cam>>camImage;
cv::resize(camImage,colorImage,cv::Size(320,240));
cv::cvtColor(colorImage,thImage,CV_RGB2HSV_FULL);
using namespace cv;
inRange(thImage,start,end,thImage);
blur(thImage, thImage, Size(blurVal,blurVal));
medianBlur(thImage,thImage,mblurVal);
GaussianBlur(thImage, thImage, Size(gblurVal,gblurVal), 0);
using namespace cv;
Mat dilatekernel;
if(kernelShape == 0)
dilatekernel = getStructuringElement(MORPH_ELLIPSE, Size(kernelSizeVal, kernelSizeVal));
if(kernelShape == 1)
dilatekernel = getStructuringElement(MORPH_RECT, Size(kernelSizeVal, kernelSizeVal));
if(kernelShape == 2)
dilatekernel = getStructuringElement(MORPH_CROSS, Size(kernelSizeVal, kernelSizeVal));
// First and second both argument selectable
if(dilate_b)
dilate(thImage, thImage, dilatekernel);
if(erode_b)
erode(thImage, thImage, dilatekernel);
/////////////////////////////////////////////////////////////
if(blob_enabled){
IplImage Iipl = thImage;
CBlobResult blobs, blobsclutter;
CBlob * currentBlob;
blobs = CBlobResult(&Iipl, NULL, bg_r);
// The last parameter indicates the background. That is the color
// on which we are searching for the clutter.
// The last parameter in fillblob is the color with which we want to fill
// the clutter that we found.
blobs.Filter(blobs, filter_arg2, CBlobGetArea(), filter_arg4, filter_arg5_area);
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob(&Iipl,Scalar(fg_r));
}
// blobsclutter = CBlobResult(&Iipl, NULL, 0);
// blobsclutter.Filter(blobsclutter, B_INCLUDE, CBlobGetArea(), B_LESS, 500);
// for (int i = 0; i < blobsclutter.GetNumBlobs(); i++ )
// {
// currentBlob = blobsclutter.GetBlob(i);
// currentBlob->FillBlob(&Iipl,Scalar(0));
// }
}
binaryImage = thImage;
Mat drawing;
if(draw_contours){
// Drawing contours
int thresh = 100;
RNG rng(12345);
Mat threshold_output;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
threshold( thImage, threshold_output, thresh, 255, THRESH_BINARY );
/// Find contours
findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
/// Approximate contours to polygons + get bounding rects and circles
vector<vector<Point> > contours_poly( contours.size() );
vector<Rect> boundRect( contours.size() );
vector<Point2f>center( contours.size() );
vector<float>radius( contours.size() );
for( int i = 0; i < contours.size(); i++ )
{ approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true );
boundRect[i] = boundingRect( Mat(contours_poly[i]) );
minEnclosingCircle( (Mat)contours_poly[i], center[i], radius[i] );
}
// Draw polygonal contour + bonding rects + circles
drawing = Mat::zeros( threshold_output.size(), CV_8UC3 );
for( int i = 0; i< contours.size(); i++ )
{
Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
if(polyContours)
drawContours( colorImage, contours_poly, i, color, 1, 8, vector<Vec4i>(), 0, Point() );
if(rectContours)
rectangle( colorImage, boundRect[i].tl(), boundRect[i].br(), color, 2, 8, 0 );
if(circleContours)
circle( colorImage, center[i], (int)radius[i], color, 2, 8, 0 );
}
}
if(hough_enabled){
vector<Vec3f> circles;
/// Apply the Hough Transform to find the circles
HoughCircles( thImage, circles, CV_HOUGH_GRADIENT, 2, thImage.rows/8, 200, 100, min_radius, max_radius);
/// Draw the circles detected
for( size_t i = 0; i < circles.size(); i++ )
{
Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
// circle center
circle( colorImage, center, 3, Scalar(0,255,0), -1, 8, 0 );
// circle outline
circle( colorImage, center, radius, Scalar(0,0,255), 3, 8, 0 );
}
}
if(hough_lines_enabled){
vector<Vec4i> lines;
HoughLinesP(thImage, lines, 1, CV_PI/180, 50, 50, 10 );
for( size_t i = 0; i < lines.size(); i++ )
{
Vec4i l = lines[i];
line( colorImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0,0,255), 3, CV_AA);
}
}
displayImages();
}
}
//==============================================================================
void PanTiltCameraClass::blobTracking(IplImage* hsv_mask,
IplImage* pFour,
IplImage* pImg)
{
//--- Get blobs and filter them using the blob area
CBlobResult blobs;
CBlob *currentBlob;
//--- Create a thresholded image and display image --------------------
//--- Creates binary image
IplImage* originalThr = cvCreateImage(cvGetSize(hsv_mask), IPL_DEPTH_8U,1);
//--- Create 3-channel image
IplImage* display = cvCreateImage(cvGetSize(hsv_mask),IPL_DEPTH_8U,3);
//--- Copies the original
cvMerge( hsv_mask, hsv_mask, hsv_mask, NULL, display );
//--- Makes a copy for processing
cvCopy(hsv_mask,originalThr);
//--- Find blobs in image ---------------------------------------------
int blobThreshold = 0;
bool blobFindMoments = true;
blobs = CBlobResult( originalThr, originalThr, blobThreshold, blobFindMoments);
//--- filters blobs according to size and radius constraints
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, this->minBlobSize );
//--- display filtered blobs ------------------------------------------
//--- copies the original in (for background)
cvMerge( originalThr, originalThr, originalThr, NULL, display );
CvPoint pts[this->NUMBER_OF_CIRCLES];
//--- This sequence marks all the blobs
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
currentBlob->FillBlob( display, CV_RGB(0,0,255));
//--- Get blobs centerpoint
CvPoint bcg;
bcg.x = (int)(currentBlob->MinX()+((currentBlob->MaxX()-currentBlob->MinX())/2));
bcg.y = (int)(currentBlob->MinY()+((currentBlob->MaxY()-currentBlob->MinY())/2));
//--- Print the CG on the picture
char blobtext[40];
for(int k=0;k<this->NUMBER_OF_CIRCLES;k++)
{
sprintf(blobtext,"%d",k+1);
TargetReticle(display,&pts[k],blobtext,6,CV_RGB(255,0,0));
}//for
}//for each blob
//--- Set the ROI in the pFour image
cvSetImageROI(pFour,cvRect(pImg->width,pImg->height+80,pImg->width,pImg->height));
cvCopy(display,pFour);
//Reset region of interest
cvResetImageROI(display);
//Clean up
cvReleaseImage( &originalThr );
cvReleaseImage( &display);
}
int main(){
Scalar robotColor = CV_RGB(255, 0, 0);
Scalar rightColor = CV_RGB(0, 255, 0);
Scalar leftColor = CV_RGB(0, 0, 255);
Scalar robotColor_2 = CV_RGB(0, 255, 255);
Scalar rightColor_2 = CV_RGB(255, 0, 255);
Scalar leftColor_2 = CV_RGB(255, 255, 0);
int lowH = 0;
int highH = 14;
int top_cut = 120;
int bot_cut = 70;
int lowV = 200;
int type = 0;
int ticks = 0;
int nb_errors = 0;
int len = 150;
int trace = 25;
int sensitivity = 100;
int area = 3000;
int flip = 0; //set to 0 if no flips are needed, 1 for y axis, 2 for x axis and 3 for both
namedWindow("My Window", 1);
createTrackbar("lowH", "My Window", &lowH, 180);
createTrackbar("highH", "My Window", &highH, 180);
createTrackbar("top_cut", "My Window", &top_cut, 255);
createTrackbar("bot_cut", "My Window", &bot_cut, 255);
createTrackbar("lowV", "My Window", &lowV, 255);
createTrackbar("LEN", "My Window", &len, 300);
createTrackbar("TRACE", "My Window", &trace, 100);
createTrackbar("SENSITIVITY", "My Window", &sensitivity, 200);
createTrackbar("AREA", "My Window", &area, 7000);
createTrackbar("FLIP", "My Window", &flip, 3);
moveWindow("My Window", 0, 0);
namedWindow("kalman", 1);
moveWindow("kalman", 500, 0);
namedWindow("Blobs Image", 1);
moveWindow("Blobs Image", 500, 300);
namedWindow("frame", 1);
moveWindow("frame", 0, 500);
namedWindow("test", WINDOW_AUTOSIZE);
moveWindow("test", 0, 500);
namedWindow("white", WINDOW_AUTOSIZE);
moveWindow("white", 0, 500);
//file of video input
string filename = "testVideo_5.webm";
ofstream logs;
ofstream stats;
stats.open("stats.txt");
logs.open("logs.csv");
logs << "Left_x,Left_y,Left_holds,Right_x,Right_y,Right_holds,confirmed" << endl;
Point center_window = Point(WIDTH/2, (HEIGHT - top_cut - bot_cut)/2);
Point center_left = Point(WIDTH/4, .5*max(10, HEIGHT - top_cut - bot_cut));
Point center_right = Point(3*WIDTH/4, .5*max(10, HEIGHT - top_cut - bot_cut));
// initialize the kalman filters
KalmanFilter KF_left(4, 2, 0);
KalmanFilter KF_right(4, 2, 0);
Mat_<float> measurement_left(2,1); measurement_left.setTo(Scalar(0));
Mat_<float> measurement_right(2,1); measurement_right.setTo(Scalar(0));
initialize_kalman(&KF_left, center_left);
initialize_kalman(&KF_right, center_right);
VideoCapture cap(0);
// VideoCapture cap(filename);
Mat kf_img(HEIGHT - top_cut - bot_cut, WIDTH, CV_8UC3);
vector<Point> mousev_left,kalmanv_left;
mousev_left.clear();
kalmanv_left.clear();
vector<Point> mousev_right,kalmanv_right;
mousev_right.clear();
kalmanv_right.clear();
int counter = 0;
int nb_confirmed = 0;
int nb_total = 0;
double average_left = 0;
double average_right = 0;
double error_left = 0;
double error_right = 0;
double prev_dist = norm(center_left - center_right);
double new_dist = prev_dist;
bool left_valid = false;
bool right_valid = true;
Mat temp = Mat::zeros(100,400, CV_8UC3);
putText(temp, "Press any key to start", Point(50,50), FONT_HERSHEY_SIMPLEX, .5, Scalar(255,255,255));
putText(temp, "and ESC to end", Point(50, 75), FONT_HERSHEY_SIMPLEX, .5, Scalar(255,255,255));
imshow("Blobs Image", temp);
waitKey(-1);
int key;
bool eof = false;
for(;;){
Mat frame;
Mat prediction_left = KF_left.predict();
Point new_left(prediction_left.at<float>(0), prediction_left.at<float>(1));
measurement_left(0) = center_left.x;
measurement_left(1) = center_left.y;
Mat estimated_left = KF_left.correct(measurement_left);
Point statePt_left(estimated_left.at<float>(0),estimated_left.at<float>(1));
Point measPt_left(measurement_left(0),measurement_left(1));
Mat prediction_right = KF_right.predict();
Point new_right(prediction_right.at<float>(0), prediction_right.at<float>(1));
measurement_right(0) = center_right.x;
measurement_right(1) = center_right.y;
Mat estimated_right = KF_right.correct(measurement_right);
Point statePt_right(estimated_right.at<float>(0),estimated_right.at<float>(1));
Point measPt_right(measurement_right(0),measurement_right(1));
ticks ++;
error_left = norm(statePt_left - measPt_left);
average_left = ((average_left * (ticks - 1)) + error_left) / ticks;
error_right = norm(statePt_right - measPt_right);
average_right = ((average_right * (ticks - 1)) + error_right) / ticks;
imshow("kalman", kf_img);
// waitKey(-1);
kf_img = Scalar::all(0);
mousev_left.push_back(measPt_left);
kalmanv_left.push_back(statePt_left);
circle(kf_img, statePt_left, 1, Scalar(255,255,255), -1);
circle(kf_img, measPt_left, 1, Scalar(0,0,255), -1);
int nb_mousev_left = mousev_left.size() - 1;
int nb_kalmanv_left = mousev_left.size() - 1;
int nb_mousev_right = mousev_left.size() - 1;
int nb_kalmanv_right = mousev_left.size() - 1;
for(int i = max(0, nb_mousev_left - trace); i< nb_mousev_left; i++){
line(kf_img, mousev_left[i], mousev_left[i+1], Scalar(255,255,0), 1);
}
for(int i = max(0, nb_kalmanv_left - trace); i< nb_kalmanv_left; i++){
line(kf_img, kalmanv_left[i], kalmanv_left[i+1], Scalar(0,0,255), 1);
}
mousev_right.push_back(measPt_right);
kalmanv_right.push_back(statePt_right);
circle(kf_img, statePt_right, 1, Scalar(255,255,255), -1);
circle(kf_img, measPt_right, 1, Scalar(0,0,255), -1);
for(int i = max(0, nb_mousev_right - trace); i< nb_mousev_right; i++){
line(kf_img, mousev_right[i], mousev_right[i+1], Scalar(0,255,0), 1);
}
for(int i = max(0, nb_kalmanv_right - trace); i< nb_kalmanv_right; i++){
line(kf_img, kalmanv_right[i], kalmanv_right[i+1], Scalar(255,0,255), 1);
}
Rect border(0, top_cut, WIDTH, max(10, HEIGHT - top_cut - bot_cut));
cap >> frame;
if(!frame.empty()){
Mat image;
int flip_type = 1;
switch (flip) {
case 0: break;
case 1: break;
case 2: flip_type = 0;
break;
case 3: flip_type = -1;
break;
}
if(flip) cv::flip(frame, frame, flip_type);
resize(frame, frame, Size(WIDTH, HEIGHT));
image = frame(border);
imshow("frame", image);
//performs the skin detection
Mat converted_skin;
cvtColor(image, converted_skin, CV_BGR2HSV);
Mat skin_masked;
inRange(converted_skin, Scalar(min(lowH, highH), 48, 80),Scalar(max(lowH, highH), 255, 255), skin_masked);
imshow("test", skin_masked);
//performs the robot detection
Mat converted_white, white_masked, lights_masked;
cvtColor(image, converted_white, CV_BGR2GRAY);
inRange(converted_skin, Scalar(0, 0, 245), Scalar(180, 255, 255), lights_masked);
threshold(converted_white, white_masked, lowV, 255, type);
bitwise_or(white_masked, lights_masked, white_masked);
imshow("white", white_masked);
Mat copy(converted_skin.size(), converted_skin.type());// = converted.clone();
//detects hands as blobs
CBlobResult blobs;
IplImage temp = (IplImage)skin_masked;
blobs = CBlobResult(&temp,NULL,1);
blobs = CBlobResult(skin_masked,Mat(),NUMCORES);
int numBlobs = blobs.GetNumBlobs();
if(0 == numBlobs){
cout << "can't find blobs!" << endl;
continue;
}
// detects robot as a blob
CBlobResult robot_blobs;
IplImage robot_temp = (IplImage) white_masked;
robot_blobs = CBlobResult(&robot_temp, NULL, 1);
robot_blobs = CBlobResult(white_masked, Mat(), NUMCORES);
if(0 == robot_blobs.GetNumBlobs()){
cout << "can't find robot_blobs!" << endl;
continue;
}
CBlob *curblob;
CBlob* blob_1;
CBlob* blob_2;
CBlob* leftBlob;
CBlob* rightBlob;
CBlob* robotBlob;
copy.setTo(Vec3b(0,0,0));
// chooses the two largest blobs for the hands
Point center_1, center_2;
int max_1 = 0;
int max_2 = 0;
int maxArea_1 = 0;
int maxArea_2 = 0;
for(int i=0;i<numBlobs;i++){
int area = blobs.GetBlob(i)->Area();
if(area > maxArea_1){
maxArea_2 = maxArea_1;
maxArea_1 = area;
max_2 = max_1;
max_1 = i;
} else if(area > maxArea_2){
maxArea_2 = area;
max_2 = i;
}
}
int i_1 = max_1;
int i_2 = max_2;
double area_left, area_right;
Rect rect_1;
Rect rect_2;
//determines which hand is left/right
blob_1 = blobs.GetBlob(i_1);
blob_2 = blobs.GetBlob(i_2);
center_1 = blob_1->getCenter();
center_2 = blob_2->getCenter();
bool left_is_1 = (center_1.x < center_2.x)? true : false;
leftBlob = (left_is_1)? blob_1 : blob_2;
rightBlob = (left_is_1)? blob_2 : blob_1;
center_left = leftBlob->getCenter();
center_right = rightBlob->getCenter();
//determine the number of valid hands
//validity is decided by whether or not the hand followed a logical movement,
//and if the area of the blob is large enough to be accepted
int valids = 0;
rect_1 = leftBlob->GetBoundingBox();
rectangle(copy, rect_1.tl(), rect_1.br(), leftColor_2, 5);
error_left = norm(statePt_left - center_left);
area_left = leftBlob->Area();
left_valid = error_left < sensitivity && area_left > area;
if(left_valid){
leftBlob->FillBlob(copy,leftColor, true);
valids ++;
}
circle(copy, center_left, 5, leftColor_2, -1);
rect_2 = rightBlob->GetBoundingBox();
rectangle(copy, rect_2.tl(), rect_2.br(), rightColor_2, 5);
error_right = norm(statePt_right - center_right);
area_right = rightBlob->Area();
right_valid = error_right < sensitivity && area_right > area;
if(right_valid){
rightBlob->FillBlob(copy,rightColor, true);
valids ++;
}
circle(copy, center_right, 5, rightColor_2, -1);
//finds the blob representing the robot
//we could add a restriction to only choose a blob between the two hands
//in terms of x-coordinate
//a Kalman check can easily be done for the robot
Point robot_center;
maxArea_1 = 0;
max_1 = 0;
numBlobs = robot_blobs.GetNumBlobs();
if(0 < numBlobs){
for(int i=0;i<numBlobs;i++){
curblob = robot_blobs.GetBlob(i);
robot_center = curblob->getCenter();
double dist_1 = norm(center_1 - robot_center);
double dist_2 = norm(center_2 - robot_center);
if(dist_1 < len || dist_2 < len){
double area = robot_blobs.GetBlob(i)->Area();
if(area > maxArea_1){
max_1 = i;
maxArea_1 = area;
}
}
}
int i_3 = max_1;
curblob = robot_blobs.GetBlob(i_3);
curblob->FillBlob(copy,robotColor, true);
robot_center = curblob->getCenter();
circle(copy, robot_center, 5, robotColor_2, -1);
Rect rect_3 = curblob->GetBoundingBox();
rectangle(copy, rect_3.tl(), rect_3.br(), robotColor_2, 5);
// determines which hand is controlling the robot
// by cheching the position of the 3 blobs
// an additional check could be done by verifying if
//the center of the robot is moving in the same direction
//as the center of the hand moving it
bool is_left = false;
bool is_right = false;
bool confirmed = false;
double dist_left = norm(center_left - robot_center);
double dist_right = norm(center_right - robot_center);
double dist_both = norm(center_left - center_right);
Point robot_tl = rect_3.tl();
Point robot_br = rect_3.br();
int left_count = 0;
int right_count = 0;
if(rect_1.contains(robot_tl)) left_count++;
if(rect_1.contains(robot_br)) left_count++;
if(rect_1.contains(robot_center)) left_count++;
if(rect_2.contains(robot_tl)) right_count++;
if(rect_2.contains(robot_br)) right_count++;
if(rect_2.contains(robot_center)) right_count++;
switch(valids){
case 0: break;
case 1:{
int area_sum = area_left + area_right;
if(dist_left > 2* dist_right || dist_right > 2*dist_left){
if(area_sum > 2 * area && (area_left > 2*area_right || area_right > 2*area_left) &&
((left_valid && left_count > 0)||(right_valid && right_count > 0))){
is_left = true;
is_right = true;
if(left_count > 2 || right_count > 2) confirmed = true;
}
}
if(left_valid && left_count > 1) {
is_left = true;
if(left_count > 2) confirmed = true;
}
if(right_valid && right_count > 1) {
is_right = true;
if(right_count > 2) confirmed = true;
}
//if just one hand is on screen
if(area_right < area/2){
if(center_left.x > robot_center.x){
is_left = true;
} else{
is_right = true;
}
} else if (area_left < area/2){
if(center_right.x < robot_center.x){
is_right = true;
} else{
is_left = true;
}
}
break;}
case 2:{
int moreLeft = left_count - right_count;
int moreRight = right_count - left_count;
int countSum = left_count + right_count;
switch (countSum) {
case 3:{
switch (left_count) {
case 3: is_left = true;
confirmed = true;
break;
case 2:
case 1: is_left = true;
is_right = true;
confirmed= true;
break;
case 0: is_right = true;
confirmed = true;
break;
}
}
case 2:{
switch (left_count) {
case 2: is_left = true;
confirmed = true;
break;
case 1: is_left = true;
is_right = true;
break;
case 0: is_right = true;
confirmed = true;
break;
}
}
case 1:{
switch (left_count) {
case 1: is_left = true;
break;
case 0: is_right = true;
break;
}
}
case 0:{
break;
}
}
break;}
}
bool found = false;
for(size_t i = robot_tl.x; i<= robot_br.x && !found; i++){
for(size_t j = robot_tl.y; j<= robot_br.y && !found; j++){
int color1 = 0; int color2 = 255;
Vec3b colour = copy.at<Vec3b>(Point(i, j));
if(colour[1] == color1 && colour[0] == color2){
found = true;
is_left = true;
}
if(colour[1] == color2 && colour[0] == color1){
found = true;
is_right = true;
}
}
}
if (found) confirmed = true;
if(!is_left && !is_right){
cout << "-- none!";
if(left_count == 0 && right_count == 0) confirmed = true;
} else if(is_left && is_right){
cout << "-- both!";
} else {
if (is_left){
cout << " -- left!";
} else {
cout << " -- right!";
}
}
imshow("kalman", kf_img);
// up till here
if(confirmed){
nb_confirmed ++;
cout << " -- confirmed" << endl;
} else {
cout << endl;
}
csv(&logs, center_left.x, center_left.y, is_left, center_right.x, center_right.y, is_right, confirmed);
}
nb_total ++;
//
// //displayOverlay("Blobs Image","Multi Thread");
new_dist = norm(center_left - center_right);
// don't throw errors in the first 10 frames
if(ticks > 10){
if(error_left > 20 && error_right > 20 /*&& new_dist < prev_dist*/){
circle(copy, Point(WIDTH/2, HEIGHT/2), 100, Scalar(0, 0, 255), 30);
nb_errors ++;
}
}
prev_dist = new_dist;
imshow("Blobs Image",copy);
key = waitKey(10);
} else{
eof = true;
}
if(27 == key || 1048603 == key || eof){
double kalman_error_percentage = (nb_errors*100.0)/ticks;
double confirm_percentage = (nb_confirmed*100.0/nb_total);
stats << "kalman error frequency: " << kalman_error_percentage << "\%" << endl;
stats << "confirmed: " << confirm_percentage << "\%" << endl;
logs.close();
stats.close();
return 0;
}
}
}
IplImage* blobDetection2(IplImage* imgThreshRed, IplImage* imgThreshGreen) {
// get blobs and filter them using its area
int i, j;
// int areaBlob = 100;
float distMark = 10;
CBlobResult blobsRed, blobsGreen, whiteRedBlobs, whiteGreenBlobs;
CBlob *currentBlob;
double px, py;
// Create Image
IplImage* displayedImage = cvCreateImage(cvGetSize(imgThreshRed), IPL_DEPTH_8U, 3);
// find all the RED related blobs in the image
blobsRed = CBlobResult(imgThreshRed, NULL, 0);
// find all the GREEN related blobs in the image
blobsGreen = CBlobResult(imgThreshGreen, NULL, 0);
// select the ones with mean gray-level equal to 255 (white) and put
// them in the whiteBlobs variable
blobsRed.Filter(whiteRedBlobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 1.0);
blobsGreen.Filter(whiteGreenBlobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 1.0);
#ifdef DEBUG_PRINT
printf("White Blobs: %d\n", whiteBlobs.GetNumBlobs());
#endif
// display filtered blobs
cvMerge(imgThreshRed, imgThreshRed, imgThreshRed, NULL, displayedImage);
// RED
CvPoint2D32f redCenter[whiteRedBlobs.GetNumBlobs()];
for (i = 0; i < whiteRedBlobs.GetNumBlobs(); i++) {
currentBlob = whiteRedBlobs.GetBlob(i);
px = (currentBlob->MaxX() + currentBlob->MinX()) / 2.0;
py = (currentBlob->MaxY() + currentBlob->MinY()) / 2.0;
redCenter[i] = cvPoint2D32f(px, py);
#ifdef DEBUG_PRINT
printf("%2.2f\t%2.2f\n", px, py);
#endif
if (currentBlob->Area() > areaBlob) {
// Add Cross to the image
currentBlob->FillBlob(displayedImage, CV_RGB(255, 0, 0));
cvCircle(displayedImage, cvPointFrom32f(redCenter[i]), 2, cvScalar(255, 0, 0), 10, 8, 0);
}
}
// GREEN
CvPoint2D32f greenCenter[whiteGreenBlobs.GetNumBlobs()];
for (i = 0; i < whiteGreenBlobs.GetNumBlobs(); i++) {
currentBlob = whiteGreenBlobs.GetBlob(i);
px = (currentBlob->MaxX() + currentBlob->MinX()) / 2.0;
py = (currentBlob->MaxY() + currentBlob->MinY()) / 2.0;
greenCenter[i] = cvPoint2D32f(px, py);
#ifdef DEBUG_PRINT
printf("%2.2f\t%2.2f\n", px, py);
#endif
if (currentBlob->Area() > areaBlob) {
// Add Cross to the image
currentBlob->FillBlob(displayedImage, CV_RGB(255, 0, 0));
cvCircle(displayedImage, cvPointFrom32f(greenCenter[i]), 2, cvScalar(0, 255, 0), 10, 8, 0);
}
}
// Populating the list of potential robots
potRobList.robNum = 0;
for (i = 0; i < robMax; i++)
potRobList.robList[i].active = 0;
int redUsage[whiteRedBlobs.GetNumBlobs()];
int greenUsage[whiteGreenBlobs.GetNumBlobs()];
for (i = 0; i < whiteRedBlobs.GetNumBlobs(); i++)
redUsage[i] = 0;
for (j = 0; j < whiteGreenBlobs.GetNumBlobs(); j++)
greenUsage[j] = 0;
// Detect Robots
float distCenter[whiteRedBlobs.GetNumBlobs()][whiteGreenBlobs.GetNumBlobs()];
for (i = 0; i < min(whiteRedBlobs.GetNumBlobs(), robMax); i++) {
currentBlob = whiteRedBlobs.GetBlob(i);
if (currentBlob->Area() > areaBlob) {
for (j = 0; j < min(whiteGreenBlobs.GetNumBlobs(), robMax); j++) {
currentBlob = whiteGreenBlobs.GetBlob(j);
if (currentBlob->Area() > areaBlob) {
distCenter[i][j] = computeDist(redCenter[i], greenCenter[j]);
//printf("[%d] - [%d]: %2.2f\n", i, j, distCenter[i][j]);
//printf("[%d] - [%d]: %2.2f\n", i, j, distCenter[i][j]);
// Print a connection line if this could be a robot
if (redUsage[i] == 0 && greenUsage[j] == 0 && checkDistMarker(distCenter[i][j], distMark)) {
cvLine(displayedImage, cvPointFrom32f(redCenter[i]), cvPointFrom32f(greenCenter[j]), cvScalar(0, 255, 255), 2, 8, 0);
// Check Robot
potRobList.robList[potRobList.robNum] = createRobot(redCenter[i], greenCenter[j]);
potRobList.robNum++;
redUsage[i] = 1;
greenUsage[j] = 1;
// printRobot(potRobList.robList[potRobList.robNum - 1]);
CvBox2D tmp;
tmp.angle = potRobList.robList[potRobList.robNum - 1].orientation;
tmp.center = potRobList.robList[potRobList.robNum - 1].center;
tmp.size = cvSize2D32f(30, 50);
cvEllipseBox(displayedImage, tmp, cvScalar(255, 255, 0), 4, 3, 0);
// printRobot(potRobList.robList[potRobList.robNum-1]);
}
}
}
}
}
// Matching The List of Potential Robots with previous List of Robots
// updateRobotListAndrea(&avRobList, potRobList);
// updateRobotList(&avRobList, potRobList);
makelistRobot();
/*
// Print robots
for (i = 0; i < robMax; i++) {
if (avRobList.robList[i].active == 1) {
CvBox2D tmp;
tmp.angle = avRobList.robList[i].orientation;
tmp.center = avRobList.robList[i].center;
tmp.size = cvSize2D32f(50, 30);
cvEllipseBox(displayedImage, tmp, cvScalar(255, 255, 0), 4, 3, 0);
printRobot(avRobList.robList[i]);
}
}
*/
/* Control Law */
return displayedImage;
}
SHModel* ShapeModel( CvCapture* g_capture,StaticBGModel* BGModel , BGModelParams* BGParams){
int num_frames = 0;
int total_blobs=0;
float Sumatorio = 0;
float SumatorioDes = 0;
IplImage* frame = NULL;
STFrame* frameData = NULL;
SHModel* Shape = NULL;
CBlobResult blobs;
CBlob *currentBlob;
IplImage* ImGris = cvCreateImage(cvGetSize( BGModel->Imed ), 8, 1 );
IplImage* Imblob = cvCreateImage(cvGetSize( BGModel->Imed ), 8, 3 );
IplImage* lastBG = cvCreateImage( cvGetSize( BGModel->Imed ),8, 1 );
IplImage* lastIdes = cvCreateImage( cvGetSize( BGModel->Imed ), IPL_DEPTH_32F, 1);
cvZero(Imblob);
// Iniciar estructura para modelo de forma
Shape = ( SHModel *) malloc( sizeof( SHModel));
if ( !Shape ) {error(4);return 0;}
Shape->FlyAreaDes = 0;
Shape->FlyAreaMedia=0;
//Pone a 0 los valores del vector areas
//EXTRACCION DE LOS BLOBS Y CALCULO DE MEDIANA/MEDIA Y DESVIACION TIPICA PARA TODOS LOS FRAMES
cvSetCaptureProperty( g_capture,1,BGParams->initDelay ); // establecemos la posición
while( num_frames < ShParams->FramesTraining ){
frame = cvQueryFrame( g_capture );
if ( !frame ) {
error(2);
break;
}
if ( (cvWaitKey(10) & 255) == 27 ) break;
ImPreProcess( frame, ImGris, BGModel->ImFMask, 0, BGModel->DataFROI);
// Cargamos datos del fondo
if(!frameData ) { //en la primera iteración iniciamos el modelo dinamico al estático
// Iniciar estructura para datos del nuevo frame
frameData = InitNewFrameData( frame );
cvCopy( BGModel->Imed,frameData->BGModel);
cvSet(frameData->IDesvf, cvScalar(1));
cvCopy( BGModel->Imed,lastBG);
}
else{ // cargamos los últimos parámetros del fondo.
cvCopy( lastBG, frameData->BGModel);
cvCopy( lastIdes,frameData->IDesvf );
}
// obtener la mascara del FG y la lista con los datos de sus blobs.
//// BACKGROUND UPDATE
// Actualización del fondo
// establecer parametros
UpdateBGModel( ImGris,frameData->BGModel,frameData->IDesvf, BGParams, BGModel->DataFROI, BGModel->ImFMask );
/////// BACKGROUND DIFERENCE. Obtención de la máscara del foreground
BackgroundDifference( ImGris, frameData->BGModel,frameData->IDesvf, frameData->FG ,BGParams, BGModel->DataFROI);
// guardamos las imagenes para iniciar el siguiente frame
cvCopy( frameData->BGModel, lastBG);
cvCopy( frameData->IDesvf,lastIdes);
//Obtener los Blobs y excluir aquellos que no interesan por su tamaño
// cvSetImageROI( frameData->FG , BGModel->DataFROI);
blobs = CBlobResult( frameData->FG, NULL, 100, true );
blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(),B_GREATER,100);
blobs.Filter( blobs, B_EXCLUDE, CBlobGetPerimeter(),B_GREATER,1000);
int j = blobs.GetNumBlobs();//numero de blos encontrados en el frame
total_blobs=total_blobs+j; // Contabiliza los blobs encontrados para todos los frames
//Recorrer Blob a blob y obtener las caracteristicas del AREA de cada uno de ellos
for (int i = 0; i < blobs.GetNumBlobs(); i++ ){ //for 1
currentBlob = blobs.GetBlob(i);
CBlobGetArea();
if(ShParams->SHOW_DATA_AREAS) {
//printf("Area blob %d = %f ",i,currentBlob->area);
}
//Estimar la media de las Areas
Sumatorio = Sumatorio + currentBlob->area;
SumatorioDes = SumatorioDes + currentBlob->area*currentBlob->area;
muestrearAreas( currentBlob->area);
currentBlob->FillBlob( Imblob, CV_RGB(255,0,0));
}//Fin del For 1
Shape->FlyAreaMedia = Sumatorio / total_blobs;
Shape->FlyAreaDes = (SumatorioDes / total_blobs) - Shape->FlyAreaMedia*Shape->FlyAreaMedia;
num_frames += 1;
// cvResetImageROI(frameData->FG);
DraWWindow(Imblob, frameData, BGModel, SHOW_SHAPE_MODELING, COMPLETO);
DraWWindow(Imblob, frameData, BGModel, SHAPE,SIMPLE );
}
desvanecer( NULL, 20);
Shape->FlyAreaDes = sqrt(abs(Shape->FlyAreaDes) ) ;
if( Shape->FlyAreaDes == 0){
printf("hola");
}
//Mostrar mediana y media para todos los frames
if(ShParams->SHOW_DATA_AREAS )
printf("\n MEDIA AREAS: %f \t DESVIACION AREAS: %f",Shape->FlyAreaMedia,Shape->FlyAreaDes);
free( ShParams);
liberarSTFrame( frameData );
cvReleaseImage( &ImGris);
cvReleaseImage( &Imblob);
cvReleaseImage( &lastIdes);
cvReleaseImage( &lastBG);
return Shape;
}//Fin de la función ShapeModel2
void iptask::markerDetect(void)
{
IplImage * frame,*img_hsv,*img_proc,* new1;
CvMemStorage * storage = cvCreateMemStorage(0);
ros::NodeHandle n;
ros::Publisher marker = n.advertise<ikat_ip_data::ip_marker_data>("marker_data",3);
ros::Rate looprate(5);
int count = 0;
CvSeq * contours,*final_contour;
int total_con;
double maxarea;
marker_data * Data =(marker_data *)malloc(sizeof(marker_data));
CBlobResult blobs;
CBlob * currentblob;
CvPoint2D32f vertices[4];
//CvCapture * img_video=cvCaptureFromAVI("downward-pipe-15_56_17.avi");
frame=cvQueryFrame(img);
cvNamedWindow("Image Actual");
cvNamedWindow("final Image");
img_hsv=cvCreateImage(cvGetSize(frame),8,3);
img_proc=cvCreateImage(cvGetSize(frame),8,1);
new1=cvCreateImage(cvGetSize(frame),8,1);
while(ros::ok())
{
ikat_ip_data::ip_marker_data msg;
IplImage * img_con=cvCreateImage(cvGetSize(frame),8,1);
frame=cvQueryFrame(img);
if(!frame)
break;
cvShowImage("Image Actual",frame);
cvCvtColor(frame,img_hsv,CV_RGB2HSV);
cvInRangeS(img_hsv,cvScalar(100,100,100),cvScalar(120,170,255),img_proc);
cvSmooth(img_proc,img_proc,CV_GAUSSIAN,11,11);
cvErode(img_proc,img_proc);
blobs=CBlobResult(img_proc,NULL,0);
blobs.Filter(blobs,B_EXCLUDE,CBlobGetArea(),B_LESS,75);
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentblob = blobs.GetBlob(i);
currentblob->FillBlob(img_proc,cvScalar(255));
}
cvCanny(img_proc,img_proc,10,200);
total_con=cvFindContours(img_proc,storage,&contours,sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
if(contours->total==0)
continue;
final_contour=cvApproxPoly(contours,sizeof(CvContour),storage,CV_POLY_APPROX_DP,1,1);
maxarea=0;
cvZero(img_con);
CvBox2D rect;
while(final_contour)
{
rect=cvMinAreaRect2(final_contour, storage);
if(rect.size.height*rect.size.width>maxarea)
{
Data->center.x=rect.center.x;
Data->center.y=rect.center.y;
Data->size.x=rect.size.width;
Data->size.y=rect.size.height;
Data->angle=rect.angle;
maxarea=rect.size.height*rect.size.width;
msg.Marker_data[0]=Data->center.x;
msg.Marker_data[1]=Data->center.y;
msg.Marker_data[2]=Data->angle;
}
final_contour=final_contour->h_next;
}
cvBoxPoints(rect,vertices);
cvLine(frame,cvPointFrom32f(vertices[0]),cvPointFrom32f(vertices[1]),cvScalarAll(255),2);
cvLine(frame,cvPointFrom32f(vertices[1]),cvPointFrom32f(vertices[2]),cvScalarAll(255),2);
cvLine(frame,cvPointFrom32f(vertices[2]),cvPointFrom32f(vertices[3]),cvScalarAll(255),2);
cvLine(frame,cvPointFrom32f(vertices[3]),cvPointFrom32f(vertices[0]),cvScalarAll(255),2);
ROS_INFO("center x :[%f]",msg.Marker_data[0]);
ROS_INFO("center y :[%f]",msg.Marker_data[1]);
ROS_INFO("angle : [%f]",msg.Marker_data[2]);
marker.publish(msg);
cvShowImage("final Image",frame);
char c=cvWaitKey(33);
if (c==27)
break;
ros::spinOnce();
++count;
looprate.sleep();
}
cvDestroyWindow("Image Actual");
cvDestroyWindow("final Image");
free(Data);
}
float thresholdSegmentation(Rect r, ntk::RGBDImage* current_frame, Mat& dst){
Mat depth = current_frame->depth();
Rect& rr = r;
Mat depthROI = depth(rr), maskROI;
Mat& rDepthROI = depthROI, &rMaskROI = maskROI;
double var = 0.3;
// maskROI for nonZero values in the Face Region
inRange(depthROI, Scalar::all(0.001), Scalar::all(255), maskROI);
// Mean depth of Face Region
Scalar mFace = cv::mean(rDepthROI, rMaskROI);
//mFace[0] = mFace[0] - mFace[0] * var;
inRange(depthROI, Scalar::all(0.001), mFace, maskROI);
mFace = cv::mean(rDepthROI, rMaskROI);
//inRange(depthROI, Scalar::all(0.001), mFace, maskROI);
//mFace = cv::mean(rDepthROI, rMaskROI);
// Mask for nearer than the mean of face.
inRange(depth, Scalar::all(0.001), mFace, dst);
Mat rgbImage = current_frame->rgb();
Mat outFrame = cvCreateMat(rgbImage.rows, rgbImage.cols, CV_32FC3);
rgbImage.copyTo(outFrame, dst);
Mat outFrameROI;
outFrameROI = outFrame(rr);
//cvCopy(&rgbImage, &outFrame, &dst);
//rgbImageROI = rgbImageROI(rr);
imshow("ROI", outFrameROI);
//imshow("thresholdSeg", dst);
// For debug of cvblobslib
// Display the color image
//imshow("faceRIO", maskROI);
imshow("faceRIO", outFrameROI);
bool iswrite;
const int nchannel = 1;
vector<Rect> faces;
//iswrite = imwrite("faceROI.png", maskROI);
iswrite = imwrite("faceROI.png", outFrameROI);
//iswrite = cvSaveImage("faceROI.jpeg", pOutFrame, &nchannel);
// ---- blob segmentation on maskROI by using cvblobslib ----
// --- Third Trial ---
//visualizeBlobs("faceROI.png", "faceRIO");
// --- First Trial Not Successful ---
//Mat maskROIThr=cvCreateMat(maskROI.rows, maskROI.cols, CV_8UC1);
//maskROIThr = maskROI;
//IplImage imgMaskROIThr = maskROIThr;
//IplImage* pImgMaskROIThr = &imgMaskROIThr;
//cvThreshold(pImgMaskROIThr, pImgMaskROIThr, 0.1, 255, CV_THRESH_BINARY_INV);
// --- Second Trial ---
IplImage* original = cvLoadImage("faceROI.png", 0);
IplImage* originalThr = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U, 1);
IplImage* displayBiggestBlob = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U, 3);
CBlobResult blobs;
CBlob biggestBlob;
//IplImage source = maskROIThr; IplImage* pSource = &source;
//blobs = CBlobResult(
cvThreshold(original, originalThr, 0.1, 255, CV_THRESH_BINARY_INV);
blobs = CBlobResult( originalThr, NULL, 255);
printf("%d blobs \n", blobs.GetNumBlobs());
blobs.GetNthBlob(CBlobGetArea(), 0, biggestBlob);
biggestBlob.FillBlob(displayBiggestBlob, CV_RGB(255, 0, 0));
// Drawing the eclipse and Rect on the blob
Mat mat(displayBiggestBlob);
cv::RotatedRect blobEllipseContour;
cv::Rect blobRectContour;
//RotatedRect blobEllipseContour;
blobEllipseContour = biggestBlob.GetEllipse();
blobRectContour = biggestBlob.GetBoundingBox();
//cv::ellipse(
cv::ellipse(mat, blobEllipseContour, cv::Scalar(0,255, 0), 3, CV_AA);
cv::rectangle(mat, blobRectContour, cv::Scalar(255, 0, 0), 3, CV_AA);
//cv::ellipse(mat, blobEllipseContour);
float headOritation = blobEllipseContour.angle;
if (headOritation <= 180)
headOritation = headOritation - 90;
else
headOritation = headOritation - 270;
cv::putText(mat,
cv::format("%f degree", headOritation),
Point(10,20), 0, 0.5, Scalar(255,0,0,255));
cv::imshow("faceRIO", mat);
return(headOritation);
}
/*
* thread for displaying the opencv content
*/
void *cv_threadfunc (void *ptr) {
IplImage* timg = cvCloneImage(rgbimg); // Image we do our processing on
IplImage* dimg = cvCloneImage(rgbimg); // Image we draw on
CvSize sz = cvSize( timg->width & -2, timg->height & -2);
IplImage* outimg = cvCreateImage(sz, 8, 3);
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq* squares; // Sequence for squares - sets of 4 points
CvSeq* contours; // Raw contours list
CvSeq* result; // Single contour being processed
CBlobResult blobs;
CBlob *currentBlob;
IplImage *pyr = cvCreateImage(cvSize(sz.width/2, sz.height/2), 8, 1);
// Set region of interest
cvSetImageROI(timg, cvRect(0, 0, sz.width, sz.height));
cvSetImageROI(dimg, cvRect(0, 0, sz.width, sz.height));
// Processing and contours
while (1) {
squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvPoint), storage);
pthread_mutex_lock( &mutex_rgb );
cvCopy(rgbimg, dimg, 0);
cvCopy(rgbimg, timg, 0);
pthread_mutex_unlock( &mutex_rgb );
// BLUR TEST
// cvPyrDown(dimg, pyr, 7);
// cvPyrUp(pyr, timg, 7);
// DILATE TEST
IplConvKernel* element = cvCreateStructuringElementEx(5, 5, 2, 2, 0);
IplConvKernel* element2 = cvCreateStructuringElementEx(3, 3, 1, 1, 0);
cvDilate(timg, timg, element, 2);
cvErode(timg, timg, element2, 3);
// THRESHOLD TEST
cvThreshold(timg, timg, 200, 255, CV_THRESH_BINARY);
// Output processed or raw image.
cvCvtColor(timg, outimg, CV_GRAY2BGR);
// BLOB TEST
blobs = CBlobResult( timg, (IplImage*)NULL, 0, true );
// blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 50 );
printf("Blobs: %d\n", blobs.GetNumBlobs());
CBlob biggestBlob;
blobs.GetNthBlob( CBlobGetArea(), 1, biggestBlob );
biggestBlob.FillBlob( outimg, CV_RGB(255, 0, 0) );
CvSeq* dest;
biggestBlob.GetConvexHull(dest);
// for (int i = 0; i < blobs.GetNumBlobs(); i++ )
// {
// currentBlob = blobs.GetBlob(i);
// currentBlob->FillBlob( outimg, CV_RGB(255,0,0) );
// }
// // CONTOUR FINDING
// cvFindContours(timg, storage, &contours, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
//
// while (contours)
// {
// // Approximate contour, accuracy proportional to perimeter of contour; may want to tune accuracy.
// result = cvApproxPoly(contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(contours) * 0.02, 0);
// // Filter small contours and contours w/o 4 vertices (filters noise, finds rectangles)
// if (result->total == 4 &&
// fabs(cvContourArea(result, CV_WHOLE_SEQ)) > 600 &&
// cvCheckContourConvexity(result))
// {
// // Skipped checking whether angles were close to 90 degrees here; may want to implement.
// // Probably also want to check if it's square enough to filter out ex. long windows.
//
// for (int i = 0; i < 4; i++)
// {
// // Write vertices to output sequence
// cvSeqPush(squares, (CvPoint*)cvGetSeqElem(result, i));
// }
// }
//
// // Take next contour
// contours = contours->h_next;
// }
//
//
// // DRAW RECTANGLES
// CvSeqReader reader;
// cvStartReadSeq(squares, &reader, 0);
//
// // Read 4 points at a time
// CvPoint pt[4];
// CvPoint *rect = pt;
// CvRect out[4];
// CvRect *outrect = out;
// for (int i = 0; i < squares->total; i += 4)
// {
// int count = 4;
//
// // Which point is which corner is unpredictable.
// CV_READ_SEQ_ELEM(pt[0], reader);
// CV_READ_SEQ_ELEM(pt[1], reader);
// CV_READ_SEQ_ELEM(pt[2], reader);
// CV_READ_SEQ_ELEM(pt[3], reader);
// // Draw rectangle on output
// cvPolyLine(outimg, &rect, &count, 1, 1, CV_RGB(0,255,0), 1, CV_AA, 0);
// // Make rectangles
// // Print (temporary)
// printf("Rect[0]: %d, %d\n", pt[0].x, pt[0].y);
// printf("Rect[1]: %d, %d\n", pt[1].x, pt[1].y);
// printf("Rect[2]: %d, %d\n", pt[2].x, pt[2].y);
// printf("Rect[3]: %d, %d\n\n", pt[3].x, pt[3].y);
// fflush(stdout);
//
// }
//
// Print on order
if( cvWaitKey( 15 )==27 )
{
}
cvShowImage (FREENECTOPENCV_WINDOW_N,outimg);
cvClearMemStorage(storage);
}
pthread_exit(NULL);
}
double findShadow(IplImage *l_img, int hue,int sat,int val,int threshold, double blobLowLimit,double blobHighLimit){
// Input HSV value of color blob your seeking, acceptable threshold of that color, and Min and Max blob sizes beeing sought out.
// Input HSV value of color blob your seeking, acceptable threshold of that color, and Min and Max blob sizes beeing sought out.
//Ouput: pointer to data array, size[#ofblobs*3+1]; Format data=[Number of Blobs, Area1,X of center1, y of center1, Area2,X of center2,y of center2,...,areaN,X of centerN, Y of centerN];
// Image variables
IplImage* local_copy = cvCloneImage(l_img);
IplImage* imageSmooth = cvCreateImage( cvGetSize(l_img),8,3);//Gausian Filtered image
IplImage* imageSuperSmooth = cvCreateImage( cvGetSize(l_img),8,3);//Gausian Filtered image
IplImage* imageHSV = cvCreateImage( cvGetSize(l_img),8,3); //HSV image
IplImage* i1 = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
IplImage* i2 = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
IplImage* i_ts = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
IplImage* planeH = cvCreateImage(cvGetSize(l_img),8,1); //Hue
IplImage* planeS = cvCreateImage(cvGetSize(l_img),8,1); //Saturation
IplImage* planeV = cvCreateImage(cvGetSize(l_img),8,1); //Brightness
IplImage* planeSmoothV = cvCreateImage(cvGetSize(l_img),8,1); //Brightness
IplImage* imageSmoothHSV = cvCreateImage( cvGetSize(l_img),8,3); //HSV image
IplImage* obsdetmask = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
IplImage* obsdetmask_dil = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
IplImage* obsdetmask_b = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
IplImage* obsdetmask_bdil = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
//Blob variables
CBlobResult mask_bls;
CBlob mask_bl;
CBlobResult blobs;
CBlob blob;
CBlobResult blobs1;
CBlob blob1;
CBlobGetXCenter getXCenter;
CBlobGetYCenter getYCenter;
//Output Variable
//Gausian Filter
cvSmooth(l_img,imageSmooth,CV_GAUSSIAN,13,13,0,0);
cvSmooth(l_img,imageSuperSmooth,CV_GAUSSIAN,41,41,0,0);
//cvShowImage("View2a",imageSmooth);
//Covert RGB to HSV
cvCvtColor(imageSmooth,imageHSV,CV_BGR2HSV);
cvCvtColor(imageSuperSmooth,imageSmoothHSV,CV_BGR2HSV);
cvCvtPixToPlane(imageSuperSmooth,NULL,NULL,planeSmoothV,0);
cvCvtPixToPlane(imageHSV, planeH,planeS,planeV,0);//Extract the 3 color components
cvSetImageROI(imageHSV,cvRect(0,imageHSV->height/3,imageHSV->width,imageHSV->height*2/3));
IplImage* planeH1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Hue
IplImage* planeS1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Saturation
IplImage* planeV1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Brightness
cvCvtPixToPlane(imageHSV, planeH1,planeS1,planeV1,0);//Extract the 3 color components
cvResetImageROI(imageHSV);
cvShowImage("Dark_Value",planeV);
cvShowImage("Dark_Sat",planeS);
cvShowImage("Dark_Hue",planeH);
cvSet(obsdetmask, cvScalar(0,0,0));
cv::waitKey(3);
int maxDark = 0;
int minDark = 255;
int minDarknessValue=0;
int maxDarknessValue = 0;
int midDarknessValue = 0;
//Filter image for desired Color, output image with only desired color highlighted remaining
for( int y = 0; y < planeH1->height; y++ ){
unsigned char* h = &CV_IMAGE_ELEM( planeH1, unsigned char, y, 0 );
unsigned char* s = &CV_IMAGE_ELEM( planeS1, unsigned char, y, 0 );
unsigned char* v = &CV_IMAGE_ELEM( planeV1, unsigned char, y, 0 );
for( int x = 0; x < planeH1->width*planeH1->nChannels; x += planeH1->nChannels ){
//if(x<5){ROS_INFO("hsv[x] is %d,%d,%d",h[x],v[x],x]);}
//int f= HSV_filter(h[x],s[x],v[x],threshold,minDarknessValue,maxDarknessValue,midDarknessValue,hue,sat,val);
int diff = abs((h[x]-hue));
if(((diff < threshold)||(v[x]<MIN_BRIGHT)||(s[x]<MIN_SAT)))
{
((uchar *)(obsdetmask->imageData + (y+planeH->height-planeH1->height)*obsdetmask->widthStep))[x]=255;
if(v[x]<minDark)
{minDark=v[x];}
if(v[x]>maxDark)
{maxDark=v[x];}
}
else
{
((uchar *)(obsdetmask->imageData + (y+planeH->height-planeH1->height)*obsdetmask->widthStep))[x]=0;
}
}
}//debug
cvDilate(obsdetmask,obsdetmask_dil,NULL,1);
cvShowImage("Dark_ObsDetPre",obsdetmask_dil);
mask_bls = CBlobResult(obsdetmask_dil,NULL,0);
mask_bls.Filter(mask_bls,B_EXCLUDE,CBlobGetArea(),B_LESS,MASK_MIN_BLOB); // Filter Blobs with min and max size
mask_bls.GetNthBlob( CBlobGetArea(), 0, mask_bl );
cvSet(obsdetmask_b, cvScalar(0,0,0));
mask_bl.FillBlob(obsdetmask_b,CV_RGB(255,255,255));
cvDilate(obsdetmask_b,obsdetmask_bdil,NULL,5);
cvShowImage("Dark_ObsDet",obsdetmask_bdil);
cvWaitKey(3);
minDarknessValue=((maxDark-minDark)*LOW_PERCENT)+minDark;
if(minDarknessValue<VALUE_LOW_LIM){minDarknessValue=VALUE_LOW_LIM;}
maxDarknessValue=(maxDark)-((maxDark-minDark)*HIGH_PERCENT);
midDarknessValue = .5*(minDarknessValue+maxDarknessValue);
ROS_INFO("minDark = %d, maxDark = %d, minDV = %d, maxDV = %d",minDark,maxDark,minDarknessValue,maxDarknessValue);
for( int y = 0; y < planeH->height; y++ ){
unsigned char* h = &CV_IMAGE_ELEM( planeH, unsigned char, y, 0 );
unsigned char* s = &CV_IMAGE_ELEM( planeS, unsigned char, y, 0 );
unsigned char* v = &CV_IMAGE_ELEM( planeV, unsigned char, y, 0 );
unsigned char* m = &CV_IMAGE_ELEM( obsdetmask_bdil, unsigned char, y, 0 );
for( int x = 0; x < planeH->width*planeH->nChannels; x += planeH->nChannels ){
//if(x<5){ROS_INFO("hsv[x] is %d,%d,%d",h[x],v[x],x]);}
int f = HSV_filter(h[x],s[x],v[x],m[x],threshold,minDarknessValue,maxDarknessValue,midDarknessValue,hue,sat,val);
if((f==0))//Non-floor
{
((uchar *)(i1->imageData + y*i1->widthStep))[x]=0;
((uchar *)(i_ts->imageData + y*i_ts->widthStep))[x]=0;
((uchar *)(i2->imageData + y*i2->widthStep))[x]=0;
}
else if(f==1) //dark
{
((uchar *)(i1->imageData + y*i1->widthStep))[x]=255;
((uchar *)(i_ts->imageData + y*i_ts->widthStep))[x]=64;
((uchar *)(i2->imageData + y*i2->widthStep))[x]=0;
}
else if(f==2)
{
((uchar *)(i_ts->imageData + y*i_ts->widthStep))[x]=128;
((uchar *)(i1->imageData + y*i1->widthStep))[x]=0;
((uchar *)(i2->imageData + y*i2->widthStep))[x]=0;
}
else if(f==3)
{
((uchar *)(i_ts->imageData + y*i_ts->widthStep))[x]=196;
((uchar *)(i1->imageData + y*i1->widthStep))[x]=0;
((uchar *)(i2->imageData + y*i2->widthStep))[x]=0;
}
else if(f==4) //bright
{
((uchar *)(i_ts->imageData + y*i_ts->widthStep))[x]=255;
((uchar *)(i1->imageData + y*i1->widthStep))[x]=0;
((uchar *)(i2->imageData + y*i2->widthStep))[x]=255;
}else{
}
}
}
cvShowImage("Dark_Triscale",i_ts);
cvWaitKey(3);
//Blob stuff
blobs = CBlobResult(i1,NULL,0); //Get blobs of image
blobs1 =CBlobResult(i2,NULL,0);
blobs.Filter(blobs,B_INCLUDE,CBlobGetArea(),B_INSIDE,blobLowLimit,blobHighLimit); // Filter Blobs with min and max size
blobs1.Filter(blobs1,B_INCLUDE,CBlobGetArea(),B_INSIDE,blobLowLimit,blobHighLimit);
//Set up data array
xCent = new int[blobs.GetNumBlobs()+blobs1.GetNumBlobs()];
yCent = new int[blobs.GetNumBlobs()+blobs1.GetNumBlobs()];
valCent = new int[blobs.GetNumBlobs()+blobs1.GetNumBlobs()];
ROS_INFO("size:%d ",blobs.GetNumBlobs()+blobs1.GetNumBlobs());
double data;
if(maxDark>190)
{
data=blobs.GetNumBlobs()+blobs1.GetNumBlobs();// Set first data value to total number of blobs
//cout<<data[0]<<" ";
int k=0;
//ROS_INFO("Blobs gotten.");
cvWaitKey(3);
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{ // Get Blob Data
blob = blobs.GetBlob(i);//cycle through each blob
//data[i*3+1]=blob.area;//blob areaEFF
xCent[i]= getXCenter(blob); //X min
yCent[i]= getYCenter(blob); //X max
valCent[i]= 1; //Y max
//debug
blob.FillBlob(local_copy, cvScalar(255, 0, 0)); // This line will give you a visual marker on image for the blob if you want it for testing or something
}
//ROS_INFO("loop 1 done.");
cvWaitKey(3);
for (int i = 0; i < blobs1.GetNumBlobs(); i++ )
{ // Get Blob Data
blob = blobs1.GetBlob(i);//cycle through each blob
//data[i*3+1]=blob.area;//blob area
xCent[blobs.GetNumBlobs()+i]= getXCenter(blob); //X min
yCent[blobs.GetNumBlobs()+i]= getYCenter(blob); //X max
valCent[blobs.GetNumBlobs()+i]= -1;
//debug
blob.FillBlob(local_copy, cvScalar(0, 255, 0)); // This line will give you a visual marker on image for the blob if you want it for testing or something
}
}else{
//
data=blobs.GetNumBlobs();// Set first data value to total number of blobs
//cout<<data[0]<<" ";
int k=0;
//ROS_INFO("Blobs gotten.");
cvWaitKey(3);
for (int i = 0; i < blobs.GetNumBlobs(); i++ )
{ // Get Blob Data
blob = blobs.GetBlob(i);//cycle through each blob
//data[i*3+1]=blob.area;//blob areaEFF
xCent[i]= getXCenter(blob); //X min
yCent[i]= getYCenter(blob); //X max
valCent[i]= 1; //Y max
//debug
blob.FillBlob(local_copy, cvScalar(255, 0, 0)); // This line will give you a visual marker on image for the blob if you want it for testing or something
}
}
cvShowImage("Dark_Detected",local_copy);
//cv::imshow("View",cv_ptr->image);
cv::waitKey(3);
cvReleaseImage(&local_copy);
cvReleaseImage(&imageSmooth);
cvReleaseImage(&imageSuperSmooth);
cvReleaseImage(&imageHSV);
cvReleaseImage(&i1);
cvReleaseImage(&i2);
cvReleaseImage(&planeSmoothV);
cvReleaseImage(&imageSmoothHSV);
cvReleaseImage(&i_ts);
cvReleaseImage(&planeH);
cvReleaseImage(&planeS);
cvReleaseImage(&planeV);
cvReleaseImage(&planeH1);
cvReleaseImage(&planeS1);
cvReleaseImage(&planeV1);
cvReleaseImage(&obsdetmask);
cvReleaseImage(&obsdetmask_dil);
cvReleaseImage(&obsdetmask_b);
cvReleaseImage(&obsdetmask_bdil);
return data; //return pointer to data array
}