C++による魚眼レンズを用いた平面展開プログラムの作成

Question

前提・実現したいこと

　現在、大学で光学系の授業を受けており授業の中で扱った題材を自分で実際に確かめてみたいと思いプログラミングに挑戦しています。今回、作成したいプログラムが「魚眼カメラで撮影した画像を通常のカメラで撮影したように処理(平面展開処理)をするプログラム」となっています。

実装の流れは、魚眼カメラで撮影した画像(src)、通常のカメラで撮影したようにsrcを処理した画像(dst)とすると
１、srcのピクセル座標がdstのどのピクセルに対応するかを計算で求めて、unordered_map<int,Point2d> A_dash_Mapに保存。この時、mapのキーはdstの座標(x,y)をy*画像の横幅+xのように通し番号?として表現した値。キーに対応するPoint2dはsrcのピクセル座標。

2、srcのピクセル値をA_dash_mapを参照しながらdstへとコピーする。

この流れてプログラム自体は完成したのですが無駄が多いようで処理時間が大幅にかかってしまいます。平面展開をする関数のループが多いことと、ループの中身自体でもA_dash_Mapの検索を行っているため処理速度が遅いのかと思いましたが改善方法が思いつきません。高速化できる部分があればアドバイスをいただきたいです。

初心者のため読みにくいソースコードだと思います。
申し訳ありません。

該当のソースコード

#include<iostream>
#include<opencv2/opencv.hpp>
#include<unordered_map>
#include<string>
#define _USE_MATH_DEFINES
#include<math.h>

#define ANGLE 100
#define FISHEYE_ANGLE 180
#define CAMERA_ID 1
#define CAMERAWIDTH 2448
#define CAMERAHEIGHT 2048

using namespace std;
using namespace cv;

//
//	平面展開の対応点を取得する関数
//
//この部分は、授業で習った数式で愚直に計算しました。
unordered_map<int,Point2d>getFisheyePoint()
{
	//焦点距離[pixel]
	double pixel_size = 3.45;
	double focus_mm = 1.8;
	double focus = focus_mm * 1000 / pixel_size;

	//画像中心までの距離を計算
	double angle = ANGLE * 2 * M_PI / 360;
	double d = CAMERAWIDTH / (2 * tan(angle / 2));

	//画像中心と任意の平面座標Aの距離r
	double r = 0;

	//点Aの画角を計算
	double c = 0;

	//点Aの回転角phi
	double phi = 0;

	//点Aに対応する魚眼座標A'
	Point2d A_dash;

	//A'を格納するA_dash_Map
	unordered_map<int, Point2d> A_dash_Map;
	//魚眼座標A'を計算してA_dash_Mapに保存
	for (int j = 0;j < CAMERAHEIGHT;j++)
	{
		for (int i = 0;i < CAMERAWIDTH;i++)
		{
			r = sqrt(pow((i - CAMERAWIDTH / 2), 2) + pow((j - CAMERAHEIGHT / 2), 2));
			if (r == 0)continue;
			c = atan(r / d);
			phi = acos((i - (CAMERAWIDTH / 2)) / r);
			A_dash.x = round(focus * c * cos(phi));
			if (j <= CAMERAHEIGHT / 2)
			{
				A_dash.y = round(focus * c * sin(phi));
			}
			else
			{
				A_dash.y = -round(focus * c * sin(phi));
			}
			A_dash_Map.insert(make_pair(j * CAMERAWIDTH + i, A_dash));
		}
	}
	return A_dash_Map;
}

//
//  カメラ撮影を行う関数
//
Mat getCameraImage()
{
	VideoCapture cap(CAMERA_ID);
	Mat src, gray;
	cap >> src;
	cvtColor(src, gray, COLOR_BGR2GRAY);
	return gray;
}

//
//  平面展開をする関数
//
Mat openFisheye(Mat src, Mat dst, unordered_map<int, Point2d> A_dash_Map)
{
	for (int index = 0;index < CAMERAHEIGHT * CAMERAWIDTH;index++)
	{
　　　　　　　　　//対応点を取得する際に座標系がずれているのでここで調節しています。
		dst.data[index] = src.at<uchar>((CAMERAHEIGHT / 2) - A_dash_Map[index].y, A_dash_Map[index].x + (CAMERAWIDTH / 2));
	}
	return dst;
}

