【OpenCV】ステレオカメラで物体の距離推定をしてみた【C++】

スポンサーリンク
programmingC++
スポンサーリンク

はじめに

たまたま研究室にwebカメラが2台あったので物体の距離推定をしてみました。

実行環境は次の通りです。

  • OS:macOS Catalina(ver10.15.1)
  • OpenCV:ver4.1.2

プログラムの手順としては次の通りです。

  • 映像の取り込み
  • 対象のフィルタリング
  • 重心位置取得(重心位置で距離推定を行う)
  • 三角測量に基づく計算(参考

ソースコード

グローバルに定義してある変数については次の通りです。

  • float T:カメラレンズ間の距離(実測:cm単位)
  • float F:カメラの焦点距離(Zを用いて推定値を定める)
  • float Z:物体との距離(実測:cm単位)、Fの推定値を求める際に利用

先に物体とカメラとの距離をグローバル変数のZに代入して43行目と46行目のコメントアウトを消してfを出力します(逆にzについてはコメントアウトする)。

出力されたfをグローバル変数Fとして代入するとおおよそ正確な測定ができます。

#include <iostream>
#include <opencv2/opencv.hpp>

float T = 16; //カメラ間距離
float F = 1285; //焦点距離
//float Z = 50; //物体距離

int main(){
	
	cv::VideoCapture video1(1); //動画の読み込み
	cv::VideoCapture video2(2); //動画の読み込み
	
	cv::Mat frame1,frame2; //フレーム格納用
	
	while(1){
		//1フレーム読み込み
		video1.read(frame1);
		video2.read(frame2);
		
		cv::Mat mask1, mask2; //mask格納用
		
		//フィルタリング,mask画像生成
		inRange(frame1, cv::Scalar(0, 0, 100), cv::Scalar(50, 50, 255), mask1);
		inRange(frame2, cv::Scalar(0, 0, 100), cv::Scalar(50, 50, 255), mask2);
				
		//画像1重心探索
		cv::Moments mom1 = cv::moments(mask1, 1);
		cv::Point2f pt1 = cv::Point2f(mom1.m10/mom1.m00, mom1.m01/mom1.m00);
		cv::circle(mask1, pt1, 10, cv::Scalar(100), 3, 4);
		
		//画像2重心探索
		cv::Moments mom2 = cv::moments(mask2, 1);
		cv::Point2f pt2 = cv::Point2f(mom2.m10/mom2.m00, mom2.m01/mom2.m00);
		cv::circle(mask2, pt2, 10, cv::Scalar(100), 3, 4);
		
		//画像表示
		cv::imshow("frame1", frame1);
		cv::imshow("frame2", frame2);
		cv::imshow("mask1", mask1);
		cv::imshow("mask2", mask2);
		
		//計算
		float D = pt1.x - pt2.x; //ピクセル誤差
		//float f = Z*D/T; //焦点距離
		float z = F*T/D; //推定距離
		
		//std::cout << f << std::endl; //標準出力
		std::cout << z << std::endl; //標準出力
		
		//キー入力
		int key = cv::waitKey(10);
		
		if(key == 'q'){
			
			cv::destroyAllWindows();
			break;
		}
	}
	
	return 0;
}

OpenCV関数についての簡単な説明

過去のOpenCVについての記事で触れていない関数について簡単に説明します。

cv::inRange

入力配列の要素が指定した範囲内に含まれているか確認し、含まれていれば1、含まれていなければ0を返します。

次のようにして使います。

inRange(入力配列, 最小値の配列, 最大値の配列, 出力配列);

ここでは各画素においてBGR値が(0,0,100)〜(50,50,255)であるかを判別している。これによって赤色の要素を抜き出しています。

cv::moments

モーメントを求める関数。

次のようにして使います。

cv::moments(配列, パラメータ);

ここでパラメータは画像の際にのみ有効な真偽値で、真の場合に0でないピクセルが全て1として扱われます。

重心についてはこちらにあるようにして求められます。

cv::circle

円を描く関数。

次のようにして使います。

cv::circle(描画する画像, 円の中心座標, 半径, 色, 枠線の太さ, 枠線の種類);

実行してみた

実画像の一部を少し隠しています。

マスク画像には重心に丸が描かれています。

実測32.5cmの時

画像については次の通りに出力されました。

距離推定結果は「32.4118」とかなり正確な数値が出ました。

実測18cmの時

画像については次の通りに出力されました。

距離推定結果は「18.1881」とこちらもかなり正確な数値を出ました。

その他の距離(机の都合上10cm〜80cm)でも測定しましたが、誤差は大きくて1.5cm程度に収まりました。

最後に

ラジコンカーに取り付けて自動走行とかやってみても面白そうですね。

今回は簡単な実装になりましたが、参考になれば幸いです。

コメント

タイトルとURLをコピーしました