)
功能:通过当前估计的位姿(mRi, mti)将3D世界点投影到相机坐标系,再通过相机模型投影到图像平面,计算重投影误差,并与阈值比较,确定内点。代码将mRi和mti用于变换,且mRi是3x3数组,mti是平移。x,y,z是世界点。然后调用mpCamera-project(P3Dc)得到像素坐标。重投影误差是欧氏距离平方下面给出CheckInliers函数的逐行注释,以及对应的数学原理和重投影误差公式。// 检查当前位姿下的内点(重投影误差小于阈值即为内点) void MLPnPsolver::CheckInliers() { mnInliersi = 0; // 重置当前内点计数器 // 遍历所有 3D-2D 对应点 (共 N 个) for (int i = 0; i N; i++) { // 获取世界坐标系下的 3D 点坐标 point_t p = mvP3Dw[i]; cv::Point3f P3Dw(p(0), p(1), p(2)); // 转为 OpenCV 点格式 // 获取对应的 2D 图像观测点 (特征点坐标) cv::Point2f P2D = mvP2D[i]; // 将 3D 点从世界坐标系变换到相机坐标系 // 使用当前估计的旋转矩阵 mRi 和平移向量 mti