BA本质上是一个非线性优化算法，先来看看它的原型

minxiρi(||fi(xi1,xi2,...,xik)||2)

f(Ki,Ri,Ti,Pj)=π(Ki[Ri  Ti]Pj)pij

# Ceres Solver

Ceres Solver专为求解此类问题进行了大量的优化，有很高的效率，尤其在大规模问题上，其优势更加明显。并且，Ceres内置了一些常用的函数，比如对坐标的旋转以及各类损失函数，使其在开发上也比较高效。在官网上可以找到它的编译方法和使用教程，Windows用户可以在此找到配置好的VS工程。

# 编写代码

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

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;
}
};

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 (rotations and motions)
for (size_t i = 0; i < extrinsics.size(); ++i)
{
}
// fix the first camera.
problem.SetParameterBlockConstant(extrinsics[0].ptr<double>());

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

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));

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
);
}
}

// 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.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;
}
}

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));
motions[i].copyTo(extrinsic.rowRange(3, 6));

extrinsics.push_back(extrinsic);
}

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

• 广告
• 抄袭
• 版权
• 政治
• 色情
• 无意义
• 其他

120