关键词：OpenCV::solvePnP

文章类型：方法封装、测试

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-27

@Lab: CvLab202@CSU

今天给大家带来的是一篇关于程序功能、性能测试的文章，读过《相机位姿估计1：根据四个特征点估计相机姿态》一文的同学应该会发现，直接使用OpenCV的solvePnP来估计相机位姿，在程序调用上相当麻烦，从一开始的参数设定到最后将计算出的矩阵转化为相机的位姿参数，需要花费近两百行代码。因此为了更方便地调用程序，今天我就给大家带来一个我自己对solvePnP的封装类PNPSolver，顺便将OpenCV自带的三种求解方法测试一遍。

封装的思路我就不写了，由于博客更新速度赶不上我写程序的速度，现在发上来的类已经修改过好几次了，思路也换了几次。不过大的方向没变，目的就是只需要输入参数，输入坐标点后直接可以得到相机在世界坐标系的坐标。

类的调用顺序：

1.初始化PNPSolver类；

2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数；

3.向Points3D，Points2D中添加一一对应的特征点对；

4.调用Solve()方法运行计算；

5.从属性Theta_C2W中提取旋转角，从Position_OcInW中提取出相机在世界坐标系下的坐标。

以下是类体：

PNPSolver.h

  1. #pragma once
  2. #include <opencv2\opencv.hpp>
  3. #include <math.h>
  4. #include <iostream>
  5. #include <fstream>
  6. using namespace std;
  7.  
  8.  
  9. // 本类用于快速解决PNP问题，顺带解决空间绕轴旋转以及图像系、相机系、世界系三系坐标投影问题
  10. // 默认使用Gao的P3P+重投影法，要求输入4个特征点
  11. // 调用顺序：
  12. // 1.初始化本类
  13. // 2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数
  14. // 3.向Points3D，Points2D中添加一一对应的特征点对
  15. // 4.调用Solve()方法运行计算
  16. // 5.从RoteM, TransM, W2CTheta等属性中提出结果
  17. //
  18. // 原理参见：http://www.cnblogs.com/singlex/category/911880.html
  19. // Author：VShawn
  20. // Ver:2016.11.25.0
  21. class PNPSolver
  22. {
  23. public:
  24. 	PNPSolver();
  25. 	//带参数初始化
  26. 	PNPSolver(double fx, double fy, double u0, double v0, double k_1, double  k_2, double  p_1, double  p_2, double k_3);
  27. 	~PNPSolver();
  28.  
  29. 	enum METHOD
  30. 	{
  31. 		CV_ITERATIVE = CV_ITERATIVE,
  32. 		CV_P3P = CV_P3P,
  33. 		CV_EPNP = CV_EPNP
  34. 	};
  35.  
  36. 	/***********************位姿估计所用特征点**************************/
  37. 	vector<cv::Point3f> Points3D;//存储四个点的世界坐标
  38. 	vector<cv::Point2f> Points2D;//存储四个点的图像坐标
  39.  
  40. 	/***********************位姿估计结果**************************/
  41. 	//最后求出的旋转矩阵与平移矩阵
  42. 	cv::Mat RoteM, TransM;
  43. 	//世界系到相机系的三轴旋转欧拉角，世界系照此旋转后可以与相机坐标系完全平行。
  44. 	//旋转顺序为x、y、z
  45. 	cv::Point3f Theta_W2C;
  46. 	//相机系到世界系的三轴旋转欧拉角，相机坐标系照此旋转后可以与世界坐标系完全平行。
  47. 	//旋转顺序为z、y、x
  48. 	cv::Point3f Theta_C2W;
  49. 	//相机坐标系中，世界坐标系原点Ow的坐标
  50. 	cv::Point3f Position_OwInC;
  51. 	//世界坐标系中，相机坐标系原点Oc的坐标
  52. 	cv::Point3f Position_OcInW;
  53.  
  54.  
  55. 	/*********************公有方法*****************************/
  56.  
  57. 	//解PNP问题，获得位姿信息
  58. 	//调用后在RoteM, TransM, W2CTheta等属性中提取计算结果，属性说明参见注释
  59. 	//输出参数：CV_ITERATIVE，CV_P3P（默认），CV_EPNP，具体参见Opencv documentation.
  60. 	//实测
  61. 	//CV_ITERATIVE迭代法似乎只能用4个共面特征点求解，5个点或非共面4点解不出正确的解
  62. 	//CV_P3P的Gao的方法可以使用任意四个特征点，特征点数量不能少于4也不能多于4
  63. 	//CV_EPNP方法可以实现特征点数>=4的问题求解，不需要4点共面
  64. 	//返回值：
  65. 	//0正确
  66. 	//-1相机内参数或畸变参数未设置
  67. 	//-2未提供足够的特征点，或特征点数目不匹配
  68. 	//-3输入的点数据有误，详见printf信息
  69. 	int Solve(METHOD method = METHOD::CV_P3P);
  70. 	
  71. 	//根据计算出的结果将世界坐标重投影到图像，返回像素坐标点集
  72. 	//使用前需要先用Solve()解出相机位姿
  73. 	//输入为世界坐标系的点坐标集合
  74. 	//输出为点投影到图像上的图像坐标集合
  75. 	vector<cv::Point2f> WordFrame2ImageFrame(vector<cv::Point3f> WorldPoints);
  76.  
  77.  
  78.  
  79. 	//根据输入的参数将图像坐标转换到相机坐标中
  80. 	//使用前需要先用Solve()解出相机位姿
  81. 	//输入为图像上的点坐标
  82. 	//double F为镜头焦距
  83. 	//输出为点在焦距=F时的相机坐标系坐标
  84. 	cv::Point3f ImageFrame2CameraFrame(cv::Point2f p, double F);
  85.  
  86.  
  87.  
  88.  
  89. 	//设置相机内参数矩阵
  90. 	void SetCameraMatrix(double fx, double fy, double u0, double v0)
  91. 	{
  92. 		camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
  93. 		camera_matrix.ptr<double>(0)[0] = fx;
  94. 		camera_matrix.ptr<double>(0)[2] = u0;
  95. 		camera_matrix.ptr<double>(1)[1] = fy;
  96. 		camera_matrix.ptr<double>(1)[2] = v0;
  97. 		camera_matrix.ptr<double>(2)[2] = 1.0f;
  98. 	}
  99. 	//设置畸变系数矩阵
  100. 	void SetDistortionCoefficients(double k_1, double  k_2, double  p_1, double  p_2, double k_3)
  101. 	{
  102. 		distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
  103. 		distortion_coefficients.ptr<double>(0)[0] = k_1;
  104. 		distortion_coefficients.ptr<double>(1)[0] = k_2;
  105. 		distortion_coefficients.ptr<double>(2)[0] = p_1;
  106. 		distortion_coefficients.ptr<double>(3)[0] = p_2;
  107. 		distortion_coefficients.ptr<double>(4)[0] = k_3;
  108. 	}
  109.  
  110.  
  111.  
  112.  
  113.  
  114.  
  115.  
  116.  
  117.  
  118.  
  119.  
  120.  
  121. 	/********************公有静态方法*********************/
  122. 	//点绕任意向量旋转，右手系
  123. 	static cv::Point3f RotateByVector(double old_x, double old_y, double old_z, double vx, double vy, double vz, double theta)
  124. 	{
  125. 		double r = theta * CV_PI / 180;
  126. 		double c = cos(r);
  127. 		double s = sin(r);
  128. 		double new_x = (vx*vx*(1 - c) + c) * old_x + (vx*vy*(1 - c) - vz*s) * old_y + (vx*vz*(1 - c) + vy*s) * old_z;
  129. 		double new_y = (vy*vx*(1 - c) + vz*s) * old_x + (vy*vy*(1 - c) + c) * old_y + (vy*vz*(1 - c) - vx*s) * old_z;
  130. 		double new_z = (vx*vz*(1 - c) - vy*s) * old_x + (vy*vz*(1 - c) + vx*s) * old_y + (vz*vz*(1 - c) + c) * old_z;
  131. 		return cv::Point3f(new_x, new_y, new_z);
  132. 	}
  133.  
  134. 	//将空间点绕Z轴旋转
  135. 	//输入参数 x y为空间点原始x y坐标
  136. 	//thetaz为空间点绕Z轴旋转多少度，角度制范围在-180到180
  137. 	//outx outy为旋转后的结果坐标
  138. 	static void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
  139. 	{
  140. 		double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
  141. 		double y1 = y;
  142. 		double rz = thetaz * CV_PI / 180;
  143. 		outx = cos(rz) * x1 - sin(rz) * y1;
  144. 		outy = sin(rz) * x1 + cos(rz) * y1;
  145. 	}
  146.  
  147. 	//将空间点绕Y轴旋转
  148. 	//输入参数 x z为空间点原始x z坐标
  149. 	//thetay为空间点绕Y轴旋转多少度，角度制范围在-180到180
  150. 	//outx outz为旋转后的结果坐标
  151. 	static void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
  152. 	{
  153. 		double x1 = x;
  154. 		double z1 = z;
  155. 		double ry = thetay * CV_PI / 180;
  156. 		outx = cos(ry) * x1 + sin(ry) * z1;
  157. 		outz = cos(ry) * z1 - sin(ry) * x1;
  158. 	}
  159.  
  160. 	//将空间点绕X轴旋转
  161. 	//输入参数 y z为空间点原始y z坐标
  162. 	//thetax为空间点绕X轴旋转多少度，角度制，范围在-180到180
  163. 	//outy outz为旋转后的结果坐标
  164. 	static void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
  165. 	{
  166. 		double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确
  167. 		double z1 = z;
  168. 		double rx = thetax * CV_PI / 180;
  169. 		outy = cos(rx) * y1 - sin(rx) * z1;
  170. 		outz = cos(rx) * z1 + sin(rx) * y1;
  171. 	}
  172. private:
  173.  
  174. 	cv::Mat camera_matrix;//内参数矩阵
  175. 	cv::Mat distortion_coefficients;//畸变系数
  176.  
  177. 	cv::Mat rvec;//解出来的旋转向量
  178. 	cv::Mat tvec;//解出来的平移向量
  179. };

