OPENCV中的光流跟踪算法 opencv光束法平差

转载

棉花糖 2024-06-27 20:36:53

文章标签 OPENCV中的光流跟踪算法 Ceres Solver 损失函数反向投影数据 文章分类 计算机视觉人工智能

为什么使用Ceres Solver

Ceres Solver是谷歌开发的一款用于非线性优化的库，在SFM（Structure From Motion）中的最后一步，对定向和三角化后的像片位姿、相机内参以及三维空间点坐标进行Bundle Adjustment，优化像片位姿（旋转矩阵、平移矩阵，在COLMAP中，旋转矩阵以四元数的形式表示），相机内参矩阵（包括焦距、像主点、径向畸变、切向畸变），三维空间点位。

光束法平差原理简单理解

由于同名像点前方交会可能并不相交，所以取平均作为最终重建三维点，由该点及像片位姿后方交会生成的像点坐标与一开始的像点坐标不重合，因此存在一个反向投影误差，以最小化反向投影误差来迭代优化像片位姿以及空间三维坐标点。这便是光束法平差的最基本思想。

下图应该是网络上出现频率较高的：但这只是理想的前方交会结果，即两条直线刚好交会到同一个点，比如射线P1X1和P2X1交会于X1，实际并不交会，而是取平均后的结果，假设未X1附近的一个点X1'，这样反向计算出来的像点不再是原始的像点，而是像点附近的一个点，就存在了反向投影误差，即两像点之间的差值，如第二幅图所示。

OPENCV中的光流跟踪算法 opencv光束法平差_Ceres Solver

（图片来源：）

OPENCV中的光流跟踪算法 opencv光束法平差_反向投影_02

光束法平差单元

按照自己的理解，Ceres Solver进行光束法平差的函数模板中存在一个平差单元，其中cost_function的定义会涉及到这个平差单元。根据上一部分原理的简单介绍，把每张像片上观测到的像点（即SFM流程中通过SIFT匹配到的可以前方交会出空间三维点的像点），定义为Observed；其所在的像片的位姿，定义为qvec和tvec；像片对应的相机内参，定义为camera_param；该像点对应的空间三维点，定义为point_3D。上述四部分便构成了一个最基本的光束法平差单元。因为同一个空间三维点可以在多张像片上找到对应的像点，以及整个平差涉及到多张像片，因此存在多个这样的基本平差单元。

代码分析

原始代码分析：

struct SnavelyReprojectionError {
	SnavelyReprojectionError(double observed_x, double observed_y)
		: observed_x(observed_x), observed_y(observed_y) {}

	template <typename T>
	bool operator()(const T* const camera,
		const T* const point,
		T* residuals) const {
		// camera[0,1,2] are the angle-axis rotation.
		T p[3];
		ceres::AngleAxisRotatePoint(camera, point, p);

		// camera[3,4,5] are the translation.
		p[0] += camera[3];
		p[1] += camera[4];
		p[2] += camera[5];

		// Compute the center of distortion. The sign change comes from
		// the camera model that Noah Snavely's Bundler assumes, whereby
		// the camera coordinate system has a negative z axis.
		T xp = -p[0] / p[2];
		T yp = -p[1] / p[2];

		// Apply second and fourth order radial distortion.
		const T& l1 = camera[7];
		const T& l2 = camera[8];
		T r2 = xp*xp + yp*yp;
		T distortion = 1.0 + r2  * (l1 + l2  * r2);

		// Compute final projected point position.
		const T& focal = camera[6];
		T predicted_x = focal * distortion * xp;
		T predicted_y = focal * distortion * yp;


		// The error is the difference between the predicted and observed position 

		residuals[0] = predicted_x - observed_x;
		residuals[1] = predicted_y - observed_y;

		return true;
	}

	// Factory to hide the construction of the CostFunction object from
	// the client code.
	static ceres::CostFunction* Create(const double observed_x,
		const double observed_y) {
		return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9,  3>(new SnavelyReprojectionError(observed_x, observed_y)));
	}

	double observed_x;
	double observed_y;
};

以上部分定义了反向投影误差是怎么计算的，即residuals的计算，以及损失函数是怎么定义的，损失函数中的几个参数，第一个为代价函数的类型，第二个为代价的维度，第三个为每张像片外方位元素和相机内参，demo使用的相机内参为一个焦距加两个畸变参数，再加上3个旋转矩阵参数3个平移矩阵参数，正好9个，第四个为每一个空间三维点的坐标为3维。

if (!bal_problem.LoadFile("C:\\Users\\sggzg\\Desktop\\problem-49-7776-pre.txt")) {
		std::cerr << "ERROR: unable to open file " << "C:\\Users\\sggzg\\Desktop\\problem - 49 - 7776 - pre.txt" << "\n";
	}
const double* observations = bal_problem.observations();

// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.

ceres::Problem problem;

for (int i = 0; i < bal_problem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.

ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(observations[2 * i + 0],observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,NULL /* squared loss */,bal_problem.mutable_camera_for_observation(i),bal_problem.mutable_point_for_observation(i));}

// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_num_iterations = 3;
options.minimizer_progress_to_stdout = true;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";

这一部分中for循环主要就是添加每一个前面所说的光束法平差单元，AddResidualBlock函数即添加每一个平差单元的需要优化的参数，Create函数通过具体的像点观测值实例化前面定义的损失函数。平差的一些选项设置，平差报告的打印。

结合自己的实验数据进行调整：

主要是对反向投影误差计算的修改，根据自己的误差定义修改计算residuals的过程，以及根据相机的不同重新定义损失函数，如下：

ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<
				BundleAdjustment_CostFunction, 2, 4, 3, 3, 3>(
					new BundleAdjustment_CostFunction(point2D.X(), point2D.Y()));

problem.AddResidualBlock(cost_function, loss_function, psfmreconstruction->ImageDatas[img_idx].image.Qvec().data(),
				psfmreconstruction->ImageDatas[img_idx].image.Tvec().data(), psfmreconstruction->params_.data(), 
				psfmreconstruction->points3Ds_[point2D.Point3DId()].XYZ().data());

这时可以看出损失函数中的几个参数，第一个为代价函数的类型，第二个为代价的维度，第三个为每张像片外方位元素和相机内参，我使用的相机内参为一个焦距加两个像主点参数，再加上4元数表示的qvec4个参数以及平移向量3个参数，第四个为每一个空间三维点的坐标为3维。

实验数据

原始代码针对的数据集下载地址：http://grail.cs.washington.edu/projects/bal/

该数据集为txt格式，内容截图如下：

OPENCV中的光流跟踪算法 opencv光束法平差_数据_03