OpenCV SURF Robot


I built a small robot to detect an object using image processing techniques and when the object is found, it goes near to it and picks it up.

OpenCV was used for image processing and it extracted SURF keypoints of the object (a 9V pp3 battery in this case), then extracted SURF Keypoints and Descriptors of the captured frames. Then they were compared using nearest neighbours approach to find matching. And if they match, it returns the x,y coordinates of the detected object.Then the screen was partitioned into  9 zones as below:

The control algorithm is coded in such a way that the robot moves until the coordinates of the detected object are in zone 5. If the detected coordinates are in zone 6, then the robot should move right, causing the object to come in zone 5. If it was in zone 2, the robot should move forward to bring the object in zone 5. Once the object is in zone 5, the arms of the gripper are aligned with the object. Now the robot should move forward to grip the object. The gripper has an LED-LDR arrangement which detects the presence of an object in between the arms.Once the object is in between the arms, the gripper closes it and takes the object.

The image processing code is given below:

/*
Surf
*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.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 x = src_corners[i].x, y = src_corners[i].y;
 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;
}

int main(int argc, char** argv)
{
 const char* object_filename = "image.bmp"; // input image to be detected

int key;

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);
 //help();
 // cvNamedWindow("Object", 1);
 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(3);
CvMat* prevgray = 0, *image = 0, *gray =0;
while( key != 'q' )
{
int firstFrame = gray == 0;
IplImage* frame = cvQueryFrame(capture);
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..nnnnggggg...\n");
 }
 vector<int> ptpairs;
 findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 for( i = 0; i < (int)ptpairs.size(); i += 2 )
 {
 CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, ptpairs[i] );
 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;
 center.x = cvRound(r->pt.x);
 center.y = cvRound(r->pt.y);
 radius = cvRound(r->size*1.2/9.*2);
 cvCircle( object_color, center, radius, colors[0], 1, 8, 0 );
 }
 cvWaitKey(30);
}}

The control signals from the computer were sent to an AVR microcontroller through a USB to Serial interface.The control signals were “echo”ed through system calls in C to the serial port ttyUSB0. The AVR microcontroller, upon receiving commands from the PC, drives the motors and the arm using the popular H bridge L293D.

21 thoughts on “OpenCV SURF Robot

  1. dude please help me over this.its kinda urgent.
    can we use system(“echo 7 > /dev/ttyUSB0”) to send an signal to microprocessor from an windows 7 operating system????

    if yes will that 7 be sent to the microprocessor.
    thxs

    1. achu can u pleas help me please m in trouble m trying to create vec file bus i stuck in a problm coz vec file having size 0 kb plz help me m detecting vehicle coz its ma year pro can u plz help me m saba plz inform me i will b waitng

      1. acu my year project is vehicle detection count and speed measurements for high ways .now my 1st task is to detect vehicle i follow u r procedure but m stuck when i try to create vec file .then the vec file is created but it is empty and having no size, size of 0 kb

      2. acu r u getting my point???? r u getting what i am trying to say ??? i want to detect vehicles as u have stated in this blog about vec file .i stuck there my vec file hace size of 0kb can i send u its screenshot ……..

      3. The 0 Kb size error is due to the incorrect size of input array

        -First check you description file: positive_image_name should be absolute path name without spaces like “C:\content\image.jpg” not “C:\con tent\image.jpg” or relative path name.
        -Avoid empty lines in description file
        -Resolution of original positive image file should be not less, then -w -h parameters you put.
        -Check that positive images are available in your file systems and not corrupted.
        -There can be unsupported formats. Jpeg, Bmp, PPM are supported!

        courtesy:
        http://www.computer-vision-software.com/blog/2009/11/faq-opencv-haartraining/

      4. listen accu my positive file name is pos_sam and description file name is pos_desc1.dat and image name is car_001.png to car_500.png and size of each image is 24*24 and i made its descriptn file as pos_sam\car_001.png 1 0 0 24 24 isn,t that fine ????

      5. The 0 Kb size error is due to the incorrect size of input array

        -First check you description file: positive_image_name should be absolute path name without spaces like “C:\content\image.jpg” not “C:\con tent\image.jpg” or relative path name.
        -Avoid empty lines in description file
        -Resolution of original positive image file should be not less, then -w -h parameters you put.
        -Check that positive images are available in your file systems and not corrupted.
        -There can be unsupported formats. Jpeg, Bmp, PPM are supported!

        courtesy:
        http://www.computer-vision-software.com/blog/2009/11/faq-opencv-haartraining/

  2. could u explain this code,i was looking to make utilize of your algorithym in my final year project(college)but its tough for me to understand
    thanks,great blog!!!
    email id:indiafavourate@yahoo.co.in

      1. please help me m doing vehicle detection i have colectd postve negatve imags and made their desc file but when i used create sample -info to create sample it creates sample file with 0kb size help me so that i can complte ma year projct …..plz notify me at rani_tanoli555@yahoo.com

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s