視差マップを作る

おじさんの画像処理勉強シリーズも毎日こつこつひとつづつやってると、勉強することがだんだんなくなってきますね。

今日はアイサイトで使われている。視差マップに挑戦です。
二つのカメラで撮影した画像から物体の距離を求めることができます。

ほんとうはLiDARとかのレーダーレーダーを使って障害物を検出したいのですが、個人的にやるのは無理なので、簡単な複数のカメラを使った障害物検出にしました。

実際にアイサイトは特徴点にたいする視差マップを使っていますが、視差マップ自体は
特徴点がなくても作ることができます。

ということでいつものように勉強
http://ishidate.my.coocan.jp/opencv_20/opencv_20.htm

なるほど。StereoSGBM(ステレオセミグローバルブロックマッチング)ということをすればよいのね。

いつものようにコピペ流でコードを書きます。
といってもコアの処理の部分はたったの四行なので誰でも作れます。
オリジナルはRGBで処理していますが、移植をしやすいようにRGBAの32ビットで処理するように変更しました。

-----------------------------
#include <stdio.h>
#include <vector>

#define STB_IMAGE_IMPLEMENTATION
#define STB_IMAGE_WRITE_IMPLEMENTATION

#include "core.hpp"
#include "imgproc.hpp"
#include "tracking.hpp"
#include "calib3d.hpp"

#include "stb_image.h"
#include "stb_image_write.h"

#define X_DIV 10
#define Y_DIV 10



using namespace std;


void changeb_g(unsigned char* p,int x,int y,int c)
{
int ct = x*y;
int i;
unsigned char t;
for (i = 0; i < ct; i++){
t = p[0];
p[0] = p[2];
p[2] = t;
p[3] = 255;
p += c;
}
}

void changeToHueColor(cv::Mat src,cv::Mat& dst)
{
cv::Mat channels[3];
channels[0] = cv::Mat(src.size(), CV_8UC1);
channels[1] = cv::Mat(src.size(), CV_8UC1,255);
channels[2] = cv::Mat(src.size(), CV_8UC1,255);
cv::Mat hsv_image,tmp;
int d,i,j;
for (i = 0; i < src.cols;i++)
for (j = 0; j < src.rows; j++)
{
d = src.at<uchar>(j, i);
channels[0].at<uchar>(j, i) = (255 - d) / 2;
}
cv::merge(channels, 3, hsv_image);
cv::cvtColor(hsv_image, tmp, CV_HSV2BGR);
cv::cvtColor(tmp, dst, CV_BGR2BGRA);
}


int main()
{
printf("Hello Open CV!\n");
unsigned char *p,*q;
int x=-1, y=-1;
int xx = -1, yy = -1;
int n, nn;

//load image
//p = stbi_load("dave.jpg", &x, &y, &n, 4);
p = stbi_load("Tsukuba_L.png", &x, &y, &n, 4);
q = stbi_load("Tsukuba_R.png", &xx, &yy, &nn, 4);
if (p == NULL || q == NULL || x < 1 || y < 1 || x != xx || y != yy){
if (p)stbi_image_free(p);
if (q)stbi_image_free(q);
return 1;
}
// R<-->B
changeb_g(p, x, y, 4);
changeb_g(q, x, y, 4);

cv::Mat color = cv::Mat(y, x, CV_8UC4);
cv::Mat color2 = cv::Mat(y, x, CV_8UC4);

cv::Mat gray,gray2;

//copy data
memcpy(color.data, p, x * 4 * y);
stbi_image_free(p);
memcpy(color2.data, q, x * 4 * y);
stbi_image_free(q);

// conver to gray scale
cv::cvtColor(color, gray, CV_BGR2GRAY);
cv::cvtColor(color2, gray2, CV_BGR2GRAY);

cv::Mat disparity_data, disparity_map;
double min_, max_;

//cv::StereoBM sbm = cv::StereoBM(0, 16, 15);
//sbm.state->speckleWindowSize = 50;
//sbm.state->speckleRange = 1;
//sbm.operator()(gray, gray2, disparity_data);

cv::StereoSGBM ssgbm = cv::StereoSGBM(0, 16, 15);
ssgbm.speckleWindowSize = 200;
ssgbm.speckleRange = 1;
ssgbm.operator()(gray, gray2, disparity_data);


cv::minMaxLoc(disparity_data, &min_, &max_);
disparity_data.convertTo(disparity_map, CV_8UC1, 255 / (max_ - min_), -255 * min_ / (max_ - min_));
//cv::equalizeHist(disparity_map, disparity_map);
changeToHueColor(disparity_map, disparity_map);
changeb_g(disparity_map.data, x, y, 4);
stbi_write_png("result.png", x, y, 4, disparity_map.data, 4 * x);

return 0;
}
-----------------------------

というわけでこのふたつの画像の視差マップを作ってみました。

equalizeHist関数はGPUがないと動かないので、使わないように変更。
近い部分が赤色になるようです。

なんか正しくできたっぽい。
障害物検出も簡単。