ceres_ba.cc

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <fstream>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <vector>

DEFINE_string(problem, "problem-49-7776-pre.txt", "dataset file name");

struct Observation
{
    int cam, pt;
    double x, y;
};

struct Dataset
{
    using CameraParam = Eigen::Matrix<double, 9, 1>;
    std::vector<Observation> observations;
    std::vector<CameraParam> cameras;
    std::vector<Eigen::Vector3d> points;
};

Dataset read_dataset(const char *filename)
{
    Dataset ds;
    std::ifstream f(filename);
    CHECK(f) << filename << " not found";
    double num_cam, num_pt, num_obs;
    f >> num_cam >> num_pt >> num_obs;
    ds.observations.resize(num_obs);
    for (int i = 0; i < num_obs; ++i)
    {
        auto &obs = ds.observations[i];
        f >> obs.cam >> obs.pt >> obs.x >> obs.y;
    }
    ds.cameras.resize(num_cam);
    for (int i = 0; i < num_cam; ++i)
    {
        auto &cam = ds.cameras[i];
        f >> cam[0] >> cam[1] >> cam[2]    // R (Rodrigues)
            >> cam[3] >> cam[4] >> cam[5]  // t
            >> cam[6] >> cam[7] >> cam[8]; // f, k1, k2
    }
    ds.points.resize(num_pt);
    for (int i = 0; i < num_pt; ++i)
    {
        auto &pt = ds.points[i];
        f >> pt[0] >> pt[1] >> pt[2];
    }
    CHECK(!f.eof());
    char tmp;
    f >> tmp;
    CHECK(f.eof());
    return ds;
}

struct ReprojectionError
{
    double obs_x, obs_y;
    ReprojectionError(double obs_x, double obs_y) : obs_x(obs_x), obs_y(obs_y)
    {
    }

    template <typename T>
    bool operator()(const T *const camera, const T *const point,
                    T *residuals) const
    {
        // world coord to camera coord
        T p_in_c[3];
        ceres::AngleAxisRotatePoint(camera, point, p_in_c);
        p_in_c[0] += camera[3];
        p_in_c[1] += camera[4];
        p_in_c[2] += camera[5];

        // homo
        T xp = -p_in_c[0] / p_in_c[2];
        T yp = -p_in_c[1] / p_in_c[2];

        // distortion
        T p2 = xp * xp + yp * yp;
        T distortion = 1.0 + p2 * camera[7] + p2 * p2 * camera[8];

        // pixel
        T x = camera[6] * distortion * xp;
        T y = camera[6] * distortion * yp;

        residuals[0] = x - obs_x;
        residuals[1] = y - obs_y;
        return true;
    }
};

int main(int argc, char **argv)
{
    google::LogToStderr();
    google::InitGoogleLogging(argv[0]);
    google::ParseCommandLineFlags(&argc, &argv, true);

    Dataset dataset = read_dataset(FLAGS_problem.c_str());

    ceres::Problem problem;
    for (auto &obs : dataset.observations)
    {
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<ReprojectionError, 2, 9, 3>(
                new ReprojectionError(obs.x, obs.y)),
            nullptr, dataset.cameras[obs.cam].data(),
            dataset.points[obs.pt].data());
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    // options.linear_solver_type = ceres::SPARSE_SCHUR;
    // options.linear_solver_type = ceres::ITERATIVE_SCHUR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    LOG(INFO) << summary.FullReport();
}

ceres_sophus_ba.cc

#include <ceres/ceres.h>
#include <fstream>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <sophus/se3.hpp>

DEFINE_string(problem, "problem-49-7776-pre.txt", "dataset file name");

struct Observation
{
    int cam_idx, pt_idx;
    double x, y;
};

struct Dataset
{
    Dataset(int num_cam, int num_pt, int num_obs)
        : obses(num_obs), cams(num_cam), poses(num_cam), points(num_pt)
    {}
    std::vector<Observation> obses;
    std::vector<Eigen::Vector3d> cams;
    std::vector<Sophus::Vector7d> poses;
    std::vector<Eigen::Vector3d> points;
};

