反向投影误差的计算

感谢广大的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×4的矩阵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;

• 通过相机三维坐标到像素坐标的转换，计算经过旋转和平移后获得的三维相机坐标点所对应的像素坐标，与第二帧的所对应特征点的像素坐标的误差，∑|ui−K(RX+T)|2，该式中ui代表的是是几个特征点所对应的坐标，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)为例。

⎡⎣⎢uv1⎤⎦⎥=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥⎡⎣⎢x′y′1⎤⎦⎥(1)

　　其中x=d*x’,y=d*y’,d=depth*scale;depth是深度传感器的测量值,scale是深度测量值的比例系数.
2.自己没有明白在对极几何约束中使用基础矩阵和本质矩阵求取的误差值存在差异。

---

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