1. 首页首页
  2. 编程汇总编程汇总

OpenCV实现SfM（四）：Bundle Adjustment

编程汇总 • • 阅读 18

OpenCV实现SfM（四）：Bundle Adjustment文章目录 Bundle Adjustment Ceres Solver 编写代码 优化结果对比 Before After Before After Statistics 结语 Bundle Adjustment 在上一篇文章中 成功将三维重建扩展到了任意数量的图像 但是 随着图像的增多 累计误差会越来越大 从而影响最终的重建效果 要解决这个问题 需要用到 Bundle

文章目录

Bundle Adjustment

Ceres Solver

编写代码

优化结果对比

Before

After

Before

After

Statistics

结语

Bundle Adjustment

在上一篇文章中，成功将三维重建扩展到了任意数量的图像，但是，随着图像的增多，累计误差会越来越大，从而影响最终的重建效果。要解决这个问题，需要用到Bundle Adjustment（下文简称BA）。
BA本质上是一个非线性优化算法，先来看看它的原型
min ⁡ x ∑ i ρ i ( ∣ ∣ f i ( x i 1 , x i 2 , . . . , x i k ) ∣ ∣ 2 ) \min_x \sum_i{\rho_i(||f_i(x_{i1}, x_{i2}, …, x_{ik})||^2)} xmin​i∑​ρi​(∣∣fi​(xi1​,xi2​,...,xik​)∣∣2)
其中 x x x是我们需要优化的参数， f f f一般称为代价函数（Cost Function）, ρ \rho ρ为损失函数（Loss Function）。其中 f f f的返回值可能是一个向量，因此总的代价取该向量的2-范数。
对于三维重建中的BA，代价函数往往是反向投影误差，比如我们需要优化的参数有相机的内参（焦距、光心、畸变等）、外参（旋转和平移）以及点云，设图像 i i i的内参为 K i K_i Ki​，外参为 R i R_i Ri​和 T i T_i Ti​，点云中某一点的坐标为 P j P_j Pj​，该点在 i i i图像中的像素坐标为 p j i p_j^i pji​，则可以写出反向投影误差
f ( K i , R i , T i , P j ) = π ( K i [ R i T i ] P j ) − p j i f(K_i, R_i, T_i, P_j)=\pi(K_i[R_i\ \ T_i]P_j) – p_j^i f(Ki​,Ri​,Ti​,Pj​)=π(Ki​[Ri​ Ti​]Pj​)−pji​
上式中的 P j P_j Pj​和 p j i p_j^i pji​均为齐次坐标，其中 π \pi π为投影函数，有 π ( p ) = ( p x / p z , p y / p z , 1 ) \pi(p)=(p_x/p_z,\ p_y/p_z,\ 1) π(p)=(px​/pz​, py​/pz​, 1).
而损失函数 ρ \rho ρ的目的是为了增强算法的鲁棒性，使得算法不易受离群点（Outliers）的影响，常见的有Huber函数、Tukey函数等，这些函数的图像如下

若不使用损失函数，即 ρ ( x ) = x \rho(x)=x ρ(x)=x，那么就如上图中的黑色曲线，代价(Cost)随着误差以二次幂的速度增长，也许一个误差较大的点，就能左右算法的优化方向。其他的损失函数，可以发现，随着误差的增大，要么代价的增长是趋于线性的(Huber)，要么干脆趋于不变(Tukey)，这样就降低了误差较大的点对总代价的影响。损失函数实际上就是代价的重映射过程。

Ceres Solver

如何求解BA？总体思想是使用梯度下降，比如高斯-牛顿迭代、Levenberg-Marquardt算法等，由于BA还有自己的一些特殊性，比如稀疏性，在实现时还有很多细节需要处理，在此就不细说了。好在现在有很多用于求解非线性最小二次问题的库，文中使用的就是Google的一个开源项目——Ceres Solver.
Ceres Solver专为求解此类问题进行了大量的优化，有很高的效率，尤其在大规模问题上，其优势更加明显。并且，Ceres内置了一些常用的函数，比如对坐标的旋转以及各类损失函数，使其在开发上也比较高效。在官网上可以找到它的编译方法和使用教程，Windows用户可以在此找到配置好的VS工程。

编写代码

代码总体基本不变，我们只需要再添加一个函数用于BA即可，还有一点需要注意的是，Ceres Solver默认使用双精度浮点，如果精度不够可能导致计算梯度失败、问题无法收敛等问题，因此在原来的代码中，需要将原本问Point3f型的structure，改为Point3d类型。
首先定义一个代价函数

struct ReprojectCost

{ 

   
cv::Point2d observation;


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

   
}


template 
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];


	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;


	residuals[0] = u - T(observation.x);
	residuals[1] = v - T(observation.y);


	return true;
}

};

该代价函数就是之前所说的反向投影误差，参数分别为内参、外参还有点在空间中的坐标，最后一个参数用于输出，为反向投影误差。注意，为了使BA更高效可靠，外参当中的旋转部分使用的是旋转向量而不是旋转矩阵，这样不仅使优化参数从9个变为3个，还能保证参数始终代表一个合法的旋转（如果用矩阵，可能在优化过程中，正交性不再满足）。

