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;
		
		// 計算重投影誤差
		residuals[0] = u - T(observation.x);
		residuals[1] = v - T(observation.y);

		return true;
	}
};

接下來使用Ceres Solver求解BA，前兩張圖片的BA優化函數如下：

/*
main函數中添加：
	// BA優化
	Mat intrinsic(Matx41d(K.at<double>(0, 0), K.at<double>(1, 1), K.at<double>(0, 2), K.at<double>(1, 2)));

	Mat extrinsic(6, 1, CV_64FC1);
	Mat r;
	Rodrigues(R, r);
	r.copyTo(extrinsic.rowRange(0, 3));// [0,3)
	t.copyTo(extrinsic.rowRange(3, 6));

	bundle_adjustment(intrinsic, extrinsic, points1, points3D);
*/
void bundle_adjustment(Mat& intrinsic, Mat& extrinsics, vector<Point2d>& points, vector<Point3d>& points3D)
{
	ceres::Problem problem;

	// load extrinsics (rotations and motions)
	problem.AddParameterBlock(extrinsics.ptr<double>(), 6);

	// fix the first camera.
	problem.SetParameterBlockConstant(extrinsics.ptr<double>());

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

	// load points
	ceres::LossFunction* loss_function = new ceres::HuberLoss(4);   // loss function make bundle adjustment robuster.
	
	for (size_t i = 0; i < points3D.size(); i++)
	{
		Point2d observed = points[i];

		// 模板參數:代價函數的類型、代價的維度、代價函數第一、第二、第三個參數的維度
		ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectCost, 2, 4, 6, 3>(new ReprojectCost(observed));

		problem.AddResidualBlock(
			cost_function,
			loss_function,
			intrinsic.ptr<double>(),      // Intrinsic
			extrinsics.ptr<double>(),  // View Rotation and Translation
			&(points3D[i].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: 0\n"
			<< " #num_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;
	}
}

如下兩圖是前兩張圖片BA優化前後的結果，雖然直觀上來看，兩者並沒有什麼太大的區別。但是根據BA優化後的統計信息可知，BA優化前重投影的三維點與真實點平均偏移11.68個像素，優化後兩者幾乎沒有偏差(RMSE = 0)，可見還是有了很大的改善。

BA優化後統計信息：

多張圖片BA優化函數：

/*
main函數中添加：
	// BA優化
	Mat intrinsic(Matx41d(K.at<double>(0, 0), K.at<double>(1, 1), K.at<double>(0, 2), K.at<double>(1, 2)));
	vector<Mat> 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));
		translations[i].copyTo(extrinsic.rowRange(3, 6));

		extrinsics.push_back(extrinsic);
	}

	bundle_adjustment(intrinsic, extrinsics, correspond_struct_idx, key_points_for_all, points3D);
*/
void bundle_adjustment(Mat& intrinsic, vector<Mat>& extrinsics, vector<vector<int>>& correspond_struct_idx, vector<vector<KeyPoint>>& key_points_for_all, vector<Point3d>& structure)
{
	// 構建問題，求解
	ceres::Problem problem;

	// load extrinsics
	for (size_t i = 0; i < extrinsics.size(); ++i)
	{
		problem.AddParameterBlock(extrinsics[i].ptr<double>(), 6);
	}
	// load intrinsic
	problem.AddParameterBlock(intrinsic.ptr<double>(), 4); // fx, fy, cx, cy

	// fix the first camera.
	problem.SetParameterBlockConstant(extrinsics[0].ptr<double>());

	// 添加初始點
	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<int>& point3d_ids = correspond_struct_idx[img_idx];
		vector<KeyPoint>& 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<ReprojectCost, 2, 4, 6, 3>(new ReprojectCost(observed));

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

	// 設置優化策略
	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"
			<< " #num_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;
	}
}

多張圖片BA優化結果：

多圖BA優化後統計信息：

多張圖片經BA優化後，RMSE也由3.96降到2.23。

參考

ceres求解PnP–SLAM 十四講第七章課後題，有助於理解代價函數的定義。
OpenCV實現SfM（四）：Bundle Adjustment
下載：SfM三維重建：BA優化【VS2015+OpenCV3.4+PCL1.8+Ceres Solver】

SfM三維點雲重建：BA優化--【VS2015+OpenCV3.4+PCL1.8】

前言

代碼

參考

VS中【F5】與【Ctrl+F5】及【F7】與【Ctrl+F7】的區別

SfM兩視圖三維點雲重建--【VS2015+OpenCV3.4+PCL1.8】

PCL可視化點雲顏色特徵

新松機器人：【示教編程】+【PC離線編程】+【PLC遠程IO】相結合

Bumblebee相機：標定、分離圖像+保存成cv::Mat格式

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結