//
//	main関数	
//
int main()
{
	cout << "計算開始\n";
	unordered_map<int, Point2d > fisheyepoint = getFisheyePoint();
	Mat src;
        Mat open;
	cout << "撮影開始\n";
	src = getCameraImage();
	Mat dst = Mat::zeros(Size(CAMERAWIDTH, CAMERAHEIGHT), CV_8UC1);
	open = openFisheye(src, dst, fisheyepoint);
	imwrite("open.png", open);
	cout << "展開終了\n";
	
	return 0;
}

補足情報（FW/ツールのバージョンなど）

OS:bootcampにてMacbookair上でWindous10を動かしています
visual studio2019
CPU:第６世代core-i5
メモリ:8GB

Comments

[A_kirisaki]

unordered_map は遅そう。まだ Vector 使ったほうが速い？メモリ管理する根性があるなら多分それが一番早い。もしくは行列計算の Eigen 使うとか。いずれにしろわたしの C++ の知識が古いので識者の意見を待ちましょう。

[jbpb0]

ご質問の処理速度に関することではないのですが、srcの画素数とdstの画素数を一致させると、srcは画像の4隅あたりに無効な(被写体が写ってない)画素があるため、srcの有効な(被写体が写っている)画素数よりも、dstの画素数(処理後の画像全体)の方が多くなり、単純に対応する画素位置関係をマッピングさせて移動させるだけだと、dstに(対応するsrcの画素が無くて)値を持たない画素が発生してしまうと思うのですが、それはどのようにして回避しているのでしょうか？
dstの画素数をsrcのよりも少なくするとか、dstを補間するとかしているのでしょうか？
もし差し支えなければ、教えてください
(めんどくさければスルーしてください)

【追記】「mapのキーはdstの座標(x,y)をy*画像の横幅+xのように通し番号?として表現した値。キーに対応するPoint2dはdstのピクセル座標。」は、どちらかがsrcですよね？？
キーがsrc座標だと、srcの各画素はdstの一つの画素にしか対応しないので、上記の問題が発生するかもしれません(srcとdstの画素数の関係にもよりますが)
キーがdst座標なら、srcの各画素がdstの複数の画素に対応できるので、上記の問題は発生しません(全dst座標のA_dash_Mapを計算すれば)

[kpkq307]

>>A_kirisakさん
コメントありがとうございます。
vectorで管理するほうが早いのですね、少し挑戦してみようと思います。

[kpkq307]

>>jbpb0さん
ご指摘ありがとうございました。
追記でご指摘いただいた通りキーの部分はdst座標です。

[jbpb0]

とりあえず、main()の各行でかかっている時間を分けて測ることをお勧めします
(変数宣言や文字表示の行は、測らなくていいけど)

[jbpb0]

これも処理速度の話では無いですが、画像の中心の座標は
CAMERAWIDTH / 2
CAMERAHEIGHT / 2
ではありません

画素数が4x4の画像を想像してください
その場合の画像の中心は、画素と画素の間ですから、その位置は 4/2=2 ではありません
C言語やPythonの配列のルールに合わせて画素の並びを0から数えるとすれば、画像中心の座標は (4-1)/2=1.5 です

上記は画素数が 偶数x偶数 で説明してますが、奇数の場合も計算式は同じです

画素数が 数千x数千 とかの画像では、処理後の画像を見比べても違いは分からないでしょうけど、一応書いときます

[fana]

（実用上はキャリブレーションで得た光学中心を使うところをとりあえず簡便に適当な値を与えているだけなのだろうから，「画像の中心」とかいう無意味な座標が欲しいわけではないはず．）

Answer 1

普通に cv::remap() あたりを使うことを考えてはどうでしょうか．

Comments

[kpkq307]

fanaさん
回答頂きありがとうございます。
remapを使うことでかなり高速になりました。
そのような関数があることを知らなかったのでとても勉強になりました。
ありがとうございます。