2.3.1 Phương pháp thực hiện
Xây dựng chương trình phát hiện và định vị trí đối tượng bằng cách sử dụng các kĩ thuật xử lý ảnh. Chương trình thực hiện tìm đối tượng, nếu tìm thấy sẽ ra lệnh điều khiển thiết bị bên dưới thông qua cổng RS32 tiến lại gần vật.
Trong chương trình chúng tôi sử dụng thư viện OpenCV trong xử lý ảnh và trích xuất keypoint của đối tượng bằng thuật toán SIFT. Sau đó chiết xuất trích lọc keypoint và descriptor của từng frame hình được định vị. Tiếp theo chương trình sẽ so sánh các điểm keypoint lần cận để tìm thấy những cặp keypoint phù hợp nhất. Và nếu chúng phù hợp với nhau thì sẽ trả về tọa độ x,y của đối tượng. Với phương pháp này, chúng tôi chia màn hình ra làm 9 phần tương ứng như hình sau:
Hình 2.7. Mô tả việc chia màn hình ra làm 9 phần để định vị vị trí đối tượng
Phần điều khiển thiết bị bến dưới sử dụng các thuật toán điểu khiển thiết bị được mã hóa sao cho robot di chuyển đến đúng tọa độ của đối tượng được phát hiện là khu vực số 5. Nếu tọa độ phát hiện trong khu vực 6, thì robot sẽ di chuyển sang bên phải, đối tượng sẽ xuất hiện trong khu vực 5. Nếu đối tượng rơi vào khu vực 2, robot sẽ di chuyển về phía trước để đưa đối tượng rơi vào khu vực 5. Khi đối tượng lọt vào khu vực 5, nếu robot có cách tay bắt lấy đối tượng thì cách tay sẽ đưa ra kẹp lấy đối tượng. Khi đó, robot sẽ di chuyển về phía trước để kẹp chặc đối tượng. Trong lúc robot tiến về phía trước sẽ sử dụng kinect để đo khoảng cách đến đối tượng sao cho khoảng cách đúng với thiết kết của cánh tay robot để bắt lấy đối tượng.
Các tính hiệu điều khiển từ máy tính sẽ gửi đến các vi điều khiển AVR thông qua cổng USB hoặc cổng serial RS32 với thiết bị kết nối tiếp vào interface. Sử dụng lệnh “echo” thông qua lời gọi hệ thống trong C vào cổng nối tiếp ttyUSB0. Các vi điều khiển AVR, khi nhận được lệnh từ máy tính, các động cơ và cánh tay sẽ hoạt động.
2.3.2 Mã chương trình nhận dạng đối tượng
#include <stdlib.h> #include <stdio.h> #include <math.h> #include <string.h> #include <stdio.h> #include "libfreenect.h"
#include "libfreenect_sync.h" //#include "libfreenect_cv.h" #include <opencv2/objdetect/objdetect.hpp> #include <opencv2/features2d/features2d.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> #include <opencv2/imgproc/imgproc_c.h> #include <opencv2/video/tracking.hpp> #include <iostream> #include <vector> using namespace std; IplImage *image = 0;
double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
{
double total_cost = 0; assert( length % 4 == 0 ); for( int i = 0; i < length; i += 4 ) {
double t0 = d1[i] - d2[i]; double t1 = d1[i+1] - d2[i+1]; double t2 = d1[i+2] - d2[i+2]; double t3 = d1[i+3] - d2[i+3];
total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3; if( total_cost > best )
break; }
return total_cost; }
int naiveNearestNeighbor( const float* vec, int laplacian, const CvSeq* model_keypoints, const CvSeq* model_descriptors )
{
int length = (int)(model_descriptors->elem_size/sizeof(float)); int i, neighbor = -1;
double d, dist1 = 1e6, dist2 = 1e6; CvSeqReader reader, kreader;
cvStartReadSeq( model_keypoints, &kreader, 0 ); cvStartReadSeq( model_descriptors, &reader, 0 );
for( i = 0; i < model_descriptors->total; i++ ) {
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr; const float* mvec = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader ); CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader ); if( laplacian != kp->laplacian )
continue;
d = compareSURFDescriptors( vec, mvec, dist2, length ); if( d < dist1 ) { dist2 = dist1; dist1 = d; neighbor = i; } else if ( d < dist2 ) dist2 = d; } if ( dist1 < 0.6*dist2 ) return neighbor; return -1; }
void findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs ) {
int i;
CvSeqReader reader, kreader;
cvStartReadSeq( objectKeypoints, &kreader ); cvStartReadSeq( objectDescriptors, &reader ); ptpairs.clear();
for( i = 0; i < objectDescriptors->total; i++ ) {
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr; const float* descriptor = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader ); CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
if( nearest_neighbor >= 0 ) { ptpairs.push_back(i); ptpairs.push_back(nearest_neighbor); } } }
/* a rough implementation for object location */
int locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq*
objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, const CvPoint src_corners[4], CvPoint dst_corners[4] )
{ double h[9]; CvMat _h = cvMat(3, 3, CV_64F, h); vector<int> ptpairs; vector<CvPoint2D32f> pt1, pt2; CvMat _pt1, _pt2; int i, n;
findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs ); n = (int)(ptpairs.size()/2); if( n < 4 ) return 0; pt1.resize(n); pt2.resize(n); for( i = 0; i < n; i++ ) { pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt; pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt; } _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] ); _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 )) return 0;
for( i = 0; i < 4; i++ ) {
double Z = 1./(h[6]*x + h[7]*y + h[8]); double X = (h[0]*x + h[1]*y + h[2])*Z; double Y = (h[3]*x + h[4]*y + h[5])*Z;
dst_corners[i] = cvPoint(cvRound(X), cvRound(Y)); }
return 1; }
//////////
IplImage *freenect_sync_get_depth_cv(int index) {
static IplImage *image = 0; static char *data = 0;
if (!image) image = cvCreateImageHeader(cvSize(640,480), 16, 1); unsigned int timestamp;
if (freenect_sync_get_depth((void**)&data, ×tamp, index, FREENECT_DEPTH_11BIT))
return NULL;
cvSetData(image, data, 640*2); return image;
}
IplImage *freenect_sync_get_rgb_cv(int index) {
static IplImage *image = 0; static char *data = 0;
if (!image) image = cvCreateImageHeader(cvSize(640,480), 8, 3); unsigned int timestamp;
if (freenect_sync_get_video((void**)&data, ×tamp, index, FREENECT_VIDEO_RGB)) return NULL; cvSetData(image, data, 640*3); return image; } int ConnectKinect(void) { while (cvWaitKey(10) < 0) {
IplImage *image = freenect_sync_get_rgb_cv(0); if (!image) {
return -1; }
cvCvtColor(image, image, CV_RGB2BGR);
IplImage *depth = freenect_sync_get_depth_cv(0); if (!depth) {
printf("Error: Kinect not connected?\n"); return -1; } cvShowImage("RGB", image); //cvShowImage("Depth", GlViewColor(depth)); } return 0; } /////////
int main(int argc, char** argv) {
const char* object_filename = "a1.png"; // input image to be detected int key=0;
//int sy;
//sy=system("stty -F /dev/ttyUSB0 cs8 115200 ignbrk -brkint -icrnl -imaxbel -opost - onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoe -noflsh -ixon"); CvMemStorage* storage = cvCreateMemStorage(0);
cvNamedWindow("Object Correspond", 1); static CvScalar colors[] =
{ {{0,0,255}}, {{0,128,255}}, {{0,255,255}}, {{0,255,0}}, {{255,128,0}}, {{255,255,0}}, {{255,0,0}}, {{255,0,255}}, {{255,255,255}} };
//CvCapture* capture = cvCreateCameraCapture(0); CvMat *image = 0;//, *gray =0;//* prevgray = 0,
while( key != 'q' ) {
int gray = 0;//firstFrame
IplImage* frame = freenect_sync_get_rgb_cv(0);//cvQueryFrame(capture); //cvCvtColor(image, image, CV_RGB2BGR);
if(!frame) break; if(!gray) {
image = cvCreateMat(frame->height, frame->width, CV_8UC1); }
cvCvtColor(frame, image, CV_BGR2GRAY);
CvSeq *imageKeypoints = 0, *imageDescriptors = 0; int i;
//Extract SURF points by initializing parameters CvSURFParams params = cvSURFParams(500, 1);
cvExtractSURF( image, 0, &imageKeypoints, &imageDescriptors, storage, params ); IplImage* object = cvLoadImage( object_filename, CV_LOAD_IMAGE_GRAYSCALE ); IplImage* object_color = cvCreateImage(cvGetSize(object), 8, 3);
cvCvtColor( object, object_color, CV_GRAY2BGR ); CvSeq *objectKeypoints = 0, *objectDescriptors = 0;
cvExtractSURF( object, 0, &objectKeypoints, &objectDescriptors, storage, params ); printf("Object Descriptors: %d\n", objectDescriptors->total);
printf("Image Descriptors: %d\n", imageDescriptors->total);
CvPoint src_corners[4] = {{0,0}, {object->width,0}, {object->width, object->height}, {0, object->height}};
CvPoint dst_corners[4];
IplImage* correspond = cvCreateImage( cvSize(image->width, object->height+image- >height), 8, 1 );
cvSetImageROI( correspond, cvRect( 0, 0, object->width, object->height ) ); cvCopy( object, correspond );
cvSetImageROI( correspond, cvRect( 0, object->height, correspond->width, correspond->height ) );
cvCopy( image, correspond ); cvResetImageROI( correspond );
if( locatePlanarObject( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, src_corners, dst_corners ))
{ printf("object found\n"); for( i = 0; i < 1; i++ ) { CvPoint r1 = dst_corners[i%4]; CvPoint r2 = dst_corners[(i+1)%4];
cvLine( correspond, cvPoint(r1.x, r1.y+object->height ), cvPoint(r2.x, r2.y+object->height ), colors[8] );
printf("%d,%d\n", r1.x, r1.y); if(r1.x<290) { printf("MOVE RIGHT\n"); //sy=system("echo -n '3' > /dev/ttyUSB0"); } if(r1.x>340) { printf("MOVE LEFT\n"); //sy=system("echo -n '2' > /dev/ttyUSB0"); } if((r1.x>290)&&(r1.x<340)) { printf("MOVE FORWARD\n"); //sy=system("echo -n '1' > /dev/ttyUSB0"); } } } else { printf("searching...\n"); //sy=system("echo -n '7' > /dev/ttyUSB0"); printf("searching..doi tuong...\n"); } vector<int> ptpairs;
findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
for( i = 0; i < (int)ptpairs.size(); i += 2 ) {
CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( imageKeypoints, ptpairs[i+1] ); cvLine( correspond, cvPointFrom32f(r1->pt),
cvPoint(cvRound(r2->pt.x), cvRound(r2->pt.y+object->height)), colors[8] ); }
cvShowImage( "Object Correspond", correspond ); for( i = 0; i < objectKeypoints->total; i++ )
{
CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, i ); CvPoint center;
int radius;
3 Chương 3 Kết quả thử nghiệm và định hướng ứng dụng
Hình3.1 Tìm thấ y đố i tư ợ ng bên trái và ra lệ nh robot dị ch chuyể n sang phả i
Hình 3.2 Tìm thấy đối tượng bên phải và ra lệnh robot di chuyển qua trái
Hình 3.3 Tìm thấy đối tượng bên phía trước và ra lệnh robot đi thẳng
4 Tài liệu tham khảo
[1].Learning computer Vision with the OpenCV Library, Gary Bradski and Adrian Kaehler, University of British Comlumbia, Mỹ.
[2].OpenCV 2 Computer Vision Application Programming Cookbook, Robert Laganière, Published by Packt Publishing Ltd, 32 Lincoln Road, Olton, Birmingham, B27 6PA, UK.
[3].“Analysis of Kinect for Mobile Robots,” Mikkel Viager, Technical University of Denmark, p. 11
[4].“A Qualitative Analysis of Two Automated Registration Algorithms In Real World Scenario Using Point Clouds from the Kinect” Jacob Kjær,June 27, 2011.
[5].http://openni.org/Documentation [6].http://pointclouds.org/documentation [7].http://en.wikipedia.org/wiki/Kinect