OpenCV4每日一练day11：单目位姿估计

一.单目位姿估计

  根据相机成像模型，如果已知相机的内参矩阵、世界坐标系中若干空间点的三维坐标和空间点在图像中投影的二维坐标，那么可以计算出世界坐标系到相机坐标系的旋转向量和平移向量。如图所示，当知道点ci在世界坐标系下的三维坐标和这些点在图像中对应点的二维坐标时，结合相机的内参矩阵和畸变系数，就可以计算出世界坐标系变换到相机坐标系的旋转向量和平移向量。
  在这种情况下，可以估计相机在世界坐标系中的位姿。如果将世界坐标系看成前一时刻的相机坐标系姿态，ci在世界坐标系下的三维坐标看成ci在前一时刻相机坐标系中的坐标，就可以估计出前一时刻到当前时刻相机的运动变化，进而得到视觉里程计信息。不过需要注意的是，由于单目相机没有深度信息，因此，如果ci的三维坐标是真实物理尺度的三维坐标，那么估计出的平移向量就是真实的物理尺度，否则就是放缩后的平移向量。
  从理论上来说，只要知道世界坐标系中3个点的三维坐标和对应图像中的坐标，根据相机内参矩阵和畸变系数就可以解算世界坐标系与相机坐标系之间的转换关系。这种利用3个空间点和图像点的解算方法称为P3P方法。当然，如果点数大于3，那么可以得到更加精确的旋转向量和平移向量。当点数大于3时，计算旋转向量和平移向量的方法称为PnP方法。两种方法在OpenCV4中都有相应的函数, P3P方法对应的是solveP3P( )函数, PnP方法对应的是solvePnP( )函数。由于solvePnP( )函数包含了solveP3P( )函数的功能，因此这里只介绍solvePnP( )函数的使用方法。

原理：根据图像的情况反推出相机的运动情况
  当前时刻相机坐标系下空间三维点的坐标(如果获得的是世界坐标系下的坐标，需要先将坐标转换到相机坐标系下)(此时刻得到的是3D坐标)，移动后，另一个时刻，同样的空间三维点的像素坐标(这一时刻得到的是2D坐标)。如果相机连续运动，那么依次估计下去可以得到相机在整个空间中的运动状态，就实现了运动轨迹的估计，即一个视觉里程计。这里得到是旋转是真实的，但平移的非真实的。
  若将初始相机坐标系(即第一时刻的相机坐标系)看作是世界坐标系，此时计算得到的R和t就是相机标定时得到的外参矩阵。
附：当前时刻相机坐标系下空间三维点的坐标可以通过两帧图像之间的三角化获得。

二.OpenCV提供的函数

三.单目位姿估计(这里就是外参估计)

left01.jpg:


运行结果：

《OpenCV4快速入门》中的运行结果：

附上例代码：

#include <opencv2\opencv.hpp>
#include <iostream>
#include <vector>

using namespace std;
using namespace cv;

int main()
{
    
    
	//读取所有图像
	Mat img = imread("left01.jpg");//这张图像中有标定板，方便检测内角点的坐标，由于标定板的存在，方便人为地选取一个世界坐标系
	Mat gray;
	cvtColor(img, gray, COLOR_BGR2GRAY);
	vector<Point2f> imgPoints;
	Size boardSize = Size(9, 6);
	findChessboardCorners(gray, boardSize, imgPoints);   //计算方格标定板角点
	find4QuadCornerSubpix(gray, imgPoints, Size(5, 5));  //细化方格标定板角点坐标

														 //生成棋盘格每个内角点的空间三维坐标
	Size squareSize = Size(10, 10);  //棋盘格每个方格的真实尺寸
	vector<Point3f> PointSets;
	for (int j = 0; j < boardSize.height; j++)
	{
    
    
		for (int k = 0; k < boardSize.width; k++)
		{
    
    
			Point3f realPoint;
			// 假设标定板为世界坐标系的z平面，即z=0
			realPoint.x = j*squareSize.width;
			realPoint.y = k*squareSize.height;
			realPoint.z = 0;
			PointSets.push_back(realPoint);
		}
	}

	//输入前文计算得到的内参矩阵和畸变矩阵
	Mat cameraMatrix = (Mat_<float>(3, 3) << 532.016297, 0, 332.172519,
		0, 531.565159, 233.388075,
		0, 0, 1);
	Mat distCoeffs = (Mat_<float>(1, 5) << -0.285188, 0.080097, 0.001274,
		-0.002415, 0.106579);

	//用PnP算法计算旋转和平移量
	Mat rvec, tvec;
	solvePnP(PointSets, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);
	cout << "世界坐标系变换到相机坐标系的旋转向量：" << rvec << endl;
	//旋转向量转换旋转矩阵
	Mat R;
	Rodrigues(rvec, R);
	cout << "旋转向量转换成旋转矩阵：" << endl << R << endl;
	Mat T = tvec;
	cout << "平移矩阵：" << endl << T << endl;

	//用PnP+Ransac算法计算旋转向量和平移向量
	Mat rvecRansac, tvecRansac;
	solvePnPRansac(PointSets, imgPoints, cameraMatrix, distCoeffs, rvecRansac, tvecRansac);
	Mat RRansac;
	Rodrigues(rvecRansac, RRansac);
	cout << "旋转向量转换成旋转矩阵：" << endl << RRansac << endl;
	waitKey(0);
	return 0;
}