rm记录第二阶段（三）：AngleSolver，pnp

本文链接：https://blog.csdn.net/weixin_46421489/article/details/113756183

本文探讨了在相机应用中如何处理不同相机参数，重点关注AngleSolver和pnp（Pose from Point correspondences）算法。内容包括相机内参矩阵、畸变系数、世界与图像坐标点对应关系，以及解决旋转和平移向量的方法，用于计算目标距离和角度补偿。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

这里开始就要涉及到一些相机的东西了，我这里因为相机的不一样，所以我肯定得改代码的，我打算先弄成一个视频，一步步来吧！

这里还是从.h文件看起，然后先看属性：

前两行是相机的参数，我们在前面已经得到了，然后定义两个世界坐标点，子弹的速度，枪口和相机的距离，目标的信息,还有一些结算的东西一会看

private:

	//Camera params
	Mat CAMERA_MATRIX;    //IntrinsicMatrix		  fx,fy,cx,cy
	Mat DISTORTION_COEFF; //DistortionCoefficients k1,k2,p1,p2

	//Object points in world coordinate
	vector<Point3f> SMALL_ARMOR_POINTS_3D;
	vector<Point3f> BIG_ARMOR_POINTS_3D;

	//speed of bullet (compensation for gravity and air fru)
	double BULLET_SPEED;

	//distance between camera and barrel in y axis(positive when camera is under barrel)  barrel_y = camera_y + barrel_camera_y
	double GUN_CAM_DISTANCE_Y;

	//Targets
	vector<Point2f> targetContour;
	Point2f targetCenter;
	ArmorType targetType;

	// calculated by solvePnP
	//s[R|t]=s'  s->world coordinate;s`->camera coordinate
	Mat rVec;    //rot rotation between camera and target center
	Mat tVec;  //trans tanslation between camera and target center

	//Results
	float y_yaw;
	float x_pitch;
	double distance;

然后看方法，首先肯定是对参数的操作呀！

第一个没啥，第二个其实我们在前面也看到过，就是对xml文件读取，然后这个得看后面的xml是什么样的！

void AngleSolver::setCameraParam(const cv::Mat & camera_matrix, const cv::Mat & distortion_coeff)
{
	camera_matrix.copyTo(CAMERA_MATRIX);
	distortion_coeff.copyTo(DISTORTION_COEFF);
}

int AngleSolver::setCameraParam(const char * filePath, int camId)
{
	FileStorage fsRead;
	fsRead.open(filePath, FileStorage::READ);
	if (!fsRead.isOpened())
	{
		cout << "Failed to open xml" << endl;
		return -1;
	}

	fsRead["Y_DISTANCE_BETWEEN_GUN_AND_CAM"] >> GUN_CAM_DISTANCE_Y;

	Mat camera_matrix;
	Mat distortion_coeff;
	switch (camId)
	{
	case 1:
		fsRead["CAMERA_MATRIX_1"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_1"] >> distortion_coeff;
		break;
	case 2:
        fsRead["CAMERA_MATRIX_2"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_2"] >> distortion_coeff;
		break;
	case 3:
        fsRead["CAMERA_MATRIX_3"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_3"] >> distortion_coeff;
		break;
	default:
		cout << "WRONG CAMID GIVEN!" << endl;
		break;
	}
	setCameraParam(camera_matrix, distortion_co