-
Notifications
You must be signed in to change notification settings - Fork 91
/
test_epipolor_geometry.cpp
163 lines (142 loc) · 5.97 KB
/
test_epipolor_geometry.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
// Test functions in "include/my_slam/geometry", including:
// * Keypoint extraction and feature matching.
// * Estimate camera motion by Essential/Homography matrix (totally 1+2=3 solutions). All returned in a vector.
// * Triangulation.
// * Compute epipolar error and triangulation error in pixel.
/*
How to run:
bin/test_epipolor_geometry image0001.jpg image0002.jpg
bin/test_epipolor_geometry image0001.jpg image0015.jpg
*/
#include "my_slam/geometry/feature_match.h"
#include "my_slam/geometry/epipolar_geometry.h"
#include "my_slam/geometry/motion_estimation.h"
#include "my_slam/basics/config.h"
#include <iostream>
#include <algorithm>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace std;
using namespace my_slam;
int main(int argc, char **argv)
{
// camera intrinsics
cv::Mat K_fr1 = (cv::Mat_<double>(3, 3) << 517.3, 0, 325.1, 0, 516.5, 249.7, 0, 0, 1); // fr1 dataset
cv::Mat K_fr2 = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); // fr2 dataset
cv::Mat K_mtb = (cv::Mat_<double>(3, 3) << 615, 0, 320, 0, 615, 240, 0, 0, 1); // fr2 dataset
cv::Mat K;
// read in images
string img_file1, img_file2;
string folder = "data/test_data/";
int IDX_TEST_CASE = 1;
// Feature extraction and matching's parameters
const string kConfigFile = "config/config.yaml";
basics::Config::setParameterFile(kConfigFile); // Use Config to read .yaml
if (argc - 1 == 2)
{
// bin/test_epipolor_geometry rgb_00000.png rgb_00001.png # inliers = 90+
// bin/test_epipolor_geometry rgb_00003.png rgb_00004.png # inliers = 35
// bin/test_epipolor_geometry rgb_00004.png rgb_00005.png # inliers = 90+
// bin/test_epipolor_geometry image0001.jpg image0015.jpg # Mean Score: E=11.0, H=9.1
// bin/test_epipolor_geometry image0001.jpg image0002.jpg # Mean Score: E=11.3, H=11.0
IDX_TEST_CASE = -1;
img_file1 = argv[1];
img_file2 = argv[2];
K = K_mtb;
}
else if (IDX_TEST_CASE == 1) // keypoints are not on the same plane.
{
img_file1 = "fr1_1_1.png";
img_file2 = "fr1_1_2.png";
K = K_fr1;
}
else if (IDX_TEST_CASE == 2) // keypoints are almost on the same plane.
{
img_file1 = "fr1_2_1.png";
img_file2 = "fr1_2_1.png";
K = K_fr1;
}
else if (IDX_TEST_CASE == 3) // keypoints are almost on the same plane.
{
img_file1 = "fr2_1_1.png";
img_file2 = "fr2_1_2.png";
K = K_fr2;
}
//====================================== Main program starts =========================================
// Read image
cv::Mat img_1 = cv::imread(folder + img_file1);
cv::Mat img_2 = cv::imread(folder + img_file2);
// Extract keypoints and features. Match keypoints
vector<cv::KeyPoint> keypoints_1, keypoints_2;
vector<cv::DMatch> matches;
cv::Mat descriptors_1, descriptors_2;
// doFeatureMatching(img_1, img_2, keypoints_1, keypoints_2, descriptors_1, descriptors_2, matches);
bool is_print_res = true;
geometry::calcKeyPoints(img_1, keypoints_1);
geometry::calcKeyPoints(img_2, keypoints_2);
cout << "Number of keypoints: " << keypoints_1.size() << ", " << keypoints_2.size() << endl;
geometry::calcDescriptors(img_1, keypoints_1, descriptors_1);
geometry::calcDescriptors(img_2, keypoints_2, descriptors_2);
static const int method_index = basics::Config::get<int>("feature_match_method_index");
geometry::matchFeatures(descriptors_1, descriptors_2, matches, method_index, is_print_res,
keypoints_1, keypoints_2, 50);
printf("Number of matches: %d\n", (int)matches.size());
// Estimation motion
vector<cv::Mat> list_R, list_t, list_normal;
vector<vector<cv::DMatch>> list_matches;
vector<vector<cv::Point3f>> sols_pts3d_in_cam1_by_triang;
const bool is_calc_homo = true, is_frame_cam2_to_cam1 = true;
is_print_res = false;
int best_sol = geometry::helperEstimatePossibleRelativePosesByEpipolarGeometry(
/*Input*/
keypoints_1, keypoints_2, matches, K,
/*Output*/
list_R, list_t, list_matches, list_normal, sols_pts3d_in_cam1_by_triang,
/*settings*/
is_print_res, is_calc_homo, is_frame_cam2_to_cam1);
cout << "Best solution is: " << best_sol << endl;
// Compute [epipolar error] and [trigulation error on norm plane] for the 3 solutions (E, H1, H2)
geometry::helperEvalEppiAndTriangErrors(
keypoints_1, keypoints_2, list_matches,
sols_pts3d_in_cam1_by_triang,
list_R, list_t, list_normal,
K,
false); // print result
// plot image
cv::Mat Idst;
string window_name;
window_name = "Detected and matched keypoints";
cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, Idst);
cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);
cv::imshow(window_name, Idst);
cv::imwrite(window_name + ".png", Idst);
cv::waitKey(1);
window_name = "Detected and inliers keypoints";
cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, list_matches[0], Idst);
cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);
cv::imshow(window_name, Idst);
cv::imwrite(window_name + ".png", Idst);
cv::waitKey(1);
// -- Print error
int cnt = 0;
for (const cv::DMatch &d : list_matches[0])
{
cv::Point2f p1 = keypoints_1[d.queryIdx].pt;
cv::Point2f p2 = keypoints_2[d.trainIdx].pt;
if (abs(p1.x - p2.x) > 20)
{
cout << endl;
printf("The %dth kpt, p1(%.2f, %.2f), p2(%.2f, %.2f)\n",
cnt, p1.x, p1.y, p2.x, p2.y);
double error = geometry::computeEpipolarConsError(p1, p2, list_R[0], list_t[0], K);
printf("Print its epi error:%.6f", error);
cout << endl;
}
cnt++;
}
cv::waitKey();
cv::destroyAllWindows();
return 0;
}