SfM三维点云重建：BA优化--【VS2015+OpenCV3.4+PCL1.8】

前言

在SfM多视图三维点云重建–【VS2015+OpenCV3.4+PCL1.8】中实现的增量式SfM三维点云重建，会随着图片数量的增加而导致误差逐渐累积，最后可能无法完成重建。在三维重建中常使用Bundle Adjustment（BA）优化来解决这个问题，BA优化是一种应用广泛的非线性优化方法，其本质属于非线性最小二乘问题，目的是找到使得重投影误差最小的3D点位置和相机参数。这里对BA原理就不再赘述，而是使用Ceres Solver对SfM三维重建后的内外参、点云进行BA优化。

代码

环境：VS2015+OpenCV3.4+PCL1.8+Ceres Solver

首先定义重投影误差作为待优化的代价函数，即计算三维空间点的重投影（世界坐标系到像素坐标系），并计算重投影误差，这部分有很多种形式，但都大同小异。这里ceres::AngleAxisRotatePoint(r, pos3d, pos_proj)将空间点pos3d进行旋转变换（旋转向量r）得到pos_proj，另外，注意添加如下定义和头文件，否则会报错。

#ifndef GLOG_NO_ABBREVIATED_SEVERITIES
#define GLOG_NO_ABBREVIATED_SEVERITIES
#endif
#include <ceres/ceres.h>
#include <ceres/rotation.h>

// 代价函数
struct ReprojectCost
{
   
	cv::Point2d observation;

	ReprojectCost(cv::Point2d& observation) : observation(observation){
   }

	// 参数：内参、外参、三维点、反向投影误差
	template <typename T>
	bool operator()(const T* const intrinsic, const T* const extrinsic, const T* const pos3d, T* residuals) const 
	{
   
		const T* r = extrinsic;
		const T* t = &extrinsic[3];
		
		// 外参数矩阵：转到相机坐标系
		// Apply the camera rotation
		T pos_proj[3];
		ceres::AngleAxisRotatePoint(r, pos3d, pos_proj);

		// Apply the camera translation
		pos_proj[0] += t[0];
		pos_proj[1] += t[1];
		pos_proj[2] += t[2];

		const T x = pos_proj[0] / pos_proj[2]; // 归一化相机坐标
		const T y = pos_proj[1] / pos_proj[2];
		
		// 内参数矩阵：转到像素坐标系
		const T fx = intrinsic[0];
		const T fy = intrinsic[1];
		const T cx = intrinsic[2];
		const T cy = intrinsic[3];

		// Apply intrinsic
		const T u = fx * x + cx;
		const T v = fy * y + cy;