GoogleGit

blob: 0c6c0d6fef96d9f09768e169dc2a0d9f26bbc20c [file] [log] [blame]
  1. // Copyright (c) 2013 libmv authors.
  2. //
  3. // Permission is hereby granted, free of charge, to any person obtaining a copy
  4. // of this software and associated documentation files (the "Software"), to
  5. // deal in the Software without restriction, including without limitation the
  6. // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  7. // sell copies of the Software, and to permit persons to whom the Software is
  8. // furnished to do so, subject to the following conditions:
  9. //
  10. // The above copyright notice and this permission notice shall be included in
  11. // all copies or substantial portions of the Software.
  12. //
  13. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  18. // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  19. // IN THE SOFTWARE.
  20. //
  21. // Author: mierle@gmail.com (Keir Mierle)
  22. // sergey.vfx@gmail.com (Sergey Sharybin)
  23. //
  24. // This is an example application which contains bundle adjustment code used
  25. // in the Libmv library and Blender. It reads problems from files passed via
  26. // the command line and runs the bundle adjuster on the problem.
  27. //
  28. // File with problem a binary file, for which it is crucial to know in which
  29. // order bytes of float values are stored in. This information is provided
  30. // by a single character in the beginning of the file. There're two possible
  31. // values of this byte:
  32. // - V, which means values in the file are stored with big endian type
  33. // - v, which means values in the file are stored with little endian type
  34. //
  35. // The rest of the file contains data in the following order:
  36. // - Space in which markers' coordinates are stored in
  37. // - Camera intrinsics
  38. // - Number of cameras
  39. // - Cameras
  40. // - Number of 3D points
  41. // - 3D points
  42. // - Number of markers
  43. // - Markers
  44. //
  45. // Markers' space could either be normalized or image (pixels). This is defined
  46. // by the single character in the file. P means markers in the file is in image
  47. // space, and N means markers are in normalized space.
  48. //
  49. // Camera intrinsics are 8 described by 8 float 8.
  50. // This values goes in the following order:
  51. //
  52. // - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2
  53. //
  54. // Every camera is described by:
  55. //
  56. // - Image for which camera belongs to (single 4 bytes integer value).
  57. // - Column-major camera rotation matrix, 9 float values.
  58. // - Camera translation, 3-component vector of float values.
  59. //
  60. // Image number shall be greater or equal to zero. Order of cameras does not
  61. // matter and gaps are possible.
  62. //
  63. // Every 3D point is decribed by:
  64. //
  65. // - Track number point belongs to (single 4 bytes integer value).
  66. // - 3D position vector, 3-component vector of float values.
  67. //
  68. // Track number shall be greater or equal to zero. Order of tracks does not
  69. // matter and gaps are possible.
  70. //
  71. // Finally every marker is described by:
  72. //
  73. // - Image marker belongs to single 4 bytes integer value).
  74. // - Track marker belongs to single 4 bytes integer value).
  75. // - 2D marker position vector, (two float values).
  76. //
  77. // Marker's space is used a default value for refine_intrinsics command line
  78. // flag. This means if there's no refine_intrinsics flag passed via command
  79. // line, camera intrinsics will be refined if markers in the problem are
  80. // stored in image space and camera intrinsics will not be refined if markers
  81. // are in normalized space.
  82. //
  83. // Passing refine_intrinsics command line flag defines explicitly whether
  84. // refinement of intrinsics will happen. Currently, only none and all
  85. // intrinsics refinement is supported.
  86. //
  87. // There're existing problem files dumped from blender stored in folder
  88. // ../data/libmv-ba-problems.
  89. #include <cstdio>
  90. #include <fcntl.h>
  91. #include <sstream>
  92. #include <string>
  93. #include <vector>
  94. #ifdef _MSC_VER
  95. # include <io.h>
  96. # define open _open
  97. # define close _close
  98. typedef unsigned __int32 uint32_t;
  99. #else
  100. #include <stdint.h>
  101. #include <unistd.h>
  102. // O_BINARY is not defined on unix like platforms, as there is no
  103. // difference between binary and text files.
  104. #define O_BINARY 0
  105. #endif
  106. #include "ceres/ceres.h"
  107. #include "ceres/rotation.h"
  108. #include "gflags/gflags.h"
  109. #include "glog/logging.h"
  110. typedef Eigen::Matrix<double, 3, 3> Mat3;
  111. typedef Eigen::Matrix<double, 6, 1> Vec6;
  112. typedef Eigen::Vector3d Vec3;
  113. typedef Eigen::Vector4d Vec4;
  114. using std::vector;
  115. DEFINE_string(input, "", "Input File name");
  116. DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. "
  117. "Options are: none, radial.");
  118. namespace {
  119. // A EuclideanCamera is the location and rotation of the camera
  120. // viewing an image.
  121. //
  122. // image identifies which image this camera represents.
  123. // R is a 3x3 matrix representing the rotation of the camera.
  124. // t is a translation vector representing its positions.
  125. struct EuclideanCamera {
  126. EuclideanCamera() : image(-1) {}
  127. EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {}
  128. int image;
  129. Mat3 R;
  130. Vec3 t;
  131. };
  132. // A Point is the 3D location of a track.
  133. //
  134. // track identifies which track this point corresponds to.
  135. // X represents the 3D position of the track.
  136. struct EuclideanPoint {
  137. EuclideanPoint() : track(-1) {}
  138. EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {}
  139. int track;
  140. Vec3 X;
  141. };
  142. // A Marker is the 2D location of a tracked point in an image.
  143. //
  144. // x and y is the position of the marker in pixels from the top left corner
  145. // in the image identified by an image. All markers for to the same target
  146. // form a track identified by a common track number.
  147. struct Marker {
  148. int image;
  149. int track;
  150. double x, y;
  151. };
  152. // Cameras intrinsics to be bundled.
  153. //
  154. // BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only,
  155. // no bundling of k3 is possible at this moment.
  156. enum BundleIntrinsics {
  157. BUNDLE_NO_INTRINSICS = 0,
  158. BUNDLE_FOCAL_LENGTH = 1,
  159. BUNDLE_PRINCIPAL_POINT = 2,
  160. BUNDLE_RADIAL_K1 = 4,
  161. BUNDLE_RADIAL_K2 = 8,
  162. BUNDLE_RADIAL = 12,
  163. BUNDLE_TANGENTIAL_P1 = 16,
  164. BUNDLE_TANGENTIAL_P2 = 32,
  165. BUNDLE_TANGENTIAL = 48,
  166. };
  167. // Denotes which blocks to keep constant during bundling.
  168. // For example it is useful to keep camera translations constant
  169. // when bundling tripod motions.
  170. enum BundleConstraints {
  171. BUNDLE_NO_CONSTRAINTS = 0,
  172. BUNDLE_NO_TRANSLATION = 1,
  173. };
  174. // The intrinsics need to get combined into a single parameter block; use these
  175. // enums to index instead of numeric constants.
  176. enum {
  177. OFFSET_FOCAL_LENGTH,
  178. OFFSET_PRINCIPAL_POINT_X,
  179. OFFSET_PRINCIPAL_POINT_Y,
  180. OFFSET_K1,
  181. OFFSET_K2,
  182. OFFSET_K3,
  183. OFFSET_P1,
  184. OFFSET_P2,
  185. };
  186. // Returns a pointer to the camera corresponding to a image.
  187. EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras,
  188. const int image) {
  189. if (image < 0 || image >= all_cameras->size()) {
  190. return NULL;
  191. }
  192. EuclideanCamera *camera = &(*all_cameras)[image];
  193. if (camera->image == -1) {
  194. return NULL;
  195. }
  196. return camera;
  197. }
  198. const EuclideanCamera *CameraForImage(
  199. const vector<EuclideanCamera> &all_cameras,
  200. const int image) {
  201. if (image < 0 || image >= all_cameras.size()) {
  202. return NULL;
  203. }
  204. const EuclideanCamera *camera = &all_cameras[image];
  205. if (camera->image == -1) {
  206. return NULL;
  207. }
  208. return camera;
  209. }
  210. // Returns maximal image number at which marker exists.
  211. int MaxImage(const vector<Marker> &all_markers) {
  212. if (all_markers.size() == 0) {
  213. return -1;
  214. }
  215. int max_image = all_markers[0].image;
  216. for (int i = 1; i < all_markers.size(); i++) {
  217. max_image = std::max(max_image, all_markers[i].image);
  218. }
  219. return max_image;
  220. }
  221. // Returns a pointer to the point corresponding to a track.
  222. EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points,
  223. const int track) {
  224. if (track < 0 || track >= all_points->size()) {
  225. return NULL;
  226. }
  227. EuclideanPoint *point = &(*all_points)[track];
  228. if (point->track == -1) {
  229. return NULL;
  230. }
  231. return point;
  232. }
  233. // Reader of binary file which makes sure possibly needed endian
  234. // conversion happens when loading values like floats and integers.
  235. //
  236. // File's endian type is reading from a first character of file, which
  237. // could either be V for big endian or v for little endian. This
  238. // means you need to design file format assuming first character
  239. // denotes file endianness in this way.
  240. class EndianAwareFileReader {
  241. public:
  242. EndianAwareFileReader(void) : file_descriptor_(-1) {
  243. // Get an endian type of the host machine.
  244. union {
  245. unsigned char bytes[4];
  246. uint32_t value;
  247. } endian_test = { { 0, 1, 2, 3 } };
  248. host_endian_type_ = endian_test.value;
  249. file_endian_type_ = host_endian_type_;
  250. }
  251. ~EndianAwareFileReader(void) {
  252. if (file_descriptor_ > 0) {
  253. close(file_descriptor_);
  254. }
  255. }
  256. bool OpenFile(const std::string &file_name) {
  257. file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY);
  258. if (file_descriptor_ < 0) {
  259. return false;
  260. }
  261. // Get an endian tpye of data in the file.
  262. unsigned char file_endian_type_flag = Read<unsigned char>();
  263. if (file_endian_type_flag == 'V') {
  264. file_endian_type_ = kBigEndian;
  265. } else if (file_endian_type_flag == 'v') {
  266. file_endian_type_ = kLittleEndian;
  267. } else {
  268. LOG(FATAL) << "Problem file is stored in unknown endian type.";
  269. }
  270. return true;
  271. }
  272. // Read value from the file, will switch endian if needed.
  273. template <typename T>
  274. T Read(void) const {
  275. T value;
  276. CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
  277. // Switch endian type if file contains data in different type
  278. // that current machine.
  279. if (file_endian_type_ != host_endian_type_) {
  280. value = SwitchEndian<T>(value);
  281. }
  282. return value;
  283. }
  284. private:
  285. static const long int kLittleEndian = 0x03020100ul;
  286. static const long int kBigEndian = 0x00010203ul;
  287. // Switch endian type between big to little.
  288. template <typename T>
  289. T SwitchEndian(const T value) const {
  290. if (sizeof(T) == 4) {
  291. unsigned int temp_value = static_cast<unsigned int>(value);
  292. return ((temp_value >> 24)) |
  293. ((temp_value << 8) & 0x00ff0000) |
  294. ((temp_value >> 8) & 0x0000ff00) |
  295. ((temp_value << 24));
  296. } else if (sizeof(T) == 1) {
  297. return value;
  298. } else {
  299. LOG(FATAL) << "Entered non-implemented part of endian switching function.";
  300. }
  301. }
  302. int host_endian_type_;
  303. int file_endian_type_;
  304. int file_descriptor_;
  305. };
  306. // Read 3x3 column-major matrix from the file
  307. void ReadMatrix3x3(const EndianAwareFileReader &file_reader,
  308. Mat3 *matrix) {
  309. for (int i = 0; i < 9; i++) {
  310. (*matrix)(i % 3, i / 3) = file_reader.Read<float>();
  311. }
  312. }
  313. // Read 3-vector from file
  314. void ReadVector3(const EndianAwareFileReader &file_reader,
  315. Vec3 *vector) {
  316. for (int i = 0; i < 3; i++) {
  317. (*vector)(i) = file_reader.Read<float>();
  318. }
  319. }
  320. // Reads a bundle adjustment problem from the file.
  321. //
  322. // file_name denotes from which file to read the problem.
  323. // camera_intrinsics will contain initial camera intrinsics values.
  324. //
  325. // all_cameras is a vector of all reconstructed cameras to be optimized,
  326. // vector element with number i will contain camera for image i.
  327. //
  328. // all_points is a vector of all reconstructed 3D points to be optimized,
  329. // vector element with number i will contain point for track i.
  330. //
  331. // all_markers is a vector of all tracked markers existing in
  332. // the problem. Only used for reprojection error calculation, stay
  333. // unchanged during optimization.
  334. //
  335. // Returns false if any kind of error happened during
  336. // reading.
  337. bool ReadProblemFromFile(const std::string &file_name,
  338. double camera_intrinsics[8],
  339. vector<EuclideanCamera> *all_cameras,
  340. vector<EuclideanPoint> *all_points,
  341. bool *is_image_space,
  342. vector<Marker> *all_markers) {
  343. EndianAwareFileReader file_reader;
  344. if (!file_reader.OpenFile(file_name)) {
  345. return false;
  346. }
  347. // Read markers' space flag.
  348. unsigned char is_image_space_flag = file_reader.Read<unsigned char>();
  349. if (is_image_space_flag == 'P') {
  350. *is_image_space = true;
  351. } else if (is_image_space_flag == 'N') {
  352. *is_image_space = false;
  353. } else {
  354. LOG(FATAL) << "Problem file contains markers stored in unknown space.";
  355. }
  356. // Read camera intrinsics.
  357. for (int i = 0; i < 8; i++) {
  358. camera_intrinsics[i] = file_reader.Read<float>();
  359. }
  360. // Read all cameras.
  361. int number_of_cameras = file_reader.Read<int>();
  362. for (int i = 0; i < number_of_cameras; i++) {
  363. EuclideanCamera camera;
  364. camera.image = file_reader.Read<int>();
  365. ReadMatrix3x3(file_reader, &camera.R);
  366. ReadVector3(file_reader, &camera.t);
  367. if (camera.image >= all_cameras->size()) {
  368. all_cameras->resize(camera.image + 1);
  369. }
  370. (*all_cameras)[camera.image].image = camera.image;
  371. (*all_cameras)[camera.image].R = camera.R;
  372. (*all_cameras)[camera.image].t = camera.t;
  373. }
  374. LOG(INFO) << "Read " << number_of_cameras << " cameras.";
  375. // Read all reconstructed 3D points.
  376. int number_of_points = file_reader.Read<int>();
  377. for (int i = 0; i < number_of_points; i++) {
  378. EuclideanPoint point;
  379. point.track = file_reader.Read<int>();
  380. ReadVector3(file_reader, &point.X);
  381. if (point.track >= all_points->size()) {
  382. all_points->resize(point.track + 1);
  383. }
  384. (*all_points)[point.track].track = point.track;
  385. (*all_points)[point.track].X = point.X;
  386. }
  387. LOG(INFO) << "Read " << number_of_points << " points.";
  388. // And finally read all markers.
  389. int number_of_markers = file_reader.Read<int>();
  390. for (int i = 0; i < number_of_markers; i++) {
  391. Marker marker;
  392. marker.image = file_reader.Read<int>();
  393. marker.track = file_reader.Read<int>();
  394. marker.x = file_reader.Read<float>();
  395. marker.y = file_reader.Read<float>();
  396. all_markers->push_back(marker);
  397. }
  398. LOG(INFO) << "Read " << number_of_markers << " markers.";
  399. return true;
  400. }
  401. // Apply camera intrinsics to the normalized point to get image coordinates.
  402. // This applies the radial lens distortion to a point which is in normalized
  403. // camera coordinates (i.e. the principal point is at (0, 0)) to get image
  404. // coordinates in pixels. Templated for use with autodifferentiation.
  405. template <typename T>
  406. inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x,
  407. const T &focal_length_y,
  408. const T &principal_point_x,
  409. const T &principal_point_y,
  410. const T &k1,
  411. const T &k2,
  412. const T &k3,
  413. const T &p1,
  414. const T &p2,
  415. const T &normalized_x,
  416. const T &normalized_y,
  417. T *image_x,
  418. T *image_y) {
  419. T x = normalized_x;
  420. T y = normalized_y;
  421. // Apply distortion to the normalized points to get (xd, yd).
  422. T r2 = x*x + y*y;
  423. T r4 = r2 * r2;
  424. T r6 = r4 * r2;
  425. T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
  426. T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
  427. T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);
  428. // Apply focal length and principal point to get the final image coordinates.
  429. *image_x = focal_length_x * xd + principal_point_x;
  430. *image_y = focal_length_y * yd + principal_point_y;
  431. }
  432. // Cost functor which computes reprojection error of 3D point X
  433. // on camera defined by angle-axis rotation and it's translation
  434. // (which are in the same block due to optimization reasons).
  435. //
  436. // This functor uses a radial distortion model.
  437. struct OpenCVReprojectionError {
  438. OpenCVReprojectionError(const double observed_x, const double observed_y)
  439. : observed_x(observed_x), observed_y(observed_y) {}
  440. template <typename T>
  441. bool operator()(const T* const intrinsics,
  442. const T* const R_t, // Rotation denoted by angle axis
  443. // followed with translation
  444. const T* const X, // Point coordinates 3x1.
  445. T* residuals) const {
  446. // Unpack the intrinsics.
  447. const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
  448. const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
  449. const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
  450. const T& k1 = intrinsics[OFFSET_K1];
  451. const T& k2 = intrinsics[OFFSET_K2];
  452. const T& k3 = intrinsics[OFFSET_K3];
  453. const T& p1 = intrinsics[OFFSET_P1];
  454. const T& p2 = intrinsics[OFFSET_P2];
  455. // Compute projective coordinates: x = RX + t.
  456. T x[3];
  457. ceres::AngleAxisRotatePoint(R_t, X, x);
  458. x[0] += R_t[3];
  459. x[1] += R_t[4];
  460. x[2] += R_t[5];
  461. // Compute normalized coordinates: x /= x[2].
  462. T xn = x[0] / x[2];
  463. T yn = x[1] / x[2];
  464. T predicted_x, predicted_y;
  465. // Apply distortion to the normalized points to get (xd, yd).
  466. // TODO(keir): Do early bailouts for zero distortion; these are expensive
  467. // jet operations.
  468. ApplyRadialDistortionCameraIntrinsics(focal_length,
  469. focal_length,
  470. principal_point_x,
  471. principal_point_y,
  472. k1, k2, k3,
  473. p1, p2,
  474. xn, yn,
  475. &predicted_x,
  476. &predicted_y);
  477. // The error is the difference between the predicted and observed position.
  478. residuals[0] = predicted_x - T(observed_x);
  479. residuals[1] = predicted_y - T(observed_y);
  480. return true;
  481. }
  482. const double observed_x;
  483. const double observed_y;
  484. };
  485. // Print a message to the log which camera intrinsics are gonna to be optimized.
  486. void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
  487. if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
  488. LOG(INFO) << "Bundling only camera positions.";
  489. } else {
  490. std::string bundling_message = "";
  491. #define APPEND_BUNDLING_INTRINSICS(name, flag) \
  492. if (bundle_intrinsics & flag) { \
  493. if (!bundling_message.empty()) { \
  494. bundling_message += ", "; \
  495. } \
  496. bundling_message += name; \
  497. } (void)0
  498. APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH);
  499. APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
  500. APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1);
  501. APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2);
  502. APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1);
  503. APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2);
  504. LOG(INFO) << "Bundling " << bundling_message << ".";
  505. }
  506. }
  507. // Print a message to the log containing all the camera intriniscs values.
  508. void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) {
  509. std::ostringstream intrinsics_output;
  510. intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH];
  511. intrinsics_output <<
  512. " cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] <<
  513. " cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y];
  514. #define APPEND_DISTORTION_COEFFICIENT(name, offset) \
  515. { \
  516. if (camera_intrinsics[offset] != 0.0) { \
  517. intrinsics_output << " " name "=" << camera_intrinsics[offset]; \
  518. } \
  519. } (void)0
  520. APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1);
  521. APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2);
  522. APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3);
  523. APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1);
  524. APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2);
  525. #undef APPEND_DISTORTION_COEFFICIENT
  526. LOG(INFO) << text << intrinsics_output.str();
  527. }
  528. // Get a vector of camera's rotations denoted by angle axis
  529. // conjuncted with translations into single block
  530. //
  531. // Element with index i matches to a rotation+translation for
  532. // camera at image i.
  533. vector<Vec6> PackCamerasRotationAndTranslation(
  534. const vector<Marker> &all_markers,
  535. const vector<EuclideanCamera> &all_cameras) {
  536. vector<Vec6> all_cameras_R_t;
  537. int max_image = MaxImage(all_markers);
  538. all_cameras_R_t.resize(max_image + 1);
  539. for (int i = 0; i <= max_image; i++) {
  540. const EuclideanCamera *camera = CameraForImage(all_cameras, i);
  541. if (!camera) {
  542. continue;
  543. }
  544. ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
  545. &all_cameras_R_t[i](0));
  546. all_cameras_R_t[i].tail<3>() = camera->t;
  547. }
  548. return all_cameras_R_t;
  549. }
  550. // Convert cameras rotations fro mangle axis back to rotation matrix.
  551. void UnpackCamerasRotationAndTranslation(
  552. const vector<Marker> &all_markers,
  553. const vector<Vec6> &all_cameras_R_t,
  554. vector<EuclideanCamera> *all_cameras) {
  555. int max_image = MaxImage(all_markers);
  556. for (int i = 0; i <= max_image; i++) {
  557. EuclideanCamera *camera = CameraForImage(all_cameras, i);
  558. if (!camera) {
  559. continue;
  560. }
  561. ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
  562. &camera->R(0, 0));
  563. camera->t = all_cameras_R_t[i].tail<3>();
  564. }
  565. }
  566. void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers,
  567. const int bundle_intrinsics,
  568. const int bundle_constraints,
  569. double *camera_intrinsics,
  570. vector<EuclideanCamera> *all_cameras,
  571. vector<EuclideanPoint> *all_points) {
  572. PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
  573. ceres::Problem::Options problem_options;
  574. ceres::Problem problem(problem_options);
  575. // Convert cameras rotations to angle axis and merge with translation
  576. // into single parameter block for maximal minimization speed
  577. //
  578. // Block for minimization has got the following structure:
  579. // <3 elements for angle-axis> <3 elements for translation>
  580. vector<Vec6> all_cameras_R_t =
  581. PackCamerasRotationAndTranslation(all_markers, *all_cameras);
  582. // Parameterization used to restrict camera motion for modal solvers.
  583. ceres::SubsetParameterization *constant_transform_parameterization = NULL;
  584. if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
  585. std::vector<int> constant_translation;
  586. // First three elements are rotation, last three are translation.
  587. constant_translation.push_back(3);
  588. constant_translation.push_back(4);
  589. constant_translation.push_back(5);
  590. constant_transform_parameterization =
  591. new ceres::SubsetParameterization(6, constant_translation);
  592. }
  593. int num_residuals = 0;
  594. bool have_locked_camera = false;
  595. for (int i = 0; i < all_markers.size(); ++i) {
  596. const Marker &marker = all_markers[i];
  597. EuclideanCamera *camera = CameraForImage(all_cameras, marker.image);
  598. EuclideanPoint *point = PointForTrack(all_points, marker.track);
  599. if (camera == NULL || point == NULL) {
  600. continue;
  601. }
  602. // Rotation of camera denoted in angle axis followed with
  603. // camera translaiton.
  604. double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
  605. problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
  606. OpenCVReprojectionError, 2, 8, 6, 3>(
  607. new OpenCVReprojectionError(
  608. marker.x,
  609. marker.y)),
  610. NULL,
  611. camera_intrinsics,
  612. current_camera_R_t,
  613. &point->X(0));
  614. // We lock the first camera to better deal with scene orientation ambiguity.
  615. if (!have_locked_camera) {
  616. problem.SetParameterBlockConstant(current_camera_R_t);
  617. have_locked_camera = true;
  618. }
  619. if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
  620. problem.SetParameterization(current_camera_R_t,
  621. constant_transform_parameterization);
  622. }
  623. num_residuals++;
  624. }
  625. LOG(INFO) << "Number of residuals: " << num_residuals;
  626. if (!num_residuals) {
  627. LOG(INFO) << "Skipping running minimizer with zero residuals";
  628. return;
  629. }
  630. BundleIntrinsicsLogMessage(bundle_intrinsics);
  631. if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
  632. // No camera intrinsics are being refined,
  633. // set the whole parameter block as constant for best performance.
  634. problem.SetParameterBlockConstant(camera_intrinsics);
  635. } else {
  636. // Set the camera intrinsics that are not to be bundled as
  637. // constant using some macro trickery.
  638. std::vector<int> constant_intrinsics;
  639. #define MAYBE_SET_CONSTANT(bundle_enum, offset) \
  640. if (!(bundle_intrinsics & bundle_enum)) { \
  641. constant_intrinsics.push_back(offset); \
  642. }
  643. MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH);
  644. MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
  645. MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
  646. MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1);
  647. MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2);
  648. MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1);
  649. MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2);
  650. #undef MAYBE_SET_CONSTANT
  651. // Always set K3 constant, it's not used at the moment.
  652. constant_intrinsics.push_back(OFFSET_K3);
  653. ceres::SubsetParameterization *subset_parameterization =
  654. new ceres::SubsetParameterization(8, constant_intrinsics);
  655. problem.SetParameterization(camera_intrinsics, subset_parameterization);
  656. }
  657. // Configure the solver.
  658. ceres::Solver::Options options;
  659. options.use_nonmonotonic_steps = true;
  660. options.preconditioner_type = ceres::SCHUR_JACOBI;
  661. options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  662. options.use_inner_iterations = true;
  663. options.max_num_iterations = 100;
  664. options.minimizer_progress_to_stdout = true;
  665. // Solve!
  666. ceres::Solver::Summary summary;
  667. ceres::Solve(options, &problem, &summary);
  668. std::cout << "Final report:\n" << summary.FullReport();
  669. // Copy rotations and translations back.
  670. UnpackCamerasRotationAndTranslation(all_markers,
  671. all_cameras_R_t,
  672. all_cameras);
  673. PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics);
  674. }
  675. } // namespace
  676. int main(int argc, char **argv) {
  677. CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
  678. google::InitGoogleLogging(argv[0]);
  679. if (FLAGS_input.empty()) {
  680. LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem";
  681. return EXIT_FAILURE;
  682. }
  683. double camera_intrinsics[8];
  684. vector<EuclideanCamera> all_cameras;
  685. vector<EuclideanPoint> all_points;
  686. bool is_image_space;
  687. vector<Marker> all_markers;
  688. if (!ReadProblemFromFile(FLAGS_input,
  689. camera_intrinsics,
  690. &all_cameras,
  691. &all_points,
  692. &is_image_space,
  693. &all_markers)) {
  694. LOG(ERROR) << "Error reading problem file";
  695. return EXIT_FAILURE;
  696. }
  697. // If there's no refine_intrinsics passed via command line
  698. // (in this case FLAGS_refine_intrinsics will be an empty string)
  699. // we use problem's settings to detect whether intrinsics
  700. // shall be refined or not.
  701. //
  702. // Namely, if problem has got markers stored in image (pixel)
  703. // space, we do full intrinsics refinement. If markers are
  704. // stored in normalized space, and refine_intrinsics is not
  705. // set, no refining will happen.
  706. //
  707. // Using command line argument refine_intrinsics will explicitly
  708. // declare which intrinsics need to be refined and in this case
  709. // refining flags does not depend on problem at all.
  710. int bundle_intrinsics = BUNDLE_NO_INTRINSICS;
  711. if (FLAGS_refine_intrinsics.empty()) {
  712. if (is_image_space) {
  713. bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
  714. }
  715. } else {
  716. if (FLAGS_refine_intrinsics == "radial") {
  717. bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
  718. } else if (FLAGS_refine_intrinsics != "none") {
  719. LOG(ERROR) << "Unsupported value for refine-intrinsics";
  720. return EXIT_FAILURE;
  721. }
  722. }
  723. // Run the bundler.
  724. EuclideanBundleCommonIntrinsics(all_markers,
  725. bundle_intrinsics,
  726. BUNDLE_NO_CONSTRAINTS,
  727. camera_intrinsics,
  728. &all_cameras,
  729. &all_points);
  730. return EXIT_SUCCESS;
  731. }