目录
3.1 鱼眼相机投影的数学基础KannalaBrandt8::project
1.相机基类GeometricCamera.h
1.1 GeometricCamera类
是一个纯虚类，无法实例化对象，只能通过继承来实现。
从C++的角度来讲，判断一个类是否是纯虚类只需要看后面的虚函数是否有=0的部分：
virtual cv::Point2f project(const cv::Point3f &p3D) = 0;纯虚类无法实例化，只能通过继承来实现。纯虚函数也是没有办法直接实现的，需要通过在继承类重写才能使用。
namespace ORB_SLAM3 { class GeometricCamera { friend class boost::serialization::access; template<class Archive> void serialize(Archive& ar, const unsigned int version) { ar & mnId; ar & mnType; ar & mvParameters; } public: GeometricCamera() {} GeometricCamera(const std::vector<float> &_vParameters) : mvParameters(_vParameters) {} ~GeometricCamera() {} // 投影函数 输入:相机坐标下的三维点 输出:图片上的像素坐标 好多重载函数应用于不同类型 virtual cv::Point2f project(const cv::Point3f &p3D) = 0; virtual Eigen::Vector2d project(const Eigen::Vector3d & v3D) = 0; virtual Eigen::Vector2f project(const Eigen::Vector3f & v3D) = 0; virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) = 0; // 返回像素点的不确定性 不管输入是什么 返回都是1.0f的float类型 virtual float uncertainty2(const Eigen::Matrix<double,2,1> &p2D) = 0; // 反投影 输入的是一个像素点的坐标 输出的是在相机坐标系下归一化平面的坐标 反投影没有尺度因此深度无法求得 virtual Eigen::Vector3f unprojectEig(const cv::Point2f &p2D) = 0; virtual cv::Point3f unproject(const cv::Point2f &p2D) = 0; virtual Eigen::Matrix<double,2,3> projectJac(const Eigen::Vector3d& v3D) = 0; // 三角化恢复三维点 主要在单目初始化的时候调用 virtual bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12, Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated) = 0; //返回 K 矩阵 3*3 virtual cv::Mat toK() = 0; virtual Eigen::Matrix3f toK_() = 0; // 极线约束 三角化MP使用 virtual bool epipolarConstrain(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc) = 0; // 设置和获取相机的内参数 float getParameter(const int i){return mvParameters[i];} void setParameter(const float p, const size_t i){mvParameters[i] = p;} size_t size(){return mvParameters.size();} virtual bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2, const float sigmaLevel1, const float sigmaLevel2, Eigen::Vector3f& x3Dtriangulated) = 0; unsigned int GetId() { return mnId; } unsigned int GetType() { return mnType; } // 针孔是0,鱼眼是1 const static unsigned int CAM_PINHOLE = 0; const static unsigned int CAM_FISHEYE = 1; static long unsigned int nNextId; protected: // 存放参数 包括K的参数和畸变 std::vector<float> mvParameters; unsigned int mnId; unsigned int mnType; }; }
2.针孔相机模型 Pinhole
2.1 Pinhole.h
针孔相机模型继承于相机基类GeometricCamera：
namespace ORB_SLAM3 { // 针孔类模型 继承自相机基类 class Pinhole : public GeometricCamera { friend class boost::serialization::access; template<class Archive> void serialize(Archive& ar, const unsigned int version) { ar & boost::serialization::base_object<GeometricCamera>(*this); } public: Pinhole() { // 参数只有四个 fx fy cx cy,没有关于畸变的参数,也就是说针孔模型已经把所有的点给去畸变化了,到相机这块的时候已经是去畸变后的点了 mvParameters.resize(4); mnId=nNextId++; mnType = CAM_PINHOLE; } // _vParameters相机内参 Pinhole(const std::vector<float> _vParameters) : GeometricCamera(_vParameters), tvr(nullptr) { // fx fy cx cy 并没有畸变...也就是说针孔模型下 已经将所有的像素点去畸变化了 到相机后所有的点已经是去畸变后的了 没有保存有关畸变的数据 assert(mvParameters.size() == 4); mnId=nNextId++; mnType = CAM_PINHOLE; } Pinhole(Pinhole* pPinhole) : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) { assert(mvParameters.size() == 4); mnId=nNextId++; mnType = CAM_PINHOLE; } ~Pinhole(){ if(tvr) delete tvr; } // 投影函数 cv::Point2f project(const cv::Point3f &p3D); Eigen::Vector2d project(const Eigen::Vector3d & v3D); Eigen::Vector2f project(const Eigen::Vector3f & v3D); Eigen::Vector2f projectMat(const cv::Point3f& p3D); // 返回像素点的不确定性 不管是啥都返回1.0f float uncertainty2(const Eigen::Matrix<double,2,1> &p2D); //反投影 2D -> 3D 深度为1的相机坐标系 Eigen::Vector3f unprojectEig(const cv::Point2f &p2D); cv::Point3f unproject(const cv::Point2f &p2D); Eigen::Matrix<double,2,3> projectJac(const Eigen::Vector3d& v3D); bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12, Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated); cv::Mat toK(); Eigen::Matrix3f toK_(); bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc); bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2, const float sigmaLevel1, const float sigmaLevel2, Eigen::Vector3f& x3Dtriangulated) { return false;} friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph); friend std::istream& operator>>(std::istream& os, Pinhole& ph); bool IsEqual(GeometricCamera* pCam); private: // 两帧恢复三维点,两帧重建 //Parameters vector corresponds to // [fx, fy, cx, cy] TwoViewReconstruction* tvr; }; }
2.2 Pinhole.cpp实现函数
2.2.1 投影函数
/** * @brief 相机坐标系下的三维点投影到无畸变像素平面 * @param v3D 三维点 * @return 像素坐标 */ Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) { Eigen::Vector2d res; // u = fx * x/z + cx // v = fy * y/z + cy // mvParameters = [fx,fy,cx,cy] res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; return res; }mvParameters存放着相机内参
。通过如下方式投影：
2.2.2 反投影函数
/** * @brief 反投影 * @param p2D 特征点 * @return 归一化坐标 */ cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) { return cv::Point3f( (p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], 1.f); }由公式：
分离出
有：
又由于像素坐标系没有尺度信息，因此我们令
为1。得到归一化平面的三维坐标。
2.2.3 恢复三维点
TwoViewReconstruction::TwoViewReconstruction(const Eigen::Matrix3f &k, float sigma, int iterations) { mK = k; mSigma = sigma; mSigma2 = sigma * sigma; mMaxIterations = iterations; }其实有用的部分就是把相机的内参拷贝进去了。
/** 三角化恢复三维点 单目初始化时使用 * @param vKeys1 第一帧的关键点 * @param vKeys2 第二帧的关键点 * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 * @param R21 顾名思义 * @param t21 顾名思义 * @param vP3D 恢复出的三维点 * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 */ bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2, const std::vector<int> &vMatches12, Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated) { if (!tvr) { // 用内参K构造 TwoViewReconstruction类 Eigen::Matrix3f K = this->toK_(); tvr = new TwoViewReconstruction(K); } // 返回 两帧之间的变换矩阵T21 和成功的三角化的点vP3D 以及是否三角化成功的标志vbTriangulated return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, T21, vP3D, vbTriangulated); }
3.鱼眼相机模型
3.1 鱼眼相机投影的数学基础KannalaBrandt8::project
鱼眼相机和普通相机相比，有更大的观测空间，在针孔相机模型中，只需要利用相似三角形原理就可以实现由三维点向二维点的投影。
如图，
是归一化平面的相对于相机坐标的
偏移量，
是相机的焦距。
因此，通过这俩相似三角形就可以得到在像素平面的坐标。
在鱼眼相机模型中，我们无法通过相似三角形的方式得到像素坐标，下面我们来介绍如何获得像素坐标，下面我们先了解图中各变量的意义：
：观测到的归一化平面的三维点与相机的夹角
：处理后的夹角
下面推导过程：带c下标的表示畸变前的，带d下标的表示畸变后的。
计算归一化坐标：
计算
和
计算
：
计算去畸变后的归一化平面坐标：
最后计算像素点：
至此，我们就得到了三维点在像素坐标系上的投影。
/** * @brief 投影 * xc = Xc/Zc, yc = Yc/Zc * r^2 = xc^2 + yc^2 * θ = arctan(r) * θd = k0*θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 * xd = θd/r * xc yd = θd/r * yc * u = fx*xd + cx v = fy*yd + cy * @param p3D 三维点 * @return 像素坐标 */ cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) { const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y; const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z); const float psi = atan2f(p3D.y, p3D.x); const float theta2 = theta * theta; const float theta3 = theta * theta2; const float theta5 = theta3 * theta2; const float theta7 = theta5 * theta2; const float theta9 = theta7 * theta2; const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5 + mvParameters[6] * theta7 + mvParameters[7] * theta9; return cv::Point2f(mvParameters[0] * r * cos(psi) + mvParameters[2], mvParameters[1] * r * sin(psi) + mvParameters[3]); }
3.2 鱼眼相机反投影的数学基础
主要是一个计算与迭代的过程。
/** * @brief 反投影 * 投影过程 * xc = Xc/Zc, yc = Yc/Zc * r^2 = xc^2 + yc^2 * θ = arctan(r) * θd = k0*θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 * xd = θd/r * xc yd = θd/r * yc * u = fx*xd + cx v = fy*yd + cy * * * 已知u与v 未矫正的特征点像素坐标 * xd = (u - cx) / fx yd = (v - cy) / fy * 待求的 xc = xd * r / θd yc = yd * r / θd * 待求的 xc = xd * tan(θ) / θd yc = yd * tan(θ) / θd * 其中 θd的算法如下： * xd^2 + yd^2 = θd^2/r^2 * (xc^2 + yc^2) * θd = sqrt(xd^2 + yd^2) / sqrt(xc^2 + yc^2) * r * 其中r = sqrt(xc^2 + yc^2) * 所以 θd = sqrt(xd^2 + yd^2) 已知 * 所以待求的只有tan(θ),也就是θ * 这里θd = θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 * 直接求解θ比较麻烦，这里用迭代的方式通过误差的一阶导数求θ * θ的初始值定为了θd，设θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 = f(θ) * e(θ) = f(θ) - θd 目标是求解一个θ值另e(θ) = 0 * 泰勒展开e(θ+δθ) = e(θ) + e`(θ) * δθ = 0 * e`(θ) = 1 + 3*k1*θ^*2 + 5*k2*θ^4 + 7*k3*θ^6 + 8*k4*θ^8 * δθ = -e(θ)/e`(θ) 经过多次迭代可求得θ * 最后xc = xd * tan(θ) / θd yc = yd * tan(θ) / θd * @param p2D 特征点 * @return */ cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) { // Use Newthon method to solve for theta with good precision (err ~ e-6) cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]); float scale = 1.f; float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y); // sin(psi) = yc / r theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f); // 不能超过180度 if (theta_d > 1e-8) { // Compensate distortion iteratively // θ的初始值定为了θd float theta = theta_d; // 开始迭代 for (int j = 0; j < 10; j++) { float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta4 * theta4; float k0_theta2 = mvParameters[4] * theta2, k1_theta4 = mvParameters[5] * theta4; float k2_theta6 = mvParameters[6] * theta6, k3_theta8 = mvParameters[7] * theta8; float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8); theta = theta - theta_fix; if (fabsf(theta_fix) < precision) // 如果更新量变得很小，表示接近最终值 break; } // scale = theta - theta_d; // 求得tan(θ) / θd scale = std::tan(theta) / theta_d; } return cv::Point3f(pw.x * scale, pw.y * scale, 1.f); }
335
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
