双目视觉的例子（Stereo算法）

刚才整理了下我以前做过的Stereo算法, 给大家分享下. 在Windows VS2008, 和MacOS下用Xcode, openCV做的.
源码在下边, 如果要直接用的话, 图像路径要改改.

Left tsukuba image: http://graphics.cis.udel.edu/research/C ... reo1/L.jpg
right tsukuba image: http://graphics.cis.udel.edu/research/C ... reo1/R.jpg

大约4,5秒钟 window size=11, disparity search range=20
matching 的算法是Normal Cross Correlation...

(VS2008+OpenCV 1.1)

// cvStereoNCC.cpp : Defines the entry point for the console application.

//

#include "stdafx.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
#include <ctime>


using namespace std;
template<class T> class Image
{
   private: 
      IplImage* imgp;
   public:
      Image(IplImage* img=0){imgp=img;}
      ~Image(){imgp=0;}
      void operator=(IplImage* img){imgp=img;}
      inline T* operator[](const int rowIndx){
         return((T*)(imgp->imageData+rowIndx*imgp->widthStep));}

};

typedef struct{
   unsigned char b,g,r;
}RgbPixel;

typedef struct{
   float b,g,r;
}RgbPixelFloat;

typedef Image<RgbPixel> RgbImage;
typedef Image<RgbPixelFloat> RgbImageFloat;
typedef Image<unsigned char> BwImage;
typedef Image<float> BwImageFloat;





/*
coded and updated by Huang, Haiqiao 2010-01-07
Normalized Cross Correlation Strereo Vision algorithm
*/

void displayImageProperty(IplImage* image){ 
   cout<<"-------Image Properties--------"<<endl;
   cout<<"Image width="<<image->width<<endl;
   cout<<"Image height="<<image->height<<endl;
   cout<<"Image depth="<<image->depth<<endl;
   cout<<"Image nSize="<<image->nSize<<endl;
   cout<<"Image nChannels="<<image->nChannels<<endl; 
   char* origin;
   char* dataOrder;
   if (image->origin==0){
       origin="Top-left";
   }else{
       origin="Below-left";//image->origin=1
   }
   cout<<"Image origin="<<origin<<endl;

   if (image->dataOrder==0){
       dataOrder="Order_Pixel(Interleaved)";
   }else{
       dataOrder="Order_Plane";//image->dataOrder=1
   }
   cout<<"Image dataOrder="<<dataOrder<<endl;
   cout<<"Image widthStep="<<image->widthStep<<" Bytes"<<endl;


}

//display an image in a new window with title to be given.
void displayImageNewWindow(char* title,CvArr* img){
   cvNamedWindow(title, CV_WINDOW_AUTOSIZE );
   cvShowImage(title,img); 
}


int getMaxMin(double value[],int valueSize, int maxmin)
{
   int pos=0;
   int i=0;
   double max1=-1;//?-999999;
   double min1=999999;
   
   if (maxmin==1){//find max
      for (i=0;i<valueSize;i++){//find the index with the max ncc; 
         if (value[i]>max1){
            pos=i;
            max1=value[i];
         }
      }
   }
   
   if (maxmin==0){//find min
      for (i=0;i<valueSize;i++){//find the index with the max ncc; 
         if (value[i]<min1){
            pos=i;
            min1=value[i];
         }
      }          
   }
      
   return pos;
}




