/**
* This function fits ellipses to each set of points (contour).
*/
void MyEllipses::fitEllipses(){
int size = linkedPoints.size();
cv::vector<cv::Point> currentContour;
cv::vector<cv::RotatedRect> minEllipse(size);
cv::Scalar color = cv::Scalar( 255,0,0 );
int validEllipsesCounter = 0;
for( int i = 0; i < size; i++){
currentContour = linkedPoints[i];
if( currentContour.size()>5){
minEllipse[validEllipsesCounter] = cv::fitEllipse(cv::Mat(currentContour));
//cv::ellipse( ellipsesImage, minEllipse[i], color, 2, 8 );
validEllipsesCounter++;
}
}
minEllipse.resize(validEllipsesCounter);
fittedEllipses = minEllipse;
}
static std::vector<cv::RotatedRect> blob_finder(cv::Mat *pic){
cv::Mat cppic;
pic->copyTo(cppic);
cv::normalize(cppic,cppic,0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::Mat img(cppic.rows/4,cppic.cols/4,CV_8UC1);
cv::resize(cppic,img,img.size());
int rad=6;
cv::Point pos(0,0);
// float abs_max=0;
// float *p_res, *p_avg, *p_blob;
float *p_res, *p_blob;
cv::Mat result = cv::Mat::zeros(img.rows,img.cols,CV_32FC1);
cv::Mat res_blob,res_avg,blob_mat;
gen_blob_neu(rad,&blob_mat);
img.convertTo(img, CV_32FC1);
filter2D(img, res_blob, -1 , blob_mat, cv::Point( -1, -1 ), 0, cv::BORDER_REPLICATE );
for(int i=20; i<result.rows;i++){ //ignore 20 first becouse of advertisement
p_res=result.ptr<float>(i);
//p_avg=res_avg.ptr<float>(i);
p_blob=res_blob.ptr<float>(i);
for(int j=0; j<result.cols;j++){
if(p_blob[j]>15)
p_res[j]=p_blob[j]*p_blob[j];
else
p_res[j]=0;
}
}
//cv::normalize(result,result,0, 255, cv::NORM_MINMAX, CV_8UC1);
float lmin=100000000;
float lmax=0;
float rmin=100000000;
float rmax=0;
for(int i=0; i<result.rows;i++){
p_res=result.ptr<float>(i);
for(int j=0; j<result.cols/2;j++){
if(p_res[j]<lmin)
lmin=p_res[j];
if(p_res[j]>lmax)
lmax=p_res[j];
}
for(int j=result.cols/2; j<result.cols;j++){
if(p_res[j]<rmin)
rmin=p_res[j];
if(p_res[j]>rmax)
rmax=p_res[j];
}
}
//norm
for(int i=0; i<result.rows;i++){
p_res=result.ptr<float>(i);
for(int j=0; j<result.cols/2;j++){
p_res[j]= ((p_res[j]-lmin)/(lmax-lmin))*255;
}
for(int j=result.cols/2; j<result.cols;j++){
p_res[j]= ((p_res[j]-rmin)/(rmax-rmin))*255;
}
}
result.convertTo(result, CV_8UC1);
cv::threshold(result,result,100,255,cv::THRESH_BINARY);
std::vector<std::vector<cv::Point>> contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours( result, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
std::vector<cv::RotatedRect> minEllipse( contours.size() );
for( size_t i = 0; i < contours.size(); i++ ){
if( contours[i].size() > 5 ){
minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) );
minEllipse[i].center.x=minEllipse[i].center.x*4;
minEllipse[i].center.y=minEllipse[i].center.y*4;
minEllipse[i].size.width=minEllipse[i].size.width*10;
minEllipse[i].size.height=minEllipse[i].size.height*10;
minEllipse[i].center.x=minEllipse[i].center.x-(minEllipse[i].size.width/2);
minEllipse[i].center.y=minEllipse[i].center.y-(minEllipse[i].size.height/2);
}
}
std::vector<cv::RotatedRect> pupiEllipse;
cv::Mat pupil_region;
for( size_t k = 0; k < minEllipse.size(); k++ ){
if(minEllipse[k].size.height>0 && minEllipse[k].size.width>0){
pupil_region=cv::Mat::zeros(minEllipse[k].size.height,minEllipse[k].size.width,CV_8UC1);
for(int i=0; i<pupil_region.rows;i++)
for(int j=0; j<pupil_region.cols;j++)
if(int(minEllipse[k].center.y+i)>0 && int(minEllipse[k].center.y+i)<cppic.rows && minEllipse[k].center.x+j>0 && minEllipse[k].center.x+j<cppic.cols){
pupil_region.data[(pupil_region.cols*i)+j]=cppic.data[( cppic.cols*int(minEllipse[k].center.y+i)) + int(minEllipse[k].center.x+j)];
}
//cv::imshow("pr",pupil_region);
//cv::waitKey(10);
cv::normalize(pupil_region,pupil_region,0, 255, cv::NORM_MINMAX, CV_8UC1);
//histo seperation
int histi[256];
for(int i=0; i<256;i++)
histi[i]=0;
for(int i=0; i<pupil_region.rows;i++)
for(int j=0; j<pupil_region.cols;j++){
histi[pupil_region.data[(pupil_region.cols*i)+j]]++;
}
/*
cv::Mat sh_hist=cv::Mat::zeros(800,256, CV_8UC1);
for(int i=0; i<256;i++)
for(int j=0; j<histi[i] && j<800;j++)
sh_hist.data[(sh_hist.cols*(799-j))+i]=255;
cv::imshow("histo",sh_hist);
cv::waitKey(100);
*/
int window=25;
int maxima[100];
int maxima_idx=0;
for(int i=0; i<100;i++)
maxima[i]=0;
for(int i=0; i<256;i++){
bool bigest=true;
for(int j=-window; j<=window && bigest;j++){
if(i+j>=0 && i+j<256 && histi[i+j]>histi[i])
bigest=false;
}
if(bigest)
maxima[maxima_idx++]=i;
}
if(maxima_idx<2) return pupiEllipse;
int max1=maxima[0];
int help_idx=0;
for(int i=0; i<maxima_idx;i++){
if(histi[maxima[i]]>histi[max1]){
max1=maxima[i];
help_idx=i;
}
}
maxima[help_idx]=255;
int max2=maxima[0];
for(int i=0; i<maxima_idx;i++){
if(histi[maxima[i]]>histi[max2]){
max2=maxima[i];
}
}
int m1=(max1>max2)?max1:max2;
int m2=(max1<max2)?max1:max2;
int min_val=1000000;
int min_idx=0;
for(int i=m2+1; i<m1;i++){
int cnt=0;
int val=0;
for(int j=-window; j<=window;j++){
if(i+j>0 && i+j<256){
val+=histi[i+j];
cnt++;
}
}
if(cnt>0)
val=val/cnt;
else
val=0;
if(val<min_val){
min_val=val;
min_idx=i;
}
}
cv::threshold(pupil_region,pupil_region,min_idx,255,cv::THRESH_BINARY);
/*
std::cout<<max1<<":"<<max2<<"|"<<min_idx<<std::endl;
cv::imshow("prth",pupil_region);
cv::waitKey(1000);
*/
//contur
contours.clear();
hierarchy.clear();
//cv::findContours( pupil_region, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
cv::Mat border(pupil_region.rows,pupil_region.cols,CV_8UC1, cv::Scalar(0,0,0));
for(int i=1;i<pupil_region.rows-1;i++)
for(int j=1;j<pupil_region.cols-1;j++)
if(pupil_region.data[(pupil_region.cols*i)+j]>0){
bool has_black=false;
for(int k1=-1;k1<=1;k1++)
for(int k2=-1;k2<=1;k2++)
if(pupil_region.data[(pupil_region.cols*(i+k1))+(j+k2)]==0)
has_black=true;
if(has_black)
border.data[(border.cols*i)+j]=255;
}
/*
cv::imshow("contur",border);
cv::waitKey(10000);
*/
//collect lines
std::vector<cv::Point> ak_line;
for(int i=0;i<border.rows;i++)
for(int j=0;j<border.cols;j++)
if(border.data[(border.cols*i)+j]==255){
ak_line.clear();
border.data[(border.cols*i)+j]=200;
ak_line.push_back(cv::Point(j,i));
size_t ak_idx=0;
while(ak_idx<ak_line.size()){
cv::Point ak_pos=ak_line.at(ak_idx);
for(int k1=-1;k1<=1;k1++)
for(int k2=-1;k2<=1;k2++)
if(ak_pos.y+k1>0 && ak_pos.y+k1<border.rows && ak_pos.x+k2>0 && ak_pos.x+k2<border.cols)
if(border.data[(border.cols*(ak_pos.y+k1))+(ak_pos.x+k2)]==255){
border.data[(border.cols*(ak_pos.y+k1))+(ak_pos.x+k2)]=200;
ak_line.push_back(cv::Point(ak_pos.x+k2,ak_pos.y+k1));
}
ak_idx++;
}
if(ak_line.size()>10)
contours.push_back(ak_line);
}
int index=-1;
for( size_t i = 0; i < contours.size(); i++ )
if(index==-1 || contours[index].size()<contours[i].size())
index=i;
//ellipse fit RANSAC like only better :-)
if(index>=0)
for( int i = index; i <=index; i++ ){
if( contours[i].size() > 5 ){
cv::RotatedRect best_pupi;
best_pupi.size.width=100;
best_pupi.size.height=1000;
int step=contours[i].size()/4;
std::vector<cv::Point> ak_set;
for(int iter=0;iter<4;iter++){
cv::RotatedRect ak_pupi;
ak_set.clear();
for(int set_st=step*iter;set_st<(step*iter)+(2*step);set_st++)
ak_set.push_back(contours[i].at(set_st%contours[i].size()));
if(ak_set.size()>5){
ak_pupi=cv::fitEllipse( cv::Mat(ak_set) );
/*
cv::ellipse(pupil_region,ak_pupi,cv::Scalar(150,150,150),1,8);
cv::imshow("ellipses",pupil_region);
*/
}
if(ak_pupi.center.x>0 && ak_pupi.center.y>0 && ak_pupi.size.height>0 && ak_pupi.size.width>0 &&
ak_pupi.size.height*ak_pupi.size.width>200 && ak_pupi.size.height-ak_pupi.size.width<20){
if(abs(best_pupi.size.width-best_pupi.size.height)>abs(ak_pupi.size.width-ak_pupi.size.height))
best_pupi=ak_pupi;
}
}
if(best_pupi.center.x>0 && best_pupi.center.y>0 && best_pupi.size.height>0 && best_pupi.size.width>0 &&
best_pupi.size.height*best_pupi.size.width>200 && best_pupi.size.height-best_pupi.size.width<20){
best_pupi.center.x+=minEllipse[k].center.x;
best_pupi.center.y+=minEllipse[k].center.y;
pupiEllipse.push_back(best_pupi);
}
}
}
}
}
minEllipse.clear();
/*
cv::imshow("img",*pic);
cv::imshow("pupils",result);
cv::waitKey(200);
*/
return pupiEllipse;
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat CircleImage;
cv::resize(cv_ptr->image,CircleImage,cv::Size(cv_ptr->image.cols/processingScale,cv_ptr->image.rows/processingScale));
cv::cvtColor( CircleImage, CircleImage, CV_BGR2GRAY );
//cout << "3 " << endl;
cv::GaussianBlur( CircleImage, CircleImage, cv::Size(5, 5), 2, 2 );
// cv::vector<cv::Vec3f> circles;
// cv::imshow(WINDOW, CircleImage);
// cv::HoughCircles( CircleImage, circles, CV_HOUGH_GRADIENT, 1, CircleImage.rows/(8*processingScale), 200, 100, 0, 0 );
cv::vector<std::vector<cv::Point> > contours;
cv::vector<cv::Vec4i> hierarchy;
cv::Canny(CircleImage,CircleImage,100,250,3);
//cv::imshow(WINDOW, CircleImage);
cv::findContours(CircleImage,contours,hierarchy,CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
cv::vector<cv::RotatedRect> minEllipse( contours.size() );
int numCircles = 0;
bool circleMask [contours.size()];
for (int i = 0;i<contours.size();i++)
{
circleMask[i] = false;
}
for( int i = 0; i < contours.size(); i++ )
{
if( contours[i].size() > 5 )
{
minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) );
minEllipse[i].center.x = minEllipse[i].center.x * processingScale;
minEllipse[i].center.y = minEllipse[i].center.y * processingScale;
minEllipse[i].size.height = minEllipse[i].size.height * processingScale;
minEllipse[i].size.width = minEllipse[i].size.width * processingScale;
double approxCircle = minEllipse[i].size.height>minEllipse[i].size.width?minEllipse[i].size.height/minEllipse[i].size.width:minEllipse[i].size.width/minEllipse[i].size.height;
std::cout << i << ":" << approxCircle;
if (approxCircle<majorToMinor)
{
std::cout << ": yes" << std::endl;
numCircles++;
circleMask[i] = true;
cv::Scalar color = cv::Scalar( 255,255,0 );
cv::ellipse( cv_ptr->image, minEllipse[i], color, 2, 8 );
}
std::cout << std::endl;
}
}
// std::cout << "Sizes: " << minEllipse.size() << ":"<< numCircles << std::endl;
std::vector<double> x;
std::vector<double> y;
std::vector<double> r;
if (true)//numCircles>4 && numCircles<20 && minEllipse.size()<35) // culls out larger data sets
{
bool flag = false;
for (int i = 0; i<minEllipse.size();i++)
{
if (circleMask[i]==true)
{
for (int j=0; j<minEllipse.size();j++)
{
if (circleMask[j]==true && i!=j && ImageConverter::Distance(minEllipse[i].center.x,minEllipse[i].center.y,minEllipse[j].center.x,minEllipse[j].center.y) < 10)
{
// std::cout << i << ":" << j << "::::"<< abs( (minEllipse[i].size.height>minEllipse[j].size.height ? minEllipse[i].size.height/minEllipse[j].size.height : minEllipse[j].size.height/minEllipse[i].size.height) - 2) << std::endl;
double r0 = minEllipse[i].size.height>minEllipse[i].size.width? minEllipse[i].size.height : minEllipse[i].size.width;
double r1 = minEllipse[j].size.height>minEllipse[j].size.width? minEllipse[j].size.height : minEllipse[j].size.width;
double diff = (r0>r1?r0/r1:r1/r0);
diff = (diff-2.0)>0.0?(diff-2.0):-(diff-2.0); // manual abs
std::cout << r0 << ":" << r1 << "::::"<<diff;
if (diff < 0.5)
{
std::cout << " yes ";
// calculates distance between points
bool success = true;
for (int k=0;k<x.size();k++)
{
if( abs(x.at(k)-minEllipse[i].center.x)<10 && abs(y.at(k)-minEllipse[i].center.y)<10 )
{success=false;}
}
if(success)
{
x.push_back(minEllipse[i].center.x);
y.push_back(minEllipse[i].center.y);
r.push_back(r0>r1?r0:r1);
cv::Scalar color = cv::Scalar( 0,0,255);
cv::ellipse( cv_ptr->image, minEllipse[r0>r1?i:j], color, 2, 8 );
}
if (x.size() == 4)
{
std::cout << "Points: " << x.size() << std::endl;
for (int index = 0; index < x.size(); index++)
{
circle_data.data[3*index]=x.at(index);
circle_data.data[3*index+1]=y.at(index);
circle_data.data[3*index+2]=r.at(index);
}
circle_pub.publish(circle_data);
flag=true;
break;
}
}
std::cout << std::endl;
}
}
}
if (flag==true){break;}
}
}
cv::imshow(WINDOW, cv_ptr->image);
//std::cout<<"lbl 2"<<std::endl;
cv::waitKey(1);
// std::cout<<"lbl 3"<<std::endl;
image_pub_.publish(cv_ptr->toImageMsg());
//std::cout<<"lbl 4"<<std::endl;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::RotatedRectArrayStamped rects_msg, ellipses_msg;
rects_msg.header = msg->header;
ellipses_msg.header = msg->header;
// Do the work
cv::Mat src_gray;
/// Convert image to gray and blur it
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
} else {
src_gray = frame;
}
cv::blur( src_gray, src_gray, cv::Size(3,3) );
/// Create window
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
}
cv::Mat threshold_output;
int max_thresh = 255;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::RNG rng(12345);
/// Detect edges using Threshold
cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
/// Find contours
cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
/// Find the rotated rectangles and ellipses for each contour
std::vector<cv::RotatedRect> minRect( contours.size() );
std::vector<cv::RotatedRect> minEllipse( contours.size() );
for( size_t i = 0; i < contours.size(); i++ )
{ minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) );
if( contours[i].size() > 5 )
{ minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); }
}
/// Draw contours + rotated rects + ellipses
cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
for( size_t i = 0; i< contours.size(); i++ )
{
cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
// contour
cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
// ellipse
cv::ellipse( drawing, minEllipse[i], color, 2, 8 );
// rotated rectangle
cv::Point2f rect_points[4]; minRect[i].points( rect_points );
for( int j = 0; j < 4; j++ )
cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 );
opencv_apps::RotatedRect rect_msg;
opencv_apps::Point2D rect_center;
opencv_apps::Size rect_size;
rect_center.x = minRect[i].center.x;
rect_center.y = minRect[i].center.y;
rect_size.width = minRect[i].size.width;
rect_size.height = minRect[i].size.height;
rect_msg.center = rect_center;
rect_msg.size = rect_size;
rect_msg.angle = minRect[i].angle;
opencv_apps::RotatedRect ellipse_msg;
opencv_apps::Point2D ellipse_center;
opencv_apps::Size ellipse_size;
ellipse_center.x = minEllipse[i].center.x;
ellipse_center.y = minEllipse[i].center.y;
ellipse_size.width = minEllipse[i].size.width;
ellipse_size.height = minEllipse[i].size.height;
ellipse_msg.center = ellipse_center;
ellipse_msg.size = ellipse_size;
ellipse_msg.angle = minEllipse[i].angle;
rects_msg.rects.push_back(rect_msg);
ellipses_msg.rects.push_back(ellipse_msg);
}
/// Create a Trackbar for user to enter threshold
if( debug_view_) {
if (need_config_update_) {
config_.threshold = threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
cv::imshow( window_name_, drawing );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
img_pub_.publish(out_img);
rects_pub_.publish(rects_msg);
ellipses_pub_.publish(ellipses_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
