// Copyright (c) 2013 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//
// Author: mierle@gmail.com (Keir Mierle)
// sergey.vfx@gmail.com (Sergey Sharybin)
//
// This is an example application which contains bundle adjustment code used
// in the Libmv library and Blender. It reads problems from files passed via
// the command line and runs the bundle adjuster on the problem.
//
// File with problem a binary file, for which it is crucial to know in which
// order bytes of float values are stored in. This information is provided
// by a single character in the beginning of the file. There're two possible
// values of this byte:
// - V, which means values in the file are stored with big endian type
// - v, which means values in the file are stored with little endian type
//
// The rest of the file contains data in the following order:
// - Space in which markers' coordinates are stored in
// - Camera intrinsics
// - Number of cameras
// - Cameras
// - Number of 3D points
// - 3D points
// - Number of markers
// - Markers
//
// Markers' space could either be normalized or image (pixels). This is defined
// by the single character in the file. P means markers in the file is in image
// space, and N means markers are in normalized space.
//
// Camera intrinsics are 8 described by 8 float 8.
// This values goes in the following order:
//
// - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2
//
// Every camera is described by:
//
// - Image for which camera belongs to (single 4 bytes integer value).
// - Column-major camera rotation matrix, 9 float values.
// - Camera translation, 3-component vector of float values.
//
// Image number shall be greater or equal to zero. Order of cameras does not
// matter and gaps are possible.
//
// Every 3D point is decribed by:
//
// - Track number point belongs to (single 4 bytes integer value).
// - 3D position vector, 3-component vector of float values.
//
// Track number shall be greater or equal to zero. Order of tracks does not
// matter and gaps are possible.
//
// Finally every marker is described by:
//
// - Image marker belongs to single 4 bytes integer value).
// - Track marker belongs to single 4 bytes integer value).
// - 2D marker position vector, (two float values).
//
// Marker's space is used a default value for refine_intrinsics command line
// flag. This means if there's no refine_intrinsics flag passed via command
// line, camera intrinsics will be refined if markers in the problem are
// stored in image space and camera intrinsics will not be refined if markers
// are in normalized space.
//
// Passing refine_intrinsics command line flag defines explicitly whether
// refinement of intrinsics will happen. Currently, only none and all
// intrinsics refinement is supported.
//
// There're existing problem files dumped from blender stored in folder
// ../data/libmv-ba-problems.
#include <cstdio>
#include <fcntl.h>
#include <sstream>
#include <string>
#include <vector>
#ifdef _MSC_VER
# include <io.h>
# define open _open
# define close _close
typedef unsigned __int32 uint32_t;
#else
# include <stdint.h>
// O_BINARY is not defined on unix like platforms, as there is no
// difference between binary and text files.
#define O_BINARY 0
#endif
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
typedef Eigen::Matrix<double, 3, 3> Mat3;
typedef Eigen::Matrix<double, 6, 1> Vec6;
typedef Eigen::Vector3d Vec3;
typedef Eigen::Vector4d Vec4;
using std::vector;
DEFINE_string(input, "", "Input File name");
DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. "
"Options are: none, radial.");
namespace {
// A EuclideanCamera is the location and rotation of the camera
// viewing an image.
//
// image identifies which image this camera represents.
// R is a 3x3 matrix representing the rotation of the camera.
// t is a translation vector representing its positions.
struct EuclideanCamera {
EuclideanCamera() : image(-1) {}
EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {}
int image;
Mat3 R;
Vec3 t;
};
// A Point is the 3D location of a track.
//
// track identifies which track this point corresponds to.
// X represents the 3D position of the track.
struct EuclideanPoint {
EuclideanPoint() : track(-1) {}
EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {}
int track;
Vec3 X;
};
// A Marker is the 2D location of a tracked point in an image.
//
// x and y is the position of the marker in pixels from the top left corner
// in the image identified by an image. All markers for to the same target
// form a track identified by a common track number.
struct Marker {
int image;
int track;
double x, y;
};
// Cameras intrinsics to be bundled.
//
// BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only,
// no bundling of k3 is possible at this moment.
enum BundleIntrinsics {
BUNDLE_NO_INTRINSICS = 0,
BUNDLE_FOCAL_LENGTH = 1,
BUNDLE_PRINCIPAL_POINT = 2,
BUNDLE_RADIAL_K1 = 4,
BUNDLE_RADIAL_K2 = 8,
BUNDLE_RADIAL = 12,
BUNDLE_TANGENTIAL_P1 = 16,
BUNDLE_TANGENTIAL_P2 = 32,
BUNDLE_TANGENTIAL = 48,
};
// Denotes which blocks to keep constant during bundling.
// For example it is useful to keep camera translations constant
// when bundling tripod motions.
enum BundleConstraints {
BUNDLE_NO_CONSTRAINTS = 0,
BUNDLE_NO_TRANSLATION = 1,
};
// The intrinsics need to get combined into a single parameter block; use these
// enums to index instead of numeric constants.
enum {
OFFSET_FOCAL_LENGTH,
OFFSET_PRINCIPAL_POINT_X,
OFFSET_PRINCIPAL_POINT_Y,
OFFSET_K1,
OFFSET_K2,
OFFSET_K3,
OFFSET_P1,
OFFSET_P2,
};
// Returns a pointer to the camera corresponding to a image.
EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras,
const int image) {
if (image < 0 || image >= all_cameras->size()) {
return NULL;
}
EuclideanCamera *camera = &(*all_cameras)[image];
if (camera->image == -1) {
return NULL;
}
return camera;
}
const EuclideanCamera *CameraForImage(
const vector<EuclideanCamera> &all_cameras,
const int image) {
if (image < 0 || image >= all_cameras.size()) {
return NULL;
}
const EuclideanCamera *camera = &all_cameras[image];
if (camera->image == -1) {
return NULL;
}
return camera;
}
// Returns maximal image number at which marker exists.
int MaxImage(const vector<Marker> &all_markers) {
if (all_markers.size() == 0) {
return -1;
}
int max_image = all_markers[0].image;
for (int i = 1; i < all_markers.size(); i++) {
max_image = std::max(max_image, all_markers[i].image);
}
return max_image;
}
// Returns a pointer to the point corresponding to a track.
EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points,
const int track) {
if (track < 0 || track >= all_points->size()) {
return NULL;
}
EuclideanPoint *point = &(*all_points)[track];
if (point->track == -1) {
return NULL;
}
return point;
}
// Reader of binary file which makes sure possibly needed endian
// conversion happens when loading values like floats and integers.
//
// File's endian type is reading from a first character of file, which
// could either be V for big endian or v for little endian. This
// means you need to design file format assuming first character
// denotes file endianness in this way.
class EndianAwareFileReader {
public:
EndianAwareFileReader(void) : file_descriptor_(-1) {
// Get an endian type of the host machine.
union {
unsigned char bytes[4];
uint32_t value;
} endian_test = { { 0, 1, 2, 3 } };
host_endian_type_ = endian_test.value;
file_endian_type_ = host_endian_type_;
}
~EndianAwareFileReader(void) {
if (file_descriptor_ > 0) {
close(file_descriptor_);
}
}
bool OpenFile(const std::string &file_name) {
file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY);
if (file_descriptor_ < 0) {
return false;
}
// Get an endian tpye of data in the file.
unsigned char file_endian_type_flag = Read<unsigned char>();
if (file_endian_type_flag == 'V') {
file_endian_type_ = kBigEndian;
} else if (file_endian_type_flag == 'v') {
file_endian_type_ = kLittleEndian;
} else {
LOG(FATAL) << "Problem file is stored in unknown endian type.";
}
return true;
}
// Read value from the file, will switch endian if needed.
template <typename T>
T Read(void) const {
T value;
CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
// Switch endian type if file contains data in different type
// that current machine.
if (file_endian_type_ != host_endian_type_) {
value = SwitchEndian<T>(value);
}
return value;
}
private:
static const long int kLittleEndian = 0x03020100ul;
static const long int kBigEndian = 0x00010203ul;
// Switch endian type between big to little.
template <typename T>
T SwitchEndian(const T value) const {
if (sizeof(T) == 4) {
unsigned int temp_value = static_cast<unsigned int>(value);
return ((temp_value >> 24)) |
((temp_value << 8) & 0x00ff0000) |
((temp_value >> 8) & 0x0000ff00) |
((temp_value << 24));
} else if (sizeof(T) == 1) {
return value;
} else {
LOG(FATAL) << "Entered non-implemented part of endian switching function.";
}
}
int host_endian_type_;
int file_endian_type_;
int file_descriptor_;
};
// Read 3x3 column-major matrix from the file
void ReadMatrix3x3(const EndianAwareFileReader &file_reader,
Mat3 *matrix) {
for (int i = 0; i < 9; i++) {
(*matrix)(i % 3, i / 3) = file_reader.Read<float>();
}
}
// Read 3-vector from file
void ReadVector3(const EndianAwareFileReader &file_reader,
Vec3 *vector) {
for (int i = 0; i < 3; i++) {
(*vector)(i) = file_reader.Read<float>();
}
}
// Reads a bundle adjustment problem from the file.
//
// file_name denotes from which file to read the problem.
// camera_intrinsics will contain initial camera intrinsics values.
//
// all_cameras is a vector of all reconstructed cameras to be optimized,
// vector element with number i will contain camera for image i.
//
// all_points is a vector of all reconstructed 3D points to be optimized,
// vector element with number i will contain point for track i.
//
// all_markers is a vector of all tracked markers existing in
// the problem. Only used for reprojection error calculation, stay
// unchanged during optimization.
//
// Returns false if any kind of error happened during
// reading.
bool ReadProblemFromFile(const std::string &file_name,
double camera_intrinsics[8],
vector<EuclideanCamera> *all_cameras,
vector<EuclideanPoint> *all_points,
bool *is_image_space,
vector<Marker> *all_markers) {
EndianAwareFileReader file_reader;
if (!file_reader.OpenFile(file_name)) {
return false;
}
// Read markers' space flag.
unsigned char is_image_space_flag = file_reader.Read<unsigned char>();
if (is_image_space_flag == 'P') {
*is_image_space = true;
} else if (is_image_space_flag == 'N') {
*is_image_space = false;
} else {
LOG(FATAL) << "Problem file contains markers stored in unknown space.";
}
// Read camera intrinsics.
for (int i = 0; i < 8; i++) {
camera_intrinsics[i] = file_reader.Read<float>();
}
// Read all cameras.
int number_of_cameras = file_reader.Read<int>();
for (int i = 0; i < number_of_cameras; i++) {
EuclideanCamera camera;
camera.image = file_reader.Read<int>();
ReadMatrix3x3(file_reader, &camera.R);
ReadVector3(file_reader, &camera.t);
if (camera.image >= all_cameras->size()) {
all_cameras->resize(camera.image + 1);
}
(*all_cameras)[camera.image].image = camera.image;
(*all_cameras)[camera.image].R = camera.R;
(*all_cameras)[camera.image].t = camera.t;
}
LOG(INFO) << "Read " << number_of_cameras << " cameras.";
// Read all reconstructed 3D points.
int number_of_points = file_reader.Read<int>();
for (int i = 0; i < number_of_points; i++) {
EuclideanPoint point;
point.track = file_reader.Read<int>();
ReadVector3(file_reader, &point.X);
if (point.track >= all_points->size()) {
all_points->resize(point.track + 1);
}
(*all_points)[point.track].track = point.track;
(*all_points)[point.track].X = point.X;
}
LOG(INFO) << "Read " << number_of_points << " points.";
// And finally read all markers.
int number_of_markers = file_reader.Read<int>();
for (int i = 0; i < number_of_markers; i++) {
Marker marker;
marker.image = file_reader.Read<int>();
marker.track = file_reader.Read<int>();
marker.x = file_reader.Read<float>();
marker.y = file_reader.Read<float>();
all_markers->push_back(marker);
}
LOG(INFO) << "Read " << number_of_markers << " markers.";
return true;
}
// Apply camera intrinsics to the normalized point to get image coordinates.
// This applies the radial lens distortion to a point which is in normalized
// camera coordinates (i.e. the principal point is at (0, 0)) to get image
// coordinates in pixels. Templated for use with autodifferentiation.
template <typename T>
inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x,
const T &focal_length_y,
const T &principal_point_x,
const T &principal_point_y,
const T &k1,
const T &k2,
const T &k3,
const T &p1,
const T &p2,
const T &normalized_x,
const T &normalized_y,
T *image_x,
T *image_y) {
T x = normalized_x;
T y = normalized_y;
// Apply distortion to the normalized points to get (xd, yd).
T r2 = x*x + y*y;
T r4 = r2 * r2;
T r6 = r4 * r2;
T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);
// Apply focal length and principal point to get the final image coordinates.
*image_x = focal_length_x * xd + principal_point_x;
*image_y = focal_length_y * yd + principal_point_y;
}
// Cost functor which computes reprojection error of 3D point X
// on camera defined by angle-axis rotation and it's translation
// (which are in the same block due to optimization reasons).
//
// This functor uses a radial distortion model.
struct OpenCVReprojectionError {
OpenCVReprojectionError(const double observed_x, const double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const intrinsics,
const T* const R_t, // Rotation denoted by angle axis
// followed with translation
const T* const X, // Point coordinates 3x1.
T* residuals) const {
// Unpack the intrinsics.
const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
const T& k1 = intrinsics[OFFSET_K1];
const T& k2 = intrinsics[OFFSET_K2];
const T& k3 = intrinsics[OFFSET_K3];
const T& p1 = intrinsics[OFFSET_P1];
const T& p2 = intrinsics[OFFSET_P2];
// Compute projective coordinates: x = RX + t.
T x[3];
ceres::AngleAxisRotatePoint(R_t, X, x);
x[0] += R_t[3];
x[1] += R_t[4];
x[2] += R_t[5];
// Compute normalized coordinates: x /= x[2].
T xn = x[0] / x[2];
T yn = x[1] / x[2];
T predicted_x, predicted_y;
// Apply distortion to the normalized points to get (xd, yd).
// TODO(keir): Do early bailouts for zero distortion; these are expensive
// jet operations.
ApplyRadialDistortionCameraIntrinsics(focal_length,
focal_length,
principal_point_x,
principal_point_y,
k1, k2, k3,
p1, p2,
xn, yn,
&predicted_x,
&predicted_y);
// 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;
}
const double observed_x;
const double observed_y;
};
// Print a message to the log which camera intrinsics are gonna to be optimized.
void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
LOG(INFO) << "Bundling only camera positions.";
} else {
std::string bundling_message = "";
#define APPEND_BUNDLING_INTRINSICS(name, flag) \
if (bundle_intrinsics & flag) { \
if (!bundling_message.empty()) { \
bundling_message += ", "; \
} \
bundling_message += name; \
} (void)0
APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH);
APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1);
APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2);
APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1);
APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2);
LOG(INFO) << "Bundling " << bundling_message << ".";
}
}
// Print a message to the log containing all the camera intriniscs values.
void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) {
std::ostringstream intrinsics_output;
intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH];
intrinsics_output <<
" cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] <<
" cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y];
#define APPEND_DISTORTION_COEFFICIENT(name, offset) \
{ \
if (camera_intrinsics[offset] != 0.0) { \
intrinsics_output << " " name "=" << camera_intrinsics[offset]; \
} \
} (void)0
APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1);
APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2);
APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3);
APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1);
APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2);
#undef APPEND_DISTORTION_COEFFICIENT
LOG(INFO) << text << intrinsics_output.str();
}
// Get a vector of camera's rotations denoted by angle axis
// conjuncted with translations into single block
//
// Element with index i matches to a rotation+translation for
// camera at image i.
vector<Vec6> PackCamerasRotationAndTranslation(
const vector<Marker> &all_markers,
const vector<EuclideanCamera> &all_cameras) {
vector<Vec6> all_cameras_R_t;
int max_image = MaxImage(all_markers);
all_cameras_R_t.resize(max_image + 1);
for (int i = 0; i <= max_image; i++) {
const EuclideanCamera *camera = CameraForImage(all_cameras, i);
if (!camera) {
continue;
}
ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
&all_cameras_R_t[i](0));
all_cameras_R_t[i].tail<3>() = camera->t;
}
return all_cameras_R_t;
}
// Convert cameras rotations fro mangle axis back to rotation matrix.
void UnpackCamerasRotationAndTranslation(
const vector<Marker> &all_markers,
const vector<Vec6> &all_cameras_R_t,
vector<EuclideanCamera> *all_cameras) {
int max_image = MaxImage(all_markers);
for (int i = 0; i <= max_image; i++) {
EuclideanCamera *camera = CameraForImage(all_cameras, i);
if (!camera) {
continue;
}
ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
&camera->R(0, 0));
camera->t = all_cameras_R_t[i].tail<3>();
}
}
void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers,
const int bundle_intrinsics,
const int bundle_constraints,
double *camera_intrinsics,
vector<EuclideanCamera> *all_cameras,
vector<EuclideanPoint> *all_points) {
PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Convert cameras rotations to angle axis and merge with translation
// into single parameter block for maximal minimization speed
//
// Block for minimization has got the following structure:
// <3 elements for angle-axis> <3 elements for translation>
vector<Vec6> all_cameras_R_t =
PackCamerasRotationAndTranslation(all_markers, *all_cameras);
// Parameterization used to restrict camera motion for modal solvers.
ceres::SubsetParameterization *constant_transform_parameterization = NULL;
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
std::vector<int> constant_translation;
// First three elements are rotation, last three are translation.
constant_translation.push_back(3);
constant_translation.push_back(4);
constant_translation.push_back(5);
constant_transform_parameterization =
new ceres::SubsetParameterization(6, constant_translation);
}
int num_residuals = 0;
bool have_locked_camera = false;
for (int i = 0; i < all_markers.size(); ++i) {
const Marker &marker = all_markers[i];
EuclideanCamera *camera = CameraForImage(all_cameras, marker.image);
EuclideanPoint *point = PointForTrack(all_points, marker.track);
if (camera == NULL || point == NULL) {
continue;
}
// Rotation of camera denoted in angle axis followed with
// camera translaiton.
double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
OpenCVReprojectionError, 2, 8, 6, 3>(
new OpenCVReprojectionError(
marker.x,
marker.y)),
NULL,
camera_intrinsics,
current_camera_R_t,
&point->X(0));
// We lock the first camera to better deal with scene orientation ambiguity.
if (!have_locked_camera) {
problem.SetParameterBlockConstant(current_camera_R_t);
have_locked_camera = true;
}
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
problem.SetParameterization(current_camera_R_t,
constant_transform_parameterization);
}
num_residuals++;
}
LOG(INFO) << "Number of residuals: " << num_residuals;
if (!num_residuals) {
LOG(INFO) << "Skipping running minimizer with zero residuals";
return;
}
BundleIntrinsicsLogMessage(bundle_intrinsics);
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
// No camera intrinsics are being refined,
// set the whole parameter block as constant for best performance.
problem.SetParameterBlockConstant(camera_intrinsics);
} else {
// Set the camera intrinsics that are not to be bundled as
// constant using some macro trickery.
std::vector<int> constant_intrinsics;
#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
if (!(bundle_intrinsics & bundle_enum)) { \
constant_intrinsics.push_back(offset); \
}
MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH);
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1);
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2);
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1);
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2);
#undef MAYBE_SET_CONSTANT
// Always set K3 constant, it's not used at the moment.
constant_intrinsics.push_back(OFFSET_K3);
ceres::SubsetParameterization *subset_parameterization =
new ceres::SubsetParameterization(8, constant_intrinsics);
problem.SetParameterization(camera_intrinsics, subset_parameterization);
}
// Configure the solver.
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.preconditioner_type = ceres::SCHUR_JACOBI;
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.use_inner_iterations = true;
options.max_num_iterations = 100;
options.minimizer_progress_to_stdout = true;
// Solve!
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "Final report:\n" << summary.FullReport();
// Copy rotations and translations back.
UnpackCamerasRotationAndTranslation(all_markers,
all_cameras_R_t,
all_cameras);
PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics);
}
} // namespace
int main(int argc, char **argv) {
google::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
if (FLAGS_input.empty()) {
LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem";
return EXIT_FAILURE;
}
double camera_intrinsics[8];
vector<EuclideanCamera> all_cameras;
vector<EuclideanPoint> all_points;
bool is_image_space;
vector<Marker> all_markers;
if (!ReadProblemFromFile(FLAGS_input,
camera_intrinsics,
&all_cameras,
&all_points,
&is_image_space,
&all_markers)) {
LOG(ERROR) << "Error reading problem file";
return EXIT_FAILURE;
}
// If there's no refine_intrinsics passed via command line
// (in this case FLAGS_refine_intrinsics will be an empty string)
// we use problem's settings to detect whether intrinsics
// shall be refined or not.
//
// Namely, if problem has got markers stored in image (pixel)
// space, we do full intrinsics refinement. If markers are
// stored in normalized space, and refine_intrinsics is not
// set, no refining will happen.
//
// Using command line argument refine_intrinsics will explicitly
// declare which intrinsics need to be refined and in this case
// refining flags does not depend on problem at all.
int bundle_intrinsics = BUNDLE_NO_INTRINSICS;
if (FLAGS_refine_intrinsics.empty()) {
if (is_image_space) {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
}
} else {
if (FLAGS_refine_intrinsics == "radial") {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
} else if (FLAGS_refine_intrinsics != "none") {
LOG(ERROR) << "Unsupported value for refine-intrinsics";
return EXIT_FAILURE;
}
}
// Run the bundler.
EuclideanBundleCommonIntrinsics(all_markers,
bundle_intrinsics,
BUNDLE_NO_CONSTRAINTS,
camera_intrinsics,
&all_cameras,
&all_points);
return EXIT_SUCCESS;
}