11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright (c) 2013 libmv authors. 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Permission is hereby granted, free of charge, to any person obtaining a copy 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// of this software and associated documentation files (the "Software"), to 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// deal in the Software without restriction, including without limitation the 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sell copies of the Software, and to permit persons to whom the Software is 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// furnished to do so, subject to the following conditions: 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The above copyright notice and this permission notice shall be included in 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// all copies or substantial portions of the Software. 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IN THE SOFTWARE. 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: mierle@gmail.com (Keir Mierle) 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sergey.vfx@gmail.com (Sergey Sharybin) 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This is an example application which contains bundle adjustment code used 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// in the Libmv library and Blender. It reads problems from files passed via 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the command line and runs the bundle adjuster on the problem. 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// File with problem a binary file, for which it is crucial to know in which 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// order bytes of float values are stored in. This information is provided 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// by a single character in the beginning of the file. There're two possible 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// values of this byte: 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - V, which means values in the file are stored with big endian type 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - v, which means values in the file are stored with little endian type 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The rest of the file contains data in the following order: 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Space in which markers' coordinates are stored in 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Camera intrinsics 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Number of cameras 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Cameras 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Number of 3D points 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - 3D points 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Number of markers 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Markers 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Markers' space could either be normalized or image (pixels). This is defined 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// by the single character in the file. P means markers in the file is in image 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// space, and N means markers are in normalized space. 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Camera intrinsics are 8 described by 8 float 8. 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This values goes in the following order: 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Every camera is described by: 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Image for which camera belongs to (single 4 bytes integer value). 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Column-major camera rotation matrix, 9 float values. 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Camera translation, 3-component vector of float values. 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Image number shall be greater or equal to zero. Order of cameras does not 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// matter and gaps are possible. 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Every 3D point is decribed by: 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Track number point belongs to (single 4 bytes integer value). 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - 3D position vector, 3-component vector of float values. 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Track number shall be greater or equal to zero. Order of tracks does not 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// matter and gaps are possible. 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Finally every marker is described by: 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Image marker belongs to single 4 bytes integer value). 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - Track marker belongs to single 4 bytes integer value). 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// - 2D marker position vector, (two float values). 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Marker's space is used a default value for refine_intrinsics command line 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// flag. This means if there's no refine_intrinsics flag passed via command 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// line, camera intrinsics will be refined if markers in the problem are 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// stored in image space and camera intrinsics will not be refined if markers 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// are in normalized space. 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Passing refine_intrinsics command line flag defines explicitly whether 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// refinement of intrinsics will happen. Currently, only none and all 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// intrinsics refinement is supported. 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// There're existing problem files dumped from blender stored in folder 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ../data/libmv-ba-problems. 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cstdio> 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <fcntl.h> 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <sstream> 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <string> 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifdef _MSC_VER 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling# include <io.h> 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling# define open _open 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling# define close _close 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef unsigned __int32 uint32_t; 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#else 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling# include <stdint.h> 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// O_BINARY is not defined on unix like platforms, as there is no 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// difference between binary and text files. 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define O_BINARY 0 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/ceres.h" 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/rotation.h" 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gflags/gflags.h" 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef Eigen::Matrix<double, 3, 3> Mat3; 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef Eigen::Matrix<double, 6, 1> Vec6; 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef Eigen::Vector3d Vec3; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef Eigen::Vector4d Vec4; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing std::vector; 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingDEFINE_string(input, "", "Input File name"); 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingDEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. " 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "Options are: none, radial."); 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace { 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A EuclideanCamera is the location and rotation of the camera 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// viewing an image. 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// image identifies which image this camera represents. 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// R is a 3x3 matrix representing the rotation of the camera. 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// t is a translation vector representing its positions. 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct EuclideanCamera { 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera() : image(-1) {} 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {} 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int image; 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Mat3 R; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vec3 t; 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A Point is the 3D location of a track. 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// track identifies which track this point corresponds to. 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// X represents the 3D position of the track. 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct EuclideanPoint { 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanPoint() : track(-1) {} 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {} 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int track; 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vec3 X; 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A Marker is the 2D location of a tracked point in an image. 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x and y is the position of the marker in pixels from the top left corner 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// in the image identified by an image. All markers for to the same target 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// form a track identified by a common track number. 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct Marker { 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int image; 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int track; 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x, y; 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Cameras intrinsics to be bundled. 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only, 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// no bundling of k3 is possible at this moment. 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingenum BundleIntrinsics { 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_NO_INTRINSICS = 0, 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_FOCAL_LENGTH = 1, 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_PRINCIPAL_POINT = 2, 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_RADIAL_K1 = 4, 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_RADIAL_K2 = 8, 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_RADIAL = 12, 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_TANGENTIAL_P1 = 16, 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_TANGENTIAL_P2 = 32, 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_TANGENTIAL = 48, 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Denotes which blocks to keep constant during bundling. 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// For example it is useful to keep camera translations constant 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// when bundling tripod motions. 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingenum BundleConstraints { 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_NO_CONSTRAINTS = 0, 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_NO_TRANSLATION = 1, 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The intrinsics need to get combined into a single parameter block; use these 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// enums to index instead of numeric constants. 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingenum { 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_FOCAL_LENGTH, 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_PRINCIPAL_POINT_X, 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_PRINCIPAL_POINT_Y, 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_K1, 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_K2, 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_K3, 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_P1, 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OFFSET_P2, 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Returns a pointer to the camera corresponding to a image. 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingEuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras, 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int image) { 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (image < 0 || image >= all_cameras->size()) { 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera *camera = &(*all_cameras)[image]; 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (camera->image == -1) { 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return camera; 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingconst EuclideanCamera *CameraForImage( 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<EuclideanCamera> &all_cameras, 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int image) { 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (image < 0 || image >= all_cameras.size()) { 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const EuclideanCamera *camera = &all_cameras[image]; 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (camera->image == -1) { 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return camera; 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Returns maximal image number at which marker exists. 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint MaxImage(const vector<Marker> &all_markers) { 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (all_markers.size() == 0) { 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return -1; 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int max_image = all_markers[0].image; 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 1; i < all_markers.size(); i++) { 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling max_image = std::max(max_image, all_markers[i].image); 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return max_image; 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Returns a pointer to the point corresponding to a track. 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingEuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points, 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int track) { 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (track < 0 || track >= all_points->size()) { 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanPoint *point = &(*all_points)[track]; 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (point->track == -1) { 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return NULL; 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return point; 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Reader of binary file which makes sure possibly needed endian 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// conversion happens when loading values like floats and integers. 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// File's endian type is reading from a first character of file, which 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// could either be V for big endian or v for little endian. This 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// means you need to design file format assuming first character 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// denotes file endianness in this way. 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass EndianAwareFileReader { 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EndianAwareFileReader(void) : file_descriptor_(-1) { 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Get an endian type of the host machine. 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling union { 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling unsigned char bytes[4]; 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling uint32_t value; 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } endian_test = { { 0, 1, 2, 3 } }; 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling host_endian_type_ = endian_test.value; 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling file_endian_type_ = host_endian_type_; 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ~EndianAwareFileReader(void) { 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (file_descriptor_ > 0) { 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling close(file_descriptor_); 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool OpenFile(const std::string &file_name) { 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY); 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (file_descriptor_ < 0) { 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return false; 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Get an endian tpye of data in the file. 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling unsigned char file_endian_type_flag = Read<unsigned char>(); 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (file_endian_type_flag == 'V') { 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling file_endian_type_ = kBigEndian; 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else if (file_endian_type_flag == 'v') { 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling file_endian_type_ = kLittleEndian; 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(FATAL) << "Problem file is stored in unknown endian type."; 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Read value from the file, will switch endian if needed. 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T Read(void) const { 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T value; 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0); 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Switch endian type if file contains data in different type 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // that current machine. 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (file_endian_type_ != host_endian_type_) { 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling value = SwitchEndian<T>(value); 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return value; 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling private: 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling static const long int kLittleEndian = 0x03020100ul; 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling static const long int kBigEndian = 0x00010203ul; 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Switch endian type between big to little. 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T SwitchEndian(const T value) const { 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (sizeof(T) == 4) { 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling unsigned int temp_value = static_cast<unsigned int>(value); 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return ((temp_value >> 24)) | 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ((temp_value << 8) & 0x00ff0000) | 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ((temp_value >> 8) & 0x0000ff00) | 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ((temp_value << 24)); 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else if (sizeof(T) == 1) { 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return value; 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(FATAL) << "Entered non-implemented part of endian switching function."; 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int host_endian_type_; 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int file_endian_type_; 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int file_descriptor_; 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Read 3x3 column-major matrix from the file 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ReadMatrix3x3(const EndianAwareFileReader &file_reader, 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Mat3 *matrix) { 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 9; i++) { 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*matrix)(i % 3, i / 3) = file_reader.Read<float>(); 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Read 3-vector from file 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ReadVector3(const EndianAwareFileReader &file_reader, 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vec3 *vector) { 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; i++) { 3451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*vector)(i) = file_reader.Read<float>(); 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Reads a bundle adjustment problem from the file. 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// file_name denotes from which file to read the problem. 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// camera_intrinsics will contain initial camera intrinsics values. 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// all_cameras is a vector of all reconstructed cameras to be optimized, 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector element with number i will contain camera for image i. 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// all_points is a vector of all reconstructed 3D points to be optimized, 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector element with number i will contain point for track i. 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// all_markers is a vector of all tracked markers existing in 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the problem. Only used for reprojection error calculation, stay 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// unchanged during optimization. 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Returns false if any kind of error happened during 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// reading. 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool ReadProblemFromFile(const std::string &file_name, 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double camera_intrinsics[8], 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanCamera> *all_cameras, 3691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanPoint> *all_points, 3701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool *is_image_space, 3711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<Marker> *all_markers) { 3721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EndianAwareFileReader file_reader; 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!file_reader.OpenFile(file_name)) { 3741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return false; 3751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Read markers' space flag. 3781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling unsigned char is_image_space_flag = file_reader.Read<unsigned char>(); 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (is_image_space_flag == 'P') { 3801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *is_image_space = true; 3811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else if (is_image_space_flag == 'N') { 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *is_image_space = false; 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(FATAL) << "Problem file contains markers stored in unknown space."; 3851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Read camera intrinsics. 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 8; i++) { 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera_intrinsics[i] = file_reader.Read<float>(); 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Read all cameras. 3931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int number_of_cameras = file_reader.Read<int>(); 3941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < number_of_cameras; i++) { 3951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera camera; 3961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera.image = file_reader.Read<int>(); 3981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ReadMatrix3x3(file_reader, &camera.R); 3991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ReadVector3(file_reader, &camera.t); 4001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (camera.image >= all_cameras->size()) { 4021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_cameras->resize(camera.image + 1); 4031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*all_cameras)[camera.image].image = camera.image; 4061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*all_cameras)[camera.image].R = camera.R; 4071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*all_cameras)[camera.image].t = camera.t; 4081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Read " << number_of_cameras << " cameras."; 4111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Read all reconstructed 3D points. 4131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int number_of_points = file_reader.Read<int>(); 4141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < number_of_points; i++) { 4151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanPoint point; 4161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling point.track = file_reader.Read<int>(); 4181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ReadVector3(file_reader, &point.X); 4191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (point.track >= all_points->size()) { 4211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_points->resize(point.track + 1); 4221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*all_points)[point.track].track = point.track; 4251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*all_points)[point.track].X = point.X; 4261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Read " << number_of_points << " points."; 4291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // And finally read all markers. 4311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int number_of_markers = file_reader.Read<int>(); 4321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < number_of_markers; i++) { 4331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Marker marker; 4341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.image = file_reader.Read<int>(); 4361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.track = file_reader.Read<int>(); 4371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.x = file_reader.Read<float>(); 4381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.y = file_reader.Read<float>(); 4391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_markers->push_back(marker); 4411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Read " << number_of_markers << " markers."; 4441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 4461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Apply camera intrinsics to the normalized point to get image coordinates. 4491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This applies the radial lens distortion to a point which is in normalized 4501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// camera coordinates (i.e. the principal point is at (0, 0)) to get image 4511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// coordinates in pixels. Templated for use with autodifferentiation. 4521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T> 4531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x, 4541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &focal_length_y, 4551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &principal_point_x, 4561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &principal_point_y, 4571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &k1, 4581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &k2, 4591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &k3, 4601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &p1, 4611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &p2, 4621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &normalized_x, 4631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T &normalized_y, 4641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T *image_x, 4651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T *image_y) { 4661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T x = normalized_x; 4671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T y = normalized_y; 4681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Apply distortion to the normalized points to get (xd, yd). 4701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T r2 = x*x + y*y; 4711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T r4 = r2 * r2; 4721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T r6 = r4 * r2; 4731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6); 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x); 4751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y); 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Apply focal length and principal point to get the final image coordinates. 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *image_x = focal_length_x * xd + principal_point_x; 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *image_y = focal_length_y * yd + principal_point_y; 4801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Cost functor which computes reprojection error of 3D point X 4831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// on camera defined by angle-axis rotation and it's translation 4841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// (which are in the same block due to optimization reasons). 4851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 4861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This functor uses a radial distortion model. 4871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct OpenCVReprojectionError { 4881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OpenCVReprojectionError(const double observed_x, const double observed_y) 4891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : observed_x(observed_x), observed_y(observed_y) {} 4901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 4921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* const intrinsics, 4931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* const R_t, // Rotation denoted by angle axis 4941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // followed with translation 4951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* const X, // Point coordinates 3x1. 4961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* residuals) const { 4971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Unpack the intrinsics. 4981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH]; 4991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X]; 5001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y]; 5011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& k1 = intrinsics[OFFSET_K1]; 5021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& k2 = intrinsics[OFFSET_K2]; 5031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& k3 = intrinsics[OFFSET_K3]; 5041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& p1 = intrinsics[OFFSET_P1]; 5051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T& p2 = intrinsics[OFFSET_P2]; 5061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Compute projective coordinates: x = RX + t. 5081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T x[3]; 5091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::AngleAxisRotatePoint(R_t, X, x); 5111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[0] += R_t[3]; 5121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[1] += R_t[4]; 5131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[2] += R_t[5]; 5141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Compute normalized coordinates: x /= x[2]. 5161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T xn = x[0] / x[2]; 5171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T yn = x[1] / x[2]; 5181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T predicted_x, predicted_y; 5201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Apply distortion to the normalized points to get (xd, yd). 5221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // TODO(keir): Do early bailouts for zero distortion; these are expensive 5231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // jet operations. 5241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ApplyRadialDistortionCameraIntrinsics(focal_length, 5251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling focal_length, 5261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling principal_point_x, 5271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling principal_point_y, 5281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling k1, k2, k3, 5291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling p1, p2, 5301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling xn, yn, 5311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &predicted_x, 5321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &predicted_y); 5331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The error is the difference between the predicted and observed position. 5351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = predicted_x - T(observed_x); 5361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = predicted_y - T(observed_y); 5371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 5391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double observed_x; 5421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double observed_y; 5431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 5441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Print a message to the log which camera intrinsics are gonna to be optimized. 5461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid BundleIntrinsicsLogMessage(const int bundle_intrinsics) { 5471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) { 5481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Bundling only camera positions."; 5491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 5501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::string bundling_message = ""; 5511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define APPEND_BUNDLING_INTRINSICS(name, flag) \ 5531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (bundle_intrinsics & flag) { \ 5541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!bundling_message.empty()) { \ 5551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bundling_message += ", "; \ 5561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } \ 5571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bundling_message += name; \ 5581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } (void)0 5591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH); 5611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT); 5621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1); 5631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2); 5641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1); 5651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2); 5661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Bundling " << bundling_message << "."; 5681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Print a message to the log containing all the camera intriniscs values. 5721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) { 5731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::ostringstream intrinsics_output; 5741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH]; 5761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling intrinsics_output << 5781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling " cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] << 5791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling " cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y]; 5801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define APPEND_DISTORTION_COEFFICIENT(name, offset) \ 5821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { \ 5831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (camera_intrinsics[offset] != 0.0) { \ 5841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling intrinsics_output << " " name "=" << camera_intrinsics[offset]; \ 5851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } \ 5861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } (void)0 5871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1); 5891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2); 5901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3); 5911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1); 5921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2); 5931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef APPEND_DISTORTION_COEFFICIENT 5951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << text << intrinsics_output.str(); 5971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Get a vector of camera's rotations denoted by angle axis 6001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// conjuncted with translations into single block 6011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 6021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Element with index i matches to a rotation+translation for 6031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// camera at image i. 6041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvector<Vec6> PackCamerasRotationAndTranslation( 6051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<Marker> &all_markers, 6061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<EuclideanCamera> &all_cameras) { 6071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<Vec6> all_cameras_R_t; 6081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int max_image = MaxImage(all_markers); 6091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_cameras_R_t.resize(max_image + 1); 6111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i <= max_image; i++) { 6131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const EuclideanCamera *camera = CameraForImage(all_cameras, i); 6141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!camera) { 6161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 6171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::RotationMatrixToAngleAxis(&camera->R(0, 0), 6201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_cameras_R_t[i](0)); 6211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_cameras_R_t[i].tail<3>() = camera->t; 6221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return all_cameras_R_t; 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 6261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Convert cameras rotations fro mangle axis back to rotation matrix. 6281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid UnpackCamerasRotationAndTranslation( 6291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<Marker> &all_markers, 6301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<Vec6> &all_cameras_R_t, 6311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanCamera> *all_cameras) { 6321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int max_image = MaxImage(all_markers); 6331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i <= max_image; i++) { 6351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera *camera = CameraForImage(all_cameras, i); 6361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!camera) { 6381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 6391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0), 6421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &camera->R(0, 0)); 6431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera->t = all_cameras_R_t[i].tail<3>(); 6441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 6461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers, 6481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int bundle_intrinsics, 6491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int bundle_constraints, 6501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double *camera_intrinsics, 6511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanCamera> *all_cameras, 6521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanPoint> *all_points) { 6531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics); 6541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::Problem::Options problem_options; 6561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::Problem problem(problem_options); 6571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Convert cameras rotations to angle axis and merge with translation 6591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // into single parameter block for maximal minimization speed 6601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 6611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Block for minimization has got the following structure: 6621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // <3 elements for angle-axis> <3 elements for translation> 6631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<Vec6> all_cameras_R_t = 6641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling PackCamerasRotationAndTranslation(all_markers, *all_cameras); 6651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Parameterization used to restrict camera motion for modal solvers. 6671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::SubsetParameterization *constant_transform_parameterization = NULL; 6681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (bundle_constraints & BUNDLE_NO_TRANSLATION) { 6691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::vector<int> constant_translation; 6701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // First three elements are rotation, last three are translation. 6721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_translation.push_back(3); 6731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_translation.push_back(4); 6741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_translation.push_back(5); 6751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_transform_parameterization = 6771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new ceres::SubsetParameterization(6, constant_translation); 6781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_residuals = 0; 6811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool have_locked_camera = false; 6821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < all_markers.size(); ++i) { 6831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Marker &marker = all_markers[i]; 6841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanCamera *camera = CameraForImage(all_cameras, marker.image); 6851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanPoint *point = PointForTrack(all_points, marker.track); 6861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (camera == NULL || point == NULL) { 6871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 6881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rotation of camera denoted in angle axis followed with 6911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // camera translaiton. 6921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double *current_camera_R_t = &all_cameras_R_t[camera->image](0); 6931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddResidualBlock(new ceres::AutoDiffCostFunction< 6951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling OpenCVReprojectionError, 2, 8, 6, 3>( 6961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new OpenCVReprojectionError( 6971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.x, 6981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling marker.y)), 6991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, 7001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera_intrinsics, 7011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_camera_R_t, 7021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &point->X(0)); 7031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // We lock the first camera to better deal with scene orientation ambiguity. 7051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!have_locked_camera) { 7061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterBlockConstant(current_camera_R_t); 7071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling have_locked_camera = true; 7081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (bundle_constraints & BUNDLE_NO_TRANSLATION) { 7111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterization(current_camera_R_t, 7121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_transform_parameterization); 7131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_residuals++; 7161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Number of residuals: " << num_residuals; 7181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!num_residuals) { 7201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Skipping running minimizer with zero residuals"; 7211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 7221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BundleIntrinsicsLogMessage(bundle_intrinsics); 7251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) { 7271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // No camera intrinsics are being refined, 7281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // set the whole parameter block as constant for best performance. 7291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterBlockConstant(camera_intrinsics); 7301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 7311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Set the camera intrinsics that are not to be bundled as 7321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // constant using some macro trickery. 7331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::vector<int> constant_intrinsics; 7351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define MAYBE_SET_CONSTANT(bundle_enum, offset) \ 7361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!(bundle_intrinsics & bundle_enum)) { \ 7371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_intrinsics.push_back(offset); \ 7381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH); 7401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X); 7411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y); 7421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1); 7431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2); 7441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1); 7451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2); 7461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef MAYBE_SET_CONSTANT 7471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Always set K3 constant, it's not used at the moment. 7491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_intrinsics.push_back(OFFSET_K3); 7501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::SubsetParameterization *subset_parameterization = 7521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new ceres::SubsetParameterization(8, constant_intrinsics); 7531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterization(camera_intrinsics, subset_parameterization); 7551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Configure the solver. 7581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::Solver::Options options; 7591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.use_nonmonotonic_steps = true; 7601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.preconditioner_type = ceres::SCHUR_JACOBI; 7611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.linear_solver_type = ceres::ITERATIVE_SCHUR; 7621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.use_inner_iterations = true; 7631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_num_iterations = 100; 7641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.minimizer_progress_to_stdout = true; 7651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Solve! 7671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::Solver::Summary summary; 7681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres::Solve(options, &problem, &summary); 7691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::cout << "Final report:\n" << summary.FullReport(); 7711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Copy rotations and translations back. 7731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling UnpackCamerasRotationAndTranslation(all_markers, 7741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_cameras_R_t, 7751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_cameras); 7761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics); 7781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 7791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace 7801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint main(int argc, char **argv) { 7821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling google::ParseCommandLineFlags(&argc, &argv, true); 7831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling google::InitGoogleLogging(argv[0]); 7841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (FLAGS_input.empty()) { 7861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem"; 7871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return EXIT_FAILURE; 7881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double camera_intrinsics[8]; 7911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanCamera> all_cameras; 7921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<EuclideanPoint> all_points; 7931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool is_image_space; 7941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<Marker> all_markers; 7951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!ReadProblemFromFile(FLAGS_input, 7971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera_intrinsics, 7981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_cameras, 7991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_points, 8001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &is_image_space, 8011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_markers)) { 8021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Error reading problem file"; 8031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return EXIT_FAILURE; 8041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 8061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If there's no refine_intrinsics passed via command line 8071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (in this case FLAGS_refine_intrinsics will be an empty string) 8081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // we use problem's settings to detect whether intrinsics 8091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // shall be refined or not. 8101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 8111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Namely, if problem has got markers stored in image (pixel) 8121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // space, we do full intrinsics refinement. If markers are 8131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // stored in normalized space, and refine_intrinsics is not 8141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // set, no refining will happen. 8151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 8161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Using command line argument refine_intrinsics will explicitly 8171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // declare which intrinsics need to be refined and in this case 8181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // refining flags does not depend on problem at all. 8191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int bundle_intrinsics = BUNDLE_NO_INTRINSICS; 8201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (FLAGS_refine_intrinsics.empty()) { 8211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (is_image_space) { 8221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL; 8231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 8251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (FLAGS_refine_intrinsics == "radial") { 8261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL; 8271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else if (FLAGS_refine_intrinsics != "none") { 8281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Unsupported value for refine-intrinsics"; 8291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return EXIT_FAILURE; 8301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 8331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Run the bundler. 8341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EuclideanBundleCommonIntrinsics(all_markers, 8351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bundle_intrinsics, 8361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BUNDLE_NO_CONSTRAINTS, 8371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling camera_intrinsics, 8381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_cameras, 8391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &all_points); 8401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 8411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return EXIT_SUCCESS; 8421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 843