Libmv bundle adjuster example application
Add example application which is based on bundle
adjustment code from Libmv library, which is heavily
used in Blender.
Apart from bundle adjustment code this commit also
contains real-life optimization problems from VFX
pipeline. This files are created from production
files of Tears of Steel movie.
New code is placed to examples, and could be used
either as an example implementation of BA or for
timing investigation of problems appearing in VFX.
Problems for this application are placed to
data/libmv-ba-problems.
Usage:
./libmv_bundle_adjuster --input=/path/to/problem_file.bin
There's also optional flag --refine_intrinsics which
declares explicitly whether intrinscis shall be
refined or not. If this flag is not passed, refinement
will happen for problems stored in image space.
Structure of problem files is described in header
comment of libmv_bundle_adjuster.cc.
Change-Id: I51202848c75dcd7612b707609e5ff3708e01b625
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index c0b4ec0..e9e5cef 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -66,6 +66,10 @@
bal_problem.cc)
TARGET_LINK_LIBRARIES(bundle_adjuster ceres)
+ ADD_EXECUTABLE(libmv_bundle_adjuster
+ libmv_bundle_adjuster.cc)
+ TARGET_LINK_LIBRARIES(libmv_bundle_adjuster ceres)
+
ADD_EXECUTABLE(denoising
denoising.cc
fields_of_experts.cc)
diff --git a/examples/libmv_bundle_adjuster.cc b/examples/libmv_bundle_adjuster.cc
new file mode 100644
index 0000000..80ee604
--- /dev/null
+++ b/examples/libmv_bundle_adjuster.cc
@@ -0,0 +1,835 @@
+// 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>
+#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 endianes 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;
+ }
+
+ ~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;
+ read(file_descriptor_, &value, sizeof(value));
+ // 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 optimixed.
+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;
+
+ // Solve!
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ LOG(INFO) << "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;
+}
