Drawing of epipolar lines (known internal and external parameters of the camera, epipolar geometry)

Polar Geometry and Stereo Vision
Goals:
Constraints of Extreme Geometry There are a lot of information on the Internet, but most of them are theoretical knowledge and some derivations. However, in specific applications, ** often uses limit constraints to judge whether the camera's pose is accurate, and there are relatively few introductions to this. **This blog will introduce how to realize the limit constraints of two cameras in the case of constant internal and external parameters of the camera.

The binocular vision correspondence can also be used for motion estimation between two adjacent frames. See the figure below:

the matching point must be on the epipolar line
Baseline: the line connecting the optical centers of the left and right cameras (yellow line segment $O_1{O}_2$);
polar plane: space point, the plane determined by the optical centers of the two cameras (gray plane $O_{1}P.O{\_}_2$);
pole: the intersection point of the baseline and the image plane of the two cameras;
epipolar line: the intersection line between the polar plane and the image plane (blue line segment)

Based on the geometric relationship of the limit, it is necessary to know their parameter relationship as follows.
Transformation of two cameras to $R,T.$ If the internal and
external parameters of the two cameras are known. Their transformation matrices can be calculated. Assume that the camera on the left is the world coordinate system. They can be expressed as:

The above is assuming the relationship between the two cameras. The world coordinate system is the coordinate system of the left camera. They coincide. So by $R,The\; parameters\; of\; the\; second\; camera\; calculated\; by\; T$ (it is the camera position pose, which refers to R , TR, Tin the first coordinate system$R,T$ ),
the specific calculation can refer to the following.
$p_2$Coordinate point (originally at $O_2$coordinate system) into $O_{\mathrm{１}}$Coordinate system $p_{\mathrm{１}}$of ( $p_1$means it is at $O_1$The coordinates in the coordinate system) are:
$$p_{\mathrm{１}}=R{p}_{\mathrm{２}}+T=>p_{\mathrm{２}}=R^T{p}_{\mathrm{１}}-R^{T}T\left(1\right)$$
The above can be obtained, from$O_{\mathrm{１}}$Convert the coordinate system to $O_{\mathrm{２}}$The coordinate system is:
$[R^T-R^{T}T]$

So the two mutual transformation matrices are:
$[R,T]$ :means from$O_{\mathrm{２}}$Coordinate system to $O_{\mathrm{１}}$Transformation of the coordinate system .
$[R^T-R^{T}T]$:means from$O_{\mathrm{１}}$Coordinate system to $O_{\mathrm{２}}$Transformation of the coordinate system .

