コーナー検出を行った後にその特徴点を用いてマッチングを行いたい

前提・実現したいこと

ステレオ画像においてコーナー検出を行い、コーナー検出で得た特徴点同士を特徴点マッチングを行ってマッチングさせたい。コーナー検出にはFAST検出を用いている。また、間で特徴点厳選のため分散値による抽出を行っている。

発生している問題・エラーメッセージ

C++
1//コーナー検出
2	int threshold = 100;
3	bool nonmax = true;
4	vector<KeyPoint> src_key;
5	vector<KeyPoint> dst_key;
6	FAST(src_gry, src_key, threshold, nonmax);
7	vector<KeyPoint>::iterator src_it_kp = src_key.begin();
8	double s_max = -1.0;
9	int xs_max = 0;
10	int ys_max = 0;
11	int p = 1;
12	int xsb[1000];
13	int	ysb[1000];
14	double vsb[1000];
15	for (; src_it_kp != src_key.end(); ++src_it_kp) {
16		double s_s = 0.0;
17		double s2 = 0.0;
18		circle(src_fast, Point(src_it_kp->pt.x, src_it_kp->pt.y), 1, Scalar(0, 255, 0), -1);
19		circle(src_fast, Point(src_it_kp->pt.x, src_it_kp->pt.y), 8, Scalar(0, 255, 0));
20		xs = src_it_kp->pt.x;
21		ys = src_it_kp->pt.y;
22		int s[9] = { MI(src_gry, xs - 1, ys - 1), MI(src_gry, xs, ys - 1), MI(src_gry, xs + 1, ys - 1), MI(src_gry, xs - 1, ys), MI(src_gry, xs, ys), MI(src_gry, xs + 1, ys), MI(src_gry, xs - 1, ys + 1), MI(src_gry, xs, ys + 1), MI(src_gry, xs + 1, ys + 1) };
23		for (int n = 0; n <= 9; n++) {
24			s_s += s[n];
25			s2 += (s[n] * s[n]);
26		}
27		double s_a = 0.0;
28		s_a = s_s / 9.0;
29		double s_v = 0.0;
30		s_v = s2 / 9.0 - s_a * s_a;
31		if (s_max < s_v) {
32			s_max = s_v;
33			xs_max = xs;
34			ys_max = ys;
35		}
36		xsb[p] = xs;
37		ysb[p] = ys;
38		vsb[p] = s_v;
39		p = p + 1;
40	}
41	int tmp1,tmp2;
42	double tmp3;
43	for (int ps = 1; ps < p; ps++) {
44		for (int pt = ps + 1; pt < p + 1; pt++) {
45			if (vsb[ps] < vsb[pt]) {
46				tmp1 = xsb[ps];
47				xsb[ps] = xsb[pt];
48				xsb[pt] = tmp1;
49				tmp2 = ysb[ps];
50				ysb[ps] = ysb[pt];
51				ysb[pt] = tmp2;
52				tmp3 = vsb[ps];
53				vsb[ps] = vsb[pt];
54				vsb[pt] = tmp3;
55			}
56		}
57	}
58	for (int s = 1; s <= 10; s++) {
59		circle(src_fast, Point(xsb[s], ysb[s]), 5, Scalar(0, 0, 255), -1);
60	}
61	for (int pm = 1; pm < p + 1; pm++) {
62		printf("格納座標：%d回目, x座標：%d, 分散値：%f \n", pm,xsb[pm],vsb[pm]);
63	}
64FAST(dst_gry, dst_key, threshold, nonmax);
65	vector<KeyPoint>::iterator dst_it_kp = dst_key.begin();
66	double d_max = -1.0;
67	int xd_max = 0;
68	int yd_max = 0;
69	int o = 1;
70	int xdb[1000];
71	int	ydb[1000];
72	double vdb[1000];
73	for (; dst_it_kp != dst_key.end(); ++dst_it_kp) {
74		double d_d = 0.0;
75		double d2 = 0.0;
76		circle(dst_fast, Point(dst_it_kp->pt.x, dst_it_kp->pt.y), 1, Scalar(0, 255, 0), -1);
77		circle(dst_fast, Point(dst_it_kp->pt.x, dst_it_kp->pt.y), 8, Scalar(0, 255, 0));
78		xd = dst_it_kp->pt.x;
79		yd = dst_it_kp->pt.y;
80		int d[9] = { MI(dst_gry, xd - 1, yd - 1), MI(dst_gry, xd, yd - 1), MI(dst_gry, xd + 1, yd - 1), MI(dst_gry, xd - 1, yd), MI(dst_gry, xd, yd), MI(dst_gry, xd + 1, yd), MI(dst_gry, xd - 1, yd + 1), MI(dst_gry, xd, yd + 1), MI(dst_gry, xd + 1, yd + 1) };
81		for (int m = 0; m <= 9; m++) {
82			d_d += d[m];
83			d2 += (d[m] * d[m]);
84		}
85		double d_a = 0.0;
86		d_a = d_d / 9.0;
87		double d_v = 0.0;
88		d_v = d2 / 9.0 - d_a * d_a;
89		if (ys_max * 0.90 <= yd && ys_max * 1.10 >= yd) {
90			d_max = d_v;
91			xd_max = xd;
92			yd_max = yd;
93		}
94		xdb[p] = xd;
95		ydb[p] = yd;
96		vdb[p] = d_v;
97		o = o + 1;
98	}
99	int tmp4, tmp5;
100	double tmp6;
101	for (int os = 1; os < o; os++) {
102		for (int ot = os + 1; ot < o + 1; ot++) {
103			if (vdb[os] < vdb[ot]) {
104				tmp4 = xdb[os];
105				xdb[os] = xdb[ot];
106				xdb[ot] = tmp4;
107				tmp5 = ydb[os];
108				ydb[os] = ydb[ot];
109				ydb[ot] = tmp5;
110				tmp6 = vdb[os];
111				vdb[os] = vdb[ot];
112				vdb[ot] = tmp6;
113			}
114		}
115	}
116	for (int d = 1; d <= 10; d++) {
117		circle(dst_fast, Point(xdb[d], ydb[d]), 5, Scalar(0, 0, 255), -1);
118	}
119	for (int om = 1; om < o + 1; om++) {
120		printf("格納座標：%d回目, x座標：%d, 分散値：%f \n", om, xdb[om],vdb[om]);
121	}
122	namedWindow("srcFAST", WINDOW_AUTOSIZE);
123	imshow("srcFAST", src_fast);
124	namedWindow("dstFAST", WINDOW_AUTOSIZE);
125	imshow("dstFAST", dst_fast);
126
127	vector<DMatch> allMatch, goodMatch;
128	BFMatcher matcher(NORM_HAMMING);
129	matcher.match(src_fast, dst_fast, allMatch);
130
131	for (int u = 0; u < allMatch.size(); u++) {
132		if (allMatch[u].distance < 30)goodMatch.push_back(allMatch[u]);
133
134		int kp1 = goodMatch[u].queryIdx;
135		int kp2 = goodMatch[u].trainIdx;
136
137		Point start(src_key[kp1].pt.x, src_key[kp1].pt.y);
138		Point end(dst_key[kp2].pt.x, dst_key[kp2].pt.y);
139	}

該当のソースコード

ここまでのコードはうまく実行可能で、これ以降はエラーを表示する。
おそらく対応がうまくできていなかったりするのだと思うが、何が間違っているのかがわからないため。

C++
1vector<DMatch> allMatch, goodMatch;
2	BFMatcher matcher(NORM_HAMMING);
3	matcher.match(src_fast, dst_fast, allMatch);
4
5	for (int u = 0; u < allMatch.size(); u++) {
6		if (allMatch[u].distance < 30)goodMatch.push_back(allMatch[u]);
7
8		int kp1 = goodMatch[u].queryIdx;
9		int kp2 = goodMatch[u].trainIdx;
10
11		Point start(src_key[kp1].pt.x, src_key[kp1].pt.y);
12		Point end(dst_key[kp2].pt.x, dst_key[kp2].pt.y);
13	}