essential_mat_reconstr.cpp
14.9 KB
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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "opencv2/calib3d.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include <vector>
#include <iostream>
#include <fstream>
using namespace cv;
static double getError2EpipLines (const Mat &F, const Mat &pts1, const Mat &pts2, const Mat &mask) {
Mat points1, points2;
vconcat(pts1, Mat::ones(1, pts1.cols, pts1.type()), points1);
vconcat(pts2, Mat::ones(1, pts2.cols, pts2.type()), points2);
double mean_error = 0;
for (int pt = 0; pt < (int) mask.total(); pt++)
if (mask.at<uchar>(pt)) {
const Mat l2 = F * points1.col(pt);
const Mat l1 = F.t() * points2.col(pt);
mean_error += (fabs(points1.col(pt).dot(l1)) / sqrt(pow(l1.at<double>(0), 2) + pow(l1.at<double>(1), 2)) +
fabs(points2.col(pt).dot(l2) / sqrt(pow(l2.at<double>(0), 2) + pow(l2.at<double>(1), 2)))) / 2;
}
return mean_error / mask.total();
}
static int sgn(double val) { return (0 < val) - (val < 0); }
/*
* @points3d - vector of Point3 or Mat of size Nx3
* @planes - vector of found planes
* @labels - vector of size point3d. Every point which has non-zero label is classified to this plane.
*/
static void getPlanes (InputArray points3d_, std::vector<int> &labels, std::vector<Vec4d> &planes, int desired_num_planes, double thr_, double conf_, int max_iters_) {
Mat points3d = points3d_.getMat();
points3d.convertTo(points3d, CV_64F); // convert points to have double precision
if (points3d_.isVector())
points3d = Mat((int)points3d.total(), 3, CV_64F, points3d.data);
else {
if (points3d.type() != CV_64F)
points3d = points3d.reshape(1, (int)points3d.total()); // convert point to have 1 channel
if (points3d.rows < points3d.cols)
transpose(points3d, points3d); // transpose so points will be in rows
CV_CheckEQ(points3d.cols, 3, "Invalid dimension of point");
}
/*
* 3D plane fitting with RANSAC
* @best_model contains coefficients [a b c d] s.t. ax + by + cz = d
*
*/
auto plane_ransac = [] (const Mat &pts, double thr, double conf, int max_iters, Vec4d &best_model, std::vector<bool> &inliers) {
const int pts_size = pts.rows, max_lo_inliers = 15, max_lo_iters = 10;
int best_inls = 0;
if (pts_size < 3) return false;
RNG rng;
const auto * const points = (double *) pts.data;
std::vector<int> min_sample(3);
inliers = std::vector<bool>(pts_size);
const double log_conf = log(1-conf);
Vec4d model, lo_model;
std::vector<int> random_pool (pts_size);
for (int p = 0; p < pts_size; p++)
random_pool[p] = p;
// estimate plane coefficients using covariance matrix
auto estimate = [&] (const std::vector<int> &sample, Vec4d &model_) {
// https://www.ilikebigbits.com/2017_09_25_plane_from_points_2.html
const int n = static_cast<int>(sample.size());
if (n < 3) return false;
double sum_x = 0, sum_y = 0, sum_z = 0;
for (int s : sample) {
sum_x += points[3*s ];
sum_y += points[3*s+1];
sum_z += points[3*s+2];
}
const double c_x = sum_x / n, c_y = sum_y / n, c_z = sum_z / n;
double xx = 0, yy = 0, zz = 0, xy = 0, xz = 0, yz = 0;
for (int s : sample) {
const double x_ = points[3*s] - c_x, y_ = points[3*s+1] - c_y, z_ = points[3*s+2] - c_z;
xx += x_*x_; yy += y_*y_; zz += z_*z_; xy += x_*y_; yz += y_*z_; xz += x_*z_;
}
xx /= n; yy /= n; zz /= n; xy /= n; yz /= n; xz /= n;
Vec3d weighted_normal(0,0,0);
const double det_x = yy*zz - yz*yz, det_y = xx*zz - xz*xz, det_z = xx*yy - xy*xy;
Vec3d axis_x (det_x, xz*xz-xy*zz, xy*yz-xz*yy);
Vec3d axis_y (xz*yz-xy*zz, det_y, xy*xz-yz*xx);
Vec3d axis_z (xy*yz-xz*yy, xy*xz-yz*xx, det_z);
weighted_normal += axis_x * det_x * det_x;
weighted_normal += sgn(weighted_normal.dot(axis_y)) * axis_y * det_y * det_y;
weighted_normal += sgn(weighted_normal.dot(axis_z)) * axis_z * det_z * det_z;
weighted_normal /= norm(weighted_normal);
if (std::isinf(weighted_normal(0)) ||
std::isinf(weighted_normal(1)) ||
std::isinf(weighted_normal(2))) return false;
// find plane model from normal and centroid
model_ = Vec4d(weighted_normal(0), weighted_normal(1), weighted_normal(2),
weighted_normal.dot(Vec3d(c_x, c_y, c_z)));
return true;
};
// calculate number of inliers
auto getInliers = [&] (const Vec4d &model_) {
const double a = model_(0), b = model_(1), c = model_(2), d = model_(3);
int num_inliers = 0;
std::fill(inliers.begin(), inliers.end(), false);
for (int p = 0; p < pts_size; p++) {
inliers[p] = fabs(a * points[3*p] + b * points[3*p+1] + c * points[3*p+2] - d) < thr;
if (inliers[p]) num_inliers++;
if (num_inliers + pts_size - p < best_inls) break;
}
return num_inliers;
};
// main RANSAC loop
for (int iters = 0; iters < max_iters; iters++) {
// find minimal sample: 3 points
min_sample[0] = rng.uniform(0, pts_size);
min_sample[1] = rng.uniform(0, pts_size);
min_sample[2] = rng.uniform(0, pts_size);
if (! estimate(min_sample, model))
continue;
int num_inliers = getInliers(model);
if (num_inliers > best_inls) {
// store so-far-the-best
std::vector<bool> best_inliers = inliers;
// do Local Optimization
for (int lo_iter = 0; lo_iter < max_lo_iters; lo_iter++) {
std::vector<int> inliers_idx; inliers_idx.reserve(max_lo_inliers);
randShuffle(random_pool);
for (int p : random_pool) {
if (best_inliers[p]) {
inliers_idx.emplace_back(p);
if ((int)inliers_idx.size() >= max_lo_inliers)
break;
}
}
if (! estimate(inliers_idx, lo_model))
continue;
int lo_inls = getInliers(lo_model);
if (best_inls < lo_inls) {
best_model = lo_model;
best_inls = lo_inls;
best_inliers = inliers;
}
}
if (best_inls < num_inliers) {
best_model = model;
best_inls = num_inliers;
}
// update max iters
// because points are quite noisy we need more iterations
const double max_hyp = 3 * log_conf / log(1 - pow(double(best_inls) / pts_size, 3));
if (! std::isinf(max_hyp) && max_hyp < max_iters)
max_iters = static_cast<int>(max_hyp);
}
}
getInliers(best_model);
return best_inls != 0;
};
labels = std::vector<int>(points3d.rows, 0);
Mat pts3d_plane_fit = points3d.clone();
// keep array of indices of points corresponding to original points3d
std::vector<int> to_orig_pts_arr(pts3d_plane_fit.rows);
for (int i = 0; i < (int) to_orig_pts_arr.size(); i++)
to_orig_pts_arr[i] = i;
for (int num_planes = 1; num_planes <= desired_num_planes; num_planes++) {
Vec4d model;
std::vector<bool> inl;
if (!plane_ransac(pts3d_plane_fit, thr_, conf_, max_iters_, model, inl))
break;
planes.emplace_back(model);
const int pts3d_size = pts3d_plane_fit.rows;
pts3d_plane_fit = Mat();
pts3d_plane_fit.reserve(points3d.rows);
int cnt = 0;
for (int p = 0; p < pts3d_size; p++) {
if (! inl[p]) {
// if point is not inlier to found plane - add it to next run
to_orig_pts_arr[cnt] = to_orig_pts_arr[p];
pts3d_plane_fit.push_back(points3d.row(to_orig_pts_arr[cnt]));
cnt++;
} else labels[to_orig_pts_arr[p]] = num_planes; // otherwise label this point
}
}
}
int main(int args, char** argv) {
std::string data_file, image_dir;
if (args < 3) {
CV_Error(Error::StsBadArg,
"Path to data file and directory to image files are missing!\nData file must have"
" format:\n--------------\n image_name_1\nimage_name_2\nk11 k12 k13\n0 k22 k23\n"
"0 0 1\n--------------\nIf image_name_{1,2} are not in the same directory as "
"the data file then add argument with directory to image files.\nFor example: "
"./essential_mat_reconstr essential_mat_data.txt ./");
} else {
data_file = argv[1];
image_dir = argv[2];
}
std::ifstream file(data_file, std::ios_base::in);
CV_CheckEQ((int)file.is_open(), 1, "Data file is not found!");
std::string filename1, filename2;
std::getline(file, filename1);
std::getline(file, filename2);
Mat image1 = imread(image_dir+filename1);
Mat image2 = imread(image_dir+filename2);
CV_CheckEQ((int)image1.empty(), 0, "Image 1 is not found!");
CV_CheckEQ((int)image2.empty(), 0, "Image 2 is not found!");
// read calibration
Matx33d K;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
file >> K(i,j);
file.close();
Mat descriptors1, descriptors2;
std::vector<KeyPoint> keypoints1, keypoints2;
// detect points with SIFT
Ptr<SIFT> detector = SIFT::create();
detector->detect(image1, keypoints1);
detector->detect(image2, keypoints2);
detector->compute(image1, keypoints1, descriptors1);
detector->compute(image2, keypoints2, descriptors2);
FlannBasedMatcher matcher(makePtr<flann::KDTreeIndexParams>(5), makePtr<flann::SearchParams>(32));
// get k=2 best match that we can apply ratio test explained by D.Lowe
std::vector<std::vector<DMatch>> matches_vector;
matcher.knnMatch(descriptors1, descriptors2, matches_vector, 2);
// filter keypoints with Lowe ratio test
std::vector<Point2d> pts1, pts2;
pts1.reserve(matches_vector.size()); pts2.reserve(matches_vector.size());
for (const auto &m : matches_vector) {
// compare best and second match using Lowe ratio test
if (m[0].distance / m[1].distance < 0.75) {
pts1.emplace_back(keypoints1[m[0].queryIdx].pt);
pts2.emplace_back(keypoints2[m[0].trainIdx].pt);
}
}
Mat inliers;
const int pts_size = (int) pts1.size();
const auto begin_time = std::chrono::steady_clock::now();
// fine essential matrix
const Mat E = findEssentialMat(pts1, pts2, Mat(K), RANSAC, 0.99, 1.0, inliers);
std::cout << "RANSAC essential matrix time " << std::chrono::duration_cast<std::chrono::microseconds>
(std::chrono::steady_clock::now() - begin_time).count() <<
"mcs.\nNumber of inliers " << countNonZero(inliers) << "\n";
Mat points1 = Mat((int)pts1.size(), 2, CV_64F, pts1.data());
Mat points2 = Mat((int)pts2.size(), 2, CV_64F, pts2.data());
points1 = points1.t(); points2 = points2.t();
std::cout << "Mean error to epipolar lines " <<
getError2EpipLines(K.inv().t() * E * K.inv(), points1, points2, inliers) << "\n";
// decompose essential into rotation and translation
Mat R1, R2, t;
decomposeEssentialMat(E, R1, R2, t);
// Create two relative pose
// P1 = K [ I | 0 ]
// P2 = K [R{1,2} | {+-}t]
Mat P1;
hconcat(K, Vec3d::zeros(), P1);
std::vector<Mat> P2s(4);
hconcat(K * R1, K * t, P2s[0]);
hconcat(K * R1, -K * t, P2s[1]);
hconcat(K * R2, K * t, P2s[2]);
hconcat(K * R2, -K * t, P2s[3]);
// find objects point by enumerating over 4 different projection matrices of second camera
// vector to keep object points
std::vector<std::vector<Vec3d>> obj_pts_per_cam(4);
// vector to keep indices of image points corresponding to object points
std::vector<std::vector<int>> img_idxs_per_cam(4);
int cam_idx = 0, best_cam_idx = 0, max_obj_pts = 0;
for (const auto &P2 : P2s) {
obj_pts_per_cam[cam_idx].reserve(pts_size);
img_idxs_per_cam[cam_idx].reserve(pts_size);
for (int i = 0; i < pts_size; i++) {
// process only inliers
if (! inliers.at<uchar>(i))
continue;
Vec4d obj_pt;
// find object point using triangulation
triangulatePoints(P1, P2, points1.col(i), points2.col(i), obj_pt);
obj_pt /= obj_pt(3); // normalize 4d point
if (obj_pt(2) > 0) { // check if projected point has positive depth
obj_pts_per_cam[cam_idx].emplace_back(Vec3d(obj_pt(0), obj_pt(1), obj_pt(2)));
img_idxs_per_cam[cam_idx].emplace_back(i);
}
}
if (max_obj_pts < (int) obj_pts_per_cam[cam_idx].size()) {
max_obj_pts = (int) obj_pts_per_cam[cam_idx].size();
best_cam_idx = cam_idx;
}
cam_idx++;
}
std::cout << "Number of object points " << max_obj_pts << "\n";
const int circle_sz = 7;
// draw image points that are inliers on two images
std::vector<int> labels;
std::vector<Vec4d> planes;
getPlanes (obj_pts_per_cam[best_cam_idx], labels, planes, 4, 0.002, 0.99, 10000);
const int num_found_planes = (int) planes.size();
RNG rng;
std::vector<Scalar> plane_colors (num_found_planes);
for (int pl = 0; pl < num_found_planes; pl++)
plane_colors[pl] = Scalar (rng.uniform(0,256), rng.uniform(0,256), rng.uniform(0,256));
for (int obj_pt = 0; obj_pt < max_obj_pts; obj_pt++) {
const int pt = img_idxs_per_cam[best_cam_idx][obj_pt];
if (labels[obj_pt] > 0) { // plot plane points
circle (image1, pts1[pt], circle_sz, plane_colors[labels[obj_pt]-1], -1);
circle (image2, pts2[pt], circle_sz, plane_colors[labels[obj_pt]-1], -1);
} else { // plot inliers
circle (image1, pts1[pt], circle_sz, Scalar(0,0,0), -1);
circle (image2, pts2[pt], circle_sz, Scalar(0,0,0), -1);
}
}
// concatenate two images
hconcat(image1, image2, image1);
const int new_img_size = 1200 * 800; // for example
// resize with the same aspect ratio
resize(image1, image1, Size((int)sqrt ((double) image1.cols * new_img_size / image1.rows),
(int)sqrt ((double) image1.rows * new_img_size / image1.cols)));
imshow("image 1-2", image1);
imwrite("planes.png", image1);
waitKey(0);
}