PNPSolver.cpp

  1. #include "PNPSolver.h"
  2.  
  3.  
  4. // 本类用于快速解决PNP问题，顺带解决空间绕轴旋转以及图像系、相机系、世界系三系坐标投影问题
  5. // 调用顺序：
  6. // 1.初始化本类
  7. // 2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数
  8. // 3.向Points3D，Points2D中添加一一对应的特征点对
  9. // 4.调用Solve()方法运行计算
  10. // 5.从RoteM, TransM, W2CTheta等属性中提出结果
  11. //
  12. // 原理参见：http://www.cnblogs.com/singlex/category/911880.html
  13. // Author：VShawn
  14. // Ver:2016.11.26.0
  15. PNPSolver::PNPSolver()
  16. {
  17. 	//初始化输出矩阵
  18. 	vector<double> rv(3), tv(3);
  19. 	cv::Mat rvec(rv), tvec(tv);
  20. }
  21. PNPSolver::PNPSolver(double fx, double fy, double u0, double v0, double k_1, double  k_2, double  p_1, double  p_2, double k_3)
  22. {
  23. 	//初始化输出矩阵
  24. 	vector<double> rv(3), tv(3);
  25. 	cv::Mat rvec(rv), tvec(tv);
  26. 	SetCameraMatrix(fx, fy, u0, v0);
  27. 	SetDistortionCoefficients(k_1, k_2, p_1, p_2, k_3);
  28. }
  29.  
  30. PNPSolver::~PNPSolver()
  31. {
  32. }
  33.  
  34. int PNPSolver::Solve(METHOD method)
  35. {
  36. 	//数据校验
  37. 	if (camera_matrix.cols == 0 || distortion_coefficients.cols == 0)
  38. 	{
  39. 		printf("ErrCode:-1,相机内参数或畸变参数未设置！\r\n");
  40. 		return -1;
  41. 	}
  42.  
  43. 	if (Points3D.size() != Points2D.size())
  44. 	{
  45. 		printf("ErrCode:-2，3D点数量与2D点数量不一致！\r\n");
  46. 		return -2;
  47. 	}
  48. 	if (method == METHOD::CV_P3P || method == METHOD::CV_ITERATIVE)
  49. 	{
  50. 		if (Points3D.size() != 4)
  51. 		{
  52. 			printf("ErrCode:-2,使用CV_ITERATIVE或CV_P3P方法时输入的特征点数量应为4！\r\n");
  53. 			return -2;
  54. 		}
  55. 	}
  56. 	else
  57. 	{
  58. 		if (Points3D.size() < 4)
  59. 		{
  60. 			printf("ErrCode:-2,输入的特征点数量应大于4！\r\n");
  61. 			return -2;
  62. 		}
  63. 	}
  64.  
  65. 	////TODO::检验是否是共面的四点
  66. 	//if ((method == METHOD::CV_ITERATIVE || method == METHOD::CV_EPNP) && Points2D.size() == 4)
  67. 	//{
  68. 	//	//通过向量两两叉乘获得法向量，看法向量是否平行
  69. 	//}
  70.  
  71.  
  72.  
  73.  
  74.  
  75.  
  76. 	/*******************解决PNP问题*********************/
  77. 	//有三种方法求解
  78. 	solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, method);	//实测迭代法似乎只能用共面特征点求位置
  79. 	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE);	//实测迭代法似乎只能用共面特征点求位置
  80. 	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P);		//Gao的方法可以使用任意四个特征点
  81. 	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);
  82.  
  83.  
  84. 	/*******************提取旋转矩阵*********************/
  85. 	double rm[9];
  86. 	RoteM = cv::Mat(3, 3, CV_64FC1, rm);
  87. 	Rodrigues(rvec, RoteM);
  88. 	double r11 = RoteM.ptr<double>(0)[0];
  89. 	double r12 = RoteM.ptr<double>(0)[1];
  90. 	double r13 = RoteM.ptr<double>(0)[2];
  91. 	double r21 = RoteM.ptr<double>(1)[0];
  92. 	double r22 = RoteM.ptr<double>(1)[1];
  93. 	double r23 = RoteM.ptr<double>(1)[2];
  94. 	double r31 = RoteM.ptr<double>(2)[0];
  95. 	double r32 = RoteM.ptr<double>(2)[1];
  96. 	double r33 = RoteM.ptr<double>(2)[2];
  97. 	TransM = tvec;
  98.  
  99. 	//计算出相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。
  100. 	//旋转顺序为z、y、x
  101. 	double thetaz = atan2(r21, r11) / CV_PI * 180;
  102. 	double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
  103. 	double thetax = atan2(r32, r33) / CV_PI * 180;
  104.  
  105. 	//相机系到世界系的三轴旋转欧拉角，相机坐标系照此旋转后可以与世界坐标系完全平行。
  106. 	//旋转顺序为z、y、x
  107. 	Theta_C2W.z = thetaz;
  108. 	Theta_C2W.y = thetay;
  109. 	Theta_C2W.x = thetax;
  110.  
  111. 	//计算出世界系到相机系的三轴旋转欧拉角，世界系照此旋转后可以转出相机坐标系。
  112. 	//旋转顺序为x、y、z
  113. 	Theta_W2C.x = -1 * thetax;
  114. 	Theta_W2C.y = -1 * thetay;
  115. 	Theta_W2C.z = -1 * thetaz;
  116. 	
  117.  
  118. 	/*************************************此处计算出相机坐标系原点Oc在世界坐标系中的位置**********************************************/
  119.  
  120. 	/***********************************************************************************/
  121. 	/* 当原始坐标系经过旋转z、y、x三次旋转后，与世界坐标系平行，向量OcOw会跟着旋转 */
  122. 	/* 而我们想知道的是两个坐标系完全平行时，OcOw的值 */
  123. 	/* 因此，原始坐标系每次旋转完成后，对向量OcOw进行一次反相旋转，最终可以得到两个坐标系完全平行时的OcOw */
  124. 	/* 该向量乘以-1就是世界坐标系下相机的坐标 */
  125. 	/***********************************************************************************/
  126.  
  127. 	//提出平移矩阵，表示从相机坐标系原点，跟着向量(x,y,z)走，就到了世界坐标系原点
  128. 	double tx = tvec.ptr<double>(0)[0];
  129. 	double ty = tvec.ptr<double>(0)[1];
  130. 	double tz = tvec.ptr<double>(0)[2];
  131.  
  132. 	//x y z 为唯一向量在相机原始坐标系下的向量值
  133. 	//也就是向量OcOw在相机坐标系下的值
  134. 	double x = tx, y = ty, z = tz;
  135. 	Position_OwInC.x = x;
  136. 	Position_OwInC.y = y;
  137. 	Position_OwInC.z = z;
  138. 	//进行三次反向旋转
  139. 	CodeRotateByZ(x, y, -1 * thetaz, x, y);
  140. 	CodeRotateByY(x, z, -1 * thetay, x, z);
  141. 	CodeRotateByX(y, z, -1 * thetax, y, z);
  142.  
  143.  
  144. 	//获得相机在世界坐标系下的位置坐标
  145. 	//即向量OcOw在世界坐标系下的值
  146. 	Position_OcInW.x = x*-1;
  147. 	Position_OcInW.y = y*-1;
  148. 	Position_OcInW.z = z*-1;
  149.  
  150. 	return 0;
  151. }
  152.  
  153.  
  154. //根据计算出的结果将世界坐标重投影到图像，返回像素坐标点集
  155. //输入为世界坐标系的点坐标集合
  156. //输出为点投影到图像上的图像坐标集合
  157. vector<cv::Point2f> PNPSolver::WordFrame2ImageFrame(vector<cv::Point3f> WorldPoints)
  158. {
  159. 	vector<cv::Point2f> projectedPoints;
  160. 	cv::projectPoints(WorldPoints, rvec, tvec, camera_matrix, distortion_coefficients, projectedPoints);
  161. 	return projectedPoints;
  162. }
  163.  
  164.  
  165.  
  166. //根据输入的参数将图像坐标转换到相机坐标中
  167. //使用前需要先用Solve()解出相机位姿
  168. //输入为图像上的点坐标
  169. //double F为镜头焦距
  170. //输出为点在焦距=F时的相机坐标系坐标
  171. cv::Point3f PNPSolver::ImageFrame2CameraFrame(cv::Point2f p, double F)
  172. {
  173. 	double fx;
  174. 	double fy; 
  175. 	double u0;
  176. 	double v0;
  177.  
  178. 	fx = camera_matrix.ptr<double>(0)[0];
  179. 	u0 = camera_matrix.ptr<double>(0)[2];
  180. 	fy = camera_matrix.ptr<double>(1)[1];
  181. 	v0 = camera_matrix.ptr<double>(1)[2];
  182. 	double zc = F;
  183. 	double xc = (p.x - u0)*F / fx;
  184. 	double yc = (p.y - v0)*F / fy;
  185. 	return cv::Point3f(xc, yc, zc);
  186. }

　　

一个典型的调用示例

  1. 	//初始化PNPSolver类
  2. 	PNPSolver p4psolver;
  3. 	//初始化相机参数
  4. 	p4psolver.SetCameraMatrix(fx, fy, u0, v0);
  5. 	//设置畸变参数
  6. 	p4psolver.SetDistortionCoefficients(k1, k2, p1, p2, k3);
  7. 　　　　 //设置特征点的世界坐标
  8. 	p4psolver.Points3D.push_back(cv::Point3f(0, 0, 0));		//P1三维坐标的单位是毫米
  9. 	p4psolver.Points3D.push_back(cv::Point3f(0, 200, 0));	//P2
  10. 	p4psolver.Points3D.push_back(cv::Point3f(150, 0, 0));	//P3
  11. 	//p4psolver.Points3D.push_back(cv::Point3f(150, 200, 0));	//P4
  12. 	p4psolver.Points3D.push_back(cv::Point3f(0, 100, 105));	//P5
  13.  
  14. 	cout << "test2:特征点世界坐标 = " << endl << p4psolver.Points3D << endl;
  15. 　　　　 //设置特征点的图像坐标
  16. 	p4psolver.Points2D.push_back(cv::Point2f(2985, 1688));	//P1
  17. 	p4psolver.Points2D.push_back(cv::Point2f(5081, 1690));	//P2
  18. 	p4psolver.Points2D.push_back(cv::Point2f(2997, 2797));	//P3
  19. 	//p4psolver.Points2D.push_back(cv::Point2f(5544, 2757));	//P4
  20. 	p4psolver.Points2D.push_back(cv::Point2f(4148, 673));	//P5
  21.  
  22. 	cout << "test2:图中特征点坐标 = " << endl << p4psolver.Points2D << endl;
  23.  
  24. 	if (p4psolver.Solve(PNPSolver::METHOD::CV_P3P) == 0)
  25. 		cout << "test2:CV_P3P方法:	相机位姿→" << "Oc坐标=" << p4psolver.Position_OcInW << "	相机旋转=" << p4psolver.Theta_W2C << endl;
  26. 	if (p4psolver.Solve(PNPSolver::METHOD::CV_ITERATIVE) == 0)
  27. 		cout << "test2:CV_ITERATIVE方法:	相机位姿→" << "Oc坐标=" << p4psolver.Position_OcInW << "	相机旋转=" << p4psolver.Theta_W2C << endl;
  28. 	if (p4psolver.Solve(PNPSolver::METHOD::CV_EPNP) == 0)
  29. 		cout << "test2:CV_EPNP方法:	相机位姿→" << "Oc坐标=" << p4psolver.Position_OcInW << "	相机旋转=" << p4psolver.Theta_W2C << endl;

