Esempio n. 1
0
int main( int argc, char** argv )
{
	int KDTREE_BBF_MAX_NN_CHKS = 200;
	float NN_SQ_DIST_RATIO_THR = 0.49;
	const CvScalar color = cvScalar( 255, 255, 0 );
	ros::init(argc, argv, "SIFT");
	ros::NodeHandle n;
	ros::Rate rate(33);

	ImageConverter ic;

	while ( !ic.ready )
	{
		ros::spinOnce();
		rate.sleep();
		if ( !ros::ok() )
		{
			printf("terminated by control_c\n");
			return 0;
		}
	}

	string filepath = ros::package::getPath("sift") + "/";

	ifstream fin( (filepath + "store.txt").data(), ios::in); 
	//ifstream fin_main_pic( (filepath + "main_pic.txt").data(), ios::in);
	int pic_num = 5;
	string find;
	//cout << "how many pictures?" << endl;
	//cin >> pic_num;
	//cout << "which picture?" << endl;
	//cin >> find;

	time_t rawtime; 
	struct tm * timeinfo; 
	time ( &rawtime ); 
	timeinfo = localtime ( &rawtime ); 
	printf ( "The current date/time is: %s", asctime (timeinfo) ); 

	char line[1024] = {0}; 
	string* store = new string [pic_num+1];
	string main_pic_name;
	int pic = 0;
	int find_pic = 0;
	while(fin.getline(line, sizeof(line))) 
	{
		stringstream word(line); 
		word >> store[pic];
		store[pic] = filepath + store[pic];
		/*if (store[pic] == find)
		{
			cout << store[pic] << endl;
			find_pic = pic;
		}*/
		pic++;
	}
	//fin_main_pic.getline(line, sizeof(line));
	//stringstream word(line);
	//word >> main_pic_name;
	//fin_main_pic.clear();
	//fin_main_pic.close();
	fin.clear(); 
	fin.close();

	IplImage* img;
	//IplImage* img1;
	struct feature* features;//, * features1;

	feature** features_all = new feature*[pic];
	int* features_num = new int[pic];
	for (int i = 0; i < pic; i++ )
		features_num[i] = 0;
	IplImage** img_all = new IplImage*[pic];
	for ( int i = 0; i < pic; i++ )
	{
		//printf ( "Finding features in template picture %d\n", i );
		img_all[i] = cvLoadImage( store[i].data(), 1 );
		features_num[i] = sift_features( img_all[i], &features_all[i] );
		printf ( "%d features in template picture %d\n", features_num[i], i );
		time ( &rawtime ); 
		timeinfo = localtime ( &rawtime ); 
		printf ( "The current date/time is: %s", asctime (timeinfo) );
	}
/*
	printf ( "Finding features in main picture\n" );
	img = cvLoadImage( main_pic_name.data(), 1 );
	int n1 = sift_features( img, &features );
	printf ( "%d features in main picture\n", n1 );
*/
	
	//cvShowImage( "main", img );
	//for (int i = 0; i < n1; i++)
		//cvCircle( img, cvPoint(features[i].x, features[i].y), 5, color, 1, 8, 0 );
	//cvShowImage( "Foundmain", img );


	//cvShowImage( "template", img1 );
	//for (int i = 0; i < n2; i++)
		//cvCircle( img1, cvPoint(features1[i].x, features1[i].y), 5, color, 1, 8, 0 );
	//cvShowImage( "Foundtemplate", img1 );

	bool features_catched = false;
	while ( ros::ok() )
	{
		if ( ic.ready == true )
		{
			ic.ready = false;
			*img = ic.curr_image;
			int n1 = sift_features( img, &features );
			printf ( "%d features in main picture\n", n1 );
			time ( &rawtime ); 
			timeinfo = localtime ( &rawtime ); 
			printf ( "The current date/time is: %s", asctime (timeinfo) );
			features_catched = false;
			for ( int j = 0; j < pic ; j++ )
			{
				IplImage* stacked;
				IplImage* ransac;
				struct feature* feat;
				struct feature** nbrs;
				struct kd_node* kd_root;
				CvPoint pt1, pt2;
				double d0, d1;
				int k, i, m = 0;
				CvMat point1_test;
				CvMat point2_test;
				double point1[3];
				double point2[3] = { 0 };

				stacked = stack_imgs( img, img_all[j] );
				ransac = stack_imgs( img, img_all[j] );

				kd_root = kdtree_build( features_all[j], features_num[j] );
				for( i = 0; i < n1; i++ )
				{
					feat = features + i;
					k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
					if( k == 2 )
					{
						d0 = descr_dist_sq( feat, nbrs[0] );
						d1 = descr_dist_sq( feat, nbrs[1] );
						if( d0 < d1 * NN_SQ_DIST_RATIO_THR )
						{
							//pt1 = cvPoint( cvRound( feat->x ), cvRound( feat->y ) );
							//pt2 = cvPoint( cvRound( nbrs[0]->x ), cvRound( nbrs[0]->y ) );
							//pt2.y += img->height;
							//cvCircle( stacked, pt1, 3, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
							//cvCircle( stacked, pt2, 3, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
							//cvLine( stacked, pt1, pt2, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
							m++;
							features[i].fwd_match = nbrs[0];
						}
					}
					free( nbrs );
				}

				double accounts = m * 100 / (double)features_num[j];
				printf( "%d total matches, accounts for %f %%, in pic %d\n", m, accounts, j);
				//cvNamedWindow( "Matches", 1 );
				//cvShowImage( "Matches", stacked );

				time ( &rawtime ); 
				timeinfo = localtime ( &rawtime ); 
				printf ( "The current date/time is: %s", asctime (timeinfo) );	

				CvMat* H = ransac_xform( features, n1, FEATURE_FWD_MATCH, lsq_homog, 4, 0.01, error, 2, NULL, NULL );
			    if( H )
			    {
					for( i = 0; i < n1; i++ )
					{
						feat = features + i;
						k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
						if( k == 2 )
						{
							d0 = descr_dist_sq( feat, nbrs[0] );
							d1 = descr_dist_sq( feat, nbrs[1] );
							if( d0 < d1 * NN_SQ_DIST_RATIO_THR )
							{
								pt1 = cvPoint( cvRound( feat->x ), cvRound( feat->y ) );
								pt2 = cvPoint( cvRound( nbrs[0]->x ), cvRound( nbrs[0]->y ) );
								pt2.y += img->height;
								point1[0] = pt1.x;
								point1[1] = pt1.y;
								point1[2] = 1.0;
								cvInitMatHeader( &point1_test, 3, 1, CV_64FC1, point1, CV_AUTOSTEP );
								cvInitMatHeader( &point2_test, 3, 1, CV_64FC1, point2, CV_AUTOSTEP );
								cvMatMul( H, &point1_test, &point2_test );
								/*if ( abs( point2[0]/point2[2]-pt2.x) < 2 && abs( point2[1]/point2[2]+img->height-pt2.y) < 2 )
								{
									cvCircle( ransac, pt1, 3, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
									cvCircle( ransac, pt2, 3, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
									cvLine( ransac, pt1, pt2, cvScalar( (i*10)%255, (i*10)%255, 127 ), 1, 8, 0 );
								}*/
			//					features[i].fwd_match = nbrs[0];
							}
						}
						free( nbrs );
						//printf("features catched, going to exit\n");
					}

					//cvNamedWindow( "Xformed" );
					//cvShowImage( "Xformed", ransac );

					features_catched = true;
					time ( &rawtime ); 
					timeinfo = localtime ( &rawtime ); 
					printf ( "ransac.. The current date/time is: %s", asctime (timeinfo) ); 
			    }
				//cvWaitKey( 0 );
				cvReleaseImage( &ransac );
				cvReleaseMat( &H );
				//cvDestroyWindow( "main" );
				//cvDestroyWindow( "Foundmain" );
				//cvDestroyWindow( "template" );
				//cvDestroyWindow( "Foundtemplate" );
				//cvReleaseImage( &img_all[j] );
				cvReleaseImage( &stacked );
				kdtree_release( kd_root );
			}
			if (!features_catched)
			{
				printf("Sorry, there is no item in the picture\n");
			}
			else
			{
				printf("Item catched in the picture!\n");
			}
		}
		ros::spinOnce();
		rate.sleep();
	}
	//cvReleaseImage( &img );
	free( features );
	for ( int i = 0; i < pic; i++ )
	{
		free( features_all[i] );
	}
	free(features_all);
	return 0;
}
Esempio n. 2
0
int main( int argc, char** argv ) {
  struct feature* feat1, * feat2, * feat;
  struct feature** nbrs;
  struct kd_node* kd_root;
  double d0, d1;
  int n1, n2, k, i, m = 0;

  if( argc != 3 )
      fatal_error( "usage: %s <keydescr1> <keydescr2>", argv[0] );

  n1 = import_features( argv[1], feat_type, &feat1 );
  n2 = import_features( argv[2], feat_type, &feat2 );

  if( n1 < 0 )
      fatal_error( "unable to load key descriptors from %s", argv[1] );
  if( n2 < 0 )
      fatal_error( "unable to load key descriptors from %s", argv[2] );

  printf("%d features presenti nel modello.\n", n1);
  printf("%d features presenti nell'immagine.\n", n2);
  kd_root = kdtree_build( feat2, n2 );
  for( i = 0; i < n1; i++ ) {
      feat = feat1 + i;
      k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
      if( k == 2 ) {
        // Controlla
          d0 = descr_dist_sq( feat, nbrs[0] );
          d1 = descr_dist_sq( feat, nbrs[1] );
          if( d0 < d1 * NN_SQ_DIST_RATIO_THR ) {
              m++;
              feat1[i].fwd_match = nbrs[0];
          }
      }
      free( nbrs );
    }

#ifdef RANSAC
  {
    CvMat* H;
    IplImage* xformed;
    struct feature ** inliers;
    int n_inliers;
    H = ransac_xform( feat1, n1, FEATURE_FWD_MATCH, lsq_homog, 4, 0.01, homog_xfer_err, 8.0, &inliers, &n_inliers );
    if( H )
    {
      if ( cvmGet( H, 1, 2 ) < 120.0 &&
           cvmGet( H, 0, 2 ) < 120.0 &&
           (
            fabs(cvmGet( H, 0, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 0, 1)) >= 0.000001 ||
            fabs(cvmGet( H, 1, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 1, 1)) >= 0.000001 ||
            fabs(cvmGet( H, 2, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 2, 1)) >= 0.000001
           )
          ) {
        // TROVATO
        printf("RANSAC OK\n");
        /*
        xformed = cvCreateImage( cvGetSize( img2 ), IPL_DEPTH_8U, 3 );
        cvWarpPerspective( img1, xformed, H, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll( 0 ) );
        cvNamedWindow( "Xformed", 1 );
        cvShowImage( "Xformed", xformed );
        cvWaitKey( 0 );
        cvReleaseImage( &xformed );
        */
        printf("N. inliers: %d\n", n_inliers);
      }
      else {
        // Trovato ma probabilmente la matrice di trasformazione e' sbagliata.
        printf("RANSAC FAIL\n");
      }

      cvReleaseMat( &H );
    }
    else {
      printf("RANSAC FAIL\n");
    }
  }
#endif








    fprintf( stderr, "Found %d total matches\n", m );

    kdtree_release( kd_root );
    free( feat1 );
    free( feat2 );
    return 0;
}