IplImage* generateDisparityImage(IplImage* greyLeftImg32,
                         IplImage* greyRightImg32, 
                         int windowSize,int DSR){

    int offset=floor((double)windowSize/2); 
   int height=greyLeftImg32->height;
   int width=greyLeftImg32->width;
    double* localNCC=new double[DSR];

   int x=0, y=0,d=0,m=0;
   int N=windowSize;             

   IplImage* leftWinImg=cvCreateImage(cvSize(N,N),32,1);//mySubImage(greyLeftImg32,cvRect(0,0,N,N));
   IplImage* rightWinImg=cvCreateImage(cvSize(N,N),32,1);;//mySubImage(greyRightImg32,cvRect(0,0,N,N));
   IplImage* disparity=cvCreateImage(cvSize(width,height),8,1);//or IPL_DEPTH_8U
   BwImage imgA(disparity);
   
   for (y=0;y<height;y++){ 
      for (x=0;x<width;x++){
         imgA[y][x]=0;
      }
   }
   
     CvScalar s1;
   CvScalar s2;
   for (y=0;y<height-N;y++){ //height-N
      for (x=0;x<width-N;x++){//width-N
         //getWindow(i,j,leftim,wl,N);
         cvSetImageROI(greyLeftImg32, cvRect(x,y,N,N));
         s1=cvAvg(greyLeftImg32,NULL);
         cvSubS(greyLeftImg32,s1,leftWinImg,NULL);//zero-means
         cvNormalize(leftWinImg,leftWinImg,0,0,CV_L2,NULL);
         d=0;
         
          //initialise localNCC
          for (m=0;m<DSR;m++){localNCC[m]=0;}
          
          do{ 
            if (x-d>=0){
    
               cvSetImageROI(greyRightImg32, cvRect(x-d,y,N,N));
               s2=cvAvg(greyRightImg32,NULL);
               cvSubS(greyRightImg32,s2,rightWinImg,NULL);//zero-means 
               cvNormalize(rightWinImg,rightWinImg,0,0,CV_L2,NULL); 
            }else{
               break;
            }  
            localNCC[d]=cvDotProduct(leftWinImg,rightWinImg); 
            cvResetImageROI(greyRightImg32); 
            d++;
         }while(d<=DSR);
         
         //to find the best d and store
          imgA[y+offset][x+offset]=getMaxMin(localNCC,DSR,1)*16; 
         cvResetImageROI(greyLeftImg32);
      }//x 
      if (y%10==0)cout<<"row="<<y<<" of "<<height<<endl;  
   }//y
   
   cvReleaseImage(&leftWinImg); 
   cvReleaseImage(&rightWinImg); 
        
   return disparity;

}





int main (int argc, char * const argv[]) {
    // insert code here...
    cout << "Stereo Normalized Cross Correlation"<<endl;  
    
   //**********image input*********************//
    

   char* filename1="D:/OpenCV_stuff/SampleImages/im2_tsu.bmp";//im2_cone.png
   IplImage* greyLeftImg= cvLoadImage(filename1,0);
   char* filename2="D:/OpenCV_stuff/SampleImages/im6_tsu.bmp";
   IplImage* greyRightImg= cvLoadImage(filename2,0); 
   
   if (greyLeftImg==NULL){
      cout << "No valid image input."<<endl; 
      //char c=getchar();
      return 1;
   }else{displayImageProperty(greyLeftImg);}
   if (greyRightImg==NULL){
      cout << "No valid image input."<<endl; 
      //char c=getchar();
      return 1;
   }

   int width=greyLeftImg->width;
   int height=greyLeftImg->height;  

   /****************8U to 32F**********************/
   IplImage* greyLeftImg32=cvCreateImage(cvSize(width,height),32,1);//IPL_DEPTH_32F 
   IplImage* greyRightImg32=cvCreateImage(cvSize(width,height),32,1); 
   cvConvertScale(greyLeftImg, greyLeftImg32, 1/255.);
   cvConvertScale(greyRightImg, greyRightImg32, 1/255.);//1/255. equals to 1/255.0 
    
   //-------------Computing stereo matching---------------- 
   time_t tstart, tend; 
   tstart = time(0); 
   int windowSize=11,DSR=20;//Disparity Search Range
   IplImage* disparity32=generateDisparityImage(greyLeftImg32,greyRightImg32,windowSize,DSR); 
   tend = time(0);
   cout << "It took "<< difftime(tend, tstart) <<" second(s)."<< endl;

   displayImageNewWindow("Dispairty Image",disparity32);
   displayImageNewWindow("Left Image",greyLeftImg32);
   displayImageNewWindow("Right Image",greyRightImg32);
    //cvSaveImage("D:/OpenCV_stuff/SampleImages/disparity.jpg",disparity32);

   //********destroy window************/
   cvWaitKey(0);
   cvReleaseImage(&greyLeftImg32);
   cvReleaseImage(&greyRightImg32);
   cvReleaseImage(&greyLeftImg);
   cvReleaseImage(&greyRightImg); 
   cvReleaseImage(&disparity32); 
   cvDestroyWindow("Left Image"); 
   cvDestroyWindow("Right Image"); 
   cvDestroyWindow("Dispairty Image"); 
    return 0;

}

