This repository contains C++ examples of fundamental matrix estimation using two-point correspondences under planar motion. It is the implementation of the paper by Chou et al. cited in the reference below.
Any agent moving on a planar surface can be described as planar motion. For example, a car driving along a flat road or a multirotor flying at a constant altitude is planar motion.
It is particularly useful for robustification against outliers (e.g. in RANSAC). In contrast to the eight-point algorithm, this method requires only two-point correspondences by enforcing a planar motion constraint. This means less iteration to find the best model.
Imagine a vehicle in planar motion: its body rotates only along the yaw axis (y-axis). Conversely, there is no translation along the y-axis, so we can zero out the irrelevant movements.
After converting the above translation to the essential matrix translation component, both terms can be multiplied to get the essential matrix.
Solving the matrix above reduces to a geometric problem of finding the intersection between an ellipse and a circle. Assuming the intrinsic matrix is known, the fundamental matrix and its pose can be derived.
OpenCV is required to build the examples.
mkdir build
cd build
cmake ..
make
The simplest example is to calculate the essential matrix directly from two visual correspondences.
#include "fpsolver.h"
using pointsVec = std::vector<cv::Point2f>;
using matVec = std::vector<cv::Mat>;
int main() {
// needs to be exactly two correspondences
pointsVec points1_est = {cv::Point2f(1652, 469), cv::Point2f(1642, 656)};
pointsVec points2_est = {cv::Point2f(1712, 453), cv::Point2f(1739, 696)};
cv::Mat k = (cv::Mat_<double>(3, 3) << 1280.7, 0.0, 969.4257, 0.0, 1281.2,
639.7227, 0.0, 0.0, 1.0); // intrinsic matrix example
matVec E = PM::findEssential(points1_est, points2_est, k);
}
A more useful example is to utilize RANSAC for robustification against outliers.
// requires >= 2 correspondences via feature matching
pointsVec points1_est = { ... };
pointsVec points2_est = { ... };
int max_iter = 50;
double threshold = 20;
double confidence = 99.0;
PM::RANSACFundam ransac(max_iter, threshold, confidence);
cv::Mat F = PM::findFundam(points1_est, points2_est, k, ransac);
Results are generated from the provided example code. Below are the epipolar lines visualized from the estimated fundamental matrix using RANSAC; all points are visualized including the outliers.
The estimated fundamental matrix is useful to detect outliers in the matched features.
- Two visual correspondences yield four solutions for the essential matrix. Early rejection might be possible instead of feeding them all into RANSAC.
- Closed-form solution exists. Also discussed in the same paper below. It requires no SVD, should be way faster.
Sunglok Choi, Jong-Hwan Kim, Fast and reliable minimal relative pose estimation under planar motion, Image and Vision Computing, Volume 69, 2018.