cv::triangulatePointsがnanを返す

C++で，二つの視点の動画から背景差分法を使って移動物体の位置を三角測量で求めるプログラムを作っています．以下がそのコードです．triangulatePointsがnanを返すのですが，その理由が全くわかりません．どなたかアドバイスいただけませんでしょうか？

C++
1   #include <windows.h>
2#include <stdio.h>
3#include <stdlib.h>
4#include <opencv2/opencv.hpp>
5#include <iostream>
6#include <sstream>
7#include <vector>
8#include <fstream>
9#include <opencv2/imgproc.hpp>
10
11int main(void)
12{
13	//left
14	cv::Mat leftrvec = (cv::Mat_<double>(3, 3) <<
15		0.6028973161363299, -0.7779353310756564, -0.177006911898431,
16		-0.2253317158862821, -0.3788660989087959, 0.8976001876745426,
17		-0.7653368173630182, -0.5012754729306425, -0.4037108572069951
18		);
19	cv::Mat lefttvec = (cv::Mat_<double>(1, 3) <<
20		2.399365804970636,
21		-1.682382960652676,
22		24.21846020749925
23		);
24	cv::Mat leftoutpala = (cv::Mat_<float>(4, 4) <<
25		0.6028973161363299, -0.7779353310756564, -0.177006911898431, 2.399365804970636,
26		-0.2253317158862821, -0.3788660989087959, 0.8976001876745426, -1.682382960652676,
27		-0.7653368173630182, -0.5012754729306425, -0.4037108572069951, 24.21846020749925,
28		0, 0, 0, 1
29		);
30
31
32	//right
33	cv::Mat rightrvec = (cv::Mat_<double>(3, 3) <<
34		0.1253642194821509, 0.9858568306229998, -0.1112210501101525,
35		-0.1112210501191595, 0.1253642194881689, 0.9858568306212183,
36		0.9858568306219837, -0.1112210501033692, 0.1253642194961598
37		);
38	cv::Mat righttvec = (cv::Mat_<double>(1, 3) <<
39		-2.609984722540527e+66,
40		-5.509832469311671e+66,
41		-7.312245215731312e+66
42		);
43	cv::Mat rightoutpala = (cv::Mat_<float>(4, 4) <<
44		0.1253642194821509, 0.9858568306229998, -0.1112210501101525, -2.609984722540527e+66,
45		-0.1112210501191595, 0.1253642194881689, 0.9858568306212183, -5.509832469311671e+66,
46		0.9858568306219837, -0.1112210501033692, 0.1253642194961598, -7.312245215731312e+66,
47		0, 0, 0, 1
48		);
49
50
51
52	//camL
53	cv::Mat cameraMatrixl = (cv::Mat_<float>(3, 4) << 817.58493536953847, 0, 332.11150404462359, 0, 819.33875610640519, 230.87135563335934, 0, 0, 1, 0, 1, 0);
54	//camR
55	cv::Mat cameraMatrixr = (cv::Mat_<float>(3, 4) << 817.58493536953847, 0, 332.11150404462359, 0, 819.33875610640519, 230.87135563335934, 0, 0, 1, 0, 1, 0);
56																																											  
57
58	cv::Mat leftpala, rightpala;
59
60	leftpala = cameraMatrixl * leftoutpala;//射影行列
61	rightpala = cameraMatrixr * rightoutpala;
62
63	///変数宣言
64	cv::Mat frameL, img_diffL, img_backL, img_srcL;	// 画像変数宣言
65	cv::Mat frameR, img_diffR, img_backR, img_srcR;	// 画像変数宣言
66	//CvMat *mat_src, *mat_diff, *mat_back;	// 行列宣言
67	cv::Rect rectL, rectR;
68	cv::Mat resultdot;
69	cv::Mat leftdot = (cv::Mat_<float>(2, 1) << 0, 0);
70	cv::Mat rightdot = (cv::Mat_<float>(2, 1) << 0, 0);
71	float check;
72
73
74	cv::VideoCapture cpa1;
75	cpa1.open("videoL1228.mp4v");
76	if (cpa1.isOpened() == false) {
77		printf("ファイルが開けません\n");
78		return -1;
79	}
80	cv::VideoCapture cpa2;
81	cpa2.open("videoR1228.mp4v");
82	if (cpa2.isOpened() == false) {
83		printf("ファイルが開けません\n");
84		return -1;
85	}
86
87	//背景保存
88	cpa1 >> frameL;
89	cpa2 >> frameR;
90	cv::cvtColor(frameL, img_backL, cv::COLOR_BGR2GRAY);//グレースケール化
91	cv::cvtColor(frameR, img_backR, cv::COLOR_BGR2GRAY);
92
93
94
95	//座標保存のやつ
96
97	using namespace std;
98	cv::FileStorage fs1("test16ae.xml", cv::FileStorage::WRITE);
99	cv::FileStorage fs2("test16be.xml", cv::FileStorage::WRITE);
100	cv::FileStorage fs3("test16ce.xml", cv::FileStorage::WRITE);
101	int formatCount = 1000000;
102	fs1 << "formatCount" << formatCount;
103	fs2 << "formatCount" << formatCount;
104	fs3 << "formatCount" << formatCount;
105
106	float t = 0;
107	cv::FileStorage fs5("tushin.xml", cv::FileStorage::WRITE);
108	fs5 << "formatCount" << formatCount;
109
110
111	int count = 0;
112	BYTE x = 0;
113	std::ofstream ofs("data.csv");
114	ofs << "wifi" << "," << "X" << "," << "Y" << "," << "Z" << "," << "x1" << "," << "y1" << "," << "x2" << "," << "y2" << std::endl;
115	/////////////////////////////////////////////////////
116	while (1) {
117
118		cpa1 >> frameL;
119		cpa2 >> frameR;
120
121		count += 1;
122		if (count > 9) { //9
123			count = 0;
124			x += 1;
125		}
126
127		std::cout << "No." << (int)x << std::endl;
128
129		//if (x < 90) continue;
130		//if (x > 120) break;
131		if (x > 250) x = 0;
132
133
134
135
136		cv::cvtColor(frameL, img_srcL, cv::COLOR_BGR2GRAY);
137		cv::cvtColor(frameR, img_srcR, cv::COLOR_BGR2GRAY);
138		cv::absdiff(img_srcL, img_backL, img_diffL);		// 差分の計算
139		cv::absdiff(img_srcR, img_backR, img_diffR);		// 差分の計算
140		cv::threshold(img_diffL, img_diffL, 50, 255, cv::THRESH_BINARY); //画像の二値化
141		cv::threshold(img_diffR, img_diffR, 50, 255, cv::THRESH_BINARY); //画像の二値化
142		//cv::morphologyEx(img_diffA, img_diffA, cv::MORPH_OPEN, 1);
143		//cv::morphologyEx(img_diffA, img_diffA, cv::MORPH_CLOSE, 1);
144		//cv::morphologyEx(img_diffB, img_diffB, cv::MORPH_OPEN, 1);
145		//cv::morphologyEx(img_diffB, img_diffB, cv::MORPH_CLOSE, 1);
146		//cv::morphologyEx(img_diffL, img_diffL, cv::MORPH_CLOSE, 4);
147		//cv::morphologyEx(img_diffB, img_diffB, cv::MORPH_OPEN, 1);
148		cv::medianBlur(img_diffL, img_diffL, 7);
149		cv::medianBlur(img_diffR, img_diffR, 7);
150		rectL = cv::boundingRect(img_diffL);//直立バウンディングボックスを計算
151		rectR = cv::boundingRect(img_diffR);
152		//バウンディングボックスの描画
153		cv::rectangle(img_srcL, cv::Point(rectL.x, rectL.y), cv::Point(rectL.x + rectL.width, rectL.y + rectR.height), CV_RGB(0, 0, 250), 10);
154		cv::rectangle(img_srcR, cv::Point(rectR.x, rectR.y), cv::Point(rectR.x + rectR.width, rectR.y + rectL.height), CV_RGB(0, 0, 250), 10);
155
156		//画像上の移動物体重心の座標
157		int x1 = rectL.x + 0.5 * rectL.width;//-382.5;//画像上のx1
158		int x2 = rectR.x + 0.5 * rectR.width;//-382.5;//画像上のx2
159		int y1 = rectL.y + 0.5 * rectL.height;//-229.1;
160		int y2 = rectR.y + 0.5 * rectR.height;//-229.1;
161
162		leftdot.at<float>(0, 0) = x1;
163		leftdot.at<float>(1, 0) = y1;
164		rightdot.at<float>(0, 0) = x2;
165		rightdot.at<float>(1, 0) = y2;
166
167		cv::triangulatePoints(//三角測量を行う
168			leftpala,//左カメラの射影行列(3x4)
169			rightpala,//右カメラの射影行列(3x4)
170			leftdot,//1枚目の画像中の特徴点の配列(2x1)
171			rightdot,//2枚目の画像中の特徴点の配列(2x1)
172			resultdot//同次座標における再構成後の点の配列(4×1)
173		);
174
175		check = resultdot.at<float>(0, 0);
176		printf("check=%lf\n", check);
177
178		//座標取得
179		float X = resultdot.at<float>(0, 0) / resultdot.at<float>(3, 0);
180		float Y = resultdot.at<float>(1, 0) / resultdot.at<float>(3, 0);
181		float Z = resultdot.at<float>(2, 0) / resultdot.at<float>(3, 0);
182
183		//座標書き出し
184		fs1 << "X" << X;
185		fs2 << "Y" << Y;
186		fs3 << "Z" << Z;
187
188		short xs = (short)(X * 100); unsigned char xl = (unsigned char)xs; unsigned char xh = (unsigned char)(xs >> 8);
189		short ys = (short)(Y * 100); unsigned char yl = (unsigned char)ys; unsigned char yh = (unsigned char)(ys >> 8);
190		short zs = (short)(Z * 100); unsigned char zl = (unsigned char)zs; unsigned char zh = (unsigned char)(zs >> 8);
191
192		//BYTE head = 255;
193
194
195		//count += 1;
196		//if (count > 9) { //9
197		//	count = 0;
198		//	x += 1;
199		//}
200		//if (x > 250) {
201		//	x = 0;
202		//}
203
204		unsigned char ucx = (unsigned char)x;
205		printf("%d\n", ucx);
206		fs5 << "x" << ucx;
207		cv::imshow("カメラL", img_srcL);	// 1フレーム表示
208		cv::imshow("カメラR", img_srcR);
209
210		//////csv出力
211		ofs << (int)x << "," << X << "," << Y << "," << Z << "," << x1 << "," << y1 << "," << x2 << "," << y2 << std::endl;
212
213
214		int key = cv::waitKey(100);
215		//qボタンが押されたとき
216		//whileループから抜ける．
217		if (key == 113) break;
218	}
219
220}