Finally, add the internal reference matrix $K,K^{\text{'}}$ The overall matrix transformation can be obtained as follows:

$$M=K[I0],\_M^{\prime }=K^{\prime }[R^T-R^{T}T]$$

For the convenience of calculation, it is assumed that the camera's internal parameter matrix $K,K^{\prime }$ is the identity matrix$I$。因此
$$M=\left[I0\right],M^{\prime }=[R^T-R^{T}T]$$

From Figure 2 we can know that $p^{\prime }$ in the coordinate system$O_1$The coordinates are: $R{p}^{\prime }+T$ , since$R{p}^{\prime }+T$ this point is in$O_1$The epipolar line $O_{1}on\; P$ , because the limit plane$O_1{O}_{2}P$是平面：
$$T\times (R{p}^{\prime }+T)=T\times \left(R{p}^{\prime }\right)\left(2\right)$$
Vector$T\times \left(R{p}^{\prime }\right)$means perpendicular to the plane$O_1{O}_{2}P$
is obtained by cross product:
$$p^T\cdot [T\times (R{p}^{\prime })]=0\left(3\right)$$
where the cross product$T$ can be expressed in matrix form, generally expressed as$skew\left(T\right)$ or$[T_{\times }]$
$$[T_{\times }]=\left[\begin{array}{ccc}0& -T[2]& T[1]\\ T[2]& 0& -T[0]\\ -T[1]& T[0]& 0\end{array}\right]\left(4\right)$$
Formula$\left(3\right)$ Written in matrix form:
$$p^T\cdot [T\times (R{p}^{\prime })]=0=>p^T[T_{\times }]R{p}^{\prime }=0\left(5\right)$$
from formula$\left(5\right)$ get$\left[T_{\times }\right]R$ is Essential Matrix, this matrix is ​​written as:$[T_{\times }]R=E$ , so:
$$p^{T}E{p}^{\prime }=The\; expression\; of\; 0\left(6\right)$$
straight line is$l\cdot p=0$ (under the homogeneous coordinate representation)
can put$E{p}^{\prime }$ regarded as the equation of a straight line,$p$ is regarded as a point on the line, that is,$E{p}^{\prime }$ is$O_1$It is the epipolar line in the origin coordinate system (Figure 1 $O_{1}e$ line segment), and vice versa.

All the above are $K,K^{’}$ is set to the identity matrix. when is no longer the identity matrix. The obtained matrix is ​​"
$$M=K[I0],\_M^{\prime }=K^{\prime }[R^T-R^{T}T]\left(7\right)$$
because$K,K^{’}$ is a camera internal parameter, which represents the conversion from the camera coordinate system to the image coordinate system. Therefore:$p_I=K^{-1}p$; $p_I^{\prime }=K^{\prime -1}{p}^{\prime }$
Because of the formula:
$$p^{T}E{p}^{\prime }=0\left(6\right)$$
Then$p_I=K^{-1}p$; $p_I^{\prime }=K^{\prime -1}{p}^{\text{'}}$ into$\left(6\right)$ get:

$$p^T{K}^{-T}[T_{\times }]R{K}^{\prime -1}{p}^{\prime }=0(8)$$
将$F=K^{-T}[T_{\times }]R{K}^{\prime -1}$ , this is the Fundamental Matrix. It is also the Fundametal matrix calculated in opencv

The above is to set the first camera to be consistent with the world coordinate system. But in the case of two cameras all the time. May need to calculate $R,T$ matrix. They are:means from$O_{\mathrm{２}}$Coordinate system to $O_{\mathrm{１}}$Transformation of the coordinate system .
Known two cameras (both external parameters of the camera) are:
$$\begin{gathered} M_{\mathrm{１}}=\left[\begin{array}{cc}{R}_1& T_1\\ 0& 1\end{array}\right] \\ {M}_2=\left[\begin{array}{cc}{R}_2& T_2\\ 0& 1\end{array}\right] \end{gathered}$$
needs to be calculated from$O_{\mathrm{２}}$Coordinate system to $O_{\mathrm{１}}$Transformation matrices $R,T$
derivation:p 1 p_1
in their respective camera coordinate systems$p_1$,$p_2$
$p_1$The vertices in the world coordinate system are
$$p_w=M_1{p}_1(9)$$
$p_2$The vertices in the world coordinate system are
$$p_w=M_2{p}_2(10)$$
$\left(9\right)\left(10\right)$ Formula:
$$p_{\mathrm{１}}=M_1^{-1}{M}_2{p}_2$$
where $R,T$ is$M_1^{-1}{M}_2$Rotation matrix and translation vector.
code show as below:

void RunTwoImagesCorrspondEpilinesWithPath(int src_idx, int tgt_idx, const std::string& dir)
{
    
    
	std::cerr << "the cams num: " << cams_modes_.size() << std::endl;
	if (src_idx < 0 || tgt_idx < 0 ||
		src_idx >= cams_modes_.size() || tgt_idx >= cams_modes_.size())
	{
    
    
		std::cerr << "RunTwoImagesCorrespondEpilines invalid idx..." << std::endl;
		return;
	}
	if (!cams_modes_[src_idx].valid_ || !cams_modes_[tgt_idx].valid_)
	{
    
    
		std::cerr << "RunTwoImagesCorrespondEpilines invalid cams existed..." << std::endl;
		return;
	}
	CameraModel src_cam_model = cams_modes_[src_idx];
	CameraModel tgt_cam_model = cams_modes_[tgt_idx];
	std::string src_name = images_files_path_[src_idx];
	std::string tgt_name = images_files_path_[tgt_idx];
	std::cerr << "src_name: " << src_name << std::endl;
	std::cerr << "tgt_name: " << tgt_name << std::endl;
	std::string src_file_name = src_name.substr(src_name.find_last_of("/") + 1, src_name.find_last_of(".") - src_name.find_last_of("/") - 1);
	std::string tgt_file_name = tgt_name.substr(tgt_name.find_last_of("/") + 1, tgt_name.find_last_of(".") - tgt_name.find_last_of("/") - 1);
	cv::Mat src_rgb = cv::imread(src_name);
	cv::Mat tgt_rgb = cv::imread(tgt_name);
	//cv::imshow("tgt_rgb", tgt_rgb);
	//cv::waitKey(0);
	// used in OpenCV3
	cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
	cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
	std::vector< cv::KeyPoint > src_kp, tgt_kp;
	detector->detect(src_rgb, src_kp);
	detector->detect(tgt_rgb, tgt_kp);
	// ¼ÆËãÃèÊö×Ó
	cv::Mat src_desp, tgt_desp;
	descriptor->compute(src_rgb, src_kp, src_desp);
	descriptor->compute(tgt_rgb, tgt_kp, tgt_desp);
	// Æ¥ÅäÃèÊö×Ó
	std::vector< cv::DMatch > matches;
	cv::BFMatcher matcher;
	matcher.match(src_desp, tgt_desp, matches);
	cout << "Find total " << matches.size() << " matches." << endl;
	if (matches.size() < match_pnts_num_)
	{
    
    
		std::cerr << "match num is too much fewer..." << std::endl;
		return;
	}
	// ɸѡƥÅä¶Ô
	std:vector< cv::DMatch > goodMatches;
	double minDis = 9999;
	for (size_t i = 0; i < matches.size(); i++)
	{
    
    
		if (matches[i].distance < minDis)
			minDis = matches[i].distance;
	}
	for (size_t i = 0; i < matches.size(); i++)
	{
    
    
		if (matches[i].distance < 10 * minDis)
			goodMatches.push_back(matches[i]);
	}
	std::vector< cv::Point2f > src_pts, tgt_pts;
	for (size_t i = 0; i<goodMatches.size(); i++)
	{
    
    
		src_pts.push_back(src_kp[goodMatches[i].queryIdx].pt);
		tgt_pts.push_back(tgt_kp[goodMatches[i].trainIdx].pt);
	}
	//通过两个相机的位姿计算它们之间的e_matrix
	Eigen::Matrix4f src_pose = src_cam_model.cam_pose_;
	Eigen::Matrix4f tgt_pose = tgt_cam_model.cam_pose_;
	Eigen::Matrix4f src_extr = src_pose.inverse();
	Eigen::Matrix4f tgt_extr = tgt_pose.inverse();
	Eigen::Matrix4f relative_extr = tgt_extr*src_pose;
	Eigen::Matrix3f r_extr = relative_extr.topLeftCorner(3, 3);
	Eigen::Vector3f t_extr = relative_extr.topRightCorner(3, 1);
	//
	Eigen::Matrix3f em = ComputeCrossMatrixFromLVector(t_extr)*r_extr;
	//已知e_matrix，计算fundamental_matrix
	Eigen::Matrix3f src_km = Eigen::Matrix3f::Identity();
	Eigen::Matrix3f tgt_km = Eigen::Matrix3f::Identity();	
	src_km(0, 0) = src_cam_model.fx_;
	src_km(1, 1) = src_cam_model.fy_;
	src_km(0, 2) = src_cam_model.cx_;
	src_km(1, 2) = src_cam_model.cy_;
	tgt_km(0, 0) = src_cam_model.fx_;
	tgt_km(1, 1) = src_cam_model.fy_;
	tgt_km(0, 2) = src_cam_model.cx_;
	tgt_km(1, 2) = src_cam_model.cy_;
	Eigen::Matrix3f fm = (tgt_km.inverse()).transpose()*em*src_km.inverse();
	std::cerr << "fm: \n" << fm << std::endl;
	Eigen::Matrix3f fmt = fm;
	//²âÊÔ
	cv::Mat fundm = (cv::Mat_<float>(3, 3)
		<< fmt(0, 0), fmt(0, 1), fmt(0, 2),
		fmt(1, 0), fmt(1, 1), fmt(1, 2),
		fmt(2, 0), fmt(2, 1), fmt(2, 2));
	std::cerr << fundm << std::endl;
	std::vector<cv::Vec<float, 3>> epilines1, epilines2;
	computeCorrespondEpilines(src_pts, 1, fundm, epilines1);
	computeCorrespondEpilines(tgt_pts, 2, fundm, epilines2);
	cv::RNG &rng = cv::theRNG();
	for (int i = 0; i < match_pnts_num_; ++i) {
    
    
		//Ëæ»ú²úÉúÑÕÉ«
		std::cerr << "epilines1: " << i << ": " << epilines1[i] << std::endl;
		std::cerr << "epilines2: " << i << ": " << epilines2[i] << std::endl;
		cv::Scalar color = cv::Scalar(rng(255), rng(255), rng(255));
		circle(src_rgb, src_pts[i], 5, color, 3);
		//»æÖÆÍ⼫ÏßµÄʱºò£¬Ñ¡ÔñÁ½¸öµã£¬Ò»¸öÊÇx=0´¦µÄµã£¬Ò»¸öÊÇxΪͼƬ¿í¶È´¦
		line(src_rgb, cv::Point(0, -epilines2[i][2] / epilines2[i][1]), cv::Point(src_rgb.cols, -(epilines2[i][2] + epilines2[i][0] * src_rgb.cols) / epilines2[i][1]), color);
		circle(tgt_rgb, tgt_pts[i], 5, color, 3);
		line(tgt_rgb, cv::Point(0, -epilines1[i][2] / epilines1[i][1]), cv::Point(tgt_rgb.cols, -(epilines1[i][2] + epilines1[i][0] * tgt_rgb.cols) / epilines1[i][1]), color);
	}
	std::string abs_src_file_name = dir + "/" + src_file_name + ".png";
	std::string abs_tgt_file_name = dir + "/" + tgt_file_name + ".png";
	std::cerr << "abs_src_file_name: " << abs_src_file_name << std::endl;
	std::cerr << "abs_tgt_file_name: " << abs_tgt_file_name << std::endl;
	//std::system("pause");
	//cv::imshow("epiline1", tgt_rgb);
	//cv::waitKey(0);
	//std::cerr << "tgt_rgb rows, cols: " << tgt_rgb.rows << ", " << tgt_rgb.cols << std::endl;
	//std::system("pause");
	cv::imwrite(abs_tgt_file_name, tgt_rgb);
	//std::system("pause");
	//cv::imshow("epiline2", src_rgb);
	cv::imwrite(abs_src_file_name, src_rgb);
	cv::waitKey(0);
}

The test results are as follows: