输入：匹配点的像素坐标；相机内参 输出：相机位姿R、t, 点在世界坐标系中的位置 说明：位姿是相对位姿，是当前帧相对于参考帧的旋转、平移量构成的变换。
求解思路： 三位场景中的同一个三维点在不同视角下的像点存在着一种约束关系：对极约束，基础矩阵E是这种约束关系的代数表示，并且这种约束关系独立与场景的结构，只依赖与相机的内参K和外参R、 t。 1、可以通过通过匹配的像点对计算出两幅图像的基础矩阵E， 2、然后分解基础矩阵得到相机的相对位姿R t。
P3P求解相机位姿 输入：3对3D-2D匹配点，1对用于验证的匹配点 记3D点为A、B、C，已知这三点在世界坐标系中的坐标； 记对应匹配的2D点为a、b、c，已知这三点在像素坐标系的坐标（也可以是相机坐标系下 的坐标，已知内参的情况下可以相互转换，不行的，深度也就是z不知道） 输出：相机位姿R、t 求解思路：通过匹配点，利用三角形相似原理，结合匹配点验证，得到像素坐标点在相机坐标系下的3D坐标，把问题转换3D-3D的位姿估计问题。 在SLAM实用：先P3P估计相机位姿R t， 之后构建最小二乘优化问题对估计值进行调整
//solvePnP求解 Mat r, t; solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法；求解出的位姿被送到r(旋转向量)、t（平移向量） Mat R; cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵 思考：怎么R、t转换成李群SO3、SE3的形式distortion coefficients为相机畸变参数，当该处输入为空时，默认没有发生畸变
用Ransac解决pnp问题，可以剔除一些误匹配的点，选择一定数量的内点去调整模型参数，而将一些无法满足模型的内点作为外局点剔除掉。 最小二乘法则是尽可能地将模型参数符合尽可能多的数据点 之后可以再考虑看看 Ransac理解参考: https://blog.csdn.net/robinhjwy/article/details/79174914
/** * 函数原型： * CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, double confidence = 0.99, OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE); * 参数： * objectPoints，要匹配的3d空间点数组 * objectPoints，要匹配的2d图像点数组 * cameraMatrix，相机内参矩阵 * distCoeffs，相机畸变矩阵 * rvec，旋转向量输出承接数组 * tvec，平移向量输出承接数组 * 后面的参数跟ransac算法有关。倒是都有默认值 * useExtrinsicGuess，迭代初始值是否定为提供的rvec和tvec值，这里没有提供，所以用false * iterationsCount，迭代次数，ransac算法所必须的迭代次数 * reprojectionError，重投影误差。ransac算法迭代时也必须要规定的误差阀值，来确定是否为内点。 * confidence，置信度。ransac算法每次用于更新迭代次数的参数。一般固定选为0.995 * inliers，内点输出承接数组 * flags, Method for solving a PnP problem。 */ cv::solvePnPRansac ( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );3D-3D 输入：3对3D-2D匹配点，已知这三对点在世界坐标系中的坐标； 输出：相机的位姿，和点在世界坐标系中的坐标。 在优化求解过程中，与相机的内参无关
