コーナー検出とAKAZEを組み合わせたいです

前提・実現したいこと

今回はステレオ画像となっている2枚の画像(以下、左画像：src、右画像：dstとする)を用いて、コーナー検出とAKAZEを組み合わせ用いたい。
手順としては、
１．src、dstを入力
２．srcのみでコーナー検出(FAST)を行い、出力された各(x,y)座標を配列sに格納
３．求めたコーナーの分散値を求める(今回は無視してください)
４．srcとdstでAKAZEを用いてマッチングを行う
５．このとき、srcで得た各(x,y)座標と２で格納した(x,y)座標を照会し一致した座標のみを別の配列asに格納
６．５で格納した座標が位置する点とマッチングするdstの(x,y)座標を対応付けて、対応するもののみを配列adへ格納
７．５と６で格納した座標で表せる点のみでマッチングを行い描画する

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

・上記記載の６以降がうまくできない
・配列の個数を表示してもどの配列も6しか表示されない
・そもそも配列を対応付けてdrawMatches()へ入れることが可能なのか

該当のソースコード

c++
1#define _CRT_SECURE_NO_WARNINGS
2
3#include <opencv2/opencv.hpp>
4#include <iostream>
5#include <string>
6#include <sstream>
7#include <iomanip>
8#include <opencv2/imgcodecs.hpp>
9#include <vector>
10#include <opencv2/imgproc/imgproc.hpp>
11#include <random>
12#include <strstream>
13#include <Eigen/Core>
14#include <math.h>
15#include <opencv2/core.hpp>
16#include <opencv2/imgcodecs.hpp>
17#include <opencv2/features2d.hpp>
18#include <ratio>
19#include <list>
20#include <ctype.h>
21#include <stdio.h>
22
23#define SIZE_OF_ARRAY(array) (sizeof(array)/sizeof(array[0]))
24
25using namespace cv;
26using namespace std;
27using namespace Eigen;
28
29int main() {
30	Mat img;
31	int i = 0;
32
33	////文字列の宣言
34	//string name = "D:\img\landmark.png";
35
36
37	//撮影画像の入力
38	Mat src = imread("D:\img\landmark\mark00.png");
39	Mat dst = imread("D:\img\landmark\mark01.png");
40
41	Mat src_gry, dst_gry;
42	vector<Point2f> src_corners;
43	vector<Point2f>::iterator src_con = src_corners.begin();
44	Mat src_fast = src.clone();
45
46	//グレースケール化
47	cvtColor(src, src_gry, COLOR_BGR2GRAY);
48	cvtColor(dst, dst_gry, COLOR_BGR2GRAY);
49
50	//コーナー検出
51	int threshold = 100;
52	bool nonmax = true;
53	int xs = 0;
54	int ys = 0;
55	vector<KeyPoint> src_key;
56
57	FAST(src_gry, src_key, threshold, nonmax);
58	vector<KeyPoint>::iterator src_it_kp = src_key.begin();
59	vector<int>
60		s_x_arr,	// コーナー検出用
61		s_y_arr,
62		s_x2_arr,	// AKAZE用
63		s_y2_arr,
64		d_x_arr,
65		d_y_arr,
66		botu;       //没用
67
68	for (; src_it_kp != src_key.end(); ++src_it_kp) {
69		double s_s = 0.0;
70		double s2 = 0.0;
71		circle(src_fast, Point(src_it_kp->pt.x, src_it_kp->pt.y), 1, Scalar(0, 255, 0), -1);
72		circle(src_fast, Point(src_it_kp->pt.x, src_it_kp->pt.y), 8, Scalar(0, 255, 0));
73		xs = src_it_kp->pt.x;
74		ys = src_it_kp->pt.y;
75		s_x_arr.push_back(xs);
76		s_y_arr.push_back(ys);
77
78		//画素値の分散
79		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) };
80		for (int n = 0; n <= 9; n++) {
81			s_s += s[n];
82			s2 += (s[n] * s[n]);
83		}
84		double s_a = 0.0;
85		s_a = s_s / 9.0;
86		double s_v = 0.0;
87		s_v = s2 / 9.0 - s_a * s_a;
88		printf("x座標：%d\ny座標：%d\n分散値：%f\n", xs, ys, s_v);
89	}
90	namedWindow("FAST");
91	imshow("FAST", src_fast);
92	cout << SIZE_OF_ARRAY(s_x_arr) << endl;
93
94
95	//AKAZE
96	//アルゴリズムにAKAZEを使用する
97	auto algorithm = AKAZE::create();
98
99	//特徴点抽出
100	vector <KeyPoint> s_key, d_key;
101	algorithm->detect(src, s_key);
102	algorithm->detect(dst, d_key);
103
104	//特徴点の座標取得
105	//AKAZEのsrc
106	vector<KeyPoint>::iterator s_i_key = s_key.begin();
107	int a_xs = 0;
108	int a_ys = 0;
109	for (; s_i_key != s_key.end(); ++s_i_key) {
110		a_xs = s_i_key->pt.x;
111		a_ys = s_i_key->pt.y;
112
113		// 一致の確認
114		for (int ss = 0; ss < s_x_arr.size(); ss++) {
115			if (s_x_arr[ss] == a_xs && s_y_arr[ss] == a_ys) {
116				// 一致時の処理
117				s_x2_arr.push_back(a_xs);
118				s_y2_arr.push_back(a_ys);
119			}
120			else {
121				botu.push_back(xs);
122				botu.push_back(ys);
123				botu.push_back(a_xs);
124				botu.push_back(a_ys);
125			}
126		}
127	}
128	//AKAZEのdst
129	vector<KeyPoint>::iterator d_i_key = d_key.begin();
130	int a_xd = 0;
131	int a_yd = 0;
132	for (; d_i_key != d_key.end(); ++d_i_key) {
133		a_xd = d_i_key->pt.x;
134		a_yd = d_i_key->pt.y;
135		d_x_arr.push_back(a_xd);
136		d_y_arr.push_back(a_yd);
137
138	}
139
140	//特徴記述
141	Mat s_des, d_des;
142	algorithm->compute(src, s_key, s_des);
143	algorithm->compute(dst, d_key, d_des);
144
145	//knnマッチング
146	vector<vector<DMatch>>matches;
147	BFMatcher matcher;
148	matcher.knnMatch(s_des, d_des, matches, 2);
149	vector<DMatch>good_matches;
150	// 一致の確認
151	for (int dd = 0; dd < d_x_arr.size(); dd++) {
152		if ((sizeof(s_x2_arr) / sizeof(int)) == (sizeof(d_x_arr) / sizeof(int)) && (sizeof(s_y2_arr) / sizeof(int)) == (sizeof(d_y_arr) / sizeof(int))) {
153			for (int k = 0; k < matches.size(); ++k) {
154				const float ratio = 0.5;
155				if (matches[k][0].distance < ratio * matches[k][1].distance) {
156					good_matches.push_back(matches[k][0]);
157				}
158			}
159		}
160	}
161	
162	//マッチング結果の描画
163	Mat dest;
164	drawMatches(src, s_key, dst, d_key, good_matches, dest);
165
166	//マッチング結果の書き出し
167	namedWindow("AKAZE");
168	imshow("AKAZE", dest);
169
170	waitKey(0);
171
172	return 0;
173}