Ceres-solver学習ノート(3)
32952 ワード
Ceresの主な目的は、大きなスケールbundle adjustment問題を解決することです.余計なことを言って、直接コードをつけます:
このルーチンは比較的長く、複雑に見えますが、実際にはoptionsのいくつかのオプションを列挙しただけで、具体的な使用もよく紹介されておらず、本質的にはあまり難しくありません.optionsの設定可能なパラメータは非常に多く、具体的にはsolver.hファイル.
// file bal_problem.h
#ifndef CERES_EXAMPLES_BAL_PROBLEM_H_
#define CERES_EXAMPLES_BAL_PROBLEM_H_
#include
namespace ceres {
namespace examples {
class BALProblem {
public:
explicit BALProblem(const std::string& filename, bool use_quaternions);
~BALProblem();
void WriteToFile(const std::string& filename) const;
void WriteToPLYFile(const std::string& filename) const;
// “ ” , marginal median 。
// , median absolute deviation 100.0
//
//
void Normalize();
//
void Perturb(const double rotation_sigma,
const double translation_sigma,
const double point_sigma);
//
int camera_block_size() const { return use_quaternions_ ? 10 : 9; }
//
int point_block_size() const { return 3; }
int num_cameras() const { return num_cameras_; }
int num_points() const { return num_points_; }
int num_observations() const { return num_observations_; }
int num_parameters() const { return num_parameters_; }
const int* point_index() const { return point_index_; }
const int* camera_index() const { return camera_index_; }
const double* observations() const { return observations_; }
const double* parameters() const { return parameters_; }
const double* cameras() const { return parameters_; }
double* mutable_cameras() { return parameters_; }
double* mutable_points() {
return parameters_ + camera_block_size() * num_cameras_;
}
private:
void CameraToAngleAxisAndCenter(const double* camera,
double* angle_axis,
double* center) const;
void AngleAxisAndCenterToCamera(const double* angle_axis,
const double* center,
double* camera) const;
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
bool use_quaternions_;
int* point_index_;
int* camera_index_;
double* observations_;
// The parameter vector is laid out as follows
// [camera_1, ..., camera_n, point_1, ..., point_m]
double* parameters_;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_BAL_PROBLEM_H_
// file bundle_adjuster.cc
#include
#include
#include
#include
#include
#include
#include "bal_problem.h"
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "snavely_reprojection_error.h"
namespace ceres {
namespace examples {
void SetLinearSolver(Solver::Options* options) {
// LinearSolver, :"sparse_schur, dense_schur, iterative_schur,
// sparse_normal_cholesky, ""dense_qr, dense_normal_cholesky and cgnr."
CHECK(StringToLinearSolverType(FLAGS_linear_solver,
&options->linear_solver_type));
// PreconditionerType, :"identity, jacobi,
// schur_jacobi, cluster_jacobi, ""cluster_tridiagonal."
CHECK(StringToPreconditionerType(FLAGS_preconditioner,
&options->preconditioner_type));
// VisibilityClusteringType, :"single_linkage,
// canonical_views"
CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
&options->visibility_clustering_type));
// SparseLinearAlgebraLibraryType, :"suite_sparse,
// cx_sparse"
CHECK(StringToSparseLinearAlgebraLibraryType(
FLAGS_sparse_linear_algebra_library,
&options->sparse_linear_algebra_library_type));
// DenseLinearAlgebraLibraryType, :"eigen,
// lapack."
CHECK(StringToDenseLinearAlgebraLibraryType(
FLAGS_dense_linear_algebra_library,
&options->dense_linear_algebra_library_type));
//
options->num_linear_solver_threads = FLAGS_num_threads;
//
options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
}
void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
// 3D
const int num_points = bal_problem->num_points();
// (3 )
const int point_block_size = bal_problem->point_block_size();
//
double* points = bal_problem->mutable_points();
//
const int num_cameras = bal_problem->num_cameras();
// (9/10)
const int camera_block_size = bal_problem->camera_block_size();
//
double* cameras = bal_problem->mutable_cameras();
// true( )
// false( )
if (options->use_inner_iterations) {
// automatic, cameras, points,(points,cameras),(cameras,points)
if (FLAGS_blocks_for_inner_iterations == "cameras") {
LOG(INFO) << "Camera blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_cameras; ++i) {
//
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
}
} else if (FLAGS_blocks_for_inner_iterations == "points") {
LOG(INFO) << "Point blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_points; ++i) {
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
}
} else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
LOG(INFO) << "Camera followed by point blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
}
for (int i = 0; i < num_points; ++i) {
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
}
} else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
LOG(INFO) << "Point followed by camera blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
}
for (int i = 0; i < num_points; ++i) {
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
}
} else if (FLAGS_blocks_for_inner_iterations == "automatic") {
LOG(INFO) << "Choosing automatic blocks for inner iterations";
} else {
LOG(FATAL) << "Unknown block type for inner iterations: "
<< FLAGS_blocks_for_inner_iterations;
}
}
// BA , 、 。 、 。
//
// Options::orderingtype=ceres::SCHUR,
// ,Ceres ,
// , Options::num_eliminate_blocks。
if (FLAGS_ordering == "automatic") {
return;
}
ceres::ParameterBlockOrdering* ordering =
new ceres::ParameterBlockOrdering;
// The points come before the cameras.
for (int i = 0; i < num_points; ++i) {
ordering->AddElementToGroup(points + point_block_size * i, 0);
}
for (int i = 0; i < num_cameras; ++i) {
// When using axis-angle, there is a single parameter block for
// the entire camera.
ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
}
options->linear_solver_ordering.reset(ordering);
}
void SetMinimizerOptions(Solver::Options* options) {
//
options->max_num_iterations = FLAGS_num_iterations;
//
options->minimizer_progress_to_stdout = true;
//
options->num_threads = FLAGS_num_threads;
//
options->eta = FLAGS_eta;
//
options->max_solver_time_in_seconds = FLAGS_max_solver_time;
// /nonmonotic
options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
if (FLAGS_line_search) {
options->minimizer_type = ceres::LINE_SEARCH;
}
CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
&options->trust_region_strategy_type));
// :raditional_dogleg,subspace_dogleg
CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
options->use_inner_iterations = FLAGS_inner_iterations;
}
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
Solver::Options* options) {
SetMinimizerOptions(options);
SetLinearSolver(options);
SetOrdering(bal_problem, options);
}
void BuildProblem(BALProblem* bal_problem, Problem* problem) {
const int point_block_size = bal_problem->point_block_size();
const int camera_block_size = bal_problem->camera_block_size();
double* points = bal_problem->mutable_points();
double* cameras = bal_problem->mutable_cameras();
// Observations u v
const double* observations = bal_problem->observations();
for (int i = 0; i < bal_problem->num_observations(); ++i) {
CostFunction* cost_function;
// Each Residual block takes a point and a camera as input and
// outputs a 2 dimensional residual.
//
cost_function =
(FLAGS_use_quaternions)
? SnavelyReprojectionErrorWithQuaternions::Create(
observations[2 * i + 0],
observations[2 * i + 1])
: SnavelyReprojectionError::Create(
observations[2 * i + 0],
observations[2 * i + 1]);
// If enabled use Huber's loss function.
LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
//
//
double* camera =
cameras + camera_block_size * bal_problem->camera_index()[i];
double* point = points + point_block_size * bal_problem->point_index()[i];
problem->AddResidualBlock(cost_function, loss_function, camera, point);
}
if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
LocalParameterization* camera_parameterization =
new ProductParameterization(
new QuaternionParameterization(),
new IdentityParameterization(6));
for (int i = 0; i < bal_problem->num_cameras(); ++i) {
// ????
problem->SetParameterization(cameras + camera_block_size * i,
camera_parameterization);
}
}
}
void SolveProblem(const char* filename) {
// BALProblem
BALProblem bal_problem(filename, FLAGS_use_quaternions);
if (!FLAGS_initial_ply.empty()) {
bal_problem.WriteToPLYFile(FLAGS_initial_ply);
}
Problem problem;
srand(FLAGS_random_seed);
bal_problem.Normalize();
//
bal_problem.Perturb(FLAGS_rotation_sigma,
FLAGS_translation_sigma,
FLAGS_point_sigma);
//
BuildProblem(&bal_problem, &problem);
Solver::Options options;
//
SetSolverOptionsFromFlags(&bal_problem, &options);
options.gradient_tolerance = 1e-16;
options.function_tolerance = 1e-16;
Solver::Summary summary;
//
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "
";
if (!FLAGS_final_ply.empty()) {
bal_problem.WriteToPLYFile(FLAGS_final_ply);
}
}
} // namespace examples
}
// main
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
if (FLAGS_input.empty()) {
LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
return 1;
}
CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
<< "--use_local_parameterization can only be used with "
<< "--use_quaternions.";
ceres::examples::SolveProblem(FLAGS_input.c_str());
return 0;
}
// file snavely_reprojection_error.h
#include "ceres/rotation.h"
namespace ceres {
namespace examples {
// . camera 9 , 3 , 3 , 1 ,2
// ,
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 {
// camera[0,1,2] angle-axis .
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
//
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.
const T xp = - p[0] / p[2];
const T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
const T r2 = xp*xp + yp*yp;
const T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
// 3D
const T predicted_x = focal * distortion * xp;
const T predicted_y = focal * distortion * yp;
// 3D
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
// AutoDiffCostFunction,2 , 1 9 , 2 3
return (new ceres::AutoDiffCostFunction2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}
double observed_x;
double observed_y;
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 10 parameters. 4 for rotation, 3 for
// translation, 1 for focal length and 2 for radial distortion. The
// principal point is not modeled (i.e. it is assumed be located at
// the image center).
struct SnavelyReprojectionErrorWithQuaternions {
// (u, v): the position of the observation with respect to the image
// center point.
SnavelyReprojectionErrorWithQuaternions(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 {
// camera[0,1,2,3] is are the rotation of the camera as a quaternion.
//
// We use QuaternionRotatePoint as it does not assume that the
// quaternion is normalized, since one of the ways to run the
// bundle adjuster is to let Ceres optimize all 4 quaternion
// parameters without a local parameterization.
T p[3];
QuaternionRotatePoint(camera, point, p);
p[0] += camera[4];
p[1] += camera[5];
p[2] += camera[6];
// 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.
const T xp = - p[0] / p[2];
const T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[8];
const T& l2 = camera[9];
const T r2 = xp*xp + yp*yp;
const T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[7];
const T predicted_x = focal * distortion * xp;
const T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
return (new ceres::AutoDiffCostFunction<
SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
new SnavelyReprojectionErrorWithQuaternions(observed_x,
observed_y)));
}
double observed_x;
double observed_y;
};
このルーチンは比較的長く、複雑に見えますが、実際にはoptionsのいくつかのオプションを列挙しただけで、具体的な使用もよく紹介されておらず、本質的にはあまり難しくありません.optionsの設定可能なパラメータは非常に多く、具体的にはsolver.hファイル.