Bundle Adjustment
即最小化重投影误差,最小化用位姿估计的投影与实际投影的误差。
每个残差取决于一个三维点和一个九参数相机。定义相机的九个参数是:三个用于作为 Rodrigues 轴角矢量的旋转,三个用于平移,一个用于焦距,两个用于径向失真。
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 {
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p); //通过camera前三个参数对point进行旋转,并存到p中
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; //通过camera中间三个参数对point进行平移,并存到p中
T xp = - p[0] / p[2];
T yp = - p[1] / p[2]; //投影到归一化平面
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2); //畸变矫正的系数
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp; //利用正畸系数和内参,计算投影的像素坐标
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y); //重投影误差
return true;
}
//设置代价函数
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;
};
AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)
其中,2表示输出的残差维度,9表示第一个输入的维度,3表示第二个输入的维度。
构建problem,添加误差项
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(
bal_problem.observations()[2 * i + 0],
bal_problem.observations()[2 * i + 1]);
problem.AddResidualBlock(cost_function,
nullptr /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
BA问题很稀疏,解决此问题的一种方法是设置 Solver::Options::linear_solver_type
为 SPARSE_NORMAL_CHOLESKY
并调用Solve()
. 但BA问题有一个特殊的稀疏结构,Ceres 为此任务提供了三个专门的求解器(统称为基于 Schur 的求解器)。示例代码使用了其中最简单的一个DENSE_SCHUR。
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";