Dataset read_dataset(const char *filename)
{
    std::ifstream f(filename);
    CHECK(f) << filename << " not found";
    int num_cam, num_pt, num_obs;
    CHECK(f >> num_cam >> num_pt >> num_obs);
    Dataset ds(num_cam, num_pt, num_obs);
    for (auto &obs : ds.obses)
        f >> obs.cam_idx >> obs.pt_idx >> obs.x >> obs.y;
    Eigen::Vector3d v;
    for (int i = 0; i < num_cam; ++i)
    {
        auto &cam = ds.cams[i];
        auto &pose = ds.poses[i];
        CHECK(f >> v[0] >> v[1] >> v[2]        // R (Rodrigues)
              >> pose[4] >> pose[5] >> pose[6] // t
              >> cam[0] >> cam[1] >> cam[2]);  // f, k1, k2
        Eigen::Quaterniond q(Eigen::AngleAxisd(v.norm(), v / v.norm()));
        pose.head<4>() = q.coeffs();
    }
    for (auto &pt : ds.points)
        CHECK(f >> pt[0] >> pt[1] >> pt[2]);
    CHECK(!f.eof());
    char tmp;
    f >> tmp;
    CHECK(f.eof());
    return ds;
}

namespace Sophus
{
class LocalParameterizationSE3 : public ceres::LocalParameterization
{
  public:
    virtual ~LocalParameterizationSE3() {}

    virtual bool Plus(const double *x_, const double *delta_,
                      double *x_plus_delta) const
    {
        Eigen::Map<SE3d const> x(x_);
        Eigen::Map<Vector6d const> delta(delta_);
        Eigen::Map<SE3d> xpd(x_plus_delta);
        xpd = x * SE3d::exp(delta);
        return true;
    }

    virtual bool ComputeJacobian(const double *x_, double *jacobian) const
    {
        Eigen::Map<SE3d const> x(x_);
        Eigen::Map<Eigen::Matrix<double, SE3d::num_parameters, SE3d::DoF,
                                 Eigen::RowMajor>>
            j(jacobian);
        j = x.Dx_this_mul_exp_x_at_0();
        return true;
    }

    virtual int GlobalSize() const { return SE3d::num_parameters; }

    virtual int LocalSize() const { return SE3d::DoF; }
};
} // namespace Sophus

struct ReprojectionError
{
    double obs_x, obs_y;
    ReprojectionError(double obs_x, double obs_y) : obs_x(obs_x), obs_y(obs_y)
    {}

    template <typename T>
    bool operator()(const T *const tr, const T *const cam, const T *const point,
                    T *residuals) const
    {
        // world coord to camera coord
        Sophus::Vector3<T> p(point[0], point[1], point[2]);
        Eigen::Map<Sophus::SE3<T> const> tran(tr);
        p = tran * p;

        // homo
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // distortion
        T p2 = xp * xp + yp * yp;
        T distortion = 1.0 + p2 * cam[1] + p2 * p2 * cam[2];

        // pixel
        T x = cam[0] * distortion * xp;
        T y = cam[0] * distortion * yp;

        residuals[0] = x - obs_x;
        residuals[1] = y - obs_y;
        return true;
    }
};

int main(int argc, char **argv)
{
    google::LogToStderr();
    google::InitGoogleLogging(argv[0]);
    google::ParseCommandLineFlags(&argc, &argv, true);

    Dataset dataset = read_dataset(FLAGS_problem.c_str());

    ceres::Problem problem;

    for (auto &pose : dataset.poses)
        problem.AddParameterBlock(pose.data(), Sophus::SE3d::num_parameters,
                                  new Sophus::LocalParameterizationSE3());

    for (auto &obs : dataset.obses)
    {
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<ReprojectionError, 2, 7, 3, 3>(
                new ReprojectionError(obs.x, obs.y)),
            nullptr, dataset.poses[obs.cam_idx].data(),
            dataset.cams[obs.cam_idx].data(),
            dataset.points[obs.pt_idx].data());
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    // options.linear_solver_type = ceres::SPARSE_SCHUR;
    // options.linear_solver_type = ceres::ITERATIVE_SCHUR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    LOG(INFO) << summary.FullReport();
}