OpenCV提供了三种方法进行PNP计算，三种方法具体怎么计算的就请各位自己查询opencv documentation以及相关的论文了，我看了个大概然后结合自己实际的测试情况给出一个结论，不一定正确，仅供参考：

方法名

说明

测试结论

CV_P3P

这个方法使用非常经典的Gao方法解P3P问题，求出4组可能的解，再通过对第四个点的重投影，返回重投影误差最小的点。

论文《Complete Solution Classification for the Perspective-Three-Point Problem》

可以使用任意4个特征点求解，不要共面，特征点数量不为4时报错

CV_ITERATIVE

该方法基于Levenberg-Marquardt optimization迭代求解PNP问题，实质是迭代求出重投影误差最小的解，这个解显然不一定是正解。

实测该方法只有用4个共面的特征点时才能求出正确的解，使用5个特征点或4点非共面的特征点都得不到正确的位姿。

 

只能用4个共面的特征点来解位姿

CV_EPNP

该方法使用EfficientPNP方法求解问题，具体怎么做的当时网速不好我没下载到论文，后面又懒得去看了。

论文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》

对于N个特征点，只要N>3就能够求出正解。

测试截图：

1.使用四个共面的特征点，显然三种方法都能得到正解，但相互之间略有误差。

2使用四个非共面的特征点，CV_ITERATIVE方法解错了。

3.使用5个特征点求解，只有CV_EPNP能够用

最后对三种方法的性能进行测试，通过对test1重复执行1000次获得算法的运行时间，从结果可以看出迭代法显然是最慢的，Gao的P3P+重投影法用时最少，EPNP法其次。

综合以上的测试，推荐使用CV_P3P来解决实际问题，该方法对于有四个特征点的情况限制少、运算速度快。当特征点数大于4时，可以取多组4特征点计算出结果再求平均值，或者为了简单点就直接使用CV_EPNP法。

不推荐使用CV_ITERATIVE方法。

 

 

 

程序下载地址：Github

版权声明：本文为singlex原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://www.cnblogs.com/singlex/p/pose_estimation_1_1.html