/** * @file SURF_detector * @brief SURF keypoint detection + keypoint drawing with OpenCV functions * @author A. Huaman */
include <stdio.h>
include <iostream>
include "opencv2/core/core.hpp"
include "opencv2/features2d/features2d.hpp"
include "opencv2/highgui/highgui.hpp"
include "opencv2/nonfree/features2d.hpp"
include <sys time.h="">
include <opencv2 core="" core.hpp="">
include <opencv highgui.h="">
using namespace cv; using namespace std;
int main( int argc, char** argv ) {
cv::VideoCapture cap("tcp://192.168.1.1:5555"); // open a video file
if(!cap.isOpened()) // check if succeeded { std::cout << " not found or could not be opened" << std::endl; printf("4\n"); return -1; } // cv::namedWindow("output", cv::WINDOW_AUTOSIZE);
unsigned long counter =0;
cv::Mat frame; // read frames until end of video:
struct timeval start,end; long seconds,useconds; long mtime=0; gettimeofday(&start,NULL); cap >> frame; imwrite("image.pgm",frame); Mat img_1 = imread( "image.pgm" ); int minHessian = 500;
SurfFeatureDetector detector( minHessian,1,2,false,true );
std::vector<keypoint> keypoints_1p; std::vector<keypoint> keypoints_2p ;
detector.detect( img_1, keypoints_1p ); SurfDescriptorExtractor extractor(minHessian,1,1,1,0); Mat descriptors1, descriptors2;
extractor.compute(img_1, keypoints_1p, descriptors1); //std::cout << descriptors1.rows << std::endl; //std::cout << descriptors1.cols<< std::endl; //std::cout << descriptors1.type()<< std::endl; while(cap.read(frame)) {
// cv::imshow("output", frame);
//cv::waitKey(5); // remove this line if you don't need the live output
// adjust the filename by incrementing a counter
std::stringstream filename (std::stringstream::in | std::stringstream::out);
filename << "img-000" << counter++ << ".pgm";
//printf("filename =%s ",filename);
std::cout << "writing " << filename.str().c_str() << " to disk" << std::endl;
// save frame to file: image0.jpg, image1.jpg, and so on...
cv::imwrite(filename.str().c_str(),frame);
//gettimeofday(&end,NULL);
//seconds = end.tv_sec - start.tv_sec;
//useconds=end.tv_usec - start.tv_usec;
//mtime = (((seconds)*1000.0 )+ (useconds/1000.0)) + 0.5;
//printf("elapsed time :%ld milliseconds\n",mtime);
//if(mtime > 1000)
//break;
printf("4\n");
Mat img_2 = imread( filename.str().c_str() );
if( !img_1.data || !img_2.data )
{
std::cout<< " --(!) Error reading images " << std::endl; return -1;
}
//-- Step 1: Detect the keypoints using SURF Detector
detector.detect( img_2, keypoints_2p);
// computing descriptors
extractor.compute(img_2, keypoints_2p, descriptors2);
//std::cout << descriptors2.rows << std::endl;
//std::cout << descriptors2.cols << std::endl;
int i,j;
/*FileStorage fs("desc1.yml",FileStorage::WRITE);
for( i=0; i<descriptors1.rows; i++)="" {="" for(="" j="0;" j<descriptors1.cols;j++)="" {="" write(fs,descriptors1.at<float="">(i,j));
}
}
fs.release();
FileStorage fs1("desc2.yml",FileStorage::WRITE); for( i=0; i<descriptors2.rows; i++)="" {="" for(="" j="0;" j<descriptors2.cols;j++)="" {="" write(fs1,descriptors2.at<float="">(i,j));
}
}
fs1.release();*/
printf("7\n"); Mat a(descriptors1.rows,descriptors1.cols,descriptors1.type()); Mat b(descriptors2.rows,descriptors2.cols, descriptors2.type()); a=descriptors1; b=descriptors2;
Mat m(descriptors1.rows,2,CV_16U);
//std::cout << m.rows << std::endl;
//std::cout << m.cols << std::endl;
if(((descriptors2.rows)||(descriptors1.rows))==0)
{
m.push_back(0);
return 0;
}
Mat des2t(descriptors2.rows,descriptors2.cols,descriptors2.type()); //std::cout << des2t.rows << std::endl; //std::cout << des2t.cols << std::endl;
des2t= b.t(); //std::cout << des2t.rows << std::endl; //std::cout << des2t.cols << std::endl;
m= Mat::zeros(descriptors1.rows,1,CV_32F);
Mat dotprod = (descriptors1*des2t); //std::cout << dotprod.type() << std::endl; Mat angle(dotprod.rows,dotprod.cols,dotprod.type());
for(int k=0;k<dotprod.rows;k++) {="" for(int="" l="0;l<dotprod.cols;l++)" {<="" p="">
angle.at<float>(k,l)= acos(dotprod.at<float>(k,l));
}
}
//std::cout << dotprod.rows << std::endl; //std::cout << dotprod.cols << std::endl; printf("6\n");
int jmin,jmin2;
float minangle,min2; for(i=0;i< dotprod.rows;i++) { minangle=angle.at<float>(i,0); jmin=1; min2=angle.at<float>(i,0); jmin2=1; for(j=1;j<dotprod.rows;j++) {<="" p="">
if(minangle > angle.at<float>(i,j))
{
minangle=angle.at<float>(i,j);
jmin=j;
}
if((min2>angle.at<float>(i,j)) && (angle.at<float>(i,j) > minangle))
{
min2=angle.at<float>(i,j);
jmin2=j;
}
}
if(minangle < 0.8*min2)
{
m.at<int>(i,0)=jmin;
//m.at<int>(i,0)=i;
}
else
{
m.at<int>(i,0)=jmin2;
//m.at<int>(i,0)=i;
}
//std::cout << jmin <<std::endl;
}
printf("5\n"); vector <point2f> im1; vector <point2f> im2;
//std::cout << m.rows << std::endl; //std::cout << m.cols << std::endl;
for(int z=0;z<= m.rows;z++) { for(int y=0;y<=m.cols;y++) { //std::cout << m.at<int>(z,y) <<std::endl;< p="">
} } for(int z=0;z<= descriptors2.rows;z++) { for(int y=0;y<=descriptors2.cols;y++) {
im2.push_back(keypoints_2p[descriptors2.at<float>(z,y)].pt);
} }
printf("20\n"); for( int z=0;z<= descriptors1.rows;z++) { for( int y=0;y<=descriptors1.cols;y++) {
im1.push_back(keypoints_1p[descriptors1.at<float>(z,y)].pt);
} } printf("10\n"); //calculate the dx and dy for alll features int v; vector <float> dx,dy; Point s,h; float hx; float hy; float sx,sy; printf("1\n"); for(int z=0;z<=m.rows;z++) { for(int k=0;k<m.cols;k++) {<="" p="">
v=m.at<int>(z,k);
s=im1[k];
sx=s.x;
sy=s.y;
h=im2[v];
hx=h.x;
hy=h.y;
dx.push_back(sx-hx);
dy.push_back(sy-hy);
} } printf("3456\n"); //calculate the mean of the dx and dy float sumx,sumy; sumx=0; sumy=0; int den=m.rows;
for(int k=0;k<den-1;k++) {="" sumx="dx[k]+dx[k+1];" sumy="dy[k]+dy[k+1];" <="" p="">
} float dx1; float dy1;
dx1=sumx/den; dy1=sumy/den;
std::cout << dx1 << std::endl; std::cout << dy1 << std::endl;
float dxf=dx1(0.00144); float dyf=dy1(0.00144);
std::cout << dxf << std::endl; std::cout << dyf << std::endl;
//-- Draw keypoints //Mat img_keypoints_1; //Mat img_keypoints_2;
//drawKeypoints( img_1, keypoints_1p, img_keypoints_1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); //drawKeypoints( img_2, keypoints_2p, img_keypoints_2, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
//-- Show detected (drawn) keypoints //imshow("Keypoints 1", img_keypoints_1 ); //imshow("Keypoints 2", img_keypoints_2 );
//waitKey(0);
//return 0; } }