Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/examples/libmv_bundle_adjuster.cc b/examples/libmv_bundle_adjuster.cc
new file mode 100644
index 0000000..47d0cd7
--- /dev/null
+++ b/examples/libmv_bundle_adjuster.cc
@@ -0,0 +1,843 @@
+// 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>
+#include <unistd.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 = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
+ T xd = x * r_coeff + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x);
+ T yd = y * r_coeff + 2.0 * p2 * x * y + p1 * (r2 + 2.0 * 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 - observed_x;
+ residuals[1] = predicted_y - 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) {
+ CERES_GFLAGS_NAMESPACE::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;
+}