Skip to main content

SLAM

2D to 2D​

Step 1 - Detect Features​

#include "opencv2/opencv.hpp"
// [...]
auto img1 = cv::imread("./data/img1.ppm");
auto img2 = cv::imread("./data/img3.ppm");
assert(img1.data != nullptr && img2.data != nullptr);

std::vector<cv::KeyPoint> kp1, kp2;
cv::Mat descr1, descr2;

auto orb = cv::ORB::create();

orb->detectAndCompute(img1, cv::Mat(), kp1, descr1);
orb->detectAndCompute(img2, cv::Mat(), kp2, descr2);

Step 2 - Match Features​

auto matcher = cv::DescriptorMatcher::create(cv::DescriptorMatcher::BRUTEFORCE_HAMMING);
std::vector<cv::DMatch> matches;
matcher->match(descr1, descr2, matches);

Step 3 - Filter Matches​

auto [min, max] = std::minmax_element(
  matches.begin(),
  matches.end(),
  [](const cv::DMatch& m1, const cv::DMatch& m2){return m1.distance < m2.distance;});

std::vector<cv::DMatch> good_matches;
for (const auto& match : matches) {
  if (match.distance <= std::max(2.0 * min->distance, 30.0)) {
    good_matches.emplace_back(match);
  }
}

Step 4 - Compute Essential Matrix​

std::vector<cv::Point2f> pts1;
std::vector<cv::Point2f> pts2;

for (const auto& match : good_matches) {
  pts1.push_back(kp1[match.queryIdx].pt);
  pts2.push_back(kp2[match.trainIdx].pt);
}
cv::Mat K = (cv::Mat_<double>(3, 3) <<
  520.9,   0.0, 325.1,
    0.0, 521.0, 249.7,
    0.0,   0.0,   1.0);

auto E = cv::findEssentialMat(pts1, pts2, K);

Step 5 - Compute Camera Rotation and Translation​

cv::Mat R, t;
cv::recoverPose(E, pts1, pts2, K, R, t);

Step 6 - Triangulate Points​

cv::Point2f pixel2cam(const cv::Point2d& p, const cv::Mat& K) {
  return cv::Point2f((p.x - K.at<double>(0,2)) / K.at<double>(0,0),
                     (p.y - K.at<double>(1,2)) / K.at<double>(1,1));
}

cv::Mat T1 = (cv::Mat_<float>(3, 4) <<
  1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 1, 0);
cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
  R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
  R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
  R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));

std::vector<cv::Point2f> pts1;
std::vector<cv::Point2f> pts2;
for (const auto& match: good_matches) {
  pts1.push_back(pixel2cam(kp1[match.queryIdx].pt, K));
  pts2.push_back(pixel2cam(kp1[match.trainIdx].pt, K));
}
cv::Mat pts4d;
cv::triangulatePoints(T1, T2, pts1, pts2, pts4d);

std::vector<cv::Point3d> points;
for (int i = 0; i < pts4d.cols; i++) {
  cv::Mat x = pts4d.col(i);
  x /= x.at<float>(3, 0);
  cv::Point3d p(x.at<float>(0, 0),x.at<float>(1, 0),x.at<float>(2, 0));
  points.emplace_back(p);
}

Source​