四畳半テクノポリス

コロナのストレスで気が狂い、D進した院生

ガバガバなステレオマッチングとか

どうも、とり天です。研究室配属ですが、なんか温室とかやってる研究室でセンサネットワークを使ったようなIoTな事ができそうなので良かったなと思います。

 最近ですがアルバイトに行き詰っています、というのはOpenCVを使ったアルバイトをしているのですが、呼び出しても動かないステレオマッチングの関数メンバがあったので、ソースコードを読んでみたところ、なんと関数内が空っぽで"return 0"になっていました。結構重要な機能だったのでショックでしたね。それと同時に自力でステレオマッチングを作ってみたので紹介します。EmacsでインデントしたあとGeditで編集してめちゃくちゃなインデントになっていますが気にしない、あとステレオ対応のWEBカメラ(ZED)とかで読み込んで分割する様に書かれてますが、ステレオカメラ持っていない人は適当に左右の視差のある画像を読み込むようにしてね。

 

#include <iostream>
#include <string>
#include <sstream>
#include <iomanip>
#include <stdexcept>
#include <opencv2/core/utility.hpp>
#include <opencv2/core.hpp>
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

//#include <cstdlib> 

//dvide image
void ImgDiv(cv::Mat src, cv::Mat right, cv::Mat left){
  int width = src.cols/2;
  int hight = src.rows;
  for(int i = 0; i < hight; i++){
    cv::Vec3b *p_src   = src.ptr<cv::Vec3b>(i);
    cv::Vec3b *p_right = right.ptr<cv::Vec3b>(i);
    cv::Vec3b *p_left  = left.ptr<cv::Vec3b>(i);
    for(int j = 0; j < width; j++){
      p_left[j] = p_src[j];
      p_right[j] = p_src[j + width];
    }
  }
}

void Depth2Hsv(cv::Mat src, cv::Mat res){
  if(src.cols != res.cols || src.rows != res.rows){
    //cout << "data size is not mach" << endl;
    return ;
  }
  int width = src.cols;
  int hight = src.rows;
  for(int i = 0; i < hight; i++){
    cv::Vec3b *p_src = src.ptr<cv::Vec3b>(i);
    cv::Vec3b *p_res = res.ptr<cv::Vec3b>(i);
    for(int j = 0; j < width; j++){
         p_res[j][0] = p_src[j][0] / 2;
         p_res[j][1] = 255;
         p_res[j][2] = 220;

    }
  }
}
void my_sad(cv::Mat right, cv::Mat left, cv::Mat dist ,int block_size, int diff ){
  if(right.channels() != 1 || left.channels() != 1 ||  dist.channels() != 1){
    std::cout << "Error : bad channnel" << std::endl;
    return ;
  }
  if(right.cols != left.cols || right.rows != left.rows){
    std::cout << "Error : not mach input image size" << std::endl;
    return ;
  }

  if(block_size % 2 != 1  ){
    std::cout << "Error : block size is not  2n + 1 " << std::endl;
    return ;
  }

  dist = cv::Scalar(0,0,0);
  std::cout << "width" << right.cols << " height" << right.rows << std::endl;
  int width = right.cols - block_size;
  int hight = right.rows - block_size;
  cv::Mat tmp = dist.clone();

  std::vector<std::vector<int> > arr;
  arr.resize(hight);
  for( int i=0; i < hight; i++ ){
    arr[i].resize(width,255);
  }
  for(int d = 0; d < diff ; d+=1){
    for(int i = (block_size/2+ 1) ; i < hight - (block_size/2+ 1) ; i += 1){
      cv::Vec3b *p_dist   = dist.ptr<cv::Vec3b>(i);
      cv::Vec3b *p_right = right.ptr<cv::Vec3b>(i);
      cv::Vec3b *p_left  = left.ptr<cv::Vec3b>(i);

      for(int j = (block_size /2) + 1; j < width - (block_size/2+ 1) -diff ; j+= 1 ) {
	int right_sum = 0;
	int left_sum = 0;      
	for(int l = 0; l < block_size ; l++){
	  cv::Vec3b *p_right = right.ptr<cv::Vec3b>(i - (block_size/2) + l);
	  cv::Vec3b *p_left  = left.ptr<cv::Vec3b>(i - (block_size/2) + l);
	  for(int m = 0; m <   block_size; m++){
	    right_sum += p_right[j/3][ - (block_size/2) + m + j % 3];
	    left_sum += p_left[j/3][ - (block_size/2) + m + d + j % 3];
	  } 
	}
	int delta =  ( (double)abs(right_sum - left_sum) / (double)(block_size * block_size *255)) * 255.0 ;

	if(delta < arr[i][j]) {
	  arr[i][j] = delta;
	  cv::Vec3b *p_dist = dist.ptr<cv::Vec3b>(i);
	  p_dist[j/3][ j % 3] =  (int)(( (double)d / (double)diff) * 255.0);
	}
      }
    }
  }
}

int main (void){
  cv::Mat src;
  cv::VideoCapture cap(0);
  int block = 7;
  int diff = 12;

  while(1){
    cap >> src;
    cv::resize(src, src, cv::Size(), 0.5, 0.5);
    cv::Mat right(src.rows, src.cols*0.5,src.type());
    cv::Mat left(src.rows, src.cols*0.5,src.type());
    ImgDiv(src,right,left);
    cv::cvtColor(right, right, CV_RGB2GRAY);
    cv::cvtColor(left, left, CV_RGB2GRAY);
    cv::Mat dist = right.clone();
     my_sad( right, left, dist , block, diff );
     cv::cvtColor(dist, dist, CV_GRAY2RGB);
     cv::Mat color(dist.rows, dist.cols, CV_8UC3);
     Depth2Hsv(dist, color);
     cv::cvtColor(color, color, CV_HSV2RGB);

     cv::imshow("window_r", right);
     cv::imshow("window_l", left);
     cv::imshow("window_d", color);
     
     cv::createTrackbar("block size","window_d",&block, 13,NULL,NULL);
     block = (block < 3  )? block = 3 : (block %2 != 1)? block - 1 : block;
     cv::createTrackbar("diffarence","window_d",&diff, 255,NULL,NULL); 
    int key = cv::waitKey(1);
    if(key == 113)//q
      {
	break;//
      }
    else if(key == 115)//s
      {
	cv::imwrite("img.png",dist );
      }
  }
}

まあアルゴリズムとしてはSADを用いたブロックマッチングを行っています。すごく適当な上に並列化もなにもしていないので、すごく重いです。

結果は次の画像に示す感じです。近いものほど赤く表示して、遠くのものほど青く表示しています。なんかキャリブレーションすら面倒くさくてしていませんがそれなりに遠くのものは遠くに、近くのものは近くと認識できているんじゃないかなぁ。部屋汚いね

左の画像

f:id:toriten1024:20170128205303p:plain

右の画像f:id:toriten1024:20170128205241p:plain

深度マップ

f:id:toriten1024:20170128205444p:plain

 

まあ詳しい話とかは今度の技術書展で豊橋技科大コンピュータクラブの出す部誌に載せようと思うので、そのあとブログにも簡単に載せようかな