844 lines
28 KiB
C++
844 lines
28 KiB
C++
|
// Copyright (c) 2013 libmv authors.
|
||
|
|
//
|
||
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
|
// of this software and associated documentation files (the "Software"), to
|
||
|
|
// deal in the Software without restriction, including without limitation the
|
||
|
|
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
||
|
|
// sell copies of the Software, and to permit persons to whom the Software is
|
||
|
|
// furnished to do so, subject to the following conditions:
|
||
|
|
//
|
||
|
|
// The above copyright notice and this permission notice shall be included in
|
||
|
|
// all copies or substantial portions of the Software.
|
||
|
|
//
|
||
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
|
|
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||
|
|
// IN THE SOFTWARE.
|
||
|
|
//
|
||
|
|
// Author: mierle@gmail.com (Keir Mierle)
|
||
|
|
// sergey.vfx@gmail.com (Sergey Sharybin)
|
||
|
|
//
|
||
|
|
// This is an example application which contains bundle adjustment code used
|
||
|
|
// in the Libmv library and Blender. It reads problems from files passed via
|
||
|
|
// the command line and runs the bundle adjuster on the problem.
|
||
|
|
//
|
||
|
|
// File with problem a binary file, for which it is crucial to know in which
|
||
|
|
// order bytes of float values are stored in. This information is provided
|
||
|
|
// by a single character in the beginning of the file. There're two possible
|
||
|
|
// values of this byte:
|
||
|
|
// - V, which means values in the file are stored with big endian type
|
||
|
|
// - v, which means values in the file are stored with little endian type
|
||
|
|
//
|
||
|
|
// The rest of the file contains data in the following order:
|
||
|
|
// - Space in which markers' coordinates are stored in
|
||
|
|
// - Camera intrinsics
|
||
|
|
// - Number of cameras
|
||
|
|
// - Cameras
|
||
|
|
// - Number of 3D points
|
||
|
|
// - 3D points
|
||
|
|
// - Number of markers
|
||
|
|
// - Markers
|
||
|
|
//
|
||
|
|
// Markers' space could either be normalized or image (pixels). This is defined
|
||
|
|
// by the single character in the file. P means markers in the file is in image
|
||
|
|
// space, and N means markers are in normalized space.
|
||
|
|
//
|
||
|
|
// Camera intrinsics are 8 described by 8 float 8.
|
||
|
|
// This values goes in the following order:
|
||
|
|
//
|
||
|
|
// - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2
|
||
|
|
//
|
||
|
|
// Every camera is described by:
|
||
|
|
//
|
||
|
|
// - Image for which camera belongs to (single 4 bytes integer value).
|
||
|
|
// - Column-major camera rotation matrix, 9 float values.
|
||
|
|
// - Camera translation, 3-component vector of float values.
|
||
|
|
//
|
||
|
|
// Image number shall be greater or equal to zero. Order of cameras does not
|
||
|
|
// matter and gaps are possible.
|
||
|
|
//
|
||
|
|
// Every 3D point is decribed by:
|
||
|
|
//
|
||
|
|
// - Track number point belongs to (single 4 bytes integer value).
|
||
|
|
// - 3D position vector, 3-component vector of float values.
|
||
|
|
//
|
||
|
|
// Track number shall be greater or equal to zero. Order of tracks does not
|
||
|
|
// matter and gaps are possible.
|
||
|
|
//
|
||
|
|
// Finally every marker is described by:
|
||
|
|
//
|
||
|
|
// - Image marker belongs to single 4 bytes integer value).
|
||
|
|
// - Track marker belongs to single 4 bytes integer value).
|
||
|
|
// - 2D marker position vector, (two float values).
|
||
|
|
//
|
||
|
|
// Marker's space is used a default value for refine_intrinsics command line
|
||
|
|
// flag. This means if there's no refine_intrinsics flag passed via command
|
||
|
|
// line, camera intrinsics will be refined if markers in the problem are
|
||
|
|
// stored in image space and camera intrinsics will not be refined if markers
|
||
|
|
// are in normalized space.
|
||
|
|
//
|
||
|
|
// Passing refine_intrinsics command line flag defines explicitly whether
|
||
|
|
// refinement of intrinsics will happen. Currently, only none and all
|
||
|
|
// intrinsics refinement is supported.
|
||
|
|
//
|
||
|
|
// There're existing problem files dumped from blender stored in folder
|
||
|
|
// ../data/libmv-ba-problems.
|
||
|
|
|
||
|
|
#include <cstdio>
|
||
|
|
#include <fcntl.h>
|
||
|
|
#include <sstream>
|
||
|
|
#include <string>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
# include <io.h>
|
||
|
|
# define open _open
|
||
|
|
# define close _close
|
||
|
|
typedef unsigned __int32 uint32_t;
|
||
|
|
#else
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <unistd.h>
|
||
|
|
|
||
|
|
// O_BINARY is not defined on unix like platforms, as there is no
|
||
|
|
// difference between binary and text files.
|
||
|
|
#define O_BINARY 0
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include "ceres/ceres.h"
|
||
|
|
#include "ceres/rotation.h"
|
||
|
|
#include "gflags/gflags.h"
|
||
|
|
#include "glog/logging.h"
|
||
|
|
|
||
|
|
typedef Eigen::Matrix<double, 3, 3> Mat3;
|
||
|
|
typedef Eigen::Matrix<double, 6, 1> Vec6;
|
||
|
|
typedef Eigen::Vector3d Vec3;
|
||
|
|
typedef Eigen::Vector4d Vec4;
|
||
|
|
|
||
|
|
using std::vector;
|
||
|
|
|
||
|
|
DEFINE_string(input, "", "Input File name");
|
||
|
|
DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. "
|
||
|
|
"Options are: none, radial.");
|
||
|
|
|
||
|
|
namespace {
|
||
|
|
|
||
|
|
// A EuclideanCamera is the location and rotation of the camera
|
||
|
|
// viewing an image.
|
||
|
|
//
|
||
|
|
// image identifies which image this camera represents.
|
||
|
|
// R is a 3x3 matrix representing the rotation of the camera.
|
||
|
|
// t is a translation vector representing its positions.
|
||
|
|
struct EuclideanCamera {
|
||
|
|
EuclideanCamera() : image(-1) {}
|
||
|
|
EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {}
|
||
|
|
|
||
|
|
int image;
|
||
|
|
Mat3 R;
|
||
|
|
Vec3 t;
|
||
|
|
};
|
||
|
|
|
||
|
|
// A Point is the 3D location of a track.
|
||
|
|
//
|
||
|
|
// track identifies which track this point corresponds to.
|
||
|
|
// X represents the 3D position of the track.
|
||
|
|
struct EuclideanPoint {
|
||
|
|
EuclideanPoint() : track(-1) {}
|
||
|
|
EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {}
|
||
|
|
int track;
|
||
|
|
Vec3 X;
|
||
|
|
};
|
||
|
|
|
||
|
|
// A Marker is the 2D location of a tracked point in an image.
|
||
|
|
//
|
||
|
|
// x and y is the position of the marker in pixels from the top left corner
|
||
|
|
// in the image identified by an image. All markers for to the same target
|
||
|
|
// form a track identified by a common track number.
|
||
|
|
struct Marker {
|
||
|
|
int image;
|
||
|
|
int track;
|
||
|
|
double x, y;
|
||
|
|
};
|
||
|
|
|
||
|
|
// Cameras intrinsics to be bundled.
|
||
|
|
//
|
||
|
|
// BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only,
|
||
|
|
// no bundling of k3 is possible at this moment.
|
||
|
|
enum BundleIntrinsics {
|
||
|
|
BUNDLE_NO_INTRINSICS = 0,
|
||
|
|
BUNDLE_FOCAL_LENGTH = 1,
|
||
|
|
BUNDLE_PRINCIPAL_POINT = 2,
|
||
|
|
BUNDLE_RADIAL_K1 = 4,
|
||
|
|
BUNDLE_RADIAL_K2 = 8,
|
||
|
|
BUNDLE_RADIAL = 12,
|
||
|
|
BUNDLE_TANGENTIAL_P1 = 16,
|
||
|
|
BUNDLE_TANGENTIAL_P2 = 32,
|
||
|
|
BUNDLE_TANGENTIAL = 48,
|
||
|
|
};
|
||
|
|
|
||
|
|
// Denotes which blocks to keep constant during bundling.
|
||
|
|
// For example it is useful to keep camera translations constant
|
||
|
|
// when bundling tripod motions.
|
||
|
|
enum BundleConstraints {
|
||
|
|
BUNDLE_NO_CONSTRAINTS = 0,
|
||
|
|
BUNDLE_NO_TRANSLATION = 1,
|
||
|
|
};
|
||
|
|
|
||
|
|
// The intrinsics need to get combined into a single parameter block; use these
|
||
|
|
// enums to index instead of numeric constants.
|
||
|
|
enum {
|
||
|
|
OFFSET_FOCAL_LENGTH,
|
||
|
|
OFFSET_PRINCIPAL_POINT_X,
|
||
|
|
OFFSET_PRINCIPAL_POINT_Y,
|
||
|
|
OFFSET_K1,
|
||
|
|
OFFSET_K2,
|
||
|
|
OFFSET_K3,
|
||
|
|
OFFSET_P1,
|
||
|
|
OFFSET_P2,
|
||
|
|
};
|
||
|
|
|
||
|
|
// Returns a pointer to the camera corresponding to a image.
|
||
|
|
EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras,
|
||
|
|
const int image) {
|
||
|
|
if (image < 0 || image >= all_cameras->size()) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
EuclideanCamera *camera = &(*all_cameras)[image];
|
||
|
|
if (camera->image == -1) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
return camera;
|
||
|
|
}
|
||
|
|
|
||
|
|
const EuclideanCamera *CameraForImage(
|
||
|
|
const vector<EuclideanCamera> &all_cameras,
|
||
|
|
const int image) {
|
||
|
|
if (image < 0 || image >= all_cameras.size()) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
const EuclideanCamera *camera = &all_cameras[image];
|
||
|
|
if (camera->image == -1) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
return camera;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns maximal image number at which marker exists.
|
||
|
|
int MaxImage(const vector<Marker> &all_markers) {
|
||
|
|
if (all_markers.size() == 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
int max_image = all_markers[0].image;
|
||
|
|
for (int i = 1; i < all_markers.size(); i++) {
|
||
|
|
max_image = std::max(max_image, all_markers[i].image);
|
||
|
|
}
|
||
|
|
return max_image;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns a pointer to the point corresponding to a track.
|
||
|
|
EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points,
|
||
|
|
const int track) {
|
||
|
|
if (track < 0 || track >= all_points->size()) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
EuclideanPoint *point = &(*all_points)[track];
|
||
|
|
if (point->track == -1) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
return point;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Reader of binary file which makes sure possibly needed endian
|
||
|
|
// conversion happens when loading values like floats and integers.
|
||
|
|
//
|
||
|
|
// File's endian type is reading from a first character of file, which
|
||
|
|
// could either be V for big endian or v for little endian. This
|
||
|
|
// means you need to design file format assuming first character
|
||
|
|
// denotes file endianness in this way.
|
||
|
|
class EndianAwareFileReader {
|
||
|
|
public:
|
||
|
|
EndianAwareFileReader(void) : file_descriptor_(-1) {
|
||
|
|
// Get an endian type of the host machine.
|
||
|
|
union {
|
||
|
|
unsigned char bytes[4];
|
||
|
|
uint32_t value;
|
||
|
|
} endian_test = { { 0, 1, 2, 3 } };
|
||
|
|
host_endian_type_ = endian_test.value;
|
||
|
|
file_endian_type_ = host_endian_type_;
|
||
|
|
}
|
||
|
|
|
||
|
|
~EndianAwareFileReader(void) {
|
||
|
|
if (file_descriptor_ > 0) {
|
||
|
|
close(file_descriptor_);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
bool OpenFile(const std::string &file_name) {
|
||
|
|
file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY);
|
||
|
|
if (file_descriptor_ < 0) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
// Get an endian tpye of data in the file.
|
||
|
|
unsigned char file_endian_type_flag = Read<unsigned char>();
|
||
|
|
if (file_endian_type_flag == 'V') {
|
||
|
|
file_endian_type_ = kBigEndian;
|
||
|
|
} else if (file_endian_type_flag == 'v') {
|
||
|
|
file_endian_type_ = kLittleEndian;
|
||
|
|
} else {
|
||
|
|
LOG(FATAL) << "Problem file is stored in unknown endian type.";
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read value from the file, will switch endian if needed.
|
||
|
|
template <typename T>
|
||
|
|
T Read(void) const {
|
||
|
|
T value;
|
||
|
|
CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
|
||
|
|
// Switch endian type if file contains data in different type
|
||
|
|
// that current machine.
|
||
|
|
if (file_endian_type_ != host_endian_type_) {
|
||
|
|
value = SwitchEndian<T>(value);
|
||
|
|
}
|
||
|
|
return value;
|
||
|
|
}
|
||
|
|
private:
|
||
|
|
static const long int kLittleEndian = 0x03020100ul;
|
||
|
|
static const long int kBigEndian = 0x00010203ul;
|
||
|
|
|
||
|
|
// Switch endian type between big to little.
|
||
|
|
template <typename T>
|
||
|
|
T SwitchEndian(const T value) const {
|
||
|
|
if (sizeof(T) == 4) {
|
||
|
|
unsigned int temp_value = static_cast<unsigned int>(value);
|
||
|
|
return ((temp_value >> 24)) |
|
||
|
|
((temp_value << 8) & 0x00ff0000) |
|
||
|
|
((temp_value >> 8) & 0x0000ff00) |
|
||
|
|
((temp_value << 24));
|
||
|
|
} else if (sizeof(T) == 1) {
|
||
|
|
return value;
|
||
|
|
} else {
|
||
|
|
LOG(FATAL) << "Entered non-implemented part of endian switching function.";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int host_endian_type_;
|
||
|
|
int file_endian_type_;
|
||
|
|
int file_descriptor_;
|
||
|
|
};
|
||
|
|
|
||
|
|
// Read 3x3 column-major matrix from the file
|
||
|
|
void ReadMatrix3x3(const EndianAwareFileReader &file_reader,
|
||
|
|
Mat3 *matrix) {
|
||
|
|
for (int i = 0; i < 9; i++) {
|
||
|
|
(*matrix)(i % 3, i / 3) = file_reader.Read<float>();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read 3-vector from file
|
||
|
|
void ReadVector3(const EndianAwareFileReader &file_reader,
|
||
|
|
Vec3 *vector) {
|
||
|
|
for (int i = 0; i < 3; i++) {
|
||
|
|
(*vector)(i) = file_reader.Read<float>();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Reads a bundle adjustment problem from the file.
|
||
|
|
//
|
||
|
|
// file_name denotes from which file to read the problem.
|
||
|
|
// camera_intrinsics will contain initial camera intrinsics values.
|
||
|
|
//
|
||
|
|
// all_cameras is a vector of all reconstructed cameras to be optimized,
|
||
|
|
// vector element with number i will contain camera for image i.
|
||
|
|
//
|
||
|
|
// all_points is a vector of all reconstructed 3D points to be optimized,
|
||
|
|
// vector element with number i will contain point for track i.
|
||
|
|
//
|
||
|
|
// all_markers is a vector of all tracked markers existing in
|
||
|
|
// the problem. Only used for reprojection error calculation, stay
|
||
|
|
// unchanged during optimization.
|
||
|
|
//
|
||
|
|
// Returns false if any kind of error happened during
|
||
|
|
// reading.
|
||
|
|
bool ReadProblemFromFile(const std::string &file_name,
|
||
|
|
double camera_intrinsics[8],
|
||
|
|
vector<EuclideanCamera> *all_cameras,
|
||
|
|
vector<EuclideanPoint> *all_points,
|
||
|
|
bool *is_image_space,
|
||
|
|
vector<Marker> *all_markers) {
|
||
|
|
EndianAwareFileReader file_reader;
|
||
|
|
if (!file_reader.OpenFile(file_name)) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read markers' space flag.
|
||
|
|
unsigned char is_image_space_flag = file_reader.Read<unsigned char>();
|
||
|
|
if (is_image_space_flag == 'P') {
|
||
|
|
*is_image_space = true;
|
||
|
|
} else if (is_image_space_flag == 'N') {
|
||
|
|
*is_image_space = false;
|
||
|
|
} else {
|
||
|
|
LOG(FATAL) << "Problem file contains markers stored in unknown space.";
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read camera intrinsics.
|
||
|
|
for (int i = 0; i < 8; i++) {
|
||
|
|
camera_intrinsics[i] = file_reader.Read<float>();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read all cameras.
|
||
|
|
int number_of_cameras = file_reader.Read<int>();
|
||
|
|
for (int i = 0; i < number_of_cameras; i++) {
|
||
|
|
EuclideanCamera camera;
|
||
|
|
|
||
|
|
camera.image = file_reader.Read<int>();
|
||
|
|
ReadMatrix3x3(file_reader, &camera.R);
|
||
|
|
ReadVector3(file_reader, &camera.t);
|
||
|
|
|
||
|
|
if (camera.image >= all_cameras->size()) {
|
||
|
|
all_cameras->resize(camera.image + 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
(*all_cameras)[camera.image].image = camera.image;
|
||
|
|
(*all_cameras)[camera.image].R = camera.R;
|
||
|
|
(*all_cameras)[camera.image].t = camera.t;
|
||
|
|
}
|
||
|
|
|
||
|
|
LOG(INFO) << "Read " << number_of_cameras << " cameras.";
|
||
|
|
|
||
|
|
// Read all reconstructed 3D points.
|
||
|
|
int number_of_points = file_reader.Read<int>();
|
||
|
|
for (int i = 0; i < number_of_points; i++) {
|
||
|
|
EuclideanPoint point;
|
||
|
|
|
||
|
|
point.track = file_reader.Read<int>();
|
||
|
|
ReadVector3(file_reader, &point.X);
|
||
|
|
|
||
|
|
if (point.track >= all_points->size()) {
|
||
|
|
all_points->resize(point.track + 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
(*all_points)[point.track].track = point.track;
|
||
|
|
(*all_points)[point.track].X = point.X;
|
||
|
|
}
|
||
|
|
|
||
|
|
LOG(INFO) << "Read " << number_of_points << " points.";
|
||
|
|
|
||
|
|
// And finally read all markers.
|
||
|
|
int number_of_markers = file_reader.Read<int>();
|
||
|
|
for (int i = 0; i < number_of_markers; i++) {
|
||
|
|
Marker marker;
|
||
|
|
|
||
|
|
marker.image = file_reader.Read<int>();
|
||
|
|
marker.track = file_reader.Read<int>();
|
||
|
|
marker.x = file_reader.Read<float>();
|
||
|
|
marker.y = file_reader.Read<float>();
|
||
|
|
|
||
|
|
all_markers->push_back(marker);
|
||
|
|
}
|
||
|
|
|
||
|
|
LOG(INFO) << "Read " << number_of_markers << " markers.";
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Apply camera intrinsics to the normalized point to get image coordinates.
|
||
|
|
// This applies the radial lens distortion to a point which is in normalized
|
||
|
|
// camera coordinates (i.e. the principal point is at (0, 0)) to get image
|
||
|
|
// coordinates in pixels. Templated for use with autodifferentiation.
|
||
|
|
template <typename T>
|
||
|
|
inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x,
|
||
|
|
const T &focal_length_y,
|
||
|
|
const T &principal_point_x,
|
||
|
|
const T &principal_point_y,
|
||
|
|
const T &k1,
|
||
|
|
const T &k2,
|
||
|
|
const T &k3,
|
||
|
|
const T &p1,
|
||
|
|
const T &p2,
|
||
|
|
const T &normalized_x,
|
||
|
|
const T &normalized_y,
|
||
|
|
T *image_x,
|
||
|
|
T *image_y) {
|
||
|
|
T x = normalized_x;
|
||
|
|
T y = normalized_y;
|
||
|
|
|
||
|
|
// Apply distortion to the normalized points to get (xd, yd).
|
||
|
|
T r2 = x*x + y*y;
|
||
|
|
T r4 = r2 * r2;
|
||
|
|
T r6 = r4 * r2;
|
||
|
|
T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
|
||
|
|
T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
|
||
|
|
T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);
|
||
|
|
|
||
|
|
// Apply focal length and principal point to get the final image coordinates.
|
||
|
|
*image_x = focal_length_x * xd + principal_point_x;
|
||
|
|
*image_y = focal_length_y * yd + principal_point_y;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Cost functor which computes reprojection error of 3D point X
|
||
|
|
// on camera defined by angle-axis rotation and it's translation
|
||
|
|
// (which are in the same block due to optimization reasons).
|
||
|
|
//
|
||
|
|
// This functor uses a radial distortion model.
|
||
|
|
struct OpenCVReprojectionError {
|
||
|
|
OpenCVReprojectionError(const double observed_x, const double observed_y)
|
||
|
|
: observed_x(observed_x), observed_y(observed_y) {}
|
||
|
|
|
||
|
|
template <typename T>
|
||
|
|
bool operator()(const T* const intrinsics,
|
||
|
|
const T* const R_t, // Rotation denoted by angle axis
|
||
|
|
// followed with translation
|
||
|
|
const T* const X, // Point coordinates 3x1.
|
||
|
|
T* residuals) const {
|
||
|
|
// Unpack the intrinsics.
|
||
|
|
const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
|
||
|
|
const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
|
||
|
|
const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
|
||
|
|
const T& k1 = intrinsics[OFFSET_K1];
|
||
|
|
const T& k2 = intrinsics[OFFSET_K2];
|
||
|
|
const T& k3 = intrinsics[OFFSET_K3];
|
||
|
|
const T& p1 = intrinsics[OFFSET_P1];
|
||
|
|
const T& p2 = intrinsics[OFFSET_P2];
|
||
|
|
|
||
|
|
// Compute projective coordinates: x = RX + t.
|
||
|
|
T x[3];
|
||
|
|
|
||
|
|
ceres::AngleAxisRotatePoint(R_t, X, x);
|
||
|
|
x[0] += R_t[3];
|
||
|
|
x[1] += R_t[4];
|
||
|
|
x[2] += R_t[5];
|
||
|
|
|
||
|
|
// Compute normalized coordinates: x /= x[2].
|
||
|
|
T xn = x[0] / x[2];
|
||
|
|
T yn = x[1] / x[2];
|
||
|
|
|
||
|
|
T predicted_x, predicted_y;
|
||
|
|
|
||
|
|
// Apply distortion to the normalized points to get (xd, yd).
|
||
|
|
// TODO(keir): Do early bailouts for zero distortion; these are expensive
|
||
|
|
// jet operations.
|
||
|
|
ApplyRadialDistortionCameraIntrinsics(focal_length,
|
||
|
|
focal_length,
|
||
|
|
principal_point_x,
|
||
|
|
principal_point_y,
|
||
|
|
k1, k2, k3,
|
||
|
|
p1, p2,
|
||
|
|
xn, yn,
|
||
|
|
&predicted_x,
|
||
|
|
&predicted_y);
|
||
|
|
|
||
|
|
// The error is the difference between the predicted and observed position.
|
||
|
|
residuals[0] = predicted_x - T(observed_x);
|
||
|
|
residuals[1] = predicted_y - T(observed_y);
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
const double observed_x;
|
||
|
|
const double observed_y;
|
||
|
|
};
|
||
|
|
|
||
|
|
// Print a message to the log which camera intrinsics are gonna to be optimized.
|
||
|
|
void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
|
||
|
|
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
|
||
|
|
LOG(INFO) << "Bundling only camera positions.";
|
||
|
|
} else {
|
||
|
|
std::string bundling_message = "";
|
||
|
|
|
||
|
|
#define APPEND_BUNDLING_INTRINSICS(name, flag) \
|
||
|
|
if (bundle_intrinsics & flag) { \
|
||
|
|
if (!bundling_message.empty()) { \
|
||
|
|
bundling_message += ", "; \
|
||
|
|
} \
|
||
|
|
bundling_message += name; \
|
||
|
|
} (void)0
|
||
|
|
|
||
|
|
APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH);
|
||
|
|
APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
|
||
|
|
APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1);
|
||
|
|
APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2);
|
||
|
|
APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1);
|
||
|
|
APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2);
|
||
|
|
|
||
|
|
LOG(INFO) << "Bundling " << bundling_message << ".";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Print a message to the log containing all the camera intriniscs values.
|
||
|
|
void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) {
|
||
|
|
std::ostringstream intrinsics_output;
|
||
|
|
|
||
|
|
intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH];
|
||
|
|
|
||
|
|
intrinsics_output <<
|
||
|
|
" cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] <<
|
||
|
|
" cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y];
|
||
|
|
|
||
|
|
#define APPEND_DISTORTION_COEFFICIENT(name, offset) \
|
||
|
|
{ \
|
||
|
|
if (camera_intrinsics[offset] != 0.0) { \
|
||
|
|
intrinsics_output << " " name "=" << camera_intrinsics[offset]; \
|
||
|
|
} \
|
||
|
|
} (void)0
|
||
|
|
|
||
|
|
APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1);
|
||
|
|
APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2);
|
||
|
|
APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3);
|
||
|
|
APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1);
|
||
|
|
APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2);
|
||
|
|
|
||
|
|
#undef APPEND_DISTORTION_COEFFICIENT
|
||
|
|
|
||
|
|
LOG(INFO) << text << intrinsics_output.str();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Get a vector of camera's rotations denoted by angle axis
|
||
|
|
// conjuncted with translations into single block
|
||
|
|
//
|
||
|
|
// Element with index i matches to a rotation+translation for
|
||
|
|
// camera at image i.
|
||
|
|
vector<Vec6> PackCamerasRotationAndTranslation(
|
||
|
|
const vector<Marker> &all_markers,
|
||
|
|
const vector<EuclideanCamera> &all_cameras) {
|
||
|
|
vector<Vec6> all_cameras_R_t;
|
||
|
|
int max_image = MaxImage(all_markers);
|
||
|
|
|
||
|
|
all_cameras_R_t.resize(max_image + 1);
|
||
|
|
|
||
|
|
for (int i = 0; i <= max_image; i++) {
|
||
|
|
const EuclideanCamera *camera = CameraForImage(all_cameras, i);
|
||
|
|
|
||
|
|
if (!camera) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
|
||
|
|
&all_cameras_R_t[i](0));
|
||
|
|
all_cameras_R_t[i].tail<3>() = camera->t;
|
||
|
|
}
|
||
|
|
|
||
|
|
return all_cameras_R_t;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert cameras rotations fro mangle axis back to rotation matrix.
|
||
|
|
void UnpackCamerasRotationAndTranslation(
|
||
|
|
const vector<Marker> &all_markers,
|
||
|
|
const vector<Vec6> &all_cameras_R_t,
|
||
|
|
vector<EuclideanCamera> *all_cameras) {
|
||
|
|
int max_image = MaxImage(all_markers);
|
||
|
|
|
||
|
|
for (int i = 0; i <= max_image; i++) {
|
||
|
|
EuclideanCamera *camera = CameraForImage(all_cameras, i);
|
||
|
|
|
||
|
|
if (!camera) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
|
||
|
|
&camera->R(0, 0));
|
||
|
|
camera->t = all_cameras_R_t[i].tail<3>();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers,
|
||
|
|
const int bundle_intrinsics,
|
||
|
|
const int bundle_constraints,
|
||
|
|
double *camera_intrinsics,
|
||
|
|
vector<EuclideanCamera> *all_cameras,
|
||
|
|
vector<EuclideanPoint> *all_points) {
|
||
|
|
PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
|
||
|
|
|
||
|
|
ceres::Problem::Options problem_options;
|
||
|
|
ceres::Problem problem(problem_options);
|
||
|
|
|
||
|
|
// Convert cameras rotations to angle axis and merge with translation
|
||
|
|
// into single parameter block for maximal minimization speed
|
||
|
|
//
|
||
|
|
// Block for minimization has got the following structure:
|
||
|
|
// <3 elements for angle-axis> <3 elements for translation>
|
||
|
|
vector<Vec6> all_cameras_R_t =
|
||
|
|
PackCamerasRotationAndTranslation(all_markers, *all_cameras);
|
||
|
|
|
||
|
|
// Parameterization used to restrict camera motion for modal solvers.
|
||
|
|
ceres::SubsetParameterization *constant_transform_parameterization = NULL;
|
||
|
|
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
|
||
|
|
std::vector<int> constant_translation;
|
||
|
|
|
||
|
|
// First three elements are rotation, last three are translation.
|
||
|
|
constant_translation.push_back(3);
|
||
|
|
constant_translation.push_back(4);
|
||
|
|
constant_translation.push_back(5);
|
||
|
|
|
||
|
|
constant_transform_parameterization =
|
||
|
|
new ceres::SubsetParameterization(6, constant_translation);
|
||
|
|
}
|
||
|
|
|
||
|
|
int num_residuals = 0;
|
||
|
|
bool have_locked_camera = false;
|
||
|
|
for (int i = 0; i < all_markers.size(); ++i) {
|
||
|
|
const Marker &marker = all_markers[i];
|
||
|
|
EuclideanCamera *camera = CameraForImage(all_cameras, marker.image);
|
||
|
|
EuclideanPoint *point = PointForTrack(all_points, marker.track);
|
||
|
|
if (camera == NULL || point == NULL) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Rotation of camera denoted in angle axis followed with
|
||
|
|
// camera translaiton.
|
||
|
|
double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
|
||
|
|
|
||
|
|
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
|
||
|
|
OpenCVReprojectionError, 2, 8, 6, 3>(
|
||
|
|
new OpenCVReprojectionError(
|
||
|
|
marker.x,
|
||
|
|
marker.y)),
|
||
|
|
NULL,
|
||
|
|
camera_intrinsics,
|
||
|
|
current_camera_R_t,
|
||
|
|
&point->X(0));
|
||
|
|
|
||
|
|
// We lock the first camera to better deal with scene orientation ambiguity.
|
||
|
|
if (!have_locked_camera) {
|
||
|
|
problem.SetParameterBlockConstant(current_camera_R_t);
|
||
|
|
have_locked_camera = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
|
||
|
|
problem.SetParameterization(current_camera_R_t,
|
||
|
|
constant_transform_parameterization);
|
||
|
|
}
|
||
|
|
|
||
|
|
num_residuals++;
|
||
|
|
}
|
||
|
|
LOG(INFO) << "Number of residuals: " << num_residuals;
|
||
|
|
|
||
|
|
if (!num_residuals) {
|
||
|
|
LOG(INFO) << "Skipping running minimizer with zero residuals";
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
BundleIntrinsicsLogMessage(bundle_intrinsics);
|
||
|
|
|
||
|
|
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
|
||
|
|
// No camera intrinsics are being refined,
|
||
|
|
// set the whole parameter block as constant for best performance.
|
||
|
|
problem.SetParameterBlockConstant(camera_intrinsics);
|
||
|
|
} else {
|
||
|
|
// Set the camera intrinsics that are not to be bundled as
|
||
|
|
// constant using some macro trickery.
|
||
|
|
|
||
|
|
std::vector<int> constant_intrinsics;
|
||
|
|
#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
|
||
|
|
if (!(bundle_intrinsics & bundle_enum)) { \
|
||
|
|
constant_intrinsics.push_back(offset); \
|
||
|
|
}
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1);
|
||
|
|
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2);
|
||
|
|
#undef MAYBE_SET_CONSTANT
|
||
|
|
|
||
|
|
// Always set K3 constant, it's not used at the moment.
|
||
|
|
constant_intrinsics.push_back(OFFSET_K3);
|
||
|
|
|
||
|
|
ceres::SubsetParameterization *subset_parameterization =
|
||
|
|
new ceres::SubsetParameterization(8, constant_intrinsics);
|
||
|
|
|
||
|
|
problem.SetParameterization(camera_intrinsics, subset_parameterization);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Configure the solver.
|
||
|
|
ceres::Solver::Options options;
|
||
|
|
options.use_nonmonotonic_steps = true;
|
||
|
|
options.preconditioner_type = ceres::SCHUR_JACOBI;
|
||
|
|
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
|
||
|
|
options.use_inner_iterations = true;
|
||
|
|
options.max_num_iterations = 100;
|
||
|
|
options.minimizer_progress_to_stdout = true;
|
||
|
|
|
||
|
|
// Solve!
|
||
|
|
ceres::Solver::Summary summary;
|
||
|
|
ceres::Solve(options, &problem, &summary);
|
||
|
|
|
||
|
|
std::cout << "Final report:\n" << summary.FullReport();
|
||
|
|
|
||
|
|
// Copy rotations and translations back.
|
||
|
|
UnpackCamerasRotationAndTranslation(all_markers,
|
||
|
|
all_cameras_R_t,
|
||
|
|
all_cameras);
|
||
|
|
|
||
|
|
PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics);
|
||
|
|
}
|
||
|
|
} // namespace
|
||
|
|
|
||
|
|
int main(int argc, char **argv) {
|
||
|
|
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
|
||
|
|
google::InitGoogleLogging(argv[0]);
|
||
|
|
|
||
|
|
if (FLAGS_input.empty()) {
|
||
|
|
LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem";
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
double camera_intrinsics[8];
|
||
|
|
vector<EuclideanCamera> all_cameras;
|
||
|
|
vector<EuclideanPoint> all_points;
|
||
|
|
bool is_image_space;
|
||
|
|
vector<Marker> all_markers;
|
||
|
|
|
||
|
|
if (!ReadProblemFromFile(FLAGS_input,
|
||
|
|
camera_intrinsics,
|
||
|
|
&all_cameras,
|
||
|
|
&all_points,
|
||
|
|
&is_image_space,
|
||
|
|
&all_markers)) {
|
||
|
|
LOG(ERROR) << "Error reading problem file";
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If there's no refine_intrinsics passed via command line
|
||
|
|
// (in this case FLAGS_refine_intrinsics will be an empty string)
|
||
|
|
// we use problem's settings to detect whether intrinsics
|
||
|
|
// shall be refined or not.
|
||
|
|
//
|
||
|
|
// Namely, if problem has got markers stored in image (pixel)
|
||
|
|
// space, we do full intrinsics refinement. If markers are
|
||
|
|
// stored in normalized space, and refine_intrinsics is not
|
||
|
|
// set, no refining will happen.
|
||
|
|
//
|
||
|
|
// Using command line argument refine_intrinsics will explicitly
|
||
|
|
// declare which intrinsics need to be refined and in this case
|
||
|
|
// refining flags does not depend on problem at all.
|
||
|
|
int bundle_intrinsics = BUNDLE_NO_INTRINSICS;
|
||
|
|
if (FLAGS_refine_intrinsics.empty()) {
|
||
|
|
if (is_image_space) {
|
||
|
|
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
if (FLAGS_refine_intrinsics == "radial") {
|
||
|
|
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
|
||
|
|
} else if (FLAGS_refine_intrinsics != "none") {
|
||
|
|
LOG(ERROR) << "Unsupported value for refine-intrinsics";
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Run the bundler.
|
||
|
|
EuclideanBundleCommonIntrinsics(all_markers,
|
||
|
|
bundle_intrinsics,
|
||
|
|
BUNDLE_NO_CONSTRAINTS,
|
||
|
|
camera_intrinsics,
|
||
|
|
&all_cameras,
|
||
|
|
&all_points);
|
||
|
|
|
||
|
|
return EXIT_SUCCESS;
|
||
|
|
}
|