fana

2021/01/07 02:51

nanが「無限遠」を示す出力だということはないのでしょうか？

roritamago

2021/01/07 07:04

恥ずかしながらtriangulatePoints()の挙動を把握していないのですが無限の値が出たりすることがあるでんしょうか？

fana

2021/01/07 07:17 編集

その関数の挙動はわかりませんが，三角測量自体は入力次第で失敗し得ますよね．例えば，奥行を推定できる条件というのを「視差>0」とすれば，そうでない状況（視差≦0）な入力がされたら関数側としては困るわけで．（で，困ったとき，果たしてどんな値を出力する仕様なのか？という）

roritamago

2021/01/07 07:33

例の話に関しての質問で申し訳ないのですが，視差<0ってどういう状況で起こりえるんでしょうか？イマイチ想像がつかなくて・・・・ステレオカメラで左右逆になってしまったりしたときでしょうか？

fana

2021/01/07 07:48 編集

triangulatePoints() にいろいろと引数でデータ値を渡すのですから，極端な話，間違った値を入力してやった場合には，それを用いた計算結果というのはそのような「ありえん」値になり得ますよね．（関数triangulatePoints側の立場から見たら，まともに計算できるデータが入力されてくるとは限らない）また，奥行が大きいほど視差は小さくなるわけですが，解像度が有限な撮影画像から観測データを得ている以上は，ステレオカメラからある距離以上離れた対象の視差は画像上で観測できなくなってしまう（視差=0になる）ことになるでしょう．（そのことを「無限遠」と述べた）

fana

2021/01/07 07:55 編集

以上より，まず確認すべきことは (1)引数値の正当性 (2)奥行計算できない場合の関数からの出力値がどうなるか？という仕様となります．（しかし後者側は何故かリファレンスにも書かれてないっぽい… 関数名と「nan」でググると，nanやinfが返ってくる的な話がありそうに見えますが）