接下来直接使用Ceres Solver求解BA，其中使用了Ceres提供的Huber函数作为损失函数

void bundle_adjustment(
Mat& intrinsic,
vector& extrinsics, 
vector>& correspond_struct_idx,
vector>& key_points_for_all,
vector& structure

)

{ 

   
ceres::Problem problem;


// load extrinsics (rotations and motions)
for (size_t i = 0; i < extrinsics.size(); ++i)
{ 

   
	problem.AddParameterBlock(extrinsics[i].ptr(), 6);
}
// fix the first camera.
problem.SetParameterBlockConstant(extrinsics[0].ptr());


// load intrinsic
problem.AddParameterBlock(intrinsic.ptr(), 4); // fx, fy, cx, cy


// load points
ceres::LossFunction* loss_function = new ceres::HuberLoss(4);	// loss function make bundle adjustment robuster.
for (size_t img_idx = 0; img_idx < correspond_struct_idx.size(); ++img_idx)
{ 

   
	vector& point3d_ids = correspond_struct_idx[img_idx];
	vector& key_points = key_points_for_all[img_idx];
	for (size_t point_idx = 0; point_idx < point3d_ids.size(); ++point_idx)
	{ 

   
		int point3d_id = point3d_ids[point_idx];
		if (point3d_id < 0)
			continue;


		Point2d observed = key_points[point_idx].pt;
		// 模板参数中，第一个为代价函数的类型，第二个为代价的维度，剩下三个分别为代价函数第一第二还有第三个参数的维度
		ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new ReprojectCost(observed));


		problem.AddResidualBlock(
			cost_function,
			loss_function,
			intrinsic.ptr(),			// Intrinsic
			extrinsics[img_idx].ptr(),	// View Rotation and Translation
			&(structure[point3d_id].x)			// Point in 3D space
		);
	}
}


// Solve BA
ceres::Solver::Options ceres_config_options;
ceres_config_options.minimizer_progress_to_stdout = false;
ceres_config_options.logging_type = ceres::SILENT;
ceres_config_options.num_threads = 1;
ceres_config_options.preconditioner_type = ceres::JACOBI;
ceres_config_options.linear_solver_type = ceres::SPARSE_SCHUR;
ceres_config_options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;


ceres::Solver::Summary summary;
ceres::Solve(ceres_config_options, &problem, &summary);


if (!summary.IsSolutionUsable())
{ 

   
	std::cout << "Bundle Adjustment failed." << std::endl;
}
else
{ 

   
	// Display statistics about the minimization
	std::cout << std::endl
		<< "Bundle Adjustment statistics (approximated RMSE):\n"
		<< " #views: " << extrinsics.size() << "\n"
		<< " #residuals: " << summary.num_residuals << "\n"
		<< " Initial RMSE: " << std::sqrt(summary.initial_cost / summary.num_residuals) << "\n"
		<< " Final RMSE: " << std::sqrt(summary.final_cost / summary.num_residuals) << "\n"
		<< " Time (s): " << summary.total_time_in_seconds << "\n"
		<< std::endl;
}

}

如果求解成功，会输出一些统计信息，其中最重要的两项分别是优化前的平均反向投影误差（Initial RMSE）和优化后的该值（Final RMSE）。

main函数完全不用变，只需在最后加入如下代码，对BA进行调用即可

Mat intrinsic(Matx41d(K.at(0, 0), K.at(1, 1), K.at(0, 2), K.at(1, 2)));

vector extrinsics;

for (size_t i = 0; i < rotations.size(); ++i)

{ 

   
Mat extrinsic(6, 1, CV_64FC1);
Mat r;
Rodrigues(rotations[i], r);


r.copyTo(extrinsic.rowRange(0, 3));
motions[i].copyTo(extrinsic.rowRange(3, 6));


extrinsics.push_back(extrinsic);

}



bundle_adjustment(intrinsic, extrinsics, correspond_struct_idx, key_points_for_all, structure);

优化结果对比

Before

After

Before

After

Statistics

从统计信息可以看出，最初的重建结果，反向投影误差约为3.6个像素，BA之后，反向投影误差降为1.4个像素，如果删除一些误差过大的点，再进行一次BA，反向投影误差往往能小于0.5个像素！

这次的代码与上一篇文章的几乎一样，唯独多出来的几个函数和修改也已在上文中列出，这次就不再单独提供代码了。要运行本文的代码，需要编译Ceres Solver，还需要依赖Eigen（一个线性代数库），详细过程在Ceres的官网上均有提及。

结语

由于个人时间有限，马上就要面临毕业论文和工作等问题，这可能是该系列最后一篇文章。在此感谢各位朋友的支持，谢谢。

编程小号
上一篇 2025-03-24 07:21
下一篇 2025-02-18 14:01

相关推荐

版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 举报，一经查实，本站将立刻删除。
如需转载请保留出处：https://bianchenghao.cn/hz/123441.html