| // 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 constexpr long int kLittleEndian = 0x03020100ul; |
| static constexpr 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) { |
| 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; |
| } |
