使用Ceres库进行光束法平差(BA)求解学习记录

官方教程: http://www.ceres-solver.org/nnls_tutorial.html

在下载的源码中可以找到一些示例代码, 跟官方的教程对应,这里记录对simple_bundle_adjuster.cc这个示例的理解.

首先看所给的数据文件ceres-solver-2.1.0/data/problem-16-22106-pre.txt,由以下几个部分组成:

• 第一行3个数据为相机数量, 地图点数量(即观测到的特征点的集合的大小), 观测数量
• 第二行开始为每次观测对应的相机标号, 地图点的标号和其在相机坐标系下观测到的x与y坐标
• 在这所有的观测数据之后,对应的是只有一列的数据,首先按照相机编号的顺序包含了所有相机的参数(依次包含旋转3个值,平移3个值, 焦距1个值, 径向畸变参数2个值).
• 相机参数之后是按照点的编号的顺序的3维坐标值

总之,该文件中包含了观测(某地图点在某相机坐标系上的2维坐标)和要估计的值(相机参数和3维地图点坐标).而光束法平差要做的就是: 用相机参数将3维地图点投影到相机坐标系上,令得到的2维坐标与观测到的该点在该相机坐标系上的坐标的残差最小.

BALProblem类. 这个类主要用来读取txt文件中的数据,并确定各数据的存储地址. 注意代码中所有的数据都是用指针的方式来使用.

class BALProblem
{
public:
    // 析构函数
    ~BALProblem()
    {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }

    // 返回观测数量
    int num_observations() const
    { return num_observations_; }
    // 返回观测数据的首地址
    const double *observations() const
    { return observations_; }
    // 返回相机参数数据首地址
    double *mutable_cameras()
    { return parameters_; }
    // 返回地图点坐标数据首地址
    double *mutable_points()
    { return parameters_ + 9 * num_cameras_; }
    // 返回第i个相机数据首地址
    double *mutable_camera_for_observation(int i)
    {
        // 一组相机参数有9个数据,因此第i个相机参数数据的地址这样计算
        return mutable_cameras() + camera_index_[i] * 9;
    }
    // 返回编号为i的地图点的坐标数据地址
    double *mutable_point_for_observation(int i)
    {
        // 坐标数据包含3个值
        return mutable_points() + point_index_[i] * 3;
    }
    
    // 加载文件
    bool LoadFile(const char *filename)
    {
        FILE *fptr = fopen(filename, "r");
        if (fptr == nullptr)
        {
            return false;
        }

        // 文件中前三个数据为相机数量, 地图点数量, 观测总次数
        FscanfOrDie(fptr, "%d", &num_cameras_);
        FscanfOrDie(fptr, "%d", &num_points_);
        FscanfOrDie(fptr, "%d", &num_observations_);
        
        // 用来保存每一条观测信息中,被观测到的地图点的编号的int数组首地址
        point_index_ = new int[num_observations_];
        // 用来保存每一条观测信息中,进行观测的相机的编号
        camera_index_ = new int[num_observations_];
        // 观测数据首地址,观测数据为2维坐标,因此数组长度为2*观测次数
        observations_ = new double[2 * num_observations_];

        // 相机参数与3维地图点数据的总参数数量
        num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
        // 保存所有相机参数与3维地图点数据的数组首地址
        parameters_ = new double[num_parameters_];

        // 将每次观测的相机编号, 地图点编号, 观测的2维坐标保存到对应数组的对应位置
        for (int i = 0; i < num_observations_; ++i)
        {
            FscanfOrDie(fptr, "%d", camera_index_ + i);
            FscanfOrDie(fptr, "%d", point_index_ + i);
            for (int j = 0; j < 2; ++j)
            {
                FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
            }
        }

        // 保存所有参数
        for (int i = 0; i < num_parameters_; ++i)
        {
            FscanfOrDie(fptr, "%lf", parameters_ + i);
        }
        return true;
    }

private:
    // 模板函数, 若读取数据失败则报错
    template<typename T>
    void FscanfOrDie(FILE *fptr, const char *format, T *value)
    {
        int num_scanned = fscanf(fptr, format, value);
        if (num_scanned != 1)
        {
            LOG(FATAL) << "Invalid UW data file.";
        }
    }

    int num_cameras_;  // 相机数量
    int num_points_;  // 地图点数量
    int num_observations_;  // 观测总次数
    int num_parameters_;  // 相机参数和地图点3维坐标的总参数数量

    int *point_index_;  // 第i次观测对应的地图点编号
    int *camera_index_;  // 第i次观测对应的相机编号
    double *observations_;  // 所有的观测信息
    double *parameters_;  // 所有的相机和地图点参数

下面是重投影误差的残差块结构体SnavelyReprojectionError

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

    // 小括号运算符重载实现Functor
    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];
        // camera前三个数据为旋转, 这里放入首地址, 将point进行旋转得到p
        ceres::AngleAxisRotatePoint(camera, point, p);

        // camera[3,4,5] are the translation
        // 将p加上平移, 得到全局坐标系下的地图点在相机坐标系下的3维坐标
        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];  // 焦距
        // 地图点p重投影在相机坐标系上,并经过去畸变得到的2维坐标
        T predicted_x = focal * distortion * xp;
        T predicted_y = focal * distortion * yp;

        // The error is the difference between the predicted and observed position.
        // 重投影坐标与观测坐标x,y坐标的差
        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.
    // 创建代价函数(使用工厂模式来避免使用时的CostFuntion对象创建)
    static ceres::CostFunction *Create(const double observed_x,
                                       const double observed_y)
    {
        // 尖括号中为: 代价函数Functor, 残差数量, 每个参数块的参数个数(相机9, 地图点3)
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                new SnavelyReprojectionError(observed_x, observed_y)));
    }

    double observed_x;
    double observed_y;
};

main函数创建每个观测的残差块, 构建Problem对象并求解BA.

int main(int argc, char **argv)
{
    google::InitGoogleLogging(argv[0]);
    if (argc != 2)
    {
        std::cerr << "usage: simple_bundle_adjuster <bal_problem>\n";
        return 1;
    }
    // 创建对象, 读取数据
    BALProblem bal_problem;
    if (!bal_problem.LoadFile(argv[1]))
    {
        std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
        return 1;
    }

    // 获取观测数据
    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,
                                 nullptr /* 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.minimizer_progress_to_stdout = true;  // 输出信息

    ceres::Solver::Summary summary;  // 求解信息简报
    ceres::Solve(options, &problem, &summary);  // 求解
    std::cout << summary.FullReport() << "\n";

    return 0;
}

以上就是该代码的详解.