首先设计五个类:分别为Frame ,Camera,Mappoint,Map,和Config类。
一,在Canera类之中,我们存储相机的内参和外参,并且完成相机坐标系,像素坐标系以及世界坐标系之间的坐标变换。其中,相机的外参是变化的,由参数的形式传入。x,y,x_corrected,y_corrected(畸变纠正)都是归一化平面上的点。
将纠正的点通过内参数矩阵投影到像素平面,得到该点在图像上的正确位置。(cx cy 成像平面坐标系以及像素坐标系之间原点的关系)
fx_,fy_,cx_,cy_,depth_scale_5个变量前四个都是以像素为单位 f为焦距。
而dx和dy指的是每个像素个数的大小,即mm/pixel,那么经过换算最后得到的u、v的量纲自然也就是像素。fx和fy是由f/dx,f/dy计算而来,表示的是长度值为f时对应的像素个数,也即长度值为f(mm)时,在图像平面的x方向和y方向的像素个数
现以NiKon D700相机为例进行求解其内参数矩阵:
就算大家身边没有这款相机也无所谓,可以在网上百度一下,很方便的就知道其一些参数——
焦距 f = 35mm 最高分辨率:4256×2832 传感器尺寸:36.0×23.9 mm
根据以上定义可以有:
u0= 4256/2 = 2128 v0= 2832/2 = 1416 dx = 36.0/4256 dy = 23.9/2832
fx = f/dx = 4137.8 fy = f/dy = 4147.3
相机的位姿都是T_c_w,用Sophus::SE3来表示(世界坐标到相机坐标)。如果是相机转世界坐标系,用T_c_w的逆矩阵就可以了。
像素坐标和相机坐标之间转,定义了一个depth,z坐标直接是这个depth.:pnp估计相机位姿初始值
1:se3的六维向量前三维表示的“平移”向量并不是真正意义上的平移向量,后三维是旋转向量
2:so3的三维向量就是旋转向量
3:se3转换的4*4矩阵分为四块
左上角:so3的反对称矩阵
右上角:se3的前三维“平移向量”
左下角:0^T
右下角:0
4:se3通过函数:rotationMatrix(),translation()转换出的就是旋转矩阵,平移向量
二,