(Mac OS, Xcode)

/*************
Huang, Haiqiao 2009-10-29
*************/

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
#include <ctime>


using namespace std;

template<class T> class Image
{
   private: 
      IplImage* imgp;
   public:
      Image(IplImage* img=0){imgp=img;}
      ~Image(){imgp=0;}
      void operator=(IplImage* img){imgp=img;}
      inline T* operator[](const int rowIndx){
         return((T*)(imgp->imageData+rowIndx*imgp->widthStep));}

};
typedef struct{
   unsigned char b,g,r;
}RgbPixel;

typedef struct{
   float b,g,r;
}RgbPixelFloat;

typedef Image<RgbPixel> RgbImage;
typedef Image<RgbPixelFloat> RgbImageFloat;
typedef Image<unsigned char> BwImage;
typedef Image<float> BwImageFloat;
  


void displayImageProperty(IplImage* image){ 
   cout<<"-------Image Properties--------"<<endl;
   cout<<"Image width="<<image->width<<endl;
   cout<<"Image height="<<image->height<<endl;
   cout<<"Image depth="<<image->depth<<endl;
   cout<<"Image nSize="<<image->nSize<<endl;
   cout<<"Image nChannels="<<image->nChannels<<endl; 
   char* origin;
   char* dataOrder;
   if (image->origin==0){
       origin="Top-left";
   }else{
       origin="Below-left";//image->origin=1
   }
   cout<<"Image origin="<<origin<<endl;

   if (image->dataOrder==0){
       dataOrder="Order_Pixel(Interleaved)";
   }else{
       dataOrder="Order_Plane";//image->dataOrder=1
   }
   cout<<"Image dataOrder="<<dataOrder<<endl;
   cout<<"Image widthStep="<<image->widthStep<<" Bytes"<<endl;


}

//display an image in a new window with title to be given.
void displayImageNewWindow(char* title,CvArr* img){
   cvNamedWindow(title, CV_WINDOW_AUTOSIZE);
   cvShowImage(title,img); 
}

int getMaxMin(double value[],int valueSize, int maxmin)
{
   int pos=0;
   int i=0;
   double max1=-1;//å-999999;
   double min1=999999;
   
   if (maxmin==1){//find max
      for (i=0;i<valueSize;i++){//find the index with the max ncc; 
         if (value[i]>max1){
            pos=i;
            max1=value[i];
         }
      }
   }
   
   if (maxmin==0){//find min
      for (i=0;i<valueSize;i++){//find the index with the max ncc; 
         if (value[i]<min1){
            pos=i;
            min1=value[i];
         }
      }          
   }
      
   return pos;
}



