三维重建中利用控制点提升模型位置精确度

现有，基于BA的SFM过程，在基于image_0位置(0,0,0)完成相对位置建图incremental mapping后，通过图片对应GPS信息，通过similarity3将整个地图拉到一个相对准确的位置，在此之后不再进行BA优化（只改变了整个模型的绝对位置，pose与point的相对位置并没有发生变化，原则上不需要）。

现在为了让模型更准确，加入控制点功能。

1.人工使用定位设备对选定位置进行打点，获得一个预设位置的绝对地理坐标Ground Truth。

2.通过在image上找到打点位置，手动对图片打标签，实现多张image下的三维-二维映射关系

如图：绿色为先验控制点投影在模型图片中的坐标，红色为人为手工纠正的点。（图片仅供参考，只要能得到控制点数据结构就行了）

控制点数据结构：先验三维坐标-image_id/name-先验二维坐标

（细节：打点坐标为WGS84，需要转换为XYZ坐标）

 3.将模型针对控制点进行匹配调整：将控制点的先验xyz投影到每张image上，用二维投影坐标减去二维先验坐标，得到投影误差，使用ceres优化这个重投影误差，实现模型位置的精确调整。

优化方程：

延续BA+ceres的方法，整个模型也都要同步优化，但是重点优化GCP的重投影误差，减少对已经建好的模型进行干扰，所以给GCP加一个更大的权重(在此之前，GCP的重投影误差和3d点的冲拖影误差，两者误差是同数量级的，不需要额外做处理)，所有数据放在一起进行BA。

//固化相机内外参、固化二维控制点与三维控制点，优化相机位姿与3d点位置

F(x)=(3d点-2d特征点重投影误差)+(GCP重投影误差)*weight

ceres优化机制：

F(x)=1/2(f1(x)+f2(x)+f3(x))，各项残差同等优化，所以需要自己权衡每一项的数值量级，实测如果weight是1，优化也能达到一些效果，但是不够显著，或者优化速度过慢，考虑到已有3d点没必要被优化，将weight从1逐渐提高到20，提高到500，以提升控制点的优化效果。

（对应原理，将3像素误差优化到终止条件（比如1），和把1500优化到终止条件是不一样的，如果最终误差是1，缩放回原尺寸看，就是1比0.002，差距明显）

weight=1

 weight=20

 weight=500

最终优化效果： 

甚至没打点的图片也被纠正了