Skip to content

Instantly share code, notes, and snippets.

@scturtle scturtle/ceres_ba.cc
Last active Aug 18, 2018

Embed
What would you like to do?
#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();
}
#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();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.