IplImage* generateDisparityImage(IplImage* greyLeftImg32,
                         IplImage* greyRightImg32, 
                         int windowSize,int DSR){

    int offset=floor((double)windowSize/2); 
   int height=greyLeftImg32->height;
   int width=greyLeftImg32->width;
    double* localNCC=new double[DSR];

   int x=0, y=0,d=0,m=0;
   int N=windowSize;             

   IplImage* leftWinImg=cvCreateImage(cvSize(N,N),32,1);//mySubImage(greyLeftImg32,cvRect(0,0,N,N));
   IplImage* rightWinImg=cvCreateImage(cvSize(N,N),32,1);;//mySubImage(greyRightImg32,cvRect(0,0,N,N));
   IplImage* disparity=cvCreateImage(cvSize(width,height),8,1);//or IPL_DEPTH_8U
   BwImage imgA(disparity);
   
   for (y=0;y<height;y++){ 
      for (x=0;x<width;x++){
         imgA[y][x]=0;
      }
   }
   
     CvScalar s1;
   CvScalar s2;
   for (y=0;y<height-N;y++){ //height-N
      for (x=0;x<width-N;x++){//width-N
         //getWindow(i,j,leftim,wl,N);
         cvSetImageROI(greyLeftImg32, cvRect(x,y,N,N));
         s1=cvAvg(greyLeftImg32,NULL);
         cvSubS(greyLeftImg32,s1,leftWinImg,NULL);//zero-means
         cvNormalize(leftWinImg,leftWinImg,0,0,CV_L2,NULL);
         d=0;
         
          //initialise localNCC
          for (m=0;m<DSR;m++){localNCC[m]=0;}
          
          do{ 
            if (x-d>=0){ 
               cvSetImageROI(greyRightImg32, cvRect(x-d,y,N,N));
               s2=cvAvg(greyRightImg32,NULL);
               cvSubS(greyRightImg32,s2,rightWinImg,NULL);//zero-means 
               cvNormalize(rightWinImg,rightWinImg,0,0,CV_L2,NULL);
               
            }else{
               break;
            } 
            //localNCC[d]=NCC_val(leftWinImg,rightWinImg);
            localNCC[d]=cvDotProduct(leftWinImg,rightWinImg); 
            cvResetImageROI(greyRightImg32);

            d++;
         }while(d<=DSR);
         
         //to find the best d and store
          imgA[y+offset][x+offset]=getMaxMin(localNCC,DSR,1)*16; 
         cvResetImageROI(greyLeftImg32);
      }//x 
      if (y%10==0)cout<<"row="<<y<<endl;  
   }//y
   
   cvReleaseImage(&leftWinImg); 
   cvReleaseImage(&rightWinImg); 
        
   return disparity;

}



int main (int argc, char * const argv[]) {
    // insert code here...
    cout << "Stereo Normalized Cross Correlation"<<endl;  
   

   //**********image input*********************//
   //left_rds1.bmp,
   char* filename1="/Users/haiqiaohuang/Desktop/ProjectsOfEclipse/CppOpenCV/SampleImages/im2_tsu.bmp";//im2_cone.png
   IplImage* greyLeftImg= cvLoadImage(filename1,0); //0 means load image as grey image
   char* filename2="/Users/haiqiaohuang/Desktop/ProjectsOfEclipse/CppOpenCV/SampleImages/im6_tsu.bmp";
   IplImage* greyRightImg= cvLoadImage(filename2,0); 
   
    //----------image validity testing------------------ 
   if (greyLeftImg==NULL){
      cout << "No valid image input."<<endl; 
      //char c=getchar();
      return 1;
   }else{displayImageProperty(greyLeftImg);}
   if (greyRightImg==NULL){
      cout << "No valid image input."<<endl; 
      //char c=getchar();
      return 1;
   }

   int width=greyLeftImg->width;
   int height=greyLeftImg->height; 

   /****************8U to 32F**********************/
   IplImage* greyLeftImg32=cvCreateImage(cvSize(width,height),32,1);//IPL_DEPTH_32F 
   IplImage* greyRightImg32=cvCreateImage(cvSize(width,height),32,1); 
   cvConvertScale(greyLeftImg, greyLeftImg32, 1/255.);
   cvConvertScale(greyRightImg, greyRightImg32, 1/255.);//1/255. equals to 1/255.0 
    
   //-------------Computing disparity image---------------- 
   time_t tstart, tend; 
   tstart = time(0); //start timing
   int windowSize=13;//window size
   int DSR=20;//Disparity Search Range;
   IplImage* disparity32=generateDisparityImage(greyLeftImg32,greyRightImg32,windowSize,DSR); 
   tend = time(0);//end timing
   cout << "It took "<< difftime(tend, tstart) <<" second(s)."<< endl;


   //--------------display stereo results----------------------
   displayImageNewWindow("Dispairty Image",disparity32);
   displayImageNewWindow("Left Image",greyLeftImg32);
   displayImageNewWindow("Right Image",greyRightImg32);


   //********destroy window************/
   cvWaitKey(0);
   cvReleaseImage(&greyLeftImg32);
   cvReleaseImage(&greyRightImg32);
   cvReleaseImage(&greyLeftImg);
   cvReleaseImage(&greyRightImg); 
   cvReleaseImage(&disparity32);
   
   
   cvDestroyWindow("Left Image"); 
   cvDestroyWindow("Right Image"); 
   cvDestroyWindow("Dispairty Image");
        
    return 0;
}