双目测距步骤三：彩色深度图获取

在原来的视差图基础上改进的，去掉黑白色的和伪彩色的，不为什么，彩虹图好看，任性。

之前测试过几次，感觉还是SGBM的算法在动态摄像头视差图匹配效果最好。
然后想办法加上摄像机内外畸变参数，还有匹配算法。贴个效果图：

可以看到匹配效果有点粗糙，是因为没加上标定后摄像头的畸变参数，加上去效果会好很多。

#include <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <iostream>
​
using namespace std;
using namespace cv;
​
int main(int argc, char** argv[])
{
     cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
     cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
​
     CvCapture* capture1;
     CvCapture* capture2;
         if (argc==1)
         {
             capture1 = cvCreateCameraCapture( 1 );
             capture2 = cvCreateCameraCapture( 0 );
         }
​
     IplImage* img1;
     IplImage* img2;
​
     while(1)
     {
         img1 = cvQueryFrame( capture1 );
         img2 = cvQueryFrame( capture2 );
         if( !(img1&&img2) )
              break;
          cvShowImage( "left", img1 );
          cvShowImage( "right", img2 );
​
    cv::StereoSGBM sgbm;
    int SADWindowSize = 5;
    sgbm.preFilterCap = 63;
    sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
​
    int cn = img1->nChannels;
    int numberOfDisparities=128;
​
    sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
    sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
    sgbm.minDisparity = 0;
    sgbm.numberOfDisparities = numberOfDisparities;
    sgbm.uniquenessRatio = 10;
    sgbm.speckleWindowSize = 100;
    sgbm.speckleRange = 10;
    sgbm.disp12MaxDiff = 1;
​
    Mat disp, img;
    int64 t = getTickCount();
​
    sgbm((Mat)img1, (Mat)img2, disp);
    disp.convertTo(img, CV_8U, 255/(numberOfDisparities*16.));
​
     Mat img_color(img.rows, img.cols, CV_8UC3);//构造RGB图像
     #define IMG_B(img,y,x) img.at<Vec3b>(y,x)[0]
     #define IMG_G(img,y,x) img.at<Vec3b>(y,x)[1]
     #define IMG_R(img,y,x) img.at<Vec3b>(y,x)[2]
     uchar tmp2=0;
     for (int y=0;y<img.rows;y++)//转为彩虹图的具体算法，主要思路是把灰度图对应的0～255的数值分别转换成彩虹色：红、橙、黄、绿、青、蓝。
      {
             for (int x=0;x<img.cols;x++)
             {
                    tmp2 = img.at<uchar>(y,x);
                    if (tmp2 <= 51)
                    {
                           IMG_B(img_color,y,x) = 255;
                           IMG_G(img_color,y,x) = tmp2*5;
                           IMG_R(img_color,y,x) = 0;
                    }
                    else if (tmp2 <= 102)
                    {
                           tmp2-=51;
                           IMG_B(img_color,y,x) = 255-tmp2*5;
                           IMG_G(img_color,y,x) = 255;
                           IMG_R(img_color,y,x) = 0;
                     }
                     else if (tmp2 <= 153)
                     {
                            tmp2-=102;
                            IMG_B(img_color,y,x) = 0;
                            IMG_G(img_color,y,x) = 255;
                            IMG_R(img_color,y,x) = tmp2*5;
                     }
                    else if (tmp2 <= 204)
                     {
                             tmp2-=153;
                             IMG_B(img_color,y,x) = 0;
                             IMG_G(img_color,y,x) = 255-uchar(128.0*tmp2/51.0+0.5);
                             IMG_R(img_color,y,x) = 255;
                     }
                     else
                      {
                             tmp2-=204;
                             IMG_B(img_color,y,x) = 0;
                             IMG_G(img_color,y,x) = 127-uchar(127.0*tmp2/51.0+0.5);
                             IMG_R(img_color,y,x) = 255;
                       }
                }
          }
     namedWindow("img_ rainbowcolor");
     imshow("img_ rainbowcolor", img_color);
​
    waitKey(30);
    }
​
    cvReleaseCapture( &capture1 );
    cvReleaseCapture( &capture2 );
    cvDestroyWindow( "left" );
    cvDestroyWindow( "right" );
    cvDestroyWindow( "disparity" );
}