三维重建（10）之由世界坐标反推图像坐标

三维重建（10）之由世界坐标反推图像坐标

世界坐标系、相机坐标系、像平面坐标系、图像坐标系 -> 可参考：三维重建（2）之相机成像+单应性变换+相机标定+立体校正
——————
简介：如果已知世界坐标中的的一组期望坐标点，想要得到其在相机参数(内参与外参)下的对应图像的坐标，那么需要从相机标定过程中推理出 3D-2D的映射关系

1.最小二乘求解单应性矩阵

单应性矩阵指的是由三维世界坐标到二维相机坐标的映射关系，推理过程如下图所示;

2.由世界坐标推理图像坐标

直接点，原理如下图所示：

3.Opencv函数实现

3.实现过程

• 最小二成法求解单应性矩阵的实现在此就不列了，因为还没有着手去实现，等后边如果用到的话再来补充一下
• 下边给出一下由三维世界坐标计算得到二维坐标的中间过程实现：

将三维坐标点转为矩阵形式

cv::Mat point2mat(const std::vector<Point3f>points)
{
	int nums = points.size();
	cv::Mat res(cv::Size(nums, 4), CV_64FC1,cv::Scalar::all(0));
	for (int i = 0; i < nums; ++i)
	{
		res.at<double>(0, i) = points[i].x;
		res.at<double>(1, i) = points[i].y;
		res.at<double>(2, i) = points[i].z;
		res.at<double>(3, i) = 1.;
	}
	return res;
}

由坐标矩阵转为二位点集格式

std::vector<cv::Point2f> Calibration::mat2point2f(const cv::Mat& mat)
{
	CV_Assert(!mat.empty());
	CV_Assert(mat.type() == CV_64FC1);
	std::vector<cv::Point2f>points(mat.cols);
	for (int i = 0; i < mat.cols; ++i)
	{
		cv::Point2f p = cv::Point2f(mat.at<double>(0, i), mat.at<double>(1, i));
		points[i] = p;
	}
	return points;
}

反推相机坐标

/*
*@para ps:一组世界坐标
*@para return:双目相机对应图像的图像坐标
*/
std::vector<vector<cv::Point2f>>get_rect3(const std::vector<cv::Point3f>ps)
{
	std::vector<vector<cv::Point2f>>points(2);
	cv::Mat m1, m2, m3, m4, m5;
	//读取相机内参
	m1 = cal.point2mat(ps);
	cv::Mat L_in = cameraMatrix1; //相机内参
	cv::Mat R_in = cameraMatrix2;
	cv::Mat Rc = camera.R; //左右相机的旋转和平移矩阵
	cv::Mat tc = camera.T;

	//1.转左相机坐标系，求解做相机坐标
	m2 = trans;  //世界坐标系到相机坐标系的变换矩阵
	m3 = m2 * m1; //转像机坐标系
	m4 = m3(Rect(0, 0, m3.cols, m3.rows - 1)).clone();
	m5 = L_in * m4;
	m5.row(0) /= m5.row(2);
	m5.row(1) /= m5.row(2);
	points[0] = cal.mat2point2f(m5);

	//2.转右相机坐标系
	cv::Mat m6, m7, m8;
	hconcat(Rc, tc, m6);
	m7 = m6 * m3;
	m8 = R_in * m7;
	m8.row(0) /= m8.row(2);
	m8.row(1) /= m8.row(2);
	points[1] = cal.mat2point2f(m8);

	int row = 1536;
	int col = 2048;
	for (int i = 0; i < points.size(); ++i)
	{
		for (int j = 0; j< points[i].size(); ++j)
		{
		    //判断是否有越界的点，如果有，将其规范在图像范围内
			points[i][j].x = points[i][j].x < 0 ? 0 : points[i][j].x;
			points[i][j].x = points[i][j].x > col ? col : points[i][j].x;
			points[i][j].y = points[i][j].y < 0 ? 0 : points[i][j].y;
			points[i][j].y = points[i][j].y > row ? row : points[i][j].y;
		}
	}
	
	return points;
}

打印

void print_p(std::vector<cv::Point2f>& points)
{
	std::cout << "cout:" << std::endl;
	for (int i = 0; i < points.size(); ++i)
	{
		std::cout << points[i] << std::endl;
	}
}