demo
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/opencv_modules.hpp>
#include <opencv2/xfeatures2d.hpp>
int main(int argc, const char **argv)
{
while (true) {
cv::Mat left = cv::imread("./res/left.png");
cv::Mat right = cv::imread("./res/right.png");
cv::Mat output;
cv::hconcat(left, right, output);
cv::Mat left_debug = output.colRange(0, output.cols / 2);
cv::Mat right_debug = output.colRange(output.cols / 2, output.cols);
cv::cvtColor(left, left, CV_RGB2GRAY);
cv::cvtColor(right, right, CV_RGB2GRAY);
{
std::vector<cv::KeyPoint> keypoints_left, keypoints_right;
cv::Mat descriptors_left, descriptors_right;
auto sift = cv::xfeatures2d::SIFT::create(100);
sift->detect(left, keypoints_left);
sift->detect(right, keypoints_right);
sift->compute(left, keypoints_left, descriptors_left);
sift->compute(right, keypoints_right, descriptors_right);
cv::drawKeypoints(left_debug, keypoints_left, left_debug, cv::Scalar::all(-1));
cv::drawKeypoints(right_debug, keypoints_right, right_debug, cv::Scalar::all(-1));
#if 1
Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
Eigen::Matrix3d translation_hat = Eigen::Matrix3d::Zero();
translation_hat << double(0), -transform.col(3)(2), transform.col(3)(1), transform.col(3)(2), double(0), -transform.col(3)(0), -transform.col(3)(1), transform.col(3)(0), double(0);
Eigen::Matrix3d essential = transform.block<3, 3>(0, 0) * translation_hat;
// epipolar line au + bv + c = 0
for (auto pos = 0; pos < keypoints_left.size(); pos++) {
auto & keypoint = keypoints_left.at(pos);
Eigen::Vector3d point(keypoint.pt.x, keypoint.pt.y, 1);
Eigen::Vector3d line = essential.transpose() * point;
}
#else
std::vector<cv::DMatch> matches;
cv::BFMatcher matcher(cv::NORM_L2, true);
matcher.match(descriptors_left, descriptors_right, matches);
double min_dist = (std::numeric_limits<double>::max)(), max_dist = 0;
for (int pos = 0; pos < matches.size(); ++pos)
{
double dist = matches[pos].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
std::vector<cv::DMatch> good_matches;
for (int pos = 0; pos < matches.size(); ++pos)
{
if (matches[pos].distance <= std::max(2 * min_dist, 30.0)) {
good_matches.push_back(matches[pos]);
}
}
for (auto pos = 0; pos < good_matches.size(); pos++) {
auto &match = good_matches.at(pos);
cv::line(output, keypoints_left.at(match.queryIdx).pt, keypoints_right.at(match.trainIdx).pt + cv::Point2f(left.cols, 0), cv::Scalar::all(-1));
}
#endif
}
cv::imshow("match", output);
cv::waitKey(1);
}
return 0;
}
Reference
http://www.cse.psu.edu/~rtc12/CSE486/lecture19.pdf