相机投影过程（码—基于Opencv）

理论知识参考于基于图像的三维重建-针孔相机模型

#include<iostream>
#include<opencv2/imgproc/types_c.h>

class Camera
{
public:
	//构造函数
	Camera()
	{
		//采用归一化坐标，不考虑图像尺寸
		c_[0] = c_[1] = 0.0;
	}

	//相机投影过程
	cv::Point2d projection(cv::Point3d const &p3d)
	{
		cv::Point2d p;

		//世界坐标系到相机坐标系
		double xc = R_[0] * p3d.x + R_[1] * p3d.y + R_[2] * p3d.z + t_[0];
		double yc = R_[3] * p3d.x + R_[4] * p3d.y + R_[5] * p3d.z + t_[1];
		double zc = R_[6] * p3d.x + R_[7] * p3d.y + R_[8] * p3d.z + t_[2];

		//相机坐标系到像平面
		double x = xc / zc;
		double y = yc / zc;

		//径向畸变过程
		double r2 = x * x + y * y;
		double distort_ratio = 1 + dist_[0] * r2 + dist_[1] * r2*r2;

		//图像坐标系到屏幕坐标系
		p.x = f_ * distort_ratio*x + c_[0];
		p.y = f_ * distort_ratio*y + c_[1];

		return p;
	}

	//相机在世界坐标系中的位置 -R^T*t
	cv::Point3d pos_in_world()
	{
		cv::Point3d pos;
		pos.x = R_[0] * t_[0] + R_[3] * t_[1] + R_[6] * t_[2];
		pos.y = R_[1] * t_[0] + R_[4] * t_[1] + R_[7] * t_[2];
		pos.z = R_[2] * t_[0] + R_[5] * t_[1] + R_[8] * t_[2];

		return -pos;
	}

	//相机在世界坐标系中的方向
	cv::Point3d dir_in_world()
	{
		cv::Point3d dir(R_[6], R_[7], R_[8]);
		return dir;
	}

public:
	//焦距f
	double f_;

	//径向畸变系数k1,k2
	double dist_[2];

	//中心点坐标u0,v0;
	double c_[2];

	//旋转矩阵
	double R_[9];

	//平移向量
	double t_[3];
};

int main()
{
	Camera cam;
	 //旋转矩阵
	cam.R_[0] = 0.999796; cam.R_[1] = -0.0127375;  cam.R_[2] = 0.0156807;
	cam.R_[3] = 0.0128557; cam.R_[4] = 0.999894;  cam.R_[5] = -0.0073718;
	cam.R_[6] = -0.0155846; cam.R_[7] = 0.00757181; cam.R_[8] = 0.999854;

	//焦距
	cam.f_ = 0.920227;

	// 径向畸变系数
	cam.dist_[0] = -0.106599; cam.dist_[1] = 0.104385;

	// 平移向量
	cam.t_[0] = 0.0814358; cam.t_[1] = 0.937498;   cam.t_[2] = -0.0887441;



	// 三维点坐标
	cv::Point3d p3d = { 1.36939, -1.17123, 7.04869 };

	/*计算相机的投影点*/
	cv::Point2d p2d = cam.projection(p3d);
	std::cout << "projection coord:\n " << p2d << std::endl;
	std::cout << "result should be:\n 0.208188 -0.035398\n\n";

	/*计算相机在世界坐标系中的位置*/
	cv::Point3d pos = cam.pos_in_world();
	std::cout << "cam position in world is:\n " << pos << std::endl;
	std::cout << "result should be: \n -0.0948544 -0.935689 0.0943652\n\n";

	/*计算相机在世界坐标系中的方向*/
	cv::Point3d dir = cam.dir_in_world();
	std::cout << "cam direction in world is:\n " << dir << std::endl;
	std::cout << "result should be: \n -0.0155846 0.00757181 0.999854\n";

	return 0;
}