反向投影误差的计算

感谢广大的SLAM开源科研和工作人员，给我提供了很大的便利和捷径。当然该看的得看，该学的得学。

通过上一讲3D-2D的位姿计算后，现在求取反向投影误差

---

• 首先通过$cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );$函数获得旋转向量$rvec$和平移向量$tvec$。
• 其次通过$cv::Rodrigues( rvec ,Rot );$函数获得旋转矩阵$Rot$
• 进行BA光束平差法,通过$bundleAdjustment ( pts3d, pts2d, K, Rot, tvec );$函数获得$4\times4$的矩阵$T$。
• 通过下面的代码可以获得第一帧的三维相机坐标点和经过旋转和平移后获得的三维相机坐标点。
  
Mat point = (Mat_<double>(3,1)<<pts3d[i].x,pts3d[i].y,pts3d[i].z);
Mat R_BA=(Mat_<double>(3,3)<<T(0,0),T(0,1),T(0,2),T(1,0),T(1,1),T(1,2),T(2,0),T(2,1),T(2,2));
Mat T_BA=(Mat_<double>(3,1)<<T(0,3),T(1,3),T(2,3));
Mat BA_Point=R_BA*point+T_BA;

• 通过相机三维坐标到像素坐标的转换，计算经过旋转和平移后获得的三维相机坐标点所对应的像素坐标，与第二帧的所对应特征点的像素坐标的误差，$\sum{|u_i-K(RX+T)|^2}$，该式中$u_i$代表的是是几个特征点所对应的坐标，K代表的是相近内参，代表的从相机的三维坐标点到像素坐标的映射，R和T代码的是相机坐标点相对于第二帧的旋转和平移变换。
  
double sum_pnp=0,sum_ba=0;
for(int i=0;i<pts3d.size();i++)
{
Vector2d ba_reprojected=camera2pixel(Vector3d(BA_Point.at<double>(0,0),BA_Point.at<double>(1,0),BA_Point.at<double>(2,0)));
delt_ba=(pts2d[i].y-ba_reprojected_pixel[1])*(pts2d[i].y-ba_reprojected_pixel[1])+(pts2d[i].x-ba_reprojected_pixel[0])*(pts2d[i].x-ba_reprojected_pixel[0]);
sum_ba+=delt_ba;
}


---

• 
  
• 优点和存在的问题
  

　优点:使用$solvePnPRansac$的函数具有明显的鲁棒性，相较于$solvePnP$函数而言。
缺点:
1.自己在三维点的尺度问题上理解还是比较模糊。以三维坐标点$(x,y,d)$为例。

$$\left[ \begin{matrix}u \\ v \\ 1 \end{matrix} \right] =\left[ \begin{matrix}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix}x' \\ y' \\ 1 \end{matrix} \right] \tag{1}$$ 　　其中x=d*x’,y=d*y’,d=depth*scale;depth是深度传感器的测量值,scale是深度测量值的比例系数.
2.自己没有明白在对极几何约束中使用基础矩阵和本质矩阵求取的误差值存在差异。

---

下一步计划：
1.实现完成较为独立的BA。
2.加入ICP,比较效果。