├── CMakeLists.txt ├── README.md └── sfm ├── CMakeLists.txt ├── Makefile ├── ba_cholesky.h ├── ba_conjugate_gradient.h ├── ba_dense_vector.h ├── ba_linear_solver.cc ├── ba_linear_solver.h ├── ba_sparse_matrix.h ├── ba_types.h ├── bundle_adjustment.cc ├── bundle_adjustment.h ├── bundler_common.cc ├── bundler_common.h ├── camera_database.cc ├── camera_database.h ├── camera_pose.h ├── correspondence.h ├── defines.h ├── extract_focal_length.cc ├── extract_focal_length.h ├── feature_set.cc ├── feature_set.h ├── fundamental.cc ├── fundamental.h ├── homography.cc ├── homography.h ├── pose_p3p.cc ├── pose_p3p.h ├── ransac.cc ├── ransac.h ├── ransac_fundamental.cc ├── ransac_fundamental.h ├── ransac_homography.cc ├── ransac_homography.h ├── ransac_pose_p3p.cc ├── ransac_pose_p3p.h ├── triangulate.cc └── triangulate.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ImageBasedModellingEdu) 3 | set(CMAKE_CXX_STANDARD 11) 4 | set(CMAKE_CXX_FLAGS "-fPIC") 5 | 6 | 7 | add_subdirectory(core) 8 | #add_subdirectory(math) 9 | add_subdirectory(util) 10 | add_subdirectory(features) 11 | add_subdirectory(sfm) 12 | add_subdirectory(examples) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ImageBasedModellingEdu 2 | 基于图像的三维模型重建 3 | 4 | #### core—提供了工程项目需要的所有的基础数据结构，包括image、depthmap、mesh、view以及数据的输入输出等结构和功能。 5 | #### math—提供矩阵，向量，四元数等基本的数学运算操作。 6 | #### features—提供特征提取以及特征匹配功能，其中特征类型包括 sift 和 surf两种。 7 | #### sfm—提供了与运动恢复结构相关的功能，包括相机姿态的恢复，三维点的三角化和捆绑调整等。 8 | #### mvs—提供立体匹配功能，实现稠密点云匹配 9 | #### surface—实现点云到网格的表面重建 10 | #### texturing—实现纹理图像的创建 11 | #### examples—提供代码示例 12 | #### tmp—存储临时数据 -------------------------------------------------------------------------------- /sfm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(sfm) 2 | set(CMAKE_CXX_STANDARD 11) 3 | set(CMAKE_CXX_FLAGS "-fPIC") 4 | 5 | 6 | include_directories("..") 7 | set(HEADERS 8 | defines.h 9 | camera_pose.h 10 | camera_database.h 11 | bundler_common.h 12 | feature_set.h 13 | ransac.h 14 | fundamental.h 15 | ransac_homography.h 16 | homography.h 17 | ransac_homography.h 18 | pose_p3p.h 19 | ransac_pose_p3p.h 20 | bundle_adjustment.h 21 | ba_types.h 22 | ba_linear_solver.h 23 | ba_sparse_matrix.h 24 | ba_dense_vector.h 25 | ba_conjugate_gradient.h 26 | ba_cholesky.h 27 | extract_focal_length.h 28 | triangulate.h 29 | ) 30 | 31 | set(SOURCE_FILES 32 | camera_database.cc 33 | bundler_common.cc 34 | feature_set.cc 35 | ransac.cc 36 | fundamental.cc 37 | ransac_homography.cc 38 | homography.cc 39 | ransac_fundamental.cc 40 | pose_p3p.cc 41 | ransac_pose_p3p.cc 42 | bundle_adjustment.cc 43 | ba_linear_solver.cc 44 | extract_focal_length.cc 45 | triangulate.cc 46 | ) 47 | add_library(sfm ${HEADERS} ${SOURCE_FILES}) 48 | target_link_libraries(sfm core util features) 49 | 50 | -------------------------------------------------------------------------------- /sfm/Makefile: -------------------------------------------------------------------------------- 1 | MVE_ROOT := ../.. 2 | TARGET := libmve_sfm.a 3 | include ${MVE_ROOT}/Makefile.inc 4 | 5 | CXXFLAGS += -I${MVE_ROOT}/libs ${OPENMP} 6 | LDLIBS += -lpng -ltiff -ljpeg 7 | 8 | SOURCES := $(wildcard [^_]*.cc) 9 | ${TARGET}: ${SOURCES:.cc=.o} 10 | $(AR) rcs $@ $^ 11 | 12 | _test%: _test%.o libmve_sfm.a libmve.a libmve_util.a 13 | ${LINK.cc} -o $@ $^ ${LDLIBS} 14 | 15 | clean: 16 | ${RM} ${TARGET} *.o Makefile.dep 17 | 18 | .PHONY: clean 19 | -------------------------------------------------------------------------------- /sfm/ba_cholesky.h: -------------------------------------------------------------------------------- 1 | #ifndef SFM_BA_CHOLESKY_HEADER 2 | #define SFM_BA_CHOLESKY_HEADER 3 | 4 | #include 5 | #include 6 | 7 | #include "math/defines.h" 8 | #include "math/matrix_tools.h" 9 | #include "sfm/defines.h" 10 | 11 | SFM_NAMESPACE_BEGIN 12 | SFM_BA_NAMESPACE_BEGIN 13 | 14 | /** 15 | * Invert symmetric, positive definite matrix A using Cholesky decomposition 16 | * and inversion of the triangular matrix: A^-1 = (L^-1)^T * (L^-1). 17 | */ 18 | template 19 | void 20 | cholesky_invert (T const* A, int const cols, T* A_inv); 21 | 22 | /** 23 | * Invert symmetric, positive definite matrix A inplace using Cholesky. 24 | */ 25 | template 26 | void 27 | cholesky_invert_inplace (T* A, int const cols); 28 | 29 | /** 30 | * Cholesky decomposition of the symmetric, positive definite matrix 31 | * A = L * L^T. The resulting matrix L is a lower-triangular matrix. 32 | * If A and L are the same matrix, the decomposition is performed in-place. 33 | */ 34 | template 35 | void 36 | cholesky_decomposition (T const* A, int const cols, T* L); 37 | 38 | /** 39 | * Invert a lower-triangular matrix (e.g. obtained by Cholesky decomposition). 40 | * The inversion cannot be computed in-place. 41 | */ 42 | template 43 | void 44 | invert_lower_diagonal (T const* A, int const cols, T* A_inv); 45 | 46 | /* ------------------------ Implementation ------------------------ */ 47 | 48 | template 49 | void 50 | cholesky_invert (T const* A, int const cols, T* A_inv) 51 | { 52 | cholesky_decomposition(A, cols, A_inv); 53 | T* tmp = new T[cols * cols]; 54 | invert_lower_diagonal(A_inv, cols, tmp); 55 | math::matrix_transpose_multiply(tmp, cols, cols, A_inv); 56 | delete[] tmp; 57 | } 58 | 59 | template 60 | void 61 | cholesky_invert_inplace (T* A, int const cols) 62 | { 63 | cholesky_decomposition(A, cols, A); 64 | T* tmp = new T[cols * cols]; 65 | invert_lower_diagonal(A, cols, tmp); 66 | math::matrix_transpose_multiply(tmp, cols, cols, A); 67 | delete[] tmp; 68 | } 69 | 70 | template 71 | void 72 | cholesky_decomposition (T const* A, int const cols, T* L) 73 | { 74 | T* out_ptr = L; 75 | for (int r = 0; r < cols; ++r) 76 | { 77 | /* Compute left-of-diagonal entries. */ 78 | for (int c = 0; c < r; ++c) 79 | { 80 | T result = T(0); 81 | for (int ci = 0; ci < c; ++ci) 82 | result += L[r * cols + ci] * L[c * cols + ci]; 83 | result = A[r * cols + c] - result; 84 | (*out_ptr++) = result / L[c * cols + c]; 85 | } 86 | 87 | /* Compute diagonal entry. */ 88 | { 89 | T* L_row_ptr = L + r * cols; 90 | T result = T(0); 91 | for (int c = 0; c < r; ++c) 92 | result += MATH_POW2(L_row_ptr[c]); 93 | result = std::max(T(0), A[r * cols + r] - result); 94 | (*out_ptr++) = std::sqrt(result); 95 | } 96 | 97 | /* Set right-of-diagonal entries zero. */ 98 | for (int c = r + 1; c < cols; ++c) 99 | (*out_ptr++) = T(0); 100 | } 101 | } 102 | 103 | template 104 | void 105 | invert_lower_diagonal (T const* A, int const cols, T* A_inv) 106 | { 107 | if (A == A_inv) 108 | throw std::invalid_argument("In-place inversion not possible"); 109 | 110 | for (int r = 0; r < cols; ++r) 111 | { 112 | T const* A_row_ptr = A + r * cols; 113 | T* A_inv_row_ptr = A_inv + r * cols; 114 | 115 | /* Compute left-of-diagonal entries. */ 116 | for (int c = 0; c < r; ++c) 117 | { 118 | T result = T(0); 119 | for (int ci = 0; ci < r; ++ci) 120 | result -= A_row_ptr[ci] * A_inv[ci * cols + c]; 121 | A_inv_row_ptr[c] = result / A_row_ptr[r]; 122 | } 123 | 124 | /* Compute diagonal entry. */ 125 | A_inv_row_ptr[r] = T(1) / A_row_ptr[r]; 126 | 127 | /* Set right-of-diagonal entries zero. */ 128 | for (int c = r + 1; c < cols; ++c) 129 | A_inv_row_ptr[c] = T(0); 130 | } 131 | } 132 | 133 | SFM_BA_NAMESPACE_END 134 | SFM_NAMESPACE_END 135 | 136 | #endif // SFM_BA_CHOLESKY_HEADER 137 | -------------------------------------------------------------------------------- /sfm/ba_conjugate_gradient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Fabian Langguth, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_BA_CONJUGATE_GRADIENT_HEADER 11 | #define SFM_BA_CONJUGATE_GRADIENT_HEADER 12 | 13 | #include "sfm/defines.h" 14 | #include "sfm/ba_dense_vector.h" 15 | #include "sfm/ba_sparse_matrix.h" 16 | 17 | SFM_NAMESPACE_BEGIN 18 | SFM_BA_NAMESPACE_BEGIN 19 | 20 | template 21 | class ConjugateGradient 22 | { 23 | public: 24 | typedef SparseMatrix Matrix; 25 | typedef DenseVector Vector; 26 | 27 | enum ReturnInfo 28 | { 29 | CG_CONVERGENCE, 30 | CG_MAX_ITERATIONS, 31 | CG_INVALID_INPUT 32 | }; 33 | 34 | struct Options 35 | { 36 | Options (void); 37 | int max_iterations; 38 | T tolerance; 39 | }; 40 | 41 | struct Status 42 | { 43 | Status (void); 44 | int num_iterations; 45 | ReturnInfo info; 46 | }; 47 | 48 | class Functor 49 | { 50 | public: 51 | virtual Vector multiply (Vector const& x) const = 0; 52 | virtual std::size_t input_size (void) const = 0; 53 | virtual std::size_t output_size (void) const = 0; 54 | }; 55 | 56 | public: 57 | ConjugateGradient (Options const& opts); 58 | 59 | Status solve (Matrix const& A, Vector const& b, Vector* x, 60 | Matrix const* P = nullptr); 61 | 62 | Status solve (Functor const& A, Vector const& b, Vector* x, 63 | Functor const* P = nullptr); 64 | 65 | private: 66 | Options opts; 67 | Status status; 68 | }; 69 | 70 | template 71 | class CGBasicMatrixFunctor : public ConjugateGradient::Functor 72 | { 73 | public: 74 | CGBasicMatrixFunctor (SparseMatrix const& A); 75 | DenseVector multiply (DenseVector const& x) const; 76 | std::size_t input_size (void) const; 77 | std::size_t output_size (void) const; 78 | 79 | private: 80 | SparseMatrix const* A; 81 | }; 82 | 83 | /* ------------------------ Implementation ------------------------ */ 84 | 85 | template 86 | inline 87 | ConjugateGradient::Options::Options (void) 88 | : max_iterations(100) 89 | , tolerance(1e-20) 90 | { 91 | } 92 | 93 | template 94 | inline 95 | ConjugateGradient::Status::Status (void) 96 | : num_iterations(0) 97 | { 98 | } 99 | 100 | template 101 | inline 102 | ConjugateGradient::ConjugateGradient 103 | (Options const& options) 104 | : opts(options) 105 | { 106 | } 107 | 108 | template 109 | inline typename ConjugateGradient::Status 110 | ConjugateGradient::solve(Matrix const& A, Vector const& b, Vector* x, 111 | Matrix const* P) 112 | { 113 | CGBasicMatrixFunctor A_functor(A); 114 | CGBasicMatrixFunctor P_functor(*P); 115 | return this->solve(A_functor, b, x, P == nullptr ? nullptr : &P_functor); 116 | } 117 | 118 | template 119 | typename ConjugateGradient::Status 120 | ConjugateGradient::solve(Functor const& A, Vector const& b, Vector* x, 121 | Functor const* P) 122 | { 123 | if (x == nullptr) 124 | throw std::invalid_argument("RHS must not be null"); 125 | 126 | if (A.output_size() != b.size()) 127 | { 128 | this->status.info = CG_INVALID_INPUT; 129 | return this->status; 130 | } 131 | 132 | /* Set intial x = 0. */ 133 | if (x->size() != A.input_size()) 134 | { 135 | x->clear(); 136 | x->resize(A.input_size(), T(0)); 137 | } 138 | else 139 | { 140 | x->fill(T(0)); 141 | } 142 | 143 | /* Initial residual is r = b - Ax with x = 0. */ 144 | Vector r = b; 145 | 146 | /* Regular search direction. */ 147 | Vector d; 148 | /* Preconditioned search direction. */ 149 | Vector z; 150 | /* Norm of residual. */ 151 | T r_dot_r; 152 | 153 | /* Compute initial search direction. */ 154 | if (P == nullptr) 155 | { 156 | r_dot_r = r.dot(r); 157 | d = r; 158 | } 159 | else 160 | { 161 | z = (*P).multiply(r); 162 | r_dot_r = z.dot(r); 163 | d = z; 164 | } 165 | 166 | for (this->status.num_iterations = 0; 167 | this->status.num_iterations < this->opts.max_iterations; 168 | this->status.num_iterations += 1) 169 | { 170 | /* Compute step size in search direction. */ 171 | Vector Ad = A.multiply(d); 172 | T alpha = r_dot_r / d.dot(Ad); 173 | 174 | /* Update parameter vector. */ 175 | *x = (*x).add(d.multiply(alpha)); 176 | 177 | /* Compute new residual and its norm. */ 178 | r = r.subtract(Ad.multiply(alpha)); 179 | T new_r_dot_r = r.dot(r); 180 | 181 | /* Check tolerance condition. */ 182 | if (new_r_dot_r < this->opts.tolerance) 183 | { 184 | this->status.info = CG_CONVERGENCE; 185 | return this->status; 186 | } 187 | 188 | /* Precondition residual if necessary. */ 189 | if (P != nullptr) 190 | { 191 | z = P->multiply(r); 192 | new_r_dot_r = z.dot(r); 193 | } 194 | 195 | /* 196 | * Update search direction. 197 | * The next residual will be orthogonal to new Krylov space. 198 | */ 199 | T beta = new_r_dot_r / r_dot_r; 200 | if (P != nullptr) 201 | d = z.add(d.multiply(beta)); 202 | else 203 | d = r.add(d.multiply(beta)); 204 | 205 | /* Update residual norm. */ 206 | r_dot_r = new_r_dot_r; 207 | } 208 | 209 | this->status.info = CG_MAX_ITERATIONS; 210 | return this->status; 211 | } 212 | 213 | /* ---------------------------------------------------------------- */ 214 | 215 | template 216 | inline 217 | CGBasicMatrixFunctor::CGBasicMatrixFunctor (SparseMatrix const& A) 218 | : A(&A) 219 | { 220 | } 221 | 222 | template 223 | inline DenseVector 224 | CGBasicMatrixFunctor::multiply (DenseVector const& x) const 225 | { 226 | return this->A->multiply(x); 227 | } 228 | 229 | template 230 | inline std::size_t 231 | CGBasicMatrixFunctor::input_size (void) const 232 | { 233 | return A->num_cols(); 234 | } 235 | 236 | template 237 | inline std::size_t 238 | CGBasicMatrixFunctor::output_size (void) const 239 | { 240 | return A->num_rows(); 241 | } 242 | 243 | SFM_NAMESPACE_END 244 | SFM_BA_NAMESPACE_END 245 | 246 | #endif /* SFM_BA_CONJUGATE_GRADIENT_HEADER */ 247 | -------------------------------------------------------------------------------- /sfm/ba_dense_vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_BA_DENSE_VECTOR_HEADER 11 | #define SFM_BA_DENSE_VECTOR_HEADER 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "sfm/defines.h" 18 | 19 | SFM_NAMESPACE_BEGIN 20 | SFM_BA_NAMESPACE_BEGIN 21 | 22 | template 23 | class DenseVector 24 | { 25 | public: 26 | DenseVector (void) = default; 27 | DenseVector (std::size_t size, T const& value = T(0)); 28 | void resize (std::size_t size, T const& value = T(0)); 29 | void clear (void); 30 | void fill (T const& value); 31 | std::size_t size (void) const; 32 | 33 | T* data (void); 34 | T const* data (void) const; 35 | T* begin (void); 36 | T const* begin (void) const; 37 | T* end (void); 38 | T const* end (void) const; 39 | 40 | DenseVector operator- (void) const; 41 | bool operator== (DenseVector const& rhs) const; 42 | T& operator[] (std::size_t index); 43 | T const& operator[] (std::size_t index) const; 44 | T& at (std::size_t index); 45 | T const& at (std::size_t index) const; 46 | 47 | T norm (void) const; 48 | T squared_norm (void) const; 49 | T dot (DenseVector const& rhs) const; 50 | DenseVector add (DenseVector const& rhs) const; 51 | DenseVector subtract (DenseVector const& rhs) const; 52 | DenseVector multiply (T const& factor) const; 53 | void multiply_self (T const& factor); 54 | void negate_self (void); 55 | 56 | private: 57 | std::vector values; 58 | }; 59 | 60 | /* ------------------------ Implementation ------------------------ */ 61 | 62 | template 63 | inline 64 | DenseVector::DenseVector (std::size_t size, T const& value) 65 | { 66 | this->resize(size, value); 67 | } 68 | 69 | template 70 | inline void 71 | DenseVector::resize (std::size_t size, T const& value) 72 | { 73 | this->values.clear(); 74 | this->values.resize(size, value); 75 | } 76 | 77 | template 78 | inline void 79 | DenseVector::clear (void) 80 | { 81 | this->values.clear(); 82 | } 83 | 84 | template 85 | inline void 86 | DenseVector::fill (T const& value) 87 | { 88 | std::fill(this->values.begin(), this->values.end(), value); 89 | } 90 | 91 | template 92 | std::size_t 93 | DenseVector::size (void) const 94 | { 95 | return this->values.size(); 96 | } 97 | 98 | template 99 | T* 100 | DenseVector::data (void) 101 | { 102 | return this->values.data(); 103 | } 104 | 105 | template 106 | T const* 107 | DenseVector::data (void) const 108 | { 109 | return this->values.data(); 110 | } 111 | 112 | template 113 | T* 114 | DenseVector::begin (void) 115 | { 116 | return this->values.data(); 117 | } 118 | 119 | template 120 | T const* 121 | DenseVector::begin (void) const 122 | { 123 | return this->values.data(); 124 | } 125 | 126 | template 127 | T* 128 | DenseVector::end (void) 129 | { 130 | return this->values.data() + this->values.size(); 131 | } 132 | 133 | template 134 | T const* 135 | DenseVector::end (void) const 136 | { 137 | return this->values.data() + this->values.size(); 138 | } 139 | 140 | template 141 | DenseVector 142 | DenseVector::operator- (void) const 143 | { 144 | DenseVector ret(this->size()); 145 | for (std::size_t i = 0; i < this->size(); ++i) 146 | ret[i] = -this->at(i); 147 | return ret; 148 | } 149 | 150 | template 151 | bool 152 | DenseVector::operator== (DenseVector const& rhs) const 153 | { 154 | if (this->size() != rhs.size()) 155 | return false; 156 | for (std::size_t i = 0; i < this->size(); ++i) 157 | if (this->at(i) != rhs.at(i)) 158 | return false; 159 | return true; 160 | } 161 | 162 | template 163 | T& 164 | DenseVector::operator[] (std::size_t index) 165 | { 166 | return this->values[index]; 167 | } 168 | 169 | template 170 | T const& 171 | DenseVector::operator[] (std::size_t index) const 172 | { 173 | return this->values[index]; 174 | } 175 | 176 | template 177 | T& 178 | DenseVector::at (std::size_t index) 179 | { 180 | return this->values[index]; 181 | } 182 | 183 | template 184 | T const& 185 | DenseVector::at (std::size_t index) const 186 | { 187 | return this->values[index]; 188 | } 189 | 190 | template 191 | inline T 192 | DenseVector::norm (void) const 193 | { 194 | return std::sqrt(this->squared_norm()); 195 | } 196 | 197 | template 198 | T 199 | DenseVector::squared_norm (void) const 200 | { 201 | return this->dot(*this); 202 | } 203 | 204 | template 205 | T 206 | DenseVector::dot (DenseVector const& rhs) const 207 | { 208 | if (this->size() != rhs.size()) 209 | throw std::invalid_argument("Incompatible vector dimensions"); 210 | 211 | T ret(0); 212 | for (std::size_t i = 0; i < this->size(); ++i) 213 | ret += this->values[i] * rhs.values[i]; 214 | return ret; 215 | } 216 | 217 | template 218 | DenseVector 219 | DenseVector::subtract (DenseVector const& rhs) const 220 | { 221 | if (this->size() != rhs.size()) 222 | throw std::invalid_argument("Incompatible vector dimensions"); 223 | 224 | DenseVector ret(this->size(), T(0)); 225 | for (std::size_t i = 0; i < this->size(); ++i) 226 | ret.values[i] = this->values[i] - rhs.values[i]; 227 | return ret; 228 | } 229 | 230 | template 231 | DenseVector 232 | DenseVector::add (DenseVector const& rhs) const 233 | { 234 | if (this->size() != rhs.size()) 235 | throw std::invalid_argument("Incompatible vector dimensions"); 236 | 237 | DenseVector ret(this->size(), T(0)); 238 | for (std::size_t i = 0; i < this->size(); ++i) 239 | ret.values[i] = this->values[i] + rhs.values[i]; 240 | return ret; 241 | } 242 | 243 | template 244 | DenseVector 245 | DenseVector::multiply (T const& factor) const 246 | { 247 | DenseVector ret(this->size(), T(0)); 248 | for (std::size_t i = 0; i < this->size(); ++i) 249 | ret[i] = this->at(i) * factor; 250 | return ret; 251 | } 252 | 253 | template 254 | void 255 | DenseVector::multiply_self (T const& factor) 256 | { 257 | for (std::size_t i = 0; i < this->size(); ++i) 258 | this->at(i) *= factor; 259 | } 260 | 261 | template 262 | void 263 | DenseVector::negate_self (void) 264 | { 265 | for (std::size_t i = 0; i < this->size(); ++i) 266 | this->at(i) = -this->at(i); 267 | } 268 | 269 | SFM_BA_NAMESPACE_END 270 | SFM_NAMESPACE_END 271 | 272 | #endif // SFM_BA_DENSE_VECTOR_HEADER 273 | -------------------------------------------------------------------------------- /sfm/ba_linear_solver.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "util/timer.h" 14 | #include "math/matrix.h" 15 | #include "math/matrix_tools.h" 16 | #include "sfm/ba_linear_solver.h" 17 | #include "sfm/ba_cholesky.h" 18 | #include "sfm/ba_conjugate_gradient.h" 19 | 20 | SFM_NAMESPACE_BEGIN 21 | SFM_BA_NAMESPACE_BEGIN 22 | 23 | namespace 24 | { 25 | /* 26 | * Inverts a symmetric, positive definite matrix with NxN bocks on its 27 | * diagonal using Cholesky decomposition. All other entries must be zero. 28 | */ 29 | void 30 | invert_block_matrix_NxN_inplace (SparseMatrix* A, int blocksize) 31 | { 32 | if (A->num_rows() != A->num_cols()) 33 | throw std::invalid_argument("Block matrix must be square"); 34 | if (A->num_non_zero() != A->num_rows() * blocksize) 35 | throw std::invalid_argument("Invalid number of non-zeros"); 36 | 37 | int const bs2 = blocksize * blocksize; 38 | std::vector matrix_block(bs2); 39 | for (double* iter = A->begin(); iter != A->end(); ) 40 | { 41 | double* iter_backup = iter; 42 | for (int i = 0; i < bs2; ++i) 43 | matrix_block[i] = *(iter++); 44 | 45 | cholesky_invert_inplace(matrix_block.data(), blocksize); 46 | 47 | iter = iter_backup; 48 | for (int i = 0; i < bs2; ++i) 49 | if (std::isfinite(matrix_block[i])) 50 | *(iter++) = matrix_block[i]; 51 | else 52 | *(iter++) = 0.0; 53 | } 54 | } 55 | 56 | /* 57 | * Inverts a matrix with 3x3 bocks on its diagonal. All other entries 58 | * must be zero. Reading blocks is thus very efficient. 59 | */ 60 | void 61 | invert_block_matrix_3x3_inplace (SparseMatrix* A) 62 | { 63 | if (A->num_rows() != A->num_cols()) 64 | throw std::invalid_argument("Block matrix must be square"); 65 | if (A->num_non_zero() != A->num_rows() * 3) 66 | throw std::invalid_argument("Invalid number of non-zeros"); 67 | 68 | for (double* iter = A->begin(); iter != A->end(); ) 69 | { 70 | double* iter_backup = iter; 71 | math::Matrix rot; 72 | for (int i = 0; i < 9; ++i) 73 | rot[i] = *(iter++); 74 | 75 | double det = math::matrix_determinant(rot); 76 | if (MATH_DOUBLE_EQ(det, 0.0)) 77 | continue; 78 | 79 | rot = math::matrix_inverse(rot, det); 80 | iter = iter_backup; 81 | for (int i = 0; i < 9; ++i) 82 | *(iter++) = rot[i]; 83 | } 84 | } 85 | 86 | /* 87 | * Computes for a given matrix A the square matrix A^T * A for the 88 | * case that block columns of A only need to be multiplied with itself. 89 | * Becase the resulting matrix is symmetric, only about half the work 90 | * needs to be performed. 91 | */ 92 | void 93 | matrix_block_column_multiply (SparseMatrix const& A, 94 | std::size_t block_size, SparseMatrix* B) 95 | { 96 | SparseMatrix::Triplets triplets; 97 | triplets.reserve(A.num_cols() * block_size); 98 | for (std::size_t block = 0; block < A.num_cols(); block += block_size) 99 | { 100 | std::vector> columns(block_size); 101 | for (std::size_t col = 0; col < block_size; ++col) 102 | A.column_nonzeros(block + col, &columns[col]); 103 | for (std::size_t col = 0; col < block_size; ++col) 104 | { 105 | double dot = columns[col].dot(columns[col]); 106 | triplets.emplace_back(block + col, block + col, dot); 107 | for (std::size_t row = col + 1; row < block_size; ++row) 108 | { 109 | dot = columns[col].dot(columns[row]); 110 | triplets.emplace_back(block + row, block + col, dot); 111 | triplets.emplace_back(block + col, block + row, dot); 112 | } 113 | } 114 | } 115 | B->allocate(A.num_cols(), A.num_cols()); 116 | B->set_from_triplets(triplets); 117 | } 118 | } 119 | 120 | LinearSolver::Status 121 | LinearSolver::solve (SparseMatrixType const& jac_cams, 122 | SparseMatrixType const& jac_points, 123 | DenseVectorType const& vector_f, 124 | DenseVectorType* delta_x) 125 | { 126 | bool const has_jac_cams = jac_cams.num_rows() > 0; 127 | bool const has_jac_points = jac_points.num_rows() > 0; 128 | 129 | /* Select solver based on bundle adjustment mode. */ 130 | if (has_jac_cams && has_jac_points) 131 | return this->solve_schur(jac_cams, jac_points, vector_f, delta_x); 132 | else if (has_jac_cams && !has_jac_points) 133 | return this->solve(jac_cams, vector_f, delta_x, 0); 134 | else if (!has_jac_cams && has_jac_points) 135 | return this->solve(jac_points, vector_f, delta_x, 3); 136 | else 137 | throw std::invalid_argument("No Jacobian given"); 138 | } 139 | 140 | LinearSolver::Status 141 | LinearSolver::solve_schur (SparseMatrixType const& jac_cams, 142 | SparseMatrixType const& jac_points, 143 | DenseVectorType const& values, DenseVectorType* delta_x) 144 | { 145 | /* 146 | * Jacobian J = [ Jc Jp ] with Jc camera block, Jp point block. 147 | * Hessian H = [ B E; E^T C ] = J^T J = [ Jc^T; Jp^T ] * [ Jc Jp ] 148 | * with B = Jc^T * Jc and E = Jc^T * Jp and C = Jp^T Jp 149 | */ 150 | DenseVectorType const& F = values; 151 | SparseMatrixType const& Jc = jac_cams; 152 | SparseMatrixType const& Jp = jac_points; 153 | SparseMatrixType JcT = Jc.transpose(); 154 | SparseMatrixType JpT = Jp.transpose(); 155 | 156 | /* Compute the blocks of the Hessian. */ 157 | SparseMatrixType B, C; 158 | /* Jc^T * Jc */ 159 | matrix_block_column_multiply(Jc, this->opts.camera_block_dim, &B); 160 | /* Jp^T * Jp */ 161 | matrix_block_column_multiply(Jp, 3, &C); 162 | SparseMatrixType E = JcT.multiply(Jp); 163 | 164 | /* Assemble two values vectors. */ 165 | DenseVectorType v = JcT.multiply(F); 166 | DenseVectorType w = JpT.multiply(F); 167 | v.negate_self(); 168 | w.negate_self(); 169 | 170 | /* Save diagonal for computing predicted error decrease */ 171 | SparseMatrixType B_diag = B.diagonal_matrix(); 172 | SparseMatrixType C_diag = C.diagonal_matrix(); 173 | 174 | /* Add regularization to C and B. */ 175 | C.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius); 176 | B.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius); 177 | 178 | /* Invert C matrix. */ 179 | invert_block_matrix_3x3_inplace(&C); 180 | 181 | /* Compute the Schur complement matrix S. */ 182 | SparseMatrixType ET = E.transpose(); 183 | SparseMatrixType S = B.subtract(E.multiply(C).multiply(ET)); 184 | DenseVectorType rhs = v.subtract(E.multiply(C.multiply(w))); 185 | 186 | /* Compute pre-conditioner for linear system. */ 187 | //SparseMatrixType precond = S.diagonal_matrix(); 188 | //precond.cwise_invert(); 189 | SparseMatrixType precond = B; 190 | invert_block_matrix_NxN_inplace(&precond, this->opts.camera_block_dim); 191 | 192 | /* Solve linear system. */ 193 | DenseVectorType delta_y(Jc.num_cols()); 194 | typedef sfm::ba::ConjugateGradient CGSolver; 195 | CGSolver::Options cg_opts; 196 | cg_opts.max_iterations = this->opts.cg_max_iterations; 197 | cg_opts.tolerance = 1e-20; 198 | CGSolver solver(cg_opts); 199 | CGSolver::Status cg_status; 200 | cg_status = solver.solve(S, rhs, &delta_y, &precond); 201 | 202 | Status status; 203 | status.num_cg_iterations = cg_status.num_iterations; 204 | switch (cg_status.info) 205 | { 206 | case CGSolver::CG_CONVERGENCE: 207 | status.success = true; 208 | break; 209 | case CGSolver::CG_MAX_ITERATIONS: 210 | status.success = true; 211 | break; 212 | case CGSolver::CG_INVALID_INPUT: 213 | std::cout << "BA: CG failed (invalid input)" << std::endl; 214 | status.success = false; 215 | return status; 216 | default: 217 | break; 218 | } 219 | 220 | /* Substitute back to obtain delta z. */ 221 | DenseVectorType delta_z = C.multiply(w.subtract(ET.multiply(delta_y))); 222 | 223 | /* Fill output vector. */ 224 | std::size_t const jac_cam_cols = Jc.num_cols(); 225 | std::size_t const jac_point_cols = Jp.num_cols(); 226 | std::size_t const jac_cols = jac_cam_cols + jac_point_cols; 227 | 228 | if (delta_x->size() != jac_cols) 229 | delta_x->resize(jac_cols, 0.0); 230 | for (std::size_t i = 0; i < jac_cam_cols; ++i) 231 | delta_x->at(i) = delta_y[i]; 232 | for (std::size_t i = 0; i < jac_point_cols; ++i) 233 | delta_x->at(jac_cam_cols + i) = delta_z[i]; 234 | 235 | /* Compute predicted error decrease */ 236 | status.predicted_error_decrease = 0.0; 237 | status.predicted_error_decrease += delta_y.dot(B_diag.multiply( 238 | delta_y).multiply(1.0 / this->opts.trust_region_radius).add(v)); 239 | status.predicted_error_decrease += delta_z.dot(C_diag.multiply( 240 | delta_z).multiply(1.0 / this->opts.trust_region_radius).add(w)); 241 | 242 | return status; 243 | } 244 | 245 | LinearSolver::Status 246 | LinearSolver::solve (SparseMatrixType const& J, 247 | DenseVectorType const& vector_f, 248 | DenseVectorType* delta_x, 249 | std::size_t block_size) 250 | { 251 | DenseVectorType const& F = vector_f; 252 | SparseMatrixType Jt = J.transpose(); 253 | SparseMatrixType H = Jt.multiply(J); 254 | SparseMatrixType H_diag = H.diagonal_matrix(); 255 | 256 | /* Compute RHS. */ 257 | DenseVectorType g = Jt.multiply(F); 258 | g.negate_self(); 259 | 260 | /* Add regularization to H. */ 261 | H.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius); 262 | 263 | Status status; 264 | if (block_size == 0) 265 | { 266 | /* Use preconditioned CG using the diagonal of H. */ 267 | SparseMatrixType precond = H.diagonal_matrix(); 268 | precond.cwise_invert(); 269 | 270 | typedef sfm::ba::ConjugateGradient CGSolver; 271 | CGSolver::Options cg_opts; 272 | cg_opts.max_iterations = this->opts.cg_max_iterations; 273 | cg_opts.tolerance = 1e-20; 274 | CGSolver solver(cg_opts); 275 | CGSolver::Status cg_status; 276 | cg_status = solver.solve(H, g, delta_x, &precond); 277 | status.num_cg_iterations = cg_status.num_iterations; 278 | 279 | switch (cg_status.info) 280 | { 281 | case CGSolver::CG_CONVERGENCE: 282 | status.success = true; 283 | break; 284 | case CGSolver::CG_MAX_ITERATIONS: 285 | status.success = true; 286 | break; 287 | case CGSolver::CG_INVALID_INPUT: 288 | std::cout << "BA: CG failed (invalid input)" << std::endl; 289 | status.success = false; 290 | return status; 291 | default: 292 | break; 293 | } 294 | } 295 | else if (block_size == 3) 296 | { 297 | /* Invert blocks of H directly */ 298 | invert_block_matrix_3x3_inplace(&H); 299 | *delta_x = H.multiply(g); 300 | status.success = true; 301 | status.num_cg_iterations = 0; 302 | } 303 | else 304 | { 305 | status.success = false; 306 | throw std::invalid_argument("Unsupported block_size in linear solver"); 307 | } 308 | 309 | status.predicted_error_decrease = delta_x->dot(H_diag.multiply( 310 | *delta_x).multiply(1.0 / this->opts.trust_region_radius).add(g)); 311 | 312 | return status; 313 | } 314 | 315 | SFM_BA_NAMESPACE_END 316 | SFM_NAMESPACE_END 317 | -------------------------------------------------------------------------------- /sfm/ba_linear_solver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_BA_LINEAR_SOLVER_HEADER 11 | #define SFM_BA_LINEAR_SOLVER_HEADER 12 | 13 | #include 14 | 15 | #include "sfm/defines.h" 16 | #include "sfm/ba_sparse_matrix.h" 17 | #include "sfm/ba_dense_vector.h" 18 | 19 | SFM_NAMESPACE_BEGIN 20 | SFM_BA_NAMESPACE_BEGIN 21 | 22 | class LinearSolver 23 | { 24 | public: 25 | struct Options 26 | { 27 | Options (void); 28 | 29 | double trust_region_radius; 30 | int cg_max_iterations; 31 | int camera_block_dim; 32 | }; 33 | 34 | struct Status 35 | { 36 | Status (void); 37 | 38 | double predicted_error_decrease; 39 | int num_cg_iterations; 40 | bool success; 41 | }; 42 | 43 | typedef SparseMatrix SparseMatrixType; 44 | typedef DenseVector DenseVectorType; 45 | 46 | public: 47 | LinearSolver (Options const& options); 48 | 49 | /** 50 | * Solve the system J^T J x = -J^T f based on the bundle adjustment mode. 51 | * If the Jacobian for cameras is empty, only points are optimized. 52 | * If the Jacobian for points is empty, only cameras are optimized. 53 | * If both, Jacobian for cams and points is given, the Schur complement 54 | * trick is used to solve the linear system. 55 | */ 56 | Status solve (SparseMatrixType const& jac_cams, 57 | SparseMatrixType const& jac_points, 58 | DenseVectorType const& vector_f, 59 | DenseVectorType* delta_x); 60 | 61 | private: 62 | /** 63 | * Conjugate Gradient on Schur-complement by exploiting the block 64 | * structure of H = J^T * J. 65 | */ 66 | Status solve_schur (SparseMatrixType const& jac_cams, 67 | SparseMatrixType const& jac_points, 68 | DenseVectorType const& values, 69 | DenseVectorType* delta_x); 70 | 71 | /** 72 | * J is the Jacobian of the problem. If H = J^T * J has a block diagonal 73 | * structure (e.g. 'motion only' or 'structure only' problems in BA), 74 | * block_size can be used to directly invert H. If block_size is 0 75 | * the diagonal of H is used as a preconditioner and the linear system 76 | * is solved via conjugate gradient. 77 | */ 78 | Status solve (SparseMatrixType const& J, 79 | DenseVectorType const& vector_f, 80 | DenseVectorType* delta_x, 81 | std::size_t block_size = 0); 82 | 83 | private: 84 | Options opts; 85 | }; 86 | 87 | /* ------------------------ Implementation ------------------------ */ 88 | 89 | inline 90 | LinearSolver::Options::Options (void) 91 | : trust_region_radius(1.0) 92 | , cg_max_iterations(1000) 93 | { 94 | } 95 | 96 | inline 97 | LinearSolver::Status::Status (void) 98 | : predicted_error_decrease(0.0) 99 | , num_cg_iterations(0) 100 | , success(false) 101 | { 102 | } 103 | 104 | inline 105 | LinearSolver::LinearSolver (Options const& options) 106 | : opts(options) 107 | { 108 | } 109 | 110 | SFM_BA_NAMESPACE_END 111 | SFM_NAMESPACE_END 112 | 113 | #endif // SFM_BA_LINEAR_SOLVER_HEADER 114 | 115 | -------------------------------------------------------------------------------- /sfm/ba_sparse_matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_SPARSE_MATRIX_HEADER 11 | #define SFM_SPARSE_MATRIX_HEADER 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "sfm/ba_dense_vector.h" 19 | #include "sfm/defines.h" 20 | 21 | SFM_NAMESPACE_BEGIN 22 | SFM_BA_NAMESPACE_BEGIN 23 | 24 | /** 25 | * Sparse matrix class in Yale format for column-major matrices. 26 | */ 27 | template 28 | class SparseMatrix 29 | { 30 | public: 31 | /** Triplet with row/col index, and the actual value. */ 32 | struct Triplet 33 | { 34 | Triplet (void) = default; 35 | Triplet (std::size_t row, std::size_t col, T const& value); 36 | 37 | std::size_t row; 38 | std::size_t col; 39 | T value; 40 | }; 41 | 42 | /** List of triplets. */ 43 | typedef std::vector Triplets; 44 | 45 | public: 46 | SparseMatrix (void); 47 | SparseMatrix (std::size_t rows, std::size_t cols); 48 | void allocate (std::size_t rows, std::size_t cols); 49 | void reserve (std::size_t num_elements); 50 | void set_from_triplets (Triplets const& triplets); 51 | void mult_diagonal (T const& factor); 52 | void cwise_invert (void); 53 | void column_nonzeros (std::size_t col, DenseVector* vector) const; 54 | 55 | SparseMatrix transpose (void) const; 56 | SparseMatrix subtract (SparseMatrix const& rhs) const; 57 | SparseMatrix multiply (SparseMatrix const& rhs) const; 58 | SparseMatrix sequential_multiply (SparseMatrix const& rhs) const; 59 | SparseMatrix parallel_multiply (SparseMatrix const& rhs) const; 60 | DenseVector multiply (DenseVector const& rhs) const; 61 | SparseMatrix diagonal_matrix (void) const; 62 | 63 | std::size_t num_non_zero (void) const; 64 | std::size_t num_rows (void) const; 65 | std::size_t num_cols (void) const; 66 | T* begin (void); 67 | T* end (void); 68 | 69 | void debug (void) const; 70 | 71 | private: 72 | std::size_t rows; 73 | std::size_t cols; 74 | std::vector values; 75 | std::vector outer; 76 | std::vector inner; 77 | }; 78 | 79 | SFM_BA_NAMESPACE_END 80 | SFM_NAMESPACE_END 81 | 82 | /* ------------------------ Implementation ------------------------ */ 83 | 84 | #include 85 | 86 | SFM_NAMESPACE_BEGIN 87 | SFM_BA_NAMESPACE_BEGIN 88 | 89 | template 90 | SparseMatrix::Triplet::Triplet (std::size_t row, 91 | std::size_t col, T const& value) 92 | : row(row), col(col), value(value) 93 | { 94 | } 95 | 96 | /* --------------------------------------------------------------- */ 97 | 98 | template 99 | SparseMatrix::SparseMatrix (void) 100 | : rows(0) 101 | , cols(0) 102 | { 103 | } 104 | 105 | template 106 | SparseMatrix::SparseMatrix (std::size_t rows, std::size_t cols) 107 | { 108 | this->allocate(rows, cols); 109 | } 110 | 111 | template 112 | void 113 | SparseMatrix::allocate (std::size_t rows, std::size_t cols) 114 | { 115 | this->rows = rows; 116 | this->cols = cols; 117 | this->values.clear(); 118 | this->outer.clear(); 119 | this->inner.clear();// col id of none zero elements 120 | this->outer.resize(cols + 1, 0); 121 | } 122 | 123 | template 124 | void 125 | SparseMatrix::reserve (std::size_t num_elements) 126 | { 127 | this->inner.reserve(num_elements); 128 | this->values.reserve(num_elements); 129 | } 130 | 131 | template 132 | void 133 | SparseMatrix::set_from_triplets (Triplets const& triplets) 134 | { 135 | /* Create a temporary transposed matrix */ 136 | SparseMatrix transposed(this->cols, this->rows); 137 | transposed.values.resize(triplets.size()); 138 | transposed.inner.resize(triplets.size()); 139 | 140 | // 统计每一行 Non-zero elements 的个数 141 | /* Initialize outer indices with amount of inner values. */ 142 | for (std::size_t i = 0; i < triplets.size(); ++i) 143 | transposed.outer[triplets[i].row]++; 144 | 145 | /* Convert amounts to indices with prefix sum. */ 146 | std::size_t sum = 0; 147 | std::vector scratch(transposed.outer.size()); 148 | for (std::size_t i = 0; i < transposed.outer.size(); ++i) 149 | { 150 | std::size_t const temp = transposed.outer[i]; 151 | transposed.outer[i] = sum; 152 | scratch[i] = sum; 153 | sum += temp; 154 | } 155 | 156 | /* Add triplets, inner indices are unsorted. */ 157 | for (std::size_t i = 0; i < triplets.size(); ++i) 158 | { 159 | Triplet const& t = triplets[i]; 160 | std::size_t pos = scratch[t.row]++; 161 | transposed.values[pos] = t.value; 162 | transposed.inner[pos] = t.col; 163 | } 164 | 165 | /* Transpose matrix, implicit sorting of inner indices. */ 166 | *this = transposed.transpose(); 167 | } 168 | 169 | template 170 | SparseMatrix 171 | SparseMatrix::transpose (void) const 172 | { 173 | SparseMatrix ret(this->cols, this->rows); 174 | ret.values.resize(this->num_non_zero()); 175 | ret.inner.resize(this->num_non_zero()); 176 | 177 | /* Compute inner sizes of transposed matrix. */ 178 | for(std::size_t i = 0; i < this->inner.size(); ++i) 179 | ret.outer[this->inner[i]] += 1; 180 | 181 | /* Compute outer sizes of transposed matrix with prefix sum. */ 182 | std::size_t sum = 0; 183 | std::vector scratch(ret.outer.size()); 184 | for (std::size_t i = 0; i < ret.outer.size(); ++i) 185 | { 186 | std::size_t const temp = ret.outer[i]; 187 | ret.outer[i] = sum; 188 | scratch[i] = sum; 189 | sum += temp; 190 | } 191 | 192 | /* Write inner indices and values of transposed matrix. */ 193 | for (std::size_t i = 0; i < this->outer.size() - 1; ++i) 194 | for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j) 195 | { 196 | std::size_t pos = scratch[this->inner[j]]++; 197 | ret.inner[pos] = i; 198 | ret.values[pos] = this->values[j]; 199 | } 200 | 201 | return ret; 202 | } 203 | 204 | template 205 | SparseMatrix 206 | SparseMatrix::subtract (SparseMatrix const& rhs) const 207 | { 208 | if (this->rows != rhs.rows || this->cols != rhs.cols) 209 | throw std::invalid_argument("Incompatible matrix dimensions"); 210 | 211 | SparseMatrix ret(this->rows, this->cols); 212 | ret.reserve(this->num_non_zero() + rhs.num_non_zero()); 213 | 214 | std::size_t num_outer = this->outer.size() - 1; 215 | for (std::size_t outer = 0; outer < num_outer; ++outer) 216 | { 217 | ret.outer[outer] = ret.values.size(); 218 | 219 | std::size_t i1 = this->outer[outer]; 220 | std::size_t i2 = rhs.outer[outer]; 221 | std::size_t const i1_end = this->outer[outer + 1]; 222 | std::size_t const i2_end = rhs.outer[outer + 1]; 223 | while (i1 < i1_end || i2 < i2_end) 224 | { 225 | if (i1 >= i1_end) 226 | { 227 | ret.values.push_back(-rhs.values[i2]); 228 | ret.inner.push_back(rhs.inner[i2]); 229 | i2 += 1; 230 | continue; 231 | } 232 | if (i2 >= i2_end) 233 | { 234 | ret.values.push_back(this->values[i1]); 235 | ret.inner.push_back(this->inner[i1]); 236 | i1 += 1; 237 | continue; 238 | } 239 | 240 | std::size_t id1 = this->inner[i1]; 241 | std::size_t id2 = rhs.inner[i2]; 242 | 243 | if (id1 < id2) 244 | ret.values.push_back(this->values[i1]); 245 | else if (id2 < id1) 246 | ret.values.push_back(-rhs.values[i2]); 247 | else 248 | ret.values.push_back(this->values[i1] - rhs.values[i2]); 249 | 250 | i1 += static_cast(id1 <= id2); 251 | i2 += static_cast(id2 <= id1); 252 | ret.inner.push_back(std::min(id1, id2)); 253 | } 254 | } 255 | ret.outer.back() = ret.values.size(); 256 | 257 | return ret; 258 | } 259 | 260 | template 261 | SparseMatrix 262 | SparseMatrix::multiply (SparseMatrix const& rhs) const 263 | { 264 | #ifdef _OPENMP 265 | return this->parallel_multiply(rhs); 266 | #else 267 | return this->sequential_multiply(rhs); 268 | #endif 269 | } 270 | 271 | template 272 | SparseMatrix 273 | SparseMatrix::sequential_multiply (SparseMatrix const& rhs) const 274 | { 275 | if (this->cols != rhs.rows) 276 | throw std::invalid_argument("Incompatible matrix dimensions"); 277 | 278 | SparseMatrix ret(this->rows, rhs.cols); 279 | ret.reserve(this->num_non_zero() + rhs.num_non_zero()); 280 | 281 | /* Matrix-matrix multiplication. */ 282 | std::vector ret_col(ret.rows, T(0)); 283 | std::vector ret_nonzero(ret.rows, false); 284 | for (std::size_t col = 0; col < ret.cols; ++col) 285 | { 286 | ret.outer[col] = ret.values.size(); 287 | 288 | std::fill(ret_col.begin(), ret_col.end(), T(0)); 289 | std::fill(ret_nonzero.begin(), ret_nonzero.end(), false); 290 | std::size_t rhs_col_begin = rhs.outer[col]; 291 | std::size_t rhs_col_end = rhs.outer[col + 1]; 292 | for (std::size_t i = rhs_col_begin; i < rhs_col_end; ++i) 293 | { 294 | T const& rhs_col_value = rhs.values[i]; 295 | std::size_t const lhs_col = rhs.inner[i]; 296 | std::size_t const lhs_col_begin = this->outer[lhs_col]; 297 | std::size_t const lhs_col_end = this->outer[lhs_col + 1]; 298 | 299 | for (std::size_t j = lhs_col_begin; j < lhs_col_end; ++j) 300 | { 301 | std::size_t const id = this->inner[j]; 302 | ret_col[id] += this->values[j] * rhs_col_value; 303 | ret_nonzero[id] = true; 304 | } 305 | } 306 | for (std::size_t i = 0; i < ret.rows; ++i) 307 | if (ret_nonzero[i]) 308 | { 309 | ret.inner.push_back(i); 310 | ret.values.push_back(ret_col[i]); 311 | } 312 | } 313 | ret.outer[ret.cols] = ret.values.size(); 314 | 315 | return ret; 316 | } 317 | 318 | template 319 | SparseMatrix 320 | SparseMatrix::parallel_multiply (SparseMatrix const& rhs) const 321 | { 322 | if (this->cols != rhs.rows) 323 | throw std::invalid_argument("Incompatible matrix dimensions"); 324 | 325 | std::size_t nnz = this->num_non_zero() + rhs.num_non_zero(); 326 | SparseMatrix ret(this->rows, rhs.cols); 327 | ret.reserve(nnz); 328 | std::fill(ret.outer.begin(), ret.outer.end(), 0); 329 | 330 | std::size_t const chunk_size = 64; 331 | std::size_t const num_chunks = ret.cols / chunk_size 332 | + (ret.cols % chunk_size != 0); 333 | std::size_t const max_threads = std::max(1u, 334 | std::thread::hardware_concurrency()); 335 | std::size_t const num_threads = std::min(num_chunks, max_threads); 336 | 337 | #pragma omp parallel num_threads(num_threads) 338 | { 339 | /* Matrix-matrix multiplication. */ 340 | std::vector ret_col(ret.rows, T(0)); 341 | std::vector ret_nonzero(ret.rows, false); 342 | std::vector thread_values; 343 | thread_values.reserve(nnz / num_chunks); 344 | std::vector thread_inner; 345 | thread_inner.reserve(nnz / num_chunks); 346 | 347 | #pragma omp for ordered schedule(static, 1) 348 | for (std::size_t chunk = 0; chunk < num_chunks; ++chunk) 349 | { 350 | thread_inner.clear(); 351 | thread_values.clear(); 352 | 353 | std::size_t const begin = chunk * chunk_size; 354 | std::size_t const end = std::min(begin + chunk_size, ret.cols); 355 | for (std::size_t col = begin; col < end; ++col) 356 | { 357 | std::fill(ret_col.begin(), ret_col.end(), T(0)); 358 | std::fill(ret_nonzero.begin(), ret_nonzero.end(), false); 359 | std::size_t const rhs_col_begin = rhs.outer[col]; 360 | std::size_t const rhs_col_end = rhs.outer[col + 1]; 361 | for (std::size_t i = rhs_col_begin; i < rhs_col_end; ++i) 362 | { 363 | T const& rhs_col_value = rhs.values[i]; 364 | std::size_t const lhs_col = rhs.inner[i]; 365 | std::size_t const lhs_col_begin = this->outer[lhs_col]; 366 | std::size_t const lhs_col_end = this->outer[lhs_col + 1]; 367 | 368 | for (std::size_t j = lhs_col_begin; j < lhs_col_end; ++j) 369 | { 370 | std::size_t const id = this->inner[j]; 371 | ret_col[id] += this->values[j] * rhs_col_value; 372 | ret_nonzero[id] = true; 373 | } 374 | } 375 | for (std::size_t i = 0; i < ret.rows; ++i) 376 | if (ret_nonzero[i]) 377 | { 378 | ret.outer[col + 1] += 1; 379 | thread_inner.push_back(i); 380 | thread_values.push_back(ret_col[i]); 381 | } 382 | } 383 | 384 | #pragma omp ordered 385 | { 386 | ret.inner.insert(ret.inner.end(), 387 | thread_inner.begin(), thread_inner.end()); 388 | ret.values.insert(ret.values.end(), 389 | thread_values.begin(), thread_values.end()); 390 | } 391 | } 392 | } 393 | 394 | for (std::size_t col = 0; col < ret.cols; ++col) 395 | ret.outer[col + 1] += ret.outer[col]; 396 | 397 | return ret; 398 | } 399 | 400 | template 401 | DenseVector 402 | SparseMatrix::multiply (DenseVector const& rhs) const 403 | { 404 | if (rhs.size() != this->cols) 405 | throw std::invalid_argument("Incompatible dimensions"); 406 | 407 | DenseVector ret(this->rows, T(0)); 408 | for (std::size_t i = 0; i < this->cols; ++i) 409 | for (std::size_t id = this->outer[i]; id < this->outer[i + 1]; ++id) 410 | ret[this->inner[id]] += this->values[id] * rhs[i]; 411 | return ret; 412 | } 413 | 414 | template 415 | SparseMatrix 416 | SparseMatrix::diagonal_matrix (void) const 417 | { 418 | std::size_t const diag_size = std::min(this->rows, this->cols); 419 | SparseMatrix ret(diag_size, diag_size); 420 | ret.reserve(diag_size); 421 | for (std::size_t i = 0; i < diag_size; ++i) 422 | { 423 | ret.outer[i] = ret.values.size(); 424 | for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j) 425 | if (this->inner[j] == i) 426 | { 427 | ret.inner.push_back(i); 428 | ret.values.push_back(this->values[j]); 429 | } 430 | else if (this->inner[j] > i) 431 | break; 432 | } 433 | ret.outer[diag_size] = ret.values.size(); 434 | return ret; 435 | } 436 | 437 | template 438 | void 439 | SparseMatrix::mult_diagonal (T const& factor) 440 | { 441 | for (std::size_t i = 0; i < this->outer.size() - 1; ++i) 442 | for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j) 443 | { 444 | if (this->inner[j] == i) 445 | this->values[j] *= factor; 446 | if (this->inner[j] >= i) 447 | break; 448 | } 449 | } 450 | 451 | template 452 | void 453 | SparseMatrix::cwise_invert (void) 454 | { 455 | for (std::size_t i = 0; i < this->values.size(); ++i) 456 | this->values[i] = T(1) / this->values[i]; 457 | } 458 | 459 | template 460 | void 461 | SparseMatrix::column_nonzeros (std::size_t col, DenseVector* vector) const 462 | { 463 | std::size_t const start = this->outer[col]; 464 | std::size_t const end = this->outer[col + 1]; 465 | vector->resize(end - start); 466 | for (std::size_t row = start, i = 0; row < end; ++row, ++i) 467 | vector->at(i) = this->values[row]; 468 | } 469 | 470 | template 471 | inline std::size_t 472 | SparseMatrix::num_non_zero (void) const 473 | { 474 | return this->values.size(); 475 | } 476 | 477 | template 478 | inline std::size_t 479 | SparseMatrix::num_rows (void) const 480 | { 481 | return this->rows; 482 | } 483 | 484 | template 485 | inline std::size_t 486 | SparseMatrix::num_cols (void) const 487 | { 488 | return this->cols; 489 | } 490 | 491 | template 492 | inline T* 493 | SparseMatrix::begin (void) 494 | { 495 | return this->values.data(); 496 | } 497 | 498 | template 499 | inline T* 500 | SparseMatrix::end (void) 501 | { 502 | return this->values.data() + this->values.size(); 503 | } 504 | 505 | template 506 | void 507 | SparseMatrix::debug (void) const 508 | { 509 | std::cout << "SparseMatrix (" 510 | << this->rows << " rows, " << this->cols << " cols, " 511 | << this->num_non_zero() << " values)" << std::endl; 512 | std::cout << " Values:"; 513 | for (std::size_t i = 0; i < this->values.size(); ++i) 514 | std::cout << " " << this->values[i]; 515 | std::cout << std::endl << " Inner:"; 516 | for (std::size_t i = 0; i < this->inner.size(); ++i) 517 | std::cout << " " << this->inner[i]; 518 | std::cout << std::endl << " Outer:"; 519 | for (std::size_t i = 0; i < this->outer.size(); ++i) 520 | std::cout << " " << this->outer[i]; 521 | std::cout << std::endl; 522 | } 523 | 524 | SFM_BA_NAMESPACE_END 525 | SFM_NAMESPACE_END 526 | 527 | #endif // SFM_SPARSE_MATRIX_HEADER 528 | 529 | -------------------------------------------------------------------------------- /sfm/ba_types.h: -------------------------------------------------------------------------------- 1 | #ifndef SFM_BA_TYPES_HEADER 2 | #define SFM_BA_TYPES_HEADER 3 | 4 | #include 5 | 6 | #include "sfm/defines.h" 7 | 8 | SFM_NAMESPACE_BEGIN 9 | SFM_BA_NAMESPACE_BEGIN 10 | 11 | /** Camera representation for bundle adjustment. */ 12 | struct Camera 13 | { 14 | Camera (void); 15 | 16 | double focal_length = 0.0; 17 | double distortion[2]; 18 | double translation[3]; 19 | double rotation[9]; 20 | bool is_constant = false; 21 | }; 22 | 23 | /** 3D point representation for bundle adjustment. */ 24 | struct Point3D 25 | { 26 | double pos[3]; 27 | bool is_constant = false; 28 | }; 29 | 30 | /** Observation of a 3D point for a camera. */ 31 | struct Observation 32 | { 33 | double pos[2]; 34 | int camera_id; 35 | int point_id; 36 | }; 37 | 38 | /* ------------------------ Implementation ------------------------ */ 39 | 40 | inline 41 | Camera::Camera (void) 42 | { 43 | std::fill(this->distortion, this->distortion + 2, 0.0); 44 | std::fill(this->translation, this->translation + 3, 0.0); 45 | std::fill(this->rotation, this->rotation + 9, 0.0); 46 | } 47 | 48 | SFM_BA_NAMESPACE_END 49 | SFM_NAMESPACE_END 50 | 51 | #endif /* SFM_BA_TYPES_HEADER */ 52 | 53 | -------------------------------------------------------------------------------- /sfm/bundle_adjustment.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "math/matrix_tools.h" 15 | #include "util/timer.h" 16 | #include "sfm/ba_sparse_matrix.h" 17 | #include "sfm/ba_dense_vector.h" 18 | #include "sfm/bundle_adjustment.h" 19 | 20 | #define LOG_E this->log.error() 21 | #define LOG_W this->log.warning() 22 | #define LOG_I this->log.info() 23 | #define LOG_V this->log.verbose() 24 | #define LOG_D this->log.debug() 25 | 26 | #define TRUST_REGION_RADIUS_INIT (1000) 27 | #define TRUST_REGION_RADIUS_DECREMENT (1.0 / 2.0) 28 | 29 | SFM_NAMESPACE_BEGIN 30 | SFM_BA_NAMESPACE_BEGIN 31 | 32 | BundleAdjustment::Status 33 | BundleAdjustment::optimize (void) 34 | { 35 | util::WallTimer timer; 36 | this->sanity_checks(); 37 | this->status = Status(); 38 | this->lm_optimize(); 39 | this->status.runtime_ms = timer.get_elapsed(); 40 | return this->status; 41 | } 42 | 43 | void 44 | BundleAdjustment::sanity_checks (void) 45 | { 46 | /* Check for null arguments. */ 47 | if (this->cameras == nullptr) 48 | throw std::invalid_argument("No cameras given"); 49 | if (this->points == nullptr) 50 | throw std::invalid_argument("No tracks given"); 51 | if (this->observations == nullptr) 52 | throw std::invalid_argument("No observations given"); 53 | 54 | /* Check for valid focal lengths. */ 55 | for (std::size_t i = 0; i < this->cameras->size(); ++i) 56 | if (this->cameras->at(i).focal_length <= 0.0) 57 | throw std::invalid_argument("Camera with invalid focal length"); 58 | 59 | /* Check for valid IDs in the observations. */ 60 | for (std::size_t i = 0; i < this->observations->size(); ++i) 61 | { 62 | Observation const& obs = this->observations->at(i); 63 | if (obs.camera_id < 0 64 | || obs.camera_id >= static_cast(this->cameras->size())) 65 | throw std::invalid_argument("Observation with invalid camera ID"); 66 | if (obs.point_id < 0 67 | || obs.point_id >= static_cast(this->points->size())) 68 | throw std::invalid_argument("Observation with invalid track ID"); 69 | } 70 | } 71 | 72 | void 73 | BundleAdjustment::lm_optimize (void) 74 | { 75 | /* Setup linear solver. */ 76 | LinearSolver::Options pcg_opts; 77 | pcg_opts = this->opts.linear_opts; 78 | pcg_opts.trust_region_radius = TRUST_REGION_RADIUS_INIT; 79 | 80 | /* Compute reprojection error for the first time. */ 81 | DenseVectorType F, F_new; 82 | this->compute_reprojection_errors(&F);// todo F 是误差向量 83 | double current_mse = this->compute_mse(F); 84 | this->status.initial_mse = current_mse; 85 | this->status.final_mse = current_mse; 86 | 87 | /* Levenberg-Marquard main loop. */ 88 | for (int lm_iter = 0; ; ++lm_iter) 89 | { 90 | if (lm_iter + 1 > this->opts.lm_min_iterations 91 | && (current_mse < this->opts.lm_mse_threshold)) 92 | { 93 | LOG_V << "BA: Satisfied MSE threshold." << std::endl; 94 | break; 95 | } 96 | 97 | /* Compute Jacobian. */ // todo 计算雅各比矩阵 98 | SparseMatrixType Jc, Jp; 99 | switch (this->opts.bundle_mode) 100 | { 101 | case BA_CAMERAS_AND_POINTS: 102 | this->analytic_jacobian(&Jc, &Jp); 103 | break; 104 | case BA_CAMERAS: 105 | this->analytic_jacobian(&Jc, nullptr); 106 | break; 107 | case BA_POINTS: 108 | this->analytic_jacobian(nullptr, &Jp); 109 | break; 110 | default: 111 | throw std::runtime_error("Invalid bundle mode"); 112 | } 113 | 114 | /* Perform linear step. */ // todo 计算更新量 115 | DenseVectorType delta_x; 116 | LinearSolver pcg(pcg_opts); 117 | LinearSolver::Status cg_status = pcg.solve(Jc, Jp, F, &delta_x); 118 | 119 | /* Update reprojection errors and MSE after linear step. */ 120 | double new_mse, delta_mse, delta_mse_ratio = 1.0; 121 | if (cg_status.success) 122 | { 123 | this->compute_reprojection_errors(&F_new, &delta_x); 124 | new_mse = this->compute_mse(F_new); 125 | delta_mse = current_mse - new_mse; 126 | delta_mse_ratio = 1.0 - new_mse / current_mse; 127 | this->status.num_cg_iterations += cg_status.num_cg_iterations; 128 | } 129 | else 130 | { 131 | new_mse = current_mse; 132 | delta_mse = 0.0; 133 | } 134 | bool successful_iteration = delta_mse > 0.0; 135 | 136 | /* 137 | * Apply delta to parameters after successful step. 138 | * Adjust the trust region to increase/decrease regulariztion. 139 | */ 140 | if (successful_iteration) 141 | { 142 | LOG_V << "BA: #" << std::setw(2) << std::left << lm_iter 143 | << " success" << std::right 144 | << ", MSE " << std::setw(11) << current_mse 145 | << " -> " << std::setw(11) << new_mse 146 | << ", CG " << std::setw(3) << cg_status.num_cg_iterations 147 | << ", TRR " << pcg_opts.trust_region_radius 148 | << std::endl; 149 | 150 | this->status.num_lm_iterations += 1; 151 | this->status.num_lm_successful_iterations += 1; 152 | 153 | this->update_parameters(delta_x);//todo 更新参数 154 | 155 | std::swap(F, F_new); 156 | current_mse = new_mse; 157 | 158 | // todo trust region 是用来做什么的？ 159 | /* Compute trust region update. FIXME delta_norm or mse? */ 160 | double const gain_ratio = delta_mse * (F.size() / 2) 161 | / cg_status.predicted_error_decrease; 162 | double const trust_region_update = 1.0 / std::max(1.0 / 3.0, 163 | (1.0 - MATH_POW3(2.0 * gain_ratio - 1.0))); 164 | pcg_opts.trust_region_radius *= trust_region_update; 165 | } 166 | else 167 | { 168 | LOG_V << "BA: #" << std::setw(2) << std::left << lm_iter 169 | << " failure" << std::right 170 | << ", MSE " << std::setw(11) << current_mse 171 | << ", " << std::setw(11) << " " 172 | << " CG " << std::setw(3) << cg_status.num_cg_iterations 173 | << ", TRR " << pcg_opts.trust_region_radius 174 | << std::endl; 175 | 176 | this->status.num_lm_iterations += 1; 177 | this->status.num_lm_unsuccessful_iterations += 1; 178 | pcg_opts.trust_region_radius *= TRUST_REGION_RADIUS_DECREMENT; 179 | } 180 | 181 | /* Check termination due to LM iterations. */ 182 | if (lm_iter + 1 < this->opts.lm_min_iterations) 183 | continue; 184 | if (lm_iter + 1 >= this->opts.lm_max_iterations) 185 | { 186 | LOG_V << "BA: Reached maximum LM iterations of " 187 | << this->opts.lm_max_iterations << std::endl; 188 | break; 189 | } 190 | 191 | /* Check threshold on the norm of delta_x. */ 192 | if (successful_iteration) 193 | { 194 | if (delta_mse_ratio < this->opts.lm_delta_threshold) 195 | { 196 | LOG_V << "BA: Satisfied delta mse ratio threshold of " 197 | << this->opts.lm_delta_threshold << std::endl; 198 | break; 199 | } 200 | } 201 | } 202 | 203 | this->status.final_mse = current_mse; 204 | } 205 | 206 | void 207 | BundleAdjustment::compute_reprojection_errors (DenseVectorType* vector_f, 208 | DenseVectorType const* delta_x) 209 | { 210 | if (vector_f->size() != this->observations->size() * 2) 211 | vector_f->resize(this->observations->size() * 2); 212 | 213 | #pragma omp parallel for 214 | for (std::size_t i = 0; i < this->observations->size(); ++i) 215 | { 216 | Observation const& obs = this->observations->at(i); 217 | Point3D const& p3d = this->points->at(obs.point_id); 218 | Camera const& cam = this->cameras->at(obs.camera_id); 219 | 220 | double const* flen = &cam.focal_length; 221 | double const* dist = cam.distortion; 222 | double const* rot = cam.rotation; 223 | double const* trans = cam.translation; 224 | double const* point = p3d.pos; 225 | 226 | Point3D new_point; 227 | Camera new_camera; 228 | if (delta_x != nullptr) 229 | { 230 | std::size_t cam_id = obs.camera_id * this->num_cam_params; 231 | std::size_t pt_id = obs.point_id * 3; 232 | 233 | if (this->opts.bundle_mode & BA_CAMERAS) 234 | { 235 | this->update_camera(cam, delta_x->data() + cam_id, &new_camera); 236 | flen = &new_camera.focal_length; 237 | dist = new_camera.distortion; 238 | rot = new_camera.rotation; 239 | trans = new_camera.translation; 240 | pt_id += this->cameras->size() * this->num_cam_params; 241 | } 242 | 243 | if (this->opts.bundle_mode & BA_POINTS) 244 | { 245 | this->update_point(p3d, delta_x->data() + pt_id, &new_point); 246 | point = new_point.pos; 247 | } 248 | } 249 | 250 | /* Project point onto image plane. */ 251 | double rp[] = { 0.0, 0.0, 0.0 }; 252 | for (int d = 0; d < 3; ++d) 253 | { 254 | rp[0] += rot[0 + d] * point[d]; 255 | rp[1] += rot[3 + d] * point[d]; 256 | rp[2] += rot[6 + d] * point[d]; 257 | } 258 | rp[2] = (rp[2] + trans[2]); 259 | rp[0] = (rp[0] + trans[0]) / rp[2]; 260 | rp[1] = (rp[1] + trans[1]) / rp[2]; 261 | 262 | /* Distort reprojections. */ 263 | this->radial_distort(rp + 0, rp + 1, dist); 264 | 265 | /* Compute reprojection error. */ 266 | vector_f->at(i * 2 + 0) = rp[0] * (*flen) - obs.pos[0]; 267 | vector_f->at(i * 2 + 1) = rp[1] * (*flen) - obs.pos[1]; 268 | } 269 | } 270 | 271 | double 272 | BundleAdjustment::compute_mse (DenseVectorType const& vector_f) 273 | { 274 | double mse = 0.0; 275 | for (std::size_t i = 0; i < vector_f.size(); ++i) 276 | mse += vector_f[i] * vector_f[i]; 277 | return mse / static_cast(vector_f.size() / 2); 278 | } 279 | 280 | void 281 | BundleAdjustment::radial_distort (double* x, double* y, double const* dist) 282 | { 283 | double const radius2 = *x * *x + *y * *y; 284 | double const factor = 1.0 + radius2 * (dist[0] + dist[1] * radius2); 285 | *x *= factor; 286 | *y *= factor; 287 | } 288 | 289 | void 290 | BundleAdjustment::rodrigues_to_matrix (double const* r, double* m) 291 | { 292 | /* Obtain angle from vector length. */ 293 | double a = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 294 | /* Precompute sine and cosine terms. */ 295 | double ct = (a == 0.0) ? 0.5f : (1.0f - std::cos(a)) / (2.0 * a); 296 | double st = (a == 0.0) ? 1.0 : std::sin(a) / a; 297 | /* R = I + st * K + ct * K^2 (with cross product matrix K). */ 298 | m[0] = 1.0 - (r[1] * r[1] + r[2] * r[2]) * ct; 299 | m[1] = r[0] * r[1] * ct - r[2] * st; 300 | m[2] = r[2] * r[0] * ct + r[1] * st; 301 | m[3] = r[0] * r[1] * ct + r[2] * st; 302 | m[4] = 1.0f - (r[2] * r[2] + r[0] * r[0]) * ct; 303 | m[5] = r[1] * r[2] * ct - r[0] * st; 304 | m[6] = r[2] * r[0] * ct - r[1] * st; 305 | m[7] = r[1] * r[2] * ct + r[0] * st; 306 | m[8] = 1.0 - (r[0] * r[0] + r[1] * r[1]) * ct; 307 | } 308 | 309 | void 310 | BundleAdjustment::analytic_jacobian (SparseMatrixType* jac_cam, 311 | SparseMatrixType* jac_points) 312 | { 313 | std::size_t const camera_cols = this->cameras->size() * this->num_cam_params; 314 | std::size_t const point_cols = this->points->size() * 3; 315 | std::size_t const jacobi_rows = this->observations->size() * 2; 316 | 317 | SparseMatrixType::Triplets cam_triplets, point_triplets; 318 | if (jac_cam != nullptr) 319 | cam_triplets.resize(this->observations->size() * 2 320 | * this->num_cam_params); 321 | if (jac_points != nullptr) 322 | point_triplets.resize(this->observations->size() * 3 * 2); 323 | 324 | #pragma omp parallel 325 | { 326 | double cam_x_ptr[9], cam_y_ptr[9], point_x_ptr[3], point_y_ptr[3]; 327 | #pragma omp for 328 | for (std::size_t i = 0; i < this->observations->size(); ++i) 329 | { 330 | Observation const& obs = this->observations->at(i); 331 | Point3D const& p3d = this->points->at(obs.point_id); 332 | Camera const& cam = this->cameras->at(obs.camera_id); 333 | this->analytic_jacobian_entries(cam, p3d, 334 | cam_x_ptr, cam_y_ptr, point_x_ptr, point_y_ptr); 335 | 336 | if (p3d.is_constant) 337 | { 338 | std::fill(point_x_ptr, point_x_ptr + 3, 0.0); 339 | std::fill(point_y_ptr, point_y_ptr + 3, 0.0); 340 | } 341 | 342 | std::size_t row_x = i * 2, row_y = row_x + 1; 343 | std::size_t cam_col = obs.camera_id * this->num_cam_params; 344 | std::size_t point_col = obs.point_id * 3; 345 | std::size_t cam_offset = i * this->num_cam_params * 2; 346 | for (int j = 0; jac_cam != nullptr && j < this->num_cam_params; ++j) 347 | { 348 | cam_triplets[cam_offset + j * 2 + 0] = 349 | SparseMatrixType::Triplet(row_x, cam_col + j, cam_x_ptr[j]); 350 | cam_triplets[cam_offset + j * 2 + 1] = 351 | SparseMatrixType::Triplet(row_y, cam_col + j, cam_y_ptr[j]); 352 | } 353 | std::size_t point_offset = i * 3 * 2; 354 | for (int j = 0; jac_points != nullptr && j < 3; ++j) 355 | { 356 | point_triplets[point_offset + j * 2 + 0] = 357 | SparseMatrixType::Triplet(row_x, point_col + j, point_x_ptr[j]); 358 | point_triplets[point_offset + j * 2 + 1] = 359 | SparseMatrixType::Triplet(row_y, point_col + j, point_y_ptr[j]); 360 | } 361 | } 362 | 363 | #pragma omp sections 364 | { 365 | #pragma omp section 366 | if (jac_cam != nullptr) 367 | { 368 | jac_cam->allocate(jacobi_rows, camera_cols); 369 | jac_cam->set_from_triplets(cam_triplets); 370 | } 371 | 372 | #pragma omp section 373 | if (jac_points != nullptr) 374 | { 375 | jac_points->allocate(jacobi_rows, point_cols); 376 | jac_points->set_from_triplets(point_triplets); 377 | } 378 | } 379 | } 380 | } 381 | 382 | void 383 | BundleAdjustment::analytic_jacobian_entries (Camera const& cam, 384 | Point3D const& point, 385 | double* cam_x_ptr, double* cam_y_ptr, 386 | double* point_x_ptr, double* point_y_ptr) 387 | { 388 | /* 389 | * This function computes the Jacobian entries for the given camera and 390 | * 3D point pair that leads to one observation. 391 | * 392 | * The camera block 'cam_x_ptr' and 'cam_y_ptr' is: 393 | * - ID 0: Derivative of focal length f 394 | * - ID 1-2: Derivative of distortion parameters k0, k1 395 | * - ID 3-5: Derivative of translation t0, t1, t2 396 | * - ID 6-8: Derivative of rotation r0, r1, r2 397 | * 398 | * The 3D point block 'point_x_ptr' and 'point_y_ptr' is: 399 | * - ID 0-2: Derivative in x, y, and z direction. 400 | * 401 | * The function that leads to the observation is given as follows: 402 | * 403 | * Px = f * D(ix,iy) * ix (image observation x coordinate) 404 | * Py = f * D(ix,iy) * iy (image observation y coordinate) 405 | * 406 | * with the following definitions: 407 | * 408 | * x = R0 * X + t0 (homogeneous projection) 409 | * y = R1 * X + t1 (homogeneous projection) 410 | * z = R2 * X + t2 (homogeneous projection) 411 | * ix = x / z (central projection) 412 | * iy = y / z (central projection) 413 | * D(ix, iy) = 1 + k0 (ix^2 + iy^2) + k1 (ix^2 + iy^2)^2 (distortion) 414 | * 415 | * The derivatives for intrinsics (f, k0, k1) are easy to compute exactly. 416 | * The derivatives for extrinsics (r, t) and point coordinates Xx, Xy, Xz, 417 | * are a bit of a pain to compute. 418 | */ 419 | 420 | /* Aliases. */ 421 | double const* r = cam.rotation; 422 | double const* t = cam.translation; 423 | double const* k = cam.distortion; 424 | double const* p3d = point.pos; 425 | 426 | /* Temporary values. */ 427 | double const rx = r[0] * p3d[0] + r[1] * p3d[1] + r[2] * p3d[2]; 428 | double const ry = r[3] * p3d[0] + r[4] * p3d[1] + r[5] * p3d[2]; 429 | double const rz = r[6] * p3d[0] + r[7] * p3d[1] + r[8] * p3d[2]; 430 | double const px = rx + t[0]; 431 | double const py = ry + t[1]; 432 | double const pz = rz + t[2]; 433 | double const ix = px / pz; 434 | double const iy = py / pz; 435 | double const fz = cam.focal_length / pz; 436 | double const radius2 = ix * ix + iy * iy; 437 | double const rd_factor = 1.0 + (k[0] + k[1] * radius2) * radius2; 438 | 439 | /* Compute exact camera and point entries if intrinsics are fixed */ 440 | if (this->opts.fixed_intrinsics) 441 | { 442 | cam_x_ptr[0] = fz * rd_factor; 443 | cam_x_ptr[1] = 0.0; 444 | cam_x_ptr[2] = -fz * rd_factor * ix; 445 | cam_x_ptr[3] = -fz * rd_factor * ry * ix; 446 | cam_x_ptr[4] = fz * rd_factor * (rz + rx * ix); 447 | cam_x_ptr[5] = -fz * rd_factor * ry; 448 | 449 | cam_y_ptr[0] = 0.0; 450 | cam_y_ptr[1] = fz * rd_factor; 451 | cam_y_ptr[2] = -fz * rd_factor * iy; 452 | cam_y_ptr[3] = -fz * rd_factor * (rz + ry * iy); 453 | cam_y_ptr[4] = fz * rd_factor * rx * iy; 454 | cam_y_ptr[5] = fz * rd_factor * rx; 455 | 456 | /* 457 | * Compute point derivatives in x, y, and z. 458 | */ 459 | point_x_ptr[0] = fz * rd_factor * (r[0] - r[6] * ix); 460 | point_x_ptr[1] = fz * rd_factor * (r[1] - r[7] * ix); 461 | point_x_ptr[2] = fz * rd_factor * (r[2] - r[8] * ix); 462 | 463 | point_y_ptr[0] = fz * rd_factor * (r[3] - r[6] * iy); 464 | point_y_ptr[1] = fz * rd_factor * (r[4] - r[7] * iy); 465 | point_y_ptr[2] = fz * rd_factor * (r[5] - r[8] * iy); 466 | return; 467 | } 468 | 469 | /* The intrinsics are easy to compute exactly. */ 470 | cam_x_ptr[0] = ix * rd_factor; 471 | cam_x_ptr[1] = cam.focal_length * ix * radius2; 472 | cam_x_ptr[2] = cam.focal_length * ix * radius2 * radius2; 473 | 474 | cam_y_ptr[0] = iy * rd_factor; 475 | cam_y_ptr[1] = cam.focal_length * iy * radius2; 476 | cam_y_ptr[2] = cam.focal_length * iy * radius2 * radius2; 477 | 478 | #define JACOBIAN_APPROX_CONST_RD 0 479 | #define JACOBIAN_APPROX_PBA 0 480 | #if JACOBIAN_APPROX_CONST_RD 481 | /* 482 | * Compute approximations of the Jacobian entries for the extrinsics 483 | * by assuming the distortion coefficent D(ix, iy) is constant. 484 | */ 485 | cam_x_ptr[3] = fz * rd_factor; 486 | cam_x_ptr[4] = 0.0; 487 | cam_x_ptr[5] = -fz * rd_factor * ix; 488 | cam_x_ptr[6] = -fz * rd_factor * ry * ix; 489 | cam_x_ptr[7] = fz * rd_factor * (rz + rx * ix); 490 | cam_x_ptr[8] = -fz * rd_factor * ry; 491 | 492 | cam_y_ptr[3] = 0.0; 493 | cam_y_ptr[4] = fz * rd_factor; 494 | cam_y_ptr[5] = -fz * rd_factor * iy; 495 | cam_y_ptr[6] = -fz * rd_factor * (rz + ry * iy); 496 | cam_y_ptr[7] = fz * rd_factor * rx * iy; 497 | cam_y_ptr[8] = fz * rd_factor * rx; 498 | 499 | /* 500 | * Compute point derivatives in x, y, and z. 501 | */ 502 | point_x_ptr[0] = fz * rd_factor * (r[0] - r[6] * ix); 503 | point_x_ptr[1] = fz * rd_factor * (r[1] - r[7] * ix); 504 | point_x_ptr[2] = fz * rd_factor * (r[2] - r[8] * ix); 505 | 506 | point_y_ptr[0] = fz * rd_factor * (r[3] - r[6] * iy); 507 | point_y_ptr[1] = fz * rd_factor * (r[4] - r[7] * iy); 508 | point_y_ptr[2] = fz * rd_factor * (r[5] - r[8] * iy); 509 | #elif JACOBIAN_APPROX_PBA 510 | /* Computation of Jacobian approximation with one distortion argument. */ 511 | 512 | double rd_derivative_x; 513 | double rd_derivative_y; 514 | 515 | rd_derivative_x = 2.0 * ix * ix * (k[0] + 2.0 * k[1] * radius2); 516 | rd_derivative_y = 2.0 * iy * iy * (k[0] + 2.0 * k[1] * radius2); 517 | rd_derivative_x += rd_factor; 518 | rd_derivative_y += rd_factor; 519 | 520 | cam_x_ptr[3] = fz * rd_derivative_x; 521 | cam_x_ptr[4] = 0.0; 522 | cam_x_ptr[5] = -fz * rd_derivative_x * ix; 523 | cam_x_ptr[6] = -fz * rd_derivative_x * ry * ix; 524 | cam_x_ptr[7] = fz * rd_derivative_x * (rz + rx * ix); 525 | cam_x_ptr[8] = -fz * rd_derivative_x * ry; 526 | 527 | cam_y_ptr[3] = 0.0; 528 | cam_y_ptr[4] = fz * rd_derivative_y; 529 | cam_y_ptr[5] = -fz * rd_derivative_y * iy; 530 | cam_y_ptr[6] = -fz * rd_derivative_y * (rz + ry * iy); 531 | cam_y_ptr[7] = fz * rd_derivative_y * rx * iy; 532 | cam_y_ptr[8] = fz * rd_derivative_y * rx; 533 | 534 | /* 535 | * Compute point derivatives in x, y, and z. 536 | */ 537 | point_x_ptr[0] = fz * rd_derivative_x * (r[0] - r[6] * ix); 538 | point_x_ptr[1] = fz * rd_derivative_x * (r[1] - r[7] * ix); 539 | point_x_ptr[2] = fz * rd_derivative_x * (r[2] - r[8] * ix); 540 | 541 | point_y_ptr[0] = fz * rd_derivative_y * (r[3] - r[6] * iy); 542 | point_y_ptr[1] = fz * rd_derivative_y * (r[4] - r[7] * iy); 543 | point_y_ptr[2] = fz * rd_derivative_y * (r[5] - r[8] * iy); 544 | 545 | #else 546 | /* Computation of the full Jacobian. */ 547 | 548 | /* 549 | * To keep everything comprehensible the chain rule 550 | * is applied excessively 551 | */ 552 | double const f = cam.focal_length; 553 | 554 | double const rd_deriv_rad = k[0] + 2.0 * k[1] * radius2; 555 | 556 | double const rad_deriv_px = 2.0 * ix / pz; 557 | double const rad_deriv_py = 2.0 * iy / pz; 558 | double const rad_deriv_pz = -2.0 * radius2 / pz; 559 | 560 | double const rd_deriv_px = rd_deriv_rad * rad_deriv_px; 561 | double const rd_deriv_py = rd_deriv_rad * rad_deriv_py; 562 | double const rd_deriv_pz = rd_deriv_rad * rad_deriv_pz; 563 | 564 | double const ix_deriv_px = 1 / pz; 565 | double const ix_deriv_pz = -ix / pz; 566 | 567 | double const iy_deriv_py = 1 / pz; 568 | double const iy_deriv_pz = -iy / pz; 569 | 570 | double const ix_deriv_r0 = -ix * ry / pz; 571 | double const ix_deriv_r1 = (rz + rx * ix) / pz; 572 | double const ix_deriv_r2 = -ry / pz; 573 | 574 | double const iy_deriv_r0 = -(rz + ry * iy) / pz; 575 | double const iy_deriv_r1 = rx * iy / pz; 576 | double const iy_deriv_r2 = rx / pz; 577 | 578 | double const rad_deriv_r0 = 2.0 * ix * ix_deriv_r0 + 2.0 * iy * iy_deriv_r0; 579 | double const rad_deriv_r1 = 2.0 * ix * ix_deriv_r1 + 2.0 * iy * iy_deriv_r1; 580 | double const rad_deriv_r2 = 2.0 * ix * ix_deriv_r2 + 2.0 * iy * iy_deriv_r2; 581 | 582 | double const rd_deriv_r0 = rd_deriv_rad * rad_deriv_r0; 583 | double const rd_deriv_r1 = rd_deriv_rad * rad_deriv_r1; 584 | double const rd_deriv_r2 = rd_deriv_rad * rad_deriv_r2; 585 | 586 | double const ix_deriv_X0 = (r[0] - r[6] * ix) / pz; 587 | double const ix_deriv_X1 = (r[1] - r[7] * ix) / pz; 588 | double const ix_deriv_X2 = (r[2] - r[8] * ix) / pz; 589 | 590 | double const iy_deriv_X0 = (r[3] - r[6] * iy) / pz; 591 | double const iy_deriv_X1 = (r[4] - r[7] * iy) / pz; 592 | double const iy_deriv_X2 = (r[5] - r[8] * iy) / pz; 593 | 594 | double const rad_deriv_X0 = 2.0 * ix * ix_deriv_X0 + 2.0 * iy * iy_deriv_X0; 595 | double const rad_deriv_X1 = 2.0 * ix * ix_deriv_X1 + 2.0 * iy * iy_deriv_X1; 596 | double const rad_deriv_X2 = 2.0 * ix * ix_deriv_X2 + 2.0 * iy * iy_deriv_X2; 597 | 598 | double const rd_deriv_X0 = rd_deriv_rad * rad_deriv_X0; 599 | double const rd_deriv_X1 = rd_deriv_rad * rad_deriv_X1; 600 | double const rd_deriv_X2 = rd_deriv_rad * rad_deriv_X2; 601 | 602 | /* 603 | * Compute translation derivatives 604 | * NOTE: px_deriv_t0 = 1 605 | */ 606 | cam_x_ptr[3] = f * (rd_deriv_px * ix + rd_factor * ix_deriv_px); 607 | cam_x_ptr[4] = f * (rd_deriv_py * ix); // + rd_factor * ix_deriv_py = 0 608 | cam_x_ptr[5] = f * (rd_deriv_pz * ix + rd_factor * ix_deriv_pz); 609 | 610 | cam_y_ptr[3] = f * (rd_deriv_px * iy); // + rd_factor * iy_deriv_px = 0 611 | cam_y_ptr[4] = f * (rd_deriv_py * iy + rd_factor * iy_deriv_py); 612 | cam_y_ptr[5] = f * (rd_deriv_pz * iy + rd_factor * iy_deriv_pz); 613 | 614 | /* 615 | * Compute rotation derivatives 616 | */ 617 | cam_x_ptr[6] = f * (rd_deriv_r0 * ix + rd_factor * ix_deriv_r0); 618 | cam_x_ptr[7] = f * (rd_deriv_r1 * ix + rd_factor * ix_deriv_r1); 619 | cam_x_ptr[8] = f * (rd_deriv_r2 * ix + rd_factor * ix_deriv_r2); 620 | 621 | cam_y_ptr[6] = f * (rd_deriv_r0 * iy + rd_factor * iy_deriv_r0); 622 | cam_y_ptr[7] = f * (rd_deriv_r1 * iy + rd_factor * iy_deriv_r1); 623 | cam_y_ptr[8] = f * (rd_deriv_r2 * iy + rd_factor * iy_deriv_r2); 624 | 625 | /* 626 | * Compute point derivatives in x, y, and z. 627 | */ 628 | point_x_ptr[0] = f * (rd_deriv_X0 * ix + rd_factor * ix_deriv_X0); 629 | point_x_ptr[1] = f * (rd_deriv_X1 * ix + rd_factor * ix_deriv_X1); 630 | point_x_ptr[2] = f * (rd_deriv_X2 * ix + rd_factor * ix_deriv_X2); 631 | 632 | point_y_ptr[0] = f * (rd_deriv_X0 * iy + rd_factor * iy_deriv_X0); 633 | point_y_ptr[1] = f * (rd_deriv_X1 * iy + rd_factor * iy_deriv_X1); 634 | point_y_ptr[2] = f * (rd_deriv_X2 * iy + rd_factor * iy_deriv_X2); 635 | 636 | #endif 637 | } 638 | 639 | void 640 | BundleAdjustment::update_parameters (DenseVectorType const& delta_x) 641 | { 642 | /* Update cameras. */ 643 | std::size_t total_camera_params = 0; 644 | if (this->opts.bundle_mode & BA_CAMERAS) 645 | { 646 | for (std::size_t i = 0; i < this->cameras->size(); ++i) 647 | this->update_camera(this->cameras->at(i), 648 | delta_x.data() + this->num_cam_params * i, 649 | &this->cameras->at(i)); 650 | total_camera_params = this->cameras->size() * this->num_cam_params; 651 | } 652 | 653 | /* Update points. */ 654 | if (this->opts.bundle_mode & BA_POINTS) 655 | { 656 | for (std::size_t i = 0; i < this->points->size(); ++i) 657 | this->update_point(this->points->at(i), 658 | delta_x.data() + total_camera_params + i * 3, 659 | &this->points->at(i)); 660 | } 661 | } 662 | 663 | void 664 | BundleAdjustment::update_camera (Camera const& cam, 665 | double const* update, Camera* out) 666 | { 667 | if (opts.fixed_intrinsics) 668 | { 669 | out->focal_length = cam.focal_length; 670 | out->distortion[0] = cam.distortion[0]; 671 | out->distortion[1] = cam.distortion[1]; 672 | } 673 | else 674 | { 675 | out->focal_length = cam.focal_length + update[0]; 676 | out->distortion[0] = cam.distortion[0] + update[1]; 677 | out->distortion[1] = cam.distortion[1] + update[2]; 678 | } 679 | 680 | int const offset = this->opts.fixed_intrinsics ? 0 : 3; 681 | out->translation[0] = cam.translation[0] + update[0 + offset]; 682 | out->translation[1] = cam.translation[1] + update[1 + offset]; 683 | out->translation[2] = cam.translation[2] + update[2 + offset]; 684 | 685 | double rot_orig[9]; 686 | std::copy(cam.rotation, cam.rotation + 9, rot_orig); 687 | double rot_update[9]; 688 | this->rodrigues_to_matrix(update + 3 + offset, rot_update); 689 | math::matrix_multiply(rot_update, 3, 3, rot_orig, 3, out->rotation); 690 | } 691 | 692 | void 693 | BundleAdjustment::update_point (Point3D const& pt, 694 | double const* update, Point3D* out) 695 | { 696 | out->pos[0] = pt.pos[0] + update[0]; 697 | out->pos[1] = pt.pos[1] + update[1]; 698 | out->pos[2] = pt.pos[2] + update[2]; 699 | } 700 | 701 | void 702 | BundleAdjustment::print_status (bool detailed) const 703 | { 704 | if (!detailed) 705 | { 706 | std::cout << "BA: MSE " << this->status.initial_mse 707 | << " -> " << this->status.final_mse << ", " 708 | << this->status.num_lm_iterations << " LM iters, " 709 | << this->status.num_cg_iterations << " CG iters, " 710 | << this->status.runtime_ms << "ms." 711 | << std::endl; 712 | return; 713 | } 714 | 715 | std::cout << "Bundle Adjustment Status:" << std::endl; 716 | std::cout << " Initial MSE: " 717 | << this->status.initial_mse << std::endl; 718 | std::cout << " Final MSE: " 719 | << this->status.final_mse << std::endl; 720 | std::cout << " LM iterations: " 721 | << this->status.num_lm_iterations << " (" 722 | << this->status.num_lm_successful_iterations << " successful, " 723 | << this->status.num_lm_unsuccessful_iterations << " unsuccessful)" 724 | << std::endl; 725 | std::cout << " CG iterations: " 726 | << this->status.num_cg_iterations << std::endl; 727 | } 728 | 729 | SFM_BA_NAMESPACE_END 730 | SFM_NAMESPACE_END 731 | -------------------------------------------------------------------------------- /sfm/bundle_adjustment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_BUNDLE_ADJUSTMENT_HEADER 11 | #define SFM_BUNDLE_ADJUSTMENT_HEADER 12 | 13 | #include 14 | 15 | #include "util/logging.h" 16 | #include "sfm/defines.h" 17 | #include "sfm/ba_sparse_matrix.h" 18 | #include "sfm/ba_dense_vector.h" 19 | #include "sfm/ba_linear_solver.h" 20 | #include "sfm/ba_types.h" 21 | 22 | /* 23 | * A few notes. 24 | * 25 | * - PBA normalizes focal length and depth values before LM optimization, 26 | * and denormalizes afterwards. Is this necessary with double? 27 | * - PBA exits the LM main loop if norm of -JF is small. Useful? 28 | * - The slowest part is computing the Schur complement because of matrix 29 | * multiplications. How can this be improved? 30 | * 31 | * Actual TODOs. 32 | * 33 | * - Better preconditioner for conjugate gradient, i.e., use the 9x9 diagonal 34 | * blocks of S instead of B. Requires method in matrix. 35 | * - Properly implement and test BA_POINTS mode. 36 | * - More accurate implementations for the Jacobian (currently approximated). 37 | * - Implement block_size = 9 in linear solver, no need for CG 38 | */ 39 | 40 | SFM_NAMESPACE_BEGIN 41 | SFM_BA_NAMESPACE_BEGIN 42 | 43 | /** 44 | * A simple bundle adjustment optimization implementation. 45 | * The algorithm requires good initial camera parameters and 3D points, as 46 | * well as observations of the 3D points in the cameras. The algorithm 47 | * then optimizes the 3D point positions and camera parameters in order to 48 | * minimize the reprojection errors, i.e., the distances from the point 49 | * projections to the observations. 50 | */ 51 | class BundleAdjustment 52 | { 53 | public: 54 | enum BAMode 55 | { 56 | BA_CAMERAS = 1, 57 | BA_POINTS = 2, 58 | BA_CAMERAS_AND_POINTS = 1 | 2 59 | }; 60 | 61 | struct Options 62 | { 63 | Options (void); 64 | 65 | bool verbose_output; 66 | BAMode bundle_mode; 67 | bool fixed_intrinsics; // 固定内参数，不进行优化 68 | //bool shared_intrinsics; 69 | int lm_max_iterations; 70 | int lm_min_iterations; 71 | 72 | double lm_delta_threshold; 73 | double lm_mse_threshold; 74 | LinearSolver::Options linear_opts; 75 | }; 76 | 77 | struct Status 78 | { 79 | Status (void); 80 | 81 | double initial_mse;// chushi 82 | double final_mse; 83 | int num_lm_iterations; 84 | int num_lm_successful_iterations; 85 | int num_lm_unsuccessful_iterations; 86 | int num_cg_iterations; 87 | std::size_t runtime_ms; 88 | }; 89 | 90 | public: 91 | BundleAdjustment (Options const& options); 92 | 93 | void set_cameras (std::vector* cameras); 94 | void set_points (std::vector* points); 95 | void set_observations (std::vector* observations); 96 | 97 | Status optimize (void); 98 | void print_status (bool detailed = false) const; 99 | 100 | private: 101 | typedef SparseMatrix SparseMatrixType; 102 | typedef DenseVector DenseVectorType; 103 | 104 | private: 105 | void sanity_checks (void); 106 | void lm_optimize (void); 107 | 108 | /* Helper functions. */ 109 | void compute_reprojection_errors (DenseVectorType* vector_f, 110 | DenseVectorType const* delta_x = nullptr); 111 | double compute_mse (DenseVectorType const& vector_f); 112 | void radial_distort (double* x, double* y, double const* dist); 113 | void rodrigues_to_matrix (double const* r, double* rot); 114 | 115 | /* Analytic Jacobian. */ 116 | void analytic_jacobian (SparseMatrixType* jac_cam, 117 | SparseMatrixType* jac_points); 118 | void analytic_jacobian_entries (Camera const& cam, Point3D const& point, 119 | double* cam_x_ptr, double* cam_y_ptr, 120 | double* point_x_ptr, double* point_y_ptr); 121 | 122 | /* Update of camera/point parameters. */ 123 | void update_parameters (DenseVectorType const& delta_x); 124 | void update_camera (Camera const& cam, double const* update, Camera* out); 125 | void update_point (Point3D const& pt, double const* update, Point3D* out); 126 | 127 | private: 128 | Options opts; 129 | Status status; 130 | util::Logging log; 131 | std::vector* cameras; 132 | std::vector* points; 133 | std::vector* observations; 134 | int const num_cam_params; 135 | }; 136 | 137 | /* ------------------------ Implementation ------------------------ */ 138 | 139 | inline 140 | BundleAdjustment::Options::Options (void) 141 | : verbose_output(false) 142 | , bundle_mode(BA_CAMERAS_AND_POINTS) 143 | , lm_max_iterations(50) 144 | , lm_min_iterations(0) 145 | , lm_delta_threshold(1e-4) 146 | , lm_mse_threshold(1e-8) 147 | { 148 | } 149 | 150 | inline 151 | BundleAdjustment::Status::Status (void) 152 | : initial_mse(0.0) 153 | , final_mse(0.0) 154 | , num_lm_iterations(0) 155 | , num_lm_successful_iterations(0) 156 | , num_lm_unsuccessful_iterations(0) 157 | , num_cg_iterations(0) 158 | { 159 | } 160 | 161 | inline 162 | BundleAdjustment::BundleAdjustment (Options const& options) 163 | : opts(options) 164 | , log(options.verbose_output 165 | ? util::Logging::LOG_DEBUG 166 | : util::Logging::LOG_INFO) 167 | , cameras(nullptr) 168 | , points(nullptr) 169 | , observations(nullptr) 170 | , num_cam_params(options.fixed_intrinsics ? 6 : 9) 171 | { 172 | this->opts.linear_opts.camera_block_dim = this->num_cam_params; 173 | } 174 | 175 | inline void 176 | BundleAdjustment::set_cameras (std::vector* cameras) 177 | { 178 | this->cameras = cameras; 179 | } 180 | 181 | inline void 182 | BundleAdjustment::set_points (std::vector* points) 183 | { 184 | this->points = points; 185 | } 186 | 187 | inline void 188 | BundleAdjustment::set_observations (std::vector* points_2d) 189 | { 190 | this->observations = points_2d; 191 | } 192 | 193 | SFM_BA_NAMESPACE_END 194 | SFM_NAMESPACE_END 195 | 196 | #endif /* SFM_BUNDLE_ADJUSTMENT_HEADER */ 197 | -------------------------------------------------------------------------------- /sfm/bundler_common.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "util/exception.h" 20 | #include "sfm/bundler_common.h" 21 | 22 | #define PREBUNDLE_SIGNATURE "MVE_PREBUNDLE\n" 23 | #define PREBUNDLE_SIGNATURE_LEN 14 24 | 25 | #define SURVEY_SIGNATURE "MVE_SURVEY\n" 26 | #define SURVEY_SIGNATURE_LEN 11 27 | 28 | SFM_NAMESPACE_BEGIN 29 | SFM_BUNDLER_NAMESPACE_BEGIN 30 | 31 | 32 | /* --------------- Data Structure for Feature Tracks -------------- */ 33 | 34 | void 35 | Track::invalidate (void) 36 | { 37 | std::fill(this->pos.begin(), this->pos.end(), 38 | std::numeric_limits::quiet_NaN()); 39 | } 40 | 41 | void 42 | Track::remove_view (int view_id) 43 | { 44 | for (FeatureReferenceList::iterator iter = this->features.begin(); 45 | iter != this->features.end();) 46 | { 47 | if (iter->view_id == view_id) 48 | iter = this->features.erase(iter); 49 | else 50 | iter++; 51 | } 52 | } 53 | 54 | /* ------------------ Input/Output for Prebundle ------------------ */ 55 | 56 | void 57 | save_prebundle_data (ViewportList const& viewports, 58 | PairwiseMatching const& matching, std::ostream& out) 59 | { 60 | /* Write signature. */ 61 | out.write(PREBUNDLE_SIGNATURE, PREBUNDLE_SIGNATURE_LEN); 62 | 63 | /* Write number of viewports. */ 64 | int32_t num_viewports = static_cast(viewports.size()); 65 | out.write(reinterpret_cast(&num_viewports), sizeof(int32_t)); 66 | 67 | /* Write per-viewport data. */ 68 | for (std::size_t i = 0; i < viewports.size(); ++i) 69 | { 70 | FeatureSet const& vpf = viewports[i].features; 71 | 72 | /* Write positions. */ 73 | int32_t num_positions = static_cast(vpf.positions.size()); 74 | out.write(reinterpret_cast(&num_positions), sizeof(int32_t)); 75 | for (std::size_t j = 0; j < vpf.positions.size(); ++j) 76 | out.write(reinterpret_cast(&vpf.positions[j]), sizeof(math::Vec2f)); 77 | 78 | /* Write colors. */ 79 | int32_t num_colors = static_cast(vpf.colors.size()); 80 | out.write(reinterpret_cast(&num_colors), sizeof(int32_t)); 81 | for (std::size_t j = 0; j < vpf.colors.size(); ++j) 82 | out.write(reinterpret_cast(&vpf.colors[j]), sizeof(math::Vec3uc)); 83 | } 84 | 85 | /* Write number of matching pairs. */ 86 | int32_t num_pairs = static_cast(matching.size()); 87 | out.write(reinterpret_cast(&num_pairs), sizeof(int32_t)); 88 | 89 | /* Write per-matching pair data. */ 90 | for (std::size_t i = 0; i < matching.size(); ++i) 91 | { 92 | TwoViewMatching const& tvr = matching[i]; 93 | int32_t id1 = static_cast(tvr.view_1_id); 94 | int32_t id2 = static_cast(tvr.view_2_id); 95 | int32_t num_matches = static_cast(tvr.matches.size()); 96 | out.write(reinterpret_cast(&id1), sizeof(int32_t)); 97 | out.write(reinterpret_cast(&id2), sizeof(int32_t)); 98 | out.write(reinterpret_cast(&num_matches), sizeof(int32_t)); 99 | for (std::size_t j = 0; j < tvr.matches.size(); ++j) 100 | { 101 | CorrespondenceIndex const& c = tvr.matches[j]; 102 | int32_t i1 = static_cast(c.first); 103 | int32_t i2 = static_cast(c.second); 104 | out.write(reinterpret_cast(&i1), sizeof(int32_t)); 105 | out.write(reinterpret_cast(&i2), sizeof(int32_t)); 106 | } 107 | } 108 | } 109 | 110 | void 111 | load_prebundle_data (std::istream& in, ViewportList* viewports, 112 | PairwiseMatching* matching) 113 | { 114 | /* Read and check file signature. */ 115 | char signature[PREBUNDLE_SIGNATURE_LEN + 1]; 116 | in.read(signature, PREBUNDLE_SIGNATURE_LEN); 117 | signature[PREBUNDLE_SIGNATURE_LEN] = '\0'; 118 | if (std::string(PREBUNDLE_SIGNATURE) != signature) 119 | throw std::invalid_argument("Invalid prebundle file signature"); 120 | 121 | viewports->clear(); 122 | matching->clear(); 123 | 124 | /* Read number of viewports. */ 125 | int32_t num_viewports; 126 | in.read(reinterpret_cast(&num_viewports), sizeof(int32_t)); 127 | viewports->resize(num_viewports); 128 | 129 | /* Read per-viewport data. */ 130 | for (int i = 0; i < num_viewports; ++i) 131 | { 132 | FeatureSet& vpf = viewports->at(i).features; 133 | 134 | /* Read positions. */ 135 | int32_t num_positions; 136 | in.read(reinterpret_cast(&num_positions), sizeof(int32_t)); 137 | vpf.positions.resize(num_positions); 138 | for (int j = 0; j < num_positions; ++j) 139 | in.read(reinterpret_cast(&vpf.positions[j]), sizeof(math::Vec2f)); 140 | 141 | /* Read colors. */ 142 | int32_t num_colors; 143 | in.read(reinterpret_cast(&num_colors), sizeof(int32_t)); 144 | vpf.colors.resize(num_colors); 145 | for (int j = 0; j < num_colors; ++j) 146 | in.read(reinterpret_cast(&vpf.colors[j]), sizeof(math::Vec3uc)); 147 | } 148 | 149 | /* Read number of matching pairs. */ 150 | int32_t num_pairs; 151 | in.read(reinterpret_cast(&num_pairs), sizeof(int32_t)); 152 | 153 | /* Read per-matching pair data. */ 154 | for (int32_t i = 0; i < num_pairs; ++i) 155 | { 156 | int32_t id1, id2, num_matches; 157 | in.read(reinterpret_cast(&id1), sizeof(int32_t)); 158 | in.read(reinterpret_cast(&id2), sizeof(int32_t)); 159 | in.read(reinterpret_cast(&num_matches), sizeof(int32_t)); 160 | 161 | TwoViewMatching tvr; 162 | tvr.view_1_id = static_cast(id1); 163 | tvr.view_2_id = static_cast(id2); 164 | tvr.matches.reserve(num_matches); 165 | for (int32_t j = 0; j < num_matches; ++j) 166 | { 167 | int32_t i1, i2; 168 | in.read(reinterpret_cast(&i1), sizeof(int32_t)); 169 | in.read(reinterpret_cast(&i2), sizeof(int32_t)); 170 | CorrespondenceIndex c; 171 | c.first = static_cast(i1); 172 | c.second = static_cast(i2); 173 | tvr.matches.push_back(c); 174 | } 175 | matching->push_back(tvr); 176 | } 177 | } 178 | 179 | void 180 | save_prebundle_to_file (ViewportList const& viewports, 181 | PairwiseMatching const& matching, std::string const& filename) 182 | { 183 | std::ofstream out(filename.c_str(), std::ios::binary); 184 | if (!out.good()) 185 | throw util::FileException(filename, std::strerror(errno)); 186 | save_prebundle_data(viewports, matching, out); 187 | out.close(); 188 | } 189 | 190 | void 191 | load_prebundle_from_file (std::string const& filename, 192 | ViewportList* viewports, PairwiseMatching* matching) 193 | { 194 | std::ifstream in(filename.c_str(), std::ios::binary); 195 | if (!in.good()) 196 | throw util::FileException(filename, std::strerror(errno)); 197 | 198 | try 199 | { 200 | load_prebundle_data(in, viewports, matching); 201 | } 202 | catch (...) 203 | { 204 | in.close(); 205 | throw; 206 | } 207 | 208 | if (in.eof()) 209 | { 210 | in.close(); 211 | throw util::Exception("Premature EOF"); 212 | } 213 | in.close(); 214 | } 215 | 216 | void 217 | load_survey_from_file (std::string const& filename, 218 | SurveyPointList* survey_points) 219 | { 220 | std::ifstream in(filename.c_str()); 221 | if (!in.good()) 222 | throw util::FileException(filename, std::strerror(errno)); 223 | 224 | char signature[SURVEY_SIGNATURE_LEN + 1]; 225 | in.read(signature, SURVEY_SIGNATURE_LEN); 226 | signature[SURVEY_SIGNATURE_LEN] = '\0'; 227 | if (std::string(SURVEY_SIGNATURE) != signature) 228 | throw std::invalid_argument("Invalid survey file signature"); 229 | 230 | std::size_t num_points = 0; 231 | std::size_t num_observations = 0; 232 | in >> num_points >> num_observations; 233 | if (in.fail()) 234 | { 235 | in.close(); 236 | throw util::Exception("Invalid survey file header"); 237 | } 238 | 239 | survey_points->resize(num_points); 240 | for (std::size_t i = 0; i < num_points; ++i) 241 | { 242 | SurveyPoint& survey_point = survey_points->at(i); 243 | for (int j = 0; j < 3; ++j) 244 | in >> survey_point.pos[j]; 245 | } 246 | 247 | for (std::size_t i = 0; i < num_observations; ++i) 248 | { 249 | int point_id, view_id; 250 | float x, y; 251 | in >> point_id >> view_id >> x >> y; 252 | 253 | if (static_cast(point_id) > num_points) 254 | throw util::Exception("Invalid survey point id"); 255 | 256 | SurveyPoint& survey_point = survey_points->at(point_id); 257 | survey_point.observations.emplace_back(view_id, x, y); 258 | } 259 | 260 | if (in.fail()) 261 | { 262 | in.close(); 263 | throw util::Exception("Parsing error"); 264 | } 265 | 266 | in.close(); 267 | } 268 | 269 | SFM_BUNDLER_NAMESPACE_END 270 | SFM_NAMESPACE_END 271 | -------------------------------------------------------------------------------- /sfm/bundler_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_BUNDLER_COMMON_HEADER 11 | #define SFM_BUNDLER_COMMON_HEADER 12 | 13 | #include 14 | #include 15 | 16 | #include "math/vector.h" 17 | #include "util/aligned_memory.h" 18 | #include "core/image.h" 19 | #include "sfm/camera_pose.h" 20 | #include "sfm/correspondence.h" 21 | #include "sfm/feature_set.h" 22 | #include "features/sift.h" 23 | #include "features/surf.h" 24 | #include "sfm/defines.h" 25 | 26 | SFM_NAMESPACE_BEGIN 27 | SFM_BUNDLER_NAMESPACE_BEGIN 28 | 29 | /* -------------------- Common Data Structures -------------------- */ 30 | 31 | /** 32 | * Per-viewport information. 33 | * Not all data is required for every step. It should be populated on demand 34 | * and cleaned as early as possible to keep memory consumption in bounds. 35 | */ 36 | struct Viewport 37 | { 38 | Viewport (void); 39 | 40 | /** Initial focal length estimate for the image. */ 41 | float focal_length; 42 | 43 | /** Radial distortion parameter. */ 44 | float radial_distortion[2]; 45 | 46 | /** Camera pose for the viewport. */ 47 | CameraPose pose; 48 | 49 | /** The actual image data for debugging purposes. Usually nullptr! */ 50 | core::ByteImage::Ptr image; 51 | 52 | /** Per-feature information. */ 53 | FeatureSet features; 54 | 55 | /** Per-feature track ID, -1 if not part of a track. */ 56 | std::vector track_ids; 57 | }; 58 | 59 | /** The list of all viewports considered for bundling. */ 60 | typedef std::vector ViewportList; 61 | 62 | /* --------------- Data Structure for Feature Tracks -------------- */ 63 | 64 | /** References a 2D feature in a specific view. */ 65 | struct FeatureReference 66 | { 67 | FeatureReference (int view_id, int feature_id); 68 | 69 | int view_id; 70 | int feature_id; 71 | }; 72 | 73 | /** The list of all feature references inside a track. */ 74 | typedef std::vector FeatureReferenceList; 75 | 76 | /** Representation of a feature track. */ 77 | struct Track 78 | { 79 | bool is_valid (void) const; 80 | void invalidate (void); 81 | void remove_view (int view_id); 82 | 83 | math::Vec3f pos; 84 | math::Vec3uc color; 85 | FeatureReferenceList features; 86 | }; 87 | 88 | /** The list of all tracks. */ 89 | typedef std::vector TrackList; 90 | 91 | /* Observation of a survey point in a specific view. */ 92 | struct SurveyObservation 93 | { 94 | SurveyObservation (int view_id, float x, float y); 95 | 96 | int view_id; 97 | math::Vec2f pos; 98 | }; 99 | 100 | /** The list of all survey point observations inside a survey point. */ 101 | typedef std::vector SurveyObservationList; 102 | 103 | /** Representation of a survey point. */ 104 | struct SurveyPoint 105 | { 106 | math::Vec3f pos; 107 | SurveyObservationList observations; 108 | }; 109 | 110 | /** The list of all survey poins. */ 111 | typedef std::vector SurveyPointList; 112 | 113 | /* ------------- Data Structures for Feature Matching ------------- */ 114 | 115 | /** The matching result between two views. */ 116 | struct TwoViewMatching 117 | { 118 | bool operator< (TwoViewMatching const& rhs) const; 119 | 120 | int view_1_id; 121 | int view_2_id; 122 | CorrespondenceIndices matches; // std::vector > 123 | }; 124 | 125 | /** The matching result between several pairs of views. */ 126 | typedef std::vector PairwiseMatching; 127 | 128 | /* ------------------ Input/Output for Prebundle ------------------ */ 129 | 130 | /** 131 | * Saves the pre-bundle data to file, which records all viewport and 132 | * matching data necessary for incremental structure-from-motion. 133 | */ 134 | void 135 | save_prebundle_to_file (ViewportList const& viewports, 136 | PairwiseMatching const& matching, std::string const& filename); 137 | 138 | /** 139 | * Loads the pre-bundle data from file, initializing viewports and matching. 140 | */ 141 | void 142 | load_prebundle_from_file (std::string const& filename, 143 | ViewportList* viewports, PairwiseMatching* matching); 144 | 145 | /** 146 | * Loads survey points and their observations from file. 147 | * 148 | * Survey file are ASCII files that start with the signature 149 | * MVE_SURVEY followed by a newline, followed by the number of survey points 150 | * and survey point observations. 151 | * Each survey point is a 3D point followed by a newline. Each survey point 152 | * observation is a line starting with the index of the survey point, followed 153 | * by the view id an the 2D location within the image. The (x, y) coordinates 154 | * have to be normalized such that the center of the image is (0, 0) and the 155 | * larger image dimension is one. This means that all image coordinates are 156 | * between (-0.5,-0.5) and (0.5, 0.5) 157 | * 158 | * MVE_SURVEY 159 | * 160 | * // x y z 161 | * ... 162 | * // survey_point_id view_id x y 163 | * ... 164 | */ 165 | void 166 | load_survey_from_file (std::string const& filename, 167 | SurveyPointList* survey_points); 168 | 169 | /* ------------------------ Implementation ------------------------ */ 170 | 171 | inline 172 | FeatureReference::FeatureReference (int view_id, int feature_id) 173 | : view_id(view_id) 174 | , feature_id(feature_id) 175 | { 176 | } 177 | 178 | inline 179 | SurveyObservation::SurveyObservation (int view_id, float x, float y) 180 | : view_id(view_id) 181 | , pos(x, y) 182 | { 183 | } 184 | 185 | inline bool 186 | TwoViewMatching::operator< (TwoViewMatching const& rhs) const 187 | { 188 | return this->view_1_id == rhs.view_1_id 189 | ? this->view_2_id < rhs.view_2_id 190 | : this->view_1_id < rhs.view_1_id; 191 | } 192 | 193 | inline 194 | Viewport::Viewport (void) 195 | : focal_length(0.0f) 196 | { 197 | std::fill(this->radial_distortion, this->radial_distortion + 2, 0.0f); 198 | } 199 | 200 | inline bool 201 | Track::is_valid (void) const 202 | { 203 | return !std::isnan(this->pos[0]); 204 | } 205 | 206 | SFM_BUNDLER_NAMESPACE_END 207 | SFM_NAMESPACE_END 208 | 209 | #endif /* SFM_BUNDLER_COMMON_HEADER */ 210 | -------------------------------------------------------------------------------- /sfm/camera_database.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_CAMERA_DATABASE_HEADER 11 | #define SFM_CAMERA_DATABASE_HEADER 12 | 13 | #include 14 | #include 15 | 16 | #include "sfm/defines.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | /** 21 | * Representation of a digital camera. 22 | */ 23 | struct CameraModel 24 | { 25 | /** The manufacturer for the camera. */ 26 | std::string maker; 27 | /** The model of the camera. */ 28 | std::string model; 29 | /** The width of the sensor in milli meters. */ 30 | float sensor_width_mm; 31 | /** The height of the sensor in milli meters. */ 32 | float sensor_height_mm; 33 | /** The width of the sensor in pixels. */ 34 | int sensor_width_px; 35 | /** The height of the sensor in pixels. */ 36 | int sensor_height_px; 37 | }; 38 | 39 | /* ---------------------------------------------------------------- */ 40 | 41 | /** 42 | * Camera database which, given a maker and model string, will look for 43 | * a camera model in the database and return the model on successful lookup. 44 | * If the lookup fails, a null pointer is returned. 45 | */ 46 | class CameraDatabase 47 | { 48 | public: 49 | /** Access to the singleton object. */ 50 | static CameraDatabase* get (void); 51 | 52 | /** Lookup of a camera model. Returns null on failure. */ 53 | CameraModel const* lookup (std::string const& maker, 54 | std::string const& model) const; 55 | 56 | private: 57 | CameraDatabase (void); 58 | void add (std::string const& maker, std::string const& model, 59 | float sensor_width_mm, float sensor_height_mm, 60 | int sensor_width_px, int sensor_height_px); 61 | 62 | private: 63 | static CameraDatabase* instance; 64 | std::vector data; 65 | }; 66 | 67 | /* ------------------------ Implementation ------------------------ */ 68 | 69 | inline CameraDatabase* 70 | CameraDatabase::get (void) 71 | { 72 | if (CameraDatabase::instance == nullptr) 73 | CameraDatabase::instance = new CameraDatabase(); 74 | return CameraDatabase::instance; 75 | } 76 | 77 | SFM_NAMESPACE_END 78 | 79 | #endif /* SFM_CAMERA_DATABASE_HEADER */ 80 | -------------------------------------------------------------------------------- /sfm/camera_pose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_POSE_HEADER 11 | #define SFM_POSE_HEADER 12 | 13 | #include 14 | 15 | #include "math/vector.h" 16 | #include "math/matrix.h" 17 | #include "math/matrix_tools.h" 18 | #include "sfm/defines.h" 19 | #include "sfm/correspondence.h" 20 | 21 | SFM_NAMESPACE_BEGIN 22 | 23 | /** 24 | * The camera pose is the 3x4 matrix P = K [R | t]. K is the 3x3 calibration 25 | * matrix, R a 3x3 rotation matrix and t a 3x1 translation vector. 26 | * 27 | * | f 0 px | The calibration matrix contains the focal length f, 28 | * K = | 0 f py | and the principal point px and py. 29 | * | 0 0 1 | 30 | * 31 | * For pose estimation, the calibration matrix is assumed to be known. This 32 | * might not be the case, but even a good guess of the focal length and the 33 | * principal point set to the image center can produce reasonably good 34 | * results so that bundle adjustment can recover better parameters. 35 | */ 36 | struct CameraPose 37 | { 38 | public: 39 | CameraPose (void); 40 | 41 | /** Initializes R with identity and t with zeros. */ 42 | void init_canonical_form (void); 43 | /** Returns the P matrix as the product K [R | t]. */ 44 | void fill_p_matrix (math::Matrix* result) const; 45 | /** Initializes the K matrix from focal length and principal point. */ 46 | void set_k_matrix (double flen, double px, double py); 47 | /** Returns the focal length as average of x and y focal length. */ 48 | double get_focal_length (void) const; 49 | /** Returns the camera position (requires valid camera). */ 50 | void fill_camera_pos (math::Vector* camera_pos) const; 51 | /** Returns true if K matrix is valid (non-zero focal length). */ 52 | bool is_valid (void) const; 53 | 54 | public: 55 | math::Matrix K; 56 | math::Matrix R; 57 | math::Vector t; 58 | }; 59 | 60 | /* ------------------------ Implementation ------------------------ */ 61 | 62 | inline 63 | CameraPose::CameraPose (void) 64 | : K(0.0) 65 | , R(0.0) 66 | , t(0.0) 67 | { 68 | } 69 | 70 | inline void 71 | CameraPose::init_canonical_form (void) 72 | { 73 | math::matrix_set_identity(&this->R); 74 | this->t.fill(0.0); 75 | } 76 | 77 | inline void 78 | CameraPose::fill_p_matrix (math::Matrix* P) const 79 | { 80 | math::Matrix KR = this->K * this->R; 81 | math::Matrix Kt(*(this->K * this->t)); 82 | *P = KR.hstack(Kt); 83 | } 84 | 85 | inline void 86 | CameraPose::set_k_matrix (double flen, double px, double py) 87 | { 88 | this->K.fill(0.0); 89 | this->K[0] = flen; this->K[2] = px; 90 | this->K[4] = flen; this->K[5] = py; 91 | this->K[8] = 1.0; 92 | } 93 | 94 | inline double 95 | CameraPose::get_focal_length (void) const 96 | { 97 | return (this->K[0] + this->K[4]) / 2.0; 98 | } 99 | 100 | inline void 101 | CameraPose::fill_camera_pos (math::Vector* camera_pos) const 102 | { 103 | *camera_pos = -this->R.transposed().mult(this->t); 104 | } 105 | 106 | inline bool 107 | CameraPose::is_valid (void) const 108 | { 109 | return this->K[0] != 0.0; 110 | } 111 | 112 | SFM_NAMESPACE_END 113 | 114 | #endif /* SFM_POSE_HEADER */ 115 | -------------------------------------------------------------------------------- /sfm/correspondence.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_CORRESPONDENCE_HEADER 11 | #define SFM_CORRESPONDENCE_HEADER 12 | 13 | #include 14 | 15 | #include "math/matrix.h" 16 | #include "sfm/defines.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | struct Correspondence2D2D; 21 | typedef std::vector Correspondences2D2D; 22 | 23 | struct Correspondence2D3D; 24 | typedef std::vector Correspondences2D3D; 25 | 26 | /** The IDs of a matching feature pair in two images. */ 27 | typedef std::pair CorrespondenceIndex; 28 | /** A list of all matching feature pairs in two images. */ 29 | typedef std::vector CorrespondenceIndices; 30 | 31 | /** 32 | * Two image coordinates which correspond to each other in terms of observing 33 | * the same point in the scene. 34 | * TODO: Rename this to Correspondence2D2D. 35 | */ 36 | struct Correspondence2D2D 37 | { 38 | double p1[2]; 39 | double p2[2]; 40 | }; 41 | 42 | /** 43 | * A 3D point and an image coordinate which correspond to each other in terms 44 | * of the image observing this 3D point in the scene. 45 | */ 46 | struct Correspondence2D3D 47 | { 48 | double p3d[3]; 49 | double p2d[2]; 50 | }; 51 | 52 | SFM_NAMESPACE_END 53 | 54 | #endif // SFM_CORRESPONDENCE_HEADER 55 | -------------------------------------------------------------------------------- /sfm/defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_DEFINES_HEADER 11 | #define SFM_DEFINES_HEADER 12 | 13 | #define SFM_NAMESPACE_BEGIN namespace sfm { 14 | #define SFM_NAMESPACE_END } 15 | 16 | #define SFM_BUNDLER_NAMESPACE_BEGIN namespace bundler { 17 | #define SFM_BUNDLER_NAMESPACE_END } 18 | 19 | #define SFM_PBA_NAMESPACE_BEGIN namespace pba { 20 | #define SFM_PBA_NAMESPACE_END } 21 | 22 | #define SFM_BA_NAMESPACE_BEGIN namespace ba { 23 | #define SFM_BA_NAMESPACE_END } 24 | 25 | #ifndef STD_NAMESPACE_BEGIN 26 | # define STD_NAMESPACE_BEGIN namespace std { 27 | # define STD_NAMESPACE_END } 28 | #endif 29 | 30 | /** Structure-from-Motion library. */ 31 | SFM_NAMESPACE_BEGIN 32 | /** SfM bundler components. */ 33 | SFM_BUNDLER_NAMESPACE_BEGIN SFM_BUNDLER_NAMESPACE_END 34 | /** Parallel Bundle Adjustment components. */ 35 | SFM_PBA_NAMESPACE_BEGIN SFM_PBA_NAMESPACE_END 36 | 37 | 38 | SFM_NAMESPACE_END 39 | 40 | #endif /* MVE_DEFINES_HEADER */ 41 | -------------------------------------------------------------------------------- /sfm/extract_focal_length.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "sfm/camera_database.h" 14 | #include "sfm/extract_focal_length.h" 15 | 16 | SFM_NAMESPACE_BEGIN 17 | 18 | std::pair 19 | extract_focal_length (core::image::ExifInfo const& exif) 20 | { 21 | /* Step 1: Check for focal length info in EXIF and database entry. */ 22 | float focal_length = exif.focal_length; 23 | std::string camera_maker = exif.camera_maker; 24 | std::string camera_model = exif.camera_model; 25 | float sensor_size = -1.0f; 26 | if (focal_length > 0.0f && !camera_model.empty()) 27 | { 28 | CameraDatabase const* db = CameraDatabase::get(); 29 | CameraModel const* model = db->lookup(camera_maker, camera_model); 30 | if (model != nullptr) 31 | sensor_size = model->sensor_width_mm; 32 | } 33 | if (focal_length > 0.0f && sensor_size > 0.0f) 34 | { 35 | float flen = focal_length / sensor_size; 36 | return std::make_pair(flen, FOCAL_LENGTH_AND_DATABASE); 37 | } 38 | 39 | /* Step 2: Check for 35mm equivalent focal length. */ 40 | float focal_length_35mm = exif.focal_length_35mm; 41 | if (focal_length_35mm > 0.0f) 42 | { 43 | float flen = focal_length_35mm / 35.0f; 44 | return std::make_pair(flen, FOCAL_LENGTH_35MM_EQUIV); 45 | } 46 | 47 | /* Step 3: Fall back to default value. */ 48 | return std::make_pair(1.0f, FOCAL_LENGTH_FALLBACK_VALUE); 49 | } 50 | 51 | SFM_NAMESPACE_END 52 | -------------------------------------------------------------------------------- /sfm/extract_focal_length.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_EXTRACT_FOCAL_LENGTH_HEADER 11 | #define SFM_EXTRACT_FOCAL_LENGTH_HEADER 12 | 13 | #include 14 | 15 | #include "core/image_exif.h" 16 | #include "sfm/defines.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | /** 21 | * Indicator which focal length estimation has been used. 22 | */ 23 | enum FocalLengthMethod 24 | { 25 | FOCAL_LENGTH_AND_DATABASE, 26 | FOCAL_LENGTH_35MM_EQUIV, 27 | FOCAL_LENGTH_FALLBACK_VALUE 28 | }; 29 | 30 | /** 31 | * Datatype for the focal length estimate which reports the normalized 32 | * focal length as well as the method used to obtain the value. 33 | */ 34 | typedef std::pair FocalLengthEstimate; 35 | 36 | /** 37 | * Extracts the focal length from the EXIF tags of an image. 38 | * 39 | * The algorithm first checks for the availability of the "focal length" 40 | * in EXIF tags and computes the effective focal length using a database 41 | * of camera sensor sizes. If the camera model is unknown to the database, 42 | * the "focal length 35mm equivalent" EXIF tag is used. If this information 43 | * is also not available, a default value is used. 44 | * 45 | * This estimation can fail in numerous situations: 46 | * - The image contains no EXIF tags (default value is used) 47 | * - The camera did not specify the focal length in EXIF 48 | * - The lens specifies the wrong focal length due to lens incompatibility 49 | * - The camera is not in the database and the 35mm equivalent is missing 50 | * - The camera used digial zoom changing the effective focal length 51 | * 52 | * The resulting focal length is in normalized format, that is the quotient 53 | * of the image focal length by the sensor size. E.g. a photo taken at 70mm 54 | * with a 35mm sensor size will result in a normalized focal length of 2. 55 | */ 56 | FocalLengthEstimate 57 | extract_focal_length (core::image::ExifInfo const& exif); 58 | 59 | SFM_NAMESPACE_END 60 | 61 | #endif /* SFM_EXTRACT_FOCAL_LENGTH_HEADER */ 62 | -------------------------------------------------------------------------------- /sfm/feature_set.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "sfm/feature_set.h" 14 | 15 | SFM_NAMESPACE_BEGIN 16 | 17 | namespace 18 | { 19 | template 20 | bool 21 | compare_scale (T const& descr1, T const& descr2) 22 | { 23 | return descr1.scale > descr2.scale; 24 | } 25 | } /* namespace */ 26 | 27 | void 28 | FeatureSet::compute_features (core::ByteImage::Ptr image) 29 | { 30 | this->colors.clear(); 31 | this->positions.clear(); 32 | this->width = image->width(); 33 | this->height = image->height(); 34 | 35 | /* Make sure these are in the right order. Matching relies on it. */ 36 | if (this->opts.feature_types & FEATURE_SIFT) 37 | this->compute_sift(image); 38 | if (this->opts.feature_types & FEATURE_SURF) 39 | this->compute_surf(image); 40 | } 41 | 42 | void 43 | FeatureSet::normalize_feature_positions (void) 44 | { 45 | /* Normalize image coordinates. */ 46 | float const fwidth = static_cast(this->width); 47 | float const fheight = static_cast(this->height); 48 | float const fnorm = std::max(fwidth, fheight); 49 | for (std::size_t i = 0; i < this->positions.size(); ++i) 50 | { 51 | math::Vec2f& pos = this->positions[i]; 52 | pos[0] = (pos[0] + 0.5f - fwidth / 2.0f) / fnorm; 53 | pos[1] = (pos[1] + 0.5f - fheight / 2.0f) / fnorm; 54 | } 55 | } 56 | 57 | void 58 | FeatureSet::compute_sift (core::ByteImage::ConstPtr image) 59 | { 60 | /* Compute features. */ 61 | Sift::Descriptors descr; 62 | { 63 | Sift sift(this->opts.sift_opts); 64 | sift.set_image(image); 65 | sift.process(); 66 | descr = sift.get_descriptors(); 67 | } 68 | 69 | /* Sort features by scale for low-res matching. */ 70 | std::sort(descr.begin(), descr.end(), compare_scale); 71 | 72 | /* Prepare and copy to data structures. */ 73 | std::size_t offset = this->positions.size(); 74 | this->positions.resize(offset + descr.size()); 75 | this->colors.resize(offset + descr.size()); 76 | 77 | for (std::size_t i = 0; i < descr.size(); ++i) 78 | { 79 | Sift::Descriptor const& d = descr[i]; 80 | this->positions[offset + i] = math::Vec2f(d.x, d.y); 81 | image->linear_at(d.x, d.y, this->colors[offset + i].begin()); 82 | } 83 | 84 | /* Keep SIFT descriptors. */ 85 | std::swap(descr, this->sift_descriptors); 86 | } 87 | 88 | void 89 | FeatureSet::compute_surf (core::ByteImage::ConstPtr image) 90 | { 91 | /* Compute features. */ 92 | Surf::Descriptors descr; 93 | { 94 | Surf surf(this->opts.surf_opts); 95 | surf.set_image(image); 96 | surf.process(); 97 | descr = surf.get_descriptors(); 98 | } 99 | 100 | /* Sort features by scale for low-res matching. */ 101 | std::sort(descr.begin(), descr.end(), compare_scale); 102 | 103 | /* Prepare and copy to data structures. */ 104 | std::size_t offset = this->positions.size(); 105 | this->positions.resize(offset + descr.size()); 106 | this->colors.resize(offset + descr.size()); 107 | 108 | for (std::size_t i = 0; i < descr.size(); ++i) 109 | { 110 | Surf::Descriptor const& d = descr[i]; 111 | this->positions[offset + i] = math::Vec2f(d.x, d.y); 112 | image->linear_at(d.x, d.y, this->colors[offset + i].begin()); 113 | } 114 | 115 | /* Keep SURF descriptors. */ 116 | std::swap(descr, this->surf_descriptors); 117 | } 118 | 119 | void 120 | FeatureSet::clear_descriptors (void) 121 | { 122 | this->sift_descriptors.clear(); 123 | this->sift_descriptors.shrink_to_fit(); 124 | this->surf_descriptors.clear(); 125 | this->surf_descriptors.shrink_to_fit(); 126 | } 127 | 128 | SFM_NAMESPACE_END 129 | -------------------------------------------------------------------------------- /sfm/feature_set.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_FEATURE_SET_HEADER 11 | #define SFM_FEATURE_SET_HEADER 12 | 13 | #include 14 | 15 | #include "math/vector.h" 16 | #include "util/aligned_memory.h" 17 | #include "features/sift.h" 18 | #include "features/surf.h" 19 | #include "sfm/defines.h" 20 | #include "features/defines.h" 21 | 22 | SFM_NAMESPACE_BEGIN 23 | 24 | using namespace features; 25 | /** 26 | * The FeatureSet holds per-feature information for a single view, and 27 | * allows to transparently compute and match multiple feature types. 28 | */ 29 | class FeatureSet 30 | { 31 | public: 32 | /** Bitmask with feature types. */ 33 | enum FeatureTypes 34 | { 35 | FEATURE_SIFT = 1 << 0, 36 | FEATURE_SURF = 1 << 1, 37 | FEATURE_ALL = 0xFF 38 | }; 39 | 40 | /** Options for feature detection and matching. */ 41 | struct Options 42 | { 43 | Options (void); 44 | 45 | FeatureTypes feature_types; 46 | Sift::Options sift_opts; 47 | Surf::Options surf_opts; 48 | }; 49 | 50 | public: 51 | FeatureSet (void); 52 | explicit FeatureSet (Options const& options); 53 | void set_options (Options const& options); 54 | 55 | // todo compute features 56 | /** Computes the features specified in the options. */ 57 | void compute_features (core::ByteImage::Ptr image); 58 | 59 | // todo normalizes the features positions 60 | /** Normalizes the features positions w.r.t. the image dimensions. */ 61 | void normalize_feature_positions (void); 62 | 63 | /** Clear descriptor data. */ 64 | void clear_descriptors (void); 65 | 66 | public: 67 | /** Image dimension used for feature computation. */ 68 | int width, height; 69 | /** Per-feature image position. */ 70 | std::vector positions; 71 | /** Per-feature image color. */ 72 | std::vector colors; 73 | /** The SIFT descriptors. */ 74 | Sift::Descriptors sift_descriptors; 75 | /** The SURF descriptors. */ 76 | Surf::Descriptors surf_descriptors; 77 | 78 | private: 79 | void compute_sift (core::ByteImage::ConstPtr image); 80 | void compute_surf (core::ByteImage::ConstPtr image); 81 | 82 | private: 83 | Options opts; 84 | }; 85 | 86 | /* ------------------------ Implementation ------------------------ */ 87 | 88 | inline 89 | FeatureSet::Options::Options (void) 90 | : feature_types(FEATURE_SIFT) 91 | { 92 | } 93 | 94 | inline 95 | FeatureSet::FeatureSet (void) 96 | { 97 | } 98 | 99 | inline 100 | FeatureSet::FeatureSet (Options const& options) 101 | : opts(options) 102 | { 103 | } 104 | 105 | inline void 106 | FeatureSet::set_options (Options const& options) 107 | { 108 | this->opts = options; 109 | } 110 | SFM_NAMESPACE_END 111 | 112 | #endif /* SFM_FEATURE_SET_HEADER */ 113 | -------------------------------------------------------------------------------- /sfm/fundamental.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "math/matrix_tools.h" 14 | #include "math/matrix_svd.h" 15 | #include "math/functions.h" 16 | #include "sfm/fundamental.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | namespace 21 | { 22 | /** 23 | * Creates the cross product matrix [x] for x. With this matrix, 24 | * the cross product cross(x, y) can be expressed using matrix 25 | * multiplication [x] y. 26 | */ 27 | template 28 | void 29 | cross_product_matrix (math::Vector const& v, 30 | math::Matrix* result) 31 | { 32 | result->fill(T(0)); 33 | (*result)(0, 1) = -v[2]; 34 | (*result)(0, 2) = v[1]; 35 | (*result)(1, 0) = v[2]; 36 | (*result)(1, 2) = -v[0]; 37 | (*result)(2, 0) = -v[1]; 38 | (*result)(2, 1) = v[0]; 39 | } 40 | 41 | } // namespace 42 | 43 | bool 44 | fundamental_least_squares (Correspondences2D2D const& points, 45 | FundamentalMatrix* result) 46 | { 47 | if (points.size() < 8) 48 | throw std::invalid_argument("At least 8 points required"); 49 | 50 | /* Create Nx9 matrix A. Each correspondence creates on row in A. */ 51 | std::vector A(points.size() * 9); 52 | for (std::size_t i = 0; i < points.size(); ++i) 53 | { 54 | Correspondence2D2D const& p = points[i]; 55 | A[i * 9 + 0] = p.p2[0] * p.p1[0]; 56 | A[i * 9 + 1] = p.p2[0] * p.p1[1]; 57 | A[i * 9 + 2] = p.p2[0] * 1.0; 58 | A[i * 9 + 3] = p.p2[1] * p.p1[0]; 59 | A[i * 9 + 4] = p.p2[1] * p.p1[1]; 60 | A[i * 9 + 5] = p.p2[1] * 1.0; 61 | A[i * 9 + 6] = 1.0 * p.p1[0]; 62 | A[i * 9 + 7] = 1.0 * p.p1[1]; 63 | A[i * 9 + 8] = 1.0 * 1.0; 64 | } 65 | 66 | /* Compute fundamental matrix using SVD. */ 67 | std::vector V(9 * 9); 68 | math::matrix_svd(&A[0], points.size(), 9, nullptr, nullptr, &V[0]); 69 | 70 | /* Use last column of V as solution. */ 71 | for (int i = 0; i < 9; ++i) 72 | (*result)[i] = V[i * 9 + 8]; 73 | 74 | return true; 75 | } 76 | 77 | bool 78 | fundamental_8_point (Eight2DPoints const& points_view_1, 79 | Eight2DPoints const& points_view_2, FundamentalMatrix* result) 80 | { 81 | /* 82 | * Create 8x9 matrix A. Each pair of input points creates on row in A. 83 | */ 84 | math::Matrix A; 85 | for (int i = 0; i < 8; ++i) 86 | { 87 | math::Vector p1 = points_view_1.col(i); 88 | math::Vector p2 = points_view_2.col(i); 89 | A(i, 0) = p2[0] * p1[0]; 90 | A(i, 1) = p2[0] * p1[1]; 91 | A(i, 2) = p2[0] * 1.0; 92 | A(i, 3) = p2[1] * p1[0]; 93 | A(i, 4) = p2[1] * p1[1]; 94 | A(i, 5) = p2[1] * 1.0; 95 | A(i, 6) = 1.0 * p1[0]; 96 | A(i, 7) = 1.0 * p1[1]; 97 | A(i, 8) = 1.0 * 1.0; 98 | } 99 | 100 | /* 101 | * The fundamental matrix F is created from the singular 102 | * vector corresponding to the smallest eigenvalue of A. 103 | */ 104 | math::Matrix V; 105 | math::matrix_svd(A, nullptr, nullptr, &V); 106 | math::Vector f = V.col(8); 107 | std::copy(*f, *f + 9, **result); 108 | 109 | return true; 110 | } 111 | 112 | void 113 | enforce_fundamental_constraints (FundamentalMatrix* matrix) 114 | { 115 | /* 116 | * Constraint enforcement. The fundamental matrix F has rank 2 and 7 117 | * degrees of freedom. However, F' computed from point correspondences may 118 | * not have rank 2 and it needs to be enforced. To this end, the SVD is 119 | * used: F' = USV*, F = UDV* where D = diag(s1, s2, 0) and s1 and s2 120 | * are the largest and second largest eigenvalues of F. 121 | */ 122 | math::Matrix U, S, V; 123 | math::matrix_svd(*matrix, &U, &S, &V); 124 | S(2, 2) = 0.0; 125 | *matrix = U * S * V.transposed(); 126 | } 127 | 128 | void 129 | enforce_essential_constraints (EssentialMatrix* matrix) 130 | { 131 | /* 132 | * Constraint enforcement. The essential matrix E has rank 2 and 5 degrees 133 | * of freedom. However, E' computed from normalized image correspondences 134 | * may not have rank 2 and it needs to be enforced. To this end, the SVD is 135 | * used: F' = USV*, F = UDV* where D = diag(s, s, 0), s = (s1 + s2) / 2 136 | * and s1 and s2 are the largest and second largest eigenvalues of F. 137 | */ 138 | math::Matrix U, S, V; 139 | math::matrix_svd(*matrix, &U, &S, &V); 140 | double avg = (S(0, 0) + S(0, 1)) / 2.0; 141 | S(0, 0) = avg; 142 | S(1, 1) = avg; 143 | S(2, 2) = 0.0; 144 | *matrix = U * S * V.transposed(); 145 | } 146 | 147 | void 148 | pose_from_essential (EssentialMatrix const& matrix, 149 | std::vector* result) 150 | { 151 | /* 152 | * The pose [R|t] for the second camera is extracted from the essential 153 | * matrix E and the first camera is given in canonical form [I|0]. 154 | * The SVD of E = USV is computed. The scale of S' diagonal entries is 155 | * irrelevant and S is assumed to be diag(1,1,0). 156 | * Details can be found in [Hartley, Zisserman, Sect. 9.6.1]. 157 | */ 158 | 159 | math::Matrix W(0.0); 160 | W(0, 1) = -1.0; W(1, 0) = 1.0; W(2, 2) = 1.0; 161 | math::Matrix Wt(0.0); 162 | Wt(0, 1) = 1.0; Wt(1, 0) = -1.0; Wt(2, 2) = 1.0; 163 | 164 | math::Matrix U, S, V; 165 | math::matrix_svd(matrix, &U, &S, &V); 166 | 167 | // This seems to ensure that det(R) = 1 (instead of -1). 168 | // FIXME: Is this the correct way to do it? 169 | if (math::matrix_determinant(U) < 0.0) 170 | for (int i = 0; i < 3; ++i) 171 | U(i,2) = -U(i,2); 172 | if (math::matrix_determinant(V) < 0.0) 173 | for (int i = 0; i < 3; ++i) 174 | V(i,2) = -V(i,2); 175 | 176 | V.transpose(); 177 | result->clear(); 178 | result->resize(4); 179 | result->at(0).R = U * W * V; 180 | result->at(1).R = result->at(0).R; 181 | result->at(2).R = U * Wt * V; 182 | result->at(3).R = result->at(2).R; 183 | result->at(0).t = U.col(2); 184 | result->at(1).t = -result->at(0).t; 185 | result->at(2).t = result->at(0).t; 186 | result->at(3).t = -result->at(0).t; 187 | 188 | // FIXME: Temporary sanity check. 189 | if (!MATH_EPSILON_EQ(math::matrix_determinant(result->at(0).R), 1.0, 1e-3)) 190 | throw std::runtime_error("Invalid rotation matrix"); 191 | } 192 | 193 | void 194 | fundamental_from_pose (CameraPose const& cam1, CameraPose const& cam2, 195 | FundamentalMatrix* result) 196 | { 197 | /* 198 | * The fundamental matrix is obtained from camera matrices. 199 | * See Hartley Zisserman (9.1): F = [e2] P2 P1^. 200 | * Where P1, P2 are the camera matrices, and P^ is the inverse of P. 201 | * e2 is the epipole in the second camera and [e2] is the cross product 202 | * matrix for e2. 203 | */ 204 | math::Matrix P1, P2; 205 | cam1.fill_p_matrix(&P1); 206 | cam2.fill_p_matrix(&P2); 207 | 208 | math::Vec4d c1(cam1.R.transposed() * -cam1.t, 1.0); 209 | math::Vec3d e2 = P2 * c1; 210 | 211 | math::Matrix3d ex; 212 | cross_product_matrix(e2, &ex); 213 | 214 | // FIXME: The values in the fundamental matrix can become huge. 215 | // The input projection matrix should be given in unit coodinates, 216 | // not pixel coordinates? Test and document that. 217 | 218 | math::Matrix P1inv; 219 | math::matrix_pseudo_inverse(P1, &P1inv); 220 | *result = ex * P2 * P1inv; 221 | } 222 | 223 | 224 | double 225 | sampson_distance (FundamentalMatrix const& F, Correspondence2D2D const& m) 226 | { 227 | /* 228 | * Computes the Sampson distance SD for a given match and fundamental 229 | * matrix. SD is computed as [Sect 11.4.3, Hartley, Zisserman]: 230 | * 231 | * SD = (x'Fx)^2 / ( (Fx)_1^2 + (Fx)_2^2 + (x'F)_1^2 + (x'F)_2^2 ) 232 | */ 233 | 234 | double p2_F_p1 = 0.0; 235 | p2_F_p1 += m.p2[0] * (m.p1[0] * F[0] + m.p1[1] * F[1] + F[2]); 236 | p2_F_p1 += m.p2[1] * (m.p1[0] * F[3] + m.p1[1] * F[4] + F[5]); 237 | p2_F_p1 += 1.0 * (m.p1[0] * F[6] + m.p1[1] * F[7] + F[8]); 238 | p2_F_p1 *= p2_F_p1; 239 | 240 | double sum = 0.0; 241 | sum += math::fastpow(m.p1[0] * F[0] + m.p1[1] * F[1] + F[2], 2); 242 | sum += math::fastpow(m.p1[0] * F[3] + m.p1[1] * F[4] + F[5], 2); 243 | sum += math::fastpow(m.p2[0] * F[0] + m.p2[1] * F[3] + F[6], 2); 244 | sum += math::fastpow(m.p2[0] * F[1] + m.p2[1] * F[4] + F[7], 2); 245 | 246 | return p2_F_p1 / sum; 247 | } 248 | 249 | SFM_NAMESPACE_END 250 | -------------------------------------------------------------------------------- /sfm/fundamental.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | * 9 | * The relation between two cameras is defined by the fundamental matrix. 10 | * In the calibrated case, where the camera internal parameters (focal 11 | * length, principal point) are known, pose can be described with the essential 12 | * matrix. 13 | * 14 | * The fundamental matrix can be computed from eight point correspondences 15 | * in the images, using the 8-point algorithm. It is also possible to compute 16 | * the fundamental matrix from seven point correspondences by enforcing 17 | * further constraints -- the 7-point algorithm. 18 | * If the camera calibration is know, the essential matrix can be computed 19 | * from as few as five point correspondences -- the 5-point algorithm. 20 | * 21 | * The input points to the N-point algorithms should be normalized such that 22 | * the mean of the points is zero and the points fit in the unit square. This 23 | * makes solving for the fundamental matrix numerically stable. The inverse 24 | * transformation can then applied afterwards. That is, for transformations 25 | * T1 and T2, the de-normalized fundamental matrix is given by F' = T2* F T1, 26 | * where T* is the transpose of T. 27 | * 28 | * Camera matrix can be extracted from the essential matrix as described 29 | * in [Sect 9.6.2, Hartley, Zisserman, 2004]. 30 | * 31 | * Properties of the Fundamental matrix F: 32 | * - Rank 2 homogenous matrix with 7 degrees of freedom, det(F) = 0. 33 | * - Relates images points x, x' in two cameras: x'^T F x = 0. 34 | * - If F is the fundamental matrix for camera pair (P,P'), the transpose 35 | * F^T is the fundamental matrix for camera pair (P',P). 36 | * - Two non-zero singular values. 37 | * 38 | * Properties of the Essential matrix E: 39 | * - Rank 2 homogenous matrix with 5 degrees of freedom, det(E) = 0. 40 | * - Relation to Fundamental matrix: E = K'^T F K. 41 | * - Relates normalized image points x, x' in two cameras: x'^T E x = 0. 42 | * Normalized image point x := K^-1 x* with x* (unnormalized) image point. 43 | * - Two equal singular vales, the third one is zero. 44 | */ 45 | #ifndef SFM_FUNDAMENTAL_HEADER 46 | #define SFM_FUNDAMENTAL_HEADER 47 | 48 | #include 49 | #include 50 | 51 | #include "math/vector.h" 52 | #include "math/matrix.h" 53 | #include "sfm/defines.h" 54 | #include "sfm/camera_pose.h" 55 | #include "sfm/correspondence.h" 56 | 57 | SFM_NAMESPACE_BEGIN 58 | 59 | typedef math::Matrix Eight2DPoints; 60 | typedef math::Matrix FundamentalMatrix; 61 | typedef math::Matrix EssentialMatrix; 62 | 63 | /** 64 | * Algorithm to compute the fundamental or essential matrix from image 65 | * correspondences. This algorithm computes the least squares solution for 66 | * the fundamental matrix from at least 8 correspondences. The solution is 67 | * sensitive to outliers. 68 | * 69 | * This does not normalize the image coordinates for stability or enforces 70 | * constraints on the resulting matrix. 71 | */ 72 | bool 73 | fundamental_least_squares (Correspondences2D2D const& points, 74 | FundamentalMatrix* result); 75 | 76 | /** 77 | * Algorithm to compute the fundamental or essential matrix from 8 image 78 | * correspondences. It closely follows [Sect. 11.2, Hartley, Zisserman, 2004]. 79 | * In case of "normalized image coordinates" (i.e. x* = K^-1 x), this code 80 | * computes the essential matrix. 81 | * 82 | * This does not normalize the image coordinates for stability or enforces 83 | * constraints on the resulting matrix. 84 | * 85 | * Note: For eight points this code computes the same result as the least 86 | * squares version but with fixed matrix sizes for compile time optimizations. 87 | */ 88 | bool 89 | fundamental_8_point (Eight2DPoints const& points_view_1, 90 | Eight2DPoints const& points_view_2, FundamentalMatrix* result); 91 | 92 | /** 93 | * Constraints the given matrix to have TWO NON-ZERO eigenvalues. 94 | * This is done using SVD: F' = USV*, F = UDV* with D = diag(a, b, 0). 95 | */ 96 | void 97 | enforce_fundamental_constraints (FundamentalMatrix* matrix); 98 | 99 | /** 100 | * Constraints the given matrix to have TWO EQUAL NON-ZERO eigenvalues. 101 | * This is done using SVD: F' = USV*, F = UDV* with D = diag(a, a, 0). 102 | */ 103 | void 104 | enforce_essential_constraints (EssentialMatrix* matrix); 105 | 106 | /** 107 | * Retrieves the camera matrices from the essential matrix. This routine 108 | * recovers P' = [R|t] for the second camera where the first camera is given 109 | * in canonical form P = [I|0]. The pose can be computed up to scale and a 110 | * four-fold ambiguity. That is, the resulting translation has length one and 111 | * four possible solutions are provided. In case of two cameras in the same 112 | * location, the rotation is still reliable but the translation is unstable. 113 | * 114 | * Each of the solutions must be tested: It is sufficient to test if a single 115 | * point (triangulated from a 2D-2D correspondence) is in front of both 116 | * cameras. Note: The resulting camera pose does not contain the K matrix. 117 | * Before testing the resulting poses, the K-matrix must be set! 118 | */ 119 | void 120 | pose_from_essential (EssentialMatrix const& matrix, 121 | std::vector* result); 122 | 123 | /** 124 | * Computes the fundamental matrix corresponding to cam1 and cam2. 125 | * The function relies on the epipole to be visible in the second 126 | * camera, thus the cameras must not be in the same location. 127 | */ 128 | void 129 | fundamental_from_pose (CameraPose const& cam1, CameraPose const& cam2, 130 | FundamentalMatrix* result); 131 | 132 | /** 133 | * Computes the Sampson distance for an image correspondence given the 134 | * fundamental matrix between two views. 135 | */ 136 | double 137 | sampson_distance (FundamentalMatrix const& fundamental, 138 | Correspondence2D2D const& match); 139 | 140 | /** 141 | * Computes a transformation for 2D points in homogeneous coordinates 142 | * such that the mean of the points is zero and the points fit in the unit 143 | * square. (The third coordinate will still be 1 after normalization.) 144 | * Optimized version where number of points must be known at compile time. 145 | */ 146 | template 147 | void 148 | compute_normalization(math::Matrix const& points, 149 | math::Matrix* transformation); 150 | 151 | /* ---------------------------------------------------------------- */ 152 | 153 | #if 0 // This is not yet implemented! 154 | typedef math::Matrix Seven2DPoints; 155 | typedef math::Matrix Five2DPoints; 156 | 157 | /** 158 | * Algorithm to compute the fundamental matrix from 7 point correspondences. 159 | * The algorithm returns one or three possible solutions. 160 | * The implementation follows [Sect. 11.2, 11.1.2, Hartley, Zisserman, 2004]. 161 | */ 162 | bool 163 | pose_7_point (Seven2DPoints const& points_view_1, 164 | Seven2DPoints const& points_view_2, 165 | std::vector* result); 166 | 167 | /** 168 | * Algorithm to compute the essential matrix from 5 point correspondences. 169 | * The algorithm returns up to ten possible solutions. 170 | * The literature can be found at: http://vis.uky.edu/~stewe/FIVEPOINT/ 171 | * 172 | * Recent developments on direct relative orientation, 173 | * H. Stewenius, C. Engels, and D. Nister, ISPRS 2006. 174 | * 175 | */ 176 | bool 177 | pose_5_point (Five2DPoints const& points_view_1, 178 | Five2DPoints const& points_view_2, 179 | std::vector* result); 180 | #endif 181 | 182 | /* ---------------------------------------------------------------- */ 183 | 184 | template 185 | void 186 | compute_normalization(math::Matrix const& points, 187 | math::Matrix* transformation) 188 | { 189 | math::Vector mean(T(0)); 190 | math::Vector aabb_min(std::numeric_limits::max()); 191 | math::Vector aabb_max(-std::numeric_limits::max()); 192 | for (int i = 0; i < DIM; ++i) 193 | { 194 | for (int j = 0; j < 3; ++j) 195 | { 196 | mean[j] += points(j, i); 197 | aabb_min[j] = std::min(aabb_min[j], points(j, i)); 198 | aabb_max[j] = std::max(aabb_max[j], points(j, i)); 199 | } 200 | } 201 | mean /= static_cast(DIM); 202 | T norm = (aabb_max - aabb_min).maximum(); 203 | math::Matrix& t = *transformation; 204 | t[0] = T(1) / norm; t[1] = T(0); t[2] = -mean[0] / norm; 205 | t[3] = T(0); t[4] = T(1) / norm; t[5] = -mean[1] / norm; 206 | t[6] = T(0); t[7] = T(0); t[8] = T(1); 207 | } 208 | 209 | SFM_NAMESPACE_END 210 | 211 | #endif // SFM_FUNDAMENTAL_HEADER 212 | -------------------------------------------------------------------------------- /sfm/homography.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "math/matrix_tools.h" 15 | #include "math/matrix_svd.h" 16 | #include "sfm/homography.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | bool 21 | homography_dlt (Correspondences2D2D const& points, HomographyMatrix* result) 22 | { 23 | if (points.size() < 4) 24 | throw std::invalid_argument("At least 4 matches required"); 25 | 26 | /* Create 2Nx9 matrix A. Each correspondence creates two rows in A. */ 27 | std::vector A(2 * points.size() * 9); 28 | for (std::size_t i = 0; i < points.size(); ++i) 29 | { 30 | std::size_t const row1 = 9 * i; 31 | std::size_t const row2 = 9 * (i + points.size()); 32 | Correspondence2D2D const& match = points[i]; 33 | A[row1 + 0] = 0.0; 34 | A[row1 + 1] = 0.0; 35 | A[row1 + 2] = 0.0; 36 | A[row1 + 3] = match.p1[0]; 37 | A[row1 + 4] = match.p1[1]; 38 | A[row1 + 5] = 1.0; 39 | A[row1 + 6] = -match.p1[0] * match.p2[1]; 40 | A[row1 + 7] = -match.p1[1] * match.p2[1]; 41 | A[row1 + 8] = -match.p2[1]; 42 | A[row2 + 0] = -match.p1[0]; 43 | A[row2 + 1] = -match.p1[1]; 44 | A[row2 + 2] = -1.0; 45 | A[row2 + 3] = 0.0; 46 | A[row2 + 4] = 0.0; 47 | A[row2 + 5] = 0.0; 48 | A[row2 + 6] = match.p1[0] * match.p2[0]; 49 | A[row2 + 7] = match.p1[1] * match.p2[0]; 50 | A[row2 + 8] = match.p2[0]; 51 | } 52 | 53 | /* Compute homography matrix using SVD. */ 54 | math::Matrix V; 55 | math::matrix_svd(&A[0], 2 * points.size(), 56 | 9, nullptr, nullptr, V.begin()); 57 | 58 | /* Only consider the last column of V as the solution. */ 59 | for (int i = 0; i < 9; ++i) 60 | (*result)[i] = V[i * 9 + 8]; 61 | 62 | return true; 63 | } 64 | 65 | double 66 | symmetric_transfer_error(HomographyMatrix const& homography, 67 | Correspondence2D2D const& match) 68 | { 69 | /* 70 | * Computes the symmetric transfer error for a given match and homography 71 | * matrix. The error is computed as [Sect 4.2.2, Hartley, Zisserman]: 72 | * 73 | * e = d(x, (H^-1)x')^2 + d(x', Hx)^2 74 | */ 75 | math::Vec3d p1(match.p1[0], match.p1[1], 1.0); 76 | math::Vec3d p2(match.p2[0], match.p2[1], 1.0); 77 | 78 | math::Matrix3d invH = math::matrix_inverse(homography); 79 | math::Vec3d result = invH * p2; 80 | result /= result[2]; 81 | double error = (p1 - result).square_norm(); 82 | 83 | result = homography * p1; 84 | result /= result[2]; 85 | error += (result - p2).square_norm(); 86 | 87 | return 0.5 * error; 88 | } 89 | 90 | SFM_NAMESPACE_END 91 | -------------------------------------------------------------------------------- /sfm/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_HOMOGRAPHY_HEADER 11 | #define SFM_HOMOGRAPHY_HEADER 12 | 13 | #include "math/matrix.h" 14 | #include "sfm/defines.h" 15 | #include "sfm/correspondence.h" 16 | 17 | SFM_NAMESPACE_BEGIN 18 | 19 | typedef math::Matrix3d HomographyMatrix; 20 | 21 | /** 22 | * Direct linear transformation algorithm to compute the homography matrix from 23 | * image correspondences. This algorithm computes the least squares solution for 24 | * the homography matrix from at least 4 correspondences. 25 | */ 26 | bool 27 | homography_dlt (Correspondences2D2D const& matches, HomographyMatrix* result); 28 | 29 | /** 30 | * Computes the symmetric transfer error for an image correspondence given the 31 | * homography matrix between two views. 32 | */ 33 | double 34 | symmetric_transfer_error(HomographyMatrix const& homography, 35 | Correspondence2D2D const& match); 36 | 37 | SFM_NAMESPACE_END 38 | 39 | #endif // SFM_HOMOGRAPHY_HEADER 40 | -------------------------------------------------------------------------------- /sfm/pose_p3p.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "math/matrix.h" 14 | #include "math/vector.h" 15 | #include "sfm/pose_p3p.h" 16 | 17 | SFM_NAMESPACE_BEGIN 18 | 19 | namespace 20 | { 21 | void 22 | solve_quartic_roots (math::Vec5d const& factors, math::Vec4d* real_roots) 23 | { 24 | double const A = factors[0]; 25 | double const B = factors[1]; 26 | double const C = factors[2]; 27 | double const D = factors[3]; 28 | double const E = factors[4]; 29 | 30 | double const A2 = A * A; 31 | double const B2 = B * B; 32 | double const A3 = A2 * A; 33 | double const B3 = B2 * B; 34 | double const A4 = A3 * A; 35 | double const B4 = B3 * B; 36 | 37 | double const alpha = -3.0 * B2 / (8.0 * A2) + C / A; 38 | double const beta = B3 / (8.0 * A3)- B * C / (2.0 * A2) + D / A; 39 | double const gamma = -3.0 * B4 / (256.0 * A4) + B2 * C / (16.0 * A3) 40 | - B * D / (4.0 * A2) + E / A; 41 | 42 | double const alpha2 = alpha * alpha; 43 | double const alpha3 = alpha2 * alpha; 44 | double const beta2 = beta * beta; 45 | 46 | std::complex P(-alpha2 / 12.0 - gamma, 0.0); 47 | std::complex Q(-alpha3 / 108.0 48 | + alpha * gamma / 3.0 - beta2 / 8.0, 0.0); 49 | std::complex R = -Q / 2.0 50 | + std::sqrt(Q * Q / 4.0 + P * P * P / 27.0); 51 | 52 | std::complex U = std::pow(R, 1.0 / 3.0); 53 | std::complex y = (U.real() == 0.0) 54 | ? -5.0 * alpha / 6.0 - std::pow(Q, 1.0 / 3.0) 55 | : -5.0 * alpha / 6.0 - P / (3.0 * U) + U; 56 | 57 | std::complex w = std::sqrt(alpha + 2.0 * y); 58 | std::complex part1 = -B / (4.0 * A); 59 | std::complex part2 = 3.0 * alpha + 2.0 * y; 60 | std::complex part3 = 2.0 * beta / w; 61 | 62 | std::complex complex_roots[4]; 63 | complex_roots[0] = part1 + 0.5 * (w + std::sqrt(-(part2 + part3))); 64 | complex_roots[1] = part1 + 0.5 * (w - std::sqrt(-(part2 + part3))); 65 | complex_roots[2] = part1 + 0.5 * (-w + std::sqrt(-(part2 - part3))); 66 | complex_roots[3] = part1 + 0.5 * (-w - std::sqrt(-(part2 - part3))); 67 | 68 | for (int i = 0; i < 4; ++i) 69 | (*real_roots)[i] = complex_roots[i].real(); 70 | } 71 | } 72 | 73 | void 74 | pose_p3p_kneip ( 75 | math::Vec3d p1, math::Vec3d p2, math::Vec3d p3, 76 | math::Vec3d f1, math::Vec3d f2, math::Vec3d f3, 77 | std::vector >* solutions) 78 | { 79 | /* Check if points are co-linear. In this case return no solution. */ 80 | double const colinear_threshold = 1e-10; 81 | if ((p2 - p1).cross(p3 - p1).square_norm() < colinear_threshold) 82 | { 83 | solutions->clear(); 84 | return; 85 | } 86 | 87 | /* Normalize directions if necessary. */ 88 | double const normalize_epsilon = 1e-10; 89 | if (!MATH_EPSILON_EQ(f1.square_norm(), 1.0, normalize_epsilon)) 90 | f1.normalize(); 91 | if (!MATH_EPSILON_EQ(f2.square_norm(), 1.0, normalize_epsilon)) 92 | f2.normalize(); 93 | if (!MATH_EPSILON_EQ(f3.square_norm(), 1.0, normalize_epsilon)) 94 | f3.normalize(); 95 | 96 | /* Create camera frame. */ 97 | math::Matrix3d T; 98 | { 99 | math::Vec3d e1 = f1; 100 | math::Vec3d e3 = f1.cross(f2).normalized(); 101 | math::Vec3d e2 = e3.cross(e1); 102 | std::copy(e1.begin(), e1.end(), T.begin() + 0); 103 | std::copy(e2.begin(), e2.end(), T.begin() + 3); 104 | std::copy(e3.begin(), e3.end(), T.begin() + 6); 105 | f3 = T * f3; 106 | } 107 | 108 | /* Change camera frame and point order if f3[2] > 0. */ 109 | if (f3[2] > 0.0) 110 | { 111 | std::swap(p1, p2); 112 | std::swap(f1, f2); 113 | 114 | math::Vec3d e1 = f1; 115 | math::Vec3d e3 = f1.cross(f2).normalized(); 116 | math::Vec3d e2 = e3.cross(e1); 117 | std::copy(e1.begin(), e1.end(), T.begin() + 0); 118 | std::copy(e2.begin(), e2.end(), T.begin() + 3); 119 | std::copy(e3.begin(), e3.end(), T.begin() + 6); 120 | f3 = T * f3; 121 | } 122 | 123 | /* Create world frame. */ 124 | math::Matrix3d N; 125 | { 126 | math::Vec3d n1 = (p2 - p1).normalized(); 127 | math::Vec3d n3 = n1.cross(p3 - p1).normalized(); 128 | math::Vec3d n2 = n3.cross(n1); 129 | std::copy(n1.begin(), n1.end(), N.begin() + 0); 130 | std::copy(n2.begin(), n2.end(), N.begin() + 3); 131 | std::copy(n3.begin(), n3.end(), N.begin() + 6); 132 | } 133 | p3 = N * (p3 - p1); 134 | 135 | /* Extraction of known parameters. */ 136 | double d_12 = (p2 - p1).norm(); 137 | double f_1 = f3[0] / f3[2]; 138 | double f_2 = f3[1] / f3[2]; 139 | double p_1 = p3[0]; 140 | double p_2 = p3[1]; 141 | 142 | double cos_beta = f1.dot(f2); 143 | double b = 1.0 / (1.0 - MATH_POW2(cos_beta)) - 1; 144 | 145 | if (cos_beta < 0.0) 146 | b = -std::sqrt(b); 147 | else 148 | b = std::sqrt(b); 149 | 150 | /* Temporary pre-computed variables. */ 151 | double f_1_pw2 = MATH_POW2(f_1); 152 | double f_2_pw2 = MATH_POW2(f_2); 153 | double p_1_pw2 = MATH_POW2(p_1); 154 | double p_1_pw3 = p_1_pw2 * p_1; 155 | double p_1_pw4 = p_1_pw3 * p_1; 156 | double p_2_pw2 = MATH_POW2(p_2); 157 | double p_2_pw3 = p_2_pw2 * p_2; 158 | double p_2_pw4 = p_2_pw3 * p_2; 159 | double d_12_pw2 = MATH_POW2(d_12); 160 | double b_pw2 = MATH_POW2(b); 161 | 162 | /* Factors of the 4th degree polynomial. */ 163 | math::Vec5d factors; 164 | factors[0] = - f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4; 165 | 166 | factors[1] = 2.0 * p_2_pw3 * d_12 * b 167 | + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b 168 | - 2.0 * f_2 * p_2_pw3 * f_1 * d_12; 169 | 170 | factors[2] = - f_2_pw2 * p_2_pw2 * p_1_pw2 171 | - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 172 | - f_2_pw2 * p_2_pw2 * d_12_pw2 173 | + f_2_pw2 * p_2_pw4 174 | + p_2_pw4 * f_1_pw2 175 | + 2.0 * p_1 * p_2_pw2 * d_12 176 | + 2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b 177 | - p_2_pw2 * p_1_pw2 * f_1_pw2 178 | + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 179 | - p_2_pw2 * d_12_pw2 * b_pw2 180 | - 2.0 * p_1_pw2 * p_2_pw2; 181 | 182 | factors[3] = 2.0 * p_1_pw2 * p_2 * d_12 * b 183 | + 2.0 * f_2 * p_2_pw3 * f_1 * d_12 184 | - 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b 185 | - 2.0 * p_1 * p_2 * d_12_pw2 * b; 186 | 187 | factors[4] = -2.0 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b 188 | + f_2_pw2 * p_2_pw2 * d_12_pw2 189 | + 2.0 * p_1_pw3 * d_12 190 | - p_1_pw2 * d_12_pw2 191 | + f_2_pw2 * p_2_pw2 * p_1_pw2 192 | - p_1_pw4 193 | - 2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 194 | + p_2_pw2 * f_1_pw2 * p_1_pw2 195 | + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2; 196 | 197 | /* Solve for the roots of the polynomial. */ 198 | math::Vec4d real_roots; 199 | solve_quartic_roots(factors, &real_roots); 200 | 201 | /* Back-substitution of each solution. */ 202 | solutions->clear(); 203 | solutions->resize(4); 204 | for (int i = 0; i < 4; ++i) 205 | { 206 | double cot_alpha = (-f_1 * p_1 / f_2 - real_roots[i] * p_2 + d_12 * b) 207 | / (-f_1 * real_roots[i] * p_2 / f_2 + p_1 - d_12); 208 | 209 | double cos_theta = real_roots[i]; 210 | double sin_theta = std::sqrt(1.0 - MATH_POW2(real_roots[i])); 211 | double sin_alpha = std::sqrt(1.0 / (MATH_POW2(cot_alpha) + 1)); 212 | double cos_alpha = std::sqrt(1.0 - MATH_POW2(sin_alpha)); 213 | 214 | if (cot_alpha < 0.0) 215 | cos_alpha = -cos_alpha; 216 | 217 | math::Vec3d C( 218 | d_12 * cos_alpha * (sin_alpha * b + cos_alpha), 219 | cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha), 220 | sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha)); 221 | 222 | C = p1 + N.transposed() * C; 223 | 224 | math::Matrix3d R; 225 | R[0] = -cos_alpha; R[1] = -sin_alpha * cos_theta; R[2] = -sin_alpha * sin_theta; 226 | R[3] = sin_alpha; R[4] = -cos_alpha * cos_theta; R[5] = -cos_alpha * sin_theta; 227 | R[6] = 0.0; R[7] = -sin_theta; R[8] = cos_theta; 228 | 229 | R = N.transposed().mult(R.transposed()).mult(T); 230 | 231 | /* Convert camera position and cam-to-world rotation to pose. */ 232 | R = R.transposed(); 233 | C = -R * C; 234 | 235 | solutions->at(i) = R.hstack(C); 236 | } 237 | } 238 | 239 | SFM_NAMESPACE_END 240 | -------------------------------------------------------------------------------- /sfm/pose_p3p.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | * 9 | * Kneips original code is available here: 10 | * http://www.laurentkneip.de/research.html 11 | */ 12 | 13 | #ifndef SFM_POSE_P3P_HEADER 14 | #define SFM_POSE_P3P_HEADER 15 | 16 | #include 17 | 18 | #include "math/matrix.h" 19 | #include "sfm/defines.h" 20 | 21 | SFM_NAMESPACE_BEGIN 22 | 23 | /** 24 | * Implementation of the perspective three point (P3P) algorithm. The 25 | * algorithm computes the pose of a camera given three 2D-3D correspondences. 26 | * The implementation closely follows the implementation of Kneip et al. 27 | * and is described in: 28 | * 29 | * "A Novel Parametrization of the Perspective-Three-Point Problem for a 30 | * Direct Computation of Absolute Camera Position and Orientation", 31 | * by Laurent Kneip, Davide Scaramuzza and Roland Siegwart, CVPR 2011. 32 | * http://www.laurentkneip.de/research.html 33 | * 34 | * The algorithm assumes a given camera calibration and takes as input 35 | * three 3D points 'p' and three 2D points. Instead of 2D points, the three 36 | * directions 'f' to the given points computed in the camera frame. Four 37 | * solutions [R | t] are returned. If the points are co-linear, no solution 38 | * is returned. The correct solution can be found by back-projecting a 39 | * forth point in the camera. 40 | */ 41 | void 42 | pose_p3p_kneip ( 43 | math::Vec3d p1, math::Vec3d p2, math::Vec3d p3, 44 | math::Vec3d f1, math::Vec3d f2, math::Vec3d f3, 45 | std::vector >* solutions); 46 | 47 | SFM_NAMESPACE_END 48 | 49 | #endif /* SFM_POSE_P3P_HEADER */ 50 | -------------------------------------------------------------------------------- /sfm/ransac.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | 12 | #include "math/functions.h" 13 | #include "sfm/ransac.h" 14 | 15 | SFM_NAMESPACE_BEGIN 16 | 17 | int 18 | compute_ransac_iterations (double inlier_ratio, 19 | int num_samples, 20 | double desired_success_rate) 21 | { 22 | double prob_all_good = math::fastpow(inlier_ratio, num_samples); 23 | double num_iterations = std::log(1.0 - desired_success_rate) 24 | / std::log(1.0 - prob_all_good); 25 | return static_cast(math::round(num_iterations)); 26 | } 27 | 28 | SFM_NAMESPACE_END 29 | -------------------------------------------------------------------------------- /sfm/ransac.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_RANSAC_HEADER 11 | #define SFM_RANSAC_HEADER 12 | 13 | #include "sfm/defines.h" 14 | 15 | SFM_NAMESPACE_BEGIN 16 | 17 | /** 18 | * The function returns the required number of iterations for a desired 19 | * RANSAC success rate. If w is the probability of choosing one good sample 20 | * (the inlier ratio), then w^n is the probability that all n samples are 21 | * inliers. Then k is the number of iterations required to draw only inliers 22 | * with a certain probability of success, p: 23 | * 24 | * log(1 - p) 25 | * k = ------------ 26 | * log(1 - w^n) 27 | * 28 | * Example: For w = 50%, p = 99%, n = 8: k = log(0.001) / log(0.99609) = 1176. 29 | * Thus, it requires 1176 iterations for RANSAC to succeed with a 99% chance. 30 | */ 31 | int 32 | compute_ransac_iterations (double inlier_ratio, 33 | int num_samples, 34 | double desired_success_rate = 0.99); 35 | 36 | SFM_NAMESPACE_END 37 | 38 | #endif /* SFM_RANSAC_HEADER */ 39 | -------------------------------------------------------------------------------- /sfm/ransac_fundamental.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "util/system.h" 16 | #include "math/algo.h" 17 | #include "sfm/ransac_fundamental.h" 18 | 19 | SFM_NAMESPACE_BEGIN 20 | 21 | RansacFundamental::RansacFundamental (Options const& options) 22 | : opts(options) 23 | { 24 | } 25 | 26 | void 27 | RansacFundamental::estimate (Correspondences2D2D const& matches, Result* result) 28 | { 29 | if (this->opts.verbose_output) 30 | { 31 | std::cout << "RANSAC-F: Running for " << this->opts.max_iterations 32 | << " iterations, threshold " << this->opts.threshold 33 | << "..." << std::endl; 34 | } 35 | 36 | std::vector inliers; 37 | inliers.reserve(matches.size()); 38 | for (int iteration = 0; iteration < this->opts.max_iterations; ++iteration) 39 | { 40 | FundamentalMatrix fundamental; 41 | this->estimate_8_point(matches, &fundamental); 42 | this->find_inliers(matches, fundamental, &inliers); 43 | if (inliers.size() > result->inliers.size()) 44 | { 45 | if (this->opts.verbose_output) 46 | { 47 | std::cout << "RANSAC-F: Iteration " << iteration 48 | << ", inliers " << inliers.size() << " (" 49 | << (100.0 * inliers.size() / matches.size()) 50 | << "%)" << std::endl; 51 | } 52 | 53 | result->fundamental = fundamental; 54 | std::swap(result->inliers, inliers); 55 | inliers.reserve(matches.size()); 56 | } 57 | } 58 | } 59 | 60 | void 61 | RansacFundamental::estimate_8_point (Correspondences2D2D const& matches, 62 | FundamentalMatrix* fundamental) 63 | { 64 | if (matches.size() < 8) 65 | throw std::invalid_argument("At least 8 matches required"); 66 | 67 | /* 68 | * Draw 8 random numbers in the interval [0, matches.size() - 1] 69 | * without duplicates. This is done by keeping a set with drawn numbers. 70 | */ 71 | std::set result; 72 | while (result.size() < 8) 73 | result.insert(util::system::rand_int() % matches.size()); 74 | 75 | math::Matrix pset1, pset2; 76 | std::set::const_iterator iter = result.begin(); 77 | for (int i = 0; i < 8; ++i, ++iter) 78 | { 79 | Correspondence2D2D const& match = matches[*iter]; 80 | pset1(0, i) = match.p1[0]; 81 | pset1(1, i) = match.p1[1]; 82 | pset1(2, i) = 1.0; 83 | pset2(0, i) = match.p2[0]; 84 | pset2(1, i) = match.p2[1]; 85 | pset2(2, i) = 1.0; 86 | } 87 | 88 | /* Compute fundamental matrix using normalized 8-point. */ 89 | sfm::fundamental_8_point(pset1, pset2, fundamental); 90 | sfm::enforce_fundamental_constraints(fundamental); 91 | } 92 | 93 | void 94 | RansacFundamental::find_inliers (Correspondences2D2D const& matches, 95 | FundamentalMatrix const& fundamental, std::vector* result) 96 | { 97 | result->resize(0); 98 | double const squared_thres = this->opts.threshold * this->opts.threshold; 99 | for (std::size_t i = 0; i < matches.size(); ++i) 100 | { 101 | double error = sampson_distance(fundamental, matches[i]); 102 | if (error < squared_thres) 103 | result->push_back(i); 104 | } 105 | } 106 | 107 | SFM_NAMESPACE_END 108 | -------------------------------------------------------------------------------- /sfm/ransac_fundamental.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_RANSAC_FUNDAMENTAL_HEADER 11 | #define SFM_RANSAC_FUNDAMENTAL_HEADER 12 | 13 | #include "math/matrix.h" 14 | #include "sfm/defines.h" 15 | #include "sfm/correspondence.h" 16 | #include "sfm/fundamental.h" 17 | 18 | SFM_NAMESPACE_BEGIN 19 | 20 | /** 21 | * RANSAC pose estimation from noisy 2D-2D image correspondences. 22 | * 23 | * The fundamental matrix for two views is to be determined from a set 24 | * of image correspondences contaminated with outliers. The algorithm 25 | * randomly selects N image correspondences (where N depends on the pose 26 | * algorithm) to estimate a fundamental matrix. Running for a number of 27 | * iterations, the fundamental matrix supporting the most matches is 28 | * returned as result. 29 | */ 30 | class RansacFundamental 31 | { 32 | public: 33 | struct Options 34 | { 35 | Options (void); 36 | 37 | /** 38 | * The number of RANSAC iterations. Defaults to 1000. 39 | * Function compute_ransac_iterations() can be used to estimate the 40 | * required number of iterations for a certain RANSAC success rate. 41 | */ 42 | int max_iterations; 43 | 44 | /** 45 | * Threshold used to determine inliers. Defaults to 0.0015. 46 | * This threshold assumes that the input points are normalized. 47 | */ 48 | double threshold; 49 | 50 | /** 51 | * Produce status messages on the console. 52 | */ 53 | bool verbose_output; 54 | }; 55 | 56 | struct Result 57 | { 58 | /** 59 | * The resulting fundamental matrix which led to the inliers. 60 | * This is NOT the re-computed matrix from the inliers. 61 | */ 62 | FundamentalMatrix fundamental; 63 | 64 | /** 65 | * The indices of inliers in the correspondences which led to the 66 | * homography matrix. 67 | */ 68 | std::vector inliers; 69 | }; 70 | 71 | public: 72 | explicit RansacFundamental (Options const& options); 73 | void estimate (Correspondences2D2D const& matches, Result* result); 74 | 75 | private: 76 | void estimate_8_point (Correspondences2D2D const& matches, 77 | FundamentalMatrix* fundamental); 78 | void find_inliers (Correspondences2D2D const& matches, 79 | FundamentalMatrix const& fundamental, std::vector* result); 80 | 81 | private: 82 | Options opts; 83 | }; 84 | 85 | /* ------------------------ Implementation ------------------------ */ 86 | 87 | inline 88 | RansacFundamental::Options::Options (void) 89 | : max_iterations(1000) 90 | , threshold(0.0015) 91 | , verbose_output(false) 92 | { 93 | } 94 | 95 | SFM_NAMESPACE_END 96 | 97 | #endif /* SFM_RANSAC_FUNDAMENTAL_HEADER */ 98 | -------------------------------------------------------------------------------- /sfm/ransac_homography.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "util/system.h" 16 | #include "math/algo.h" 17 | #include "math/matrix_tools.h" 18 | #include "sfm/ransac_homography.h" 19 | 20 | SFM_NAMESPACE_BEGIN 21 | 22 | RansacHomography::RansacHomography (Options const& options) 23 | : opts(options) 24 | { 25 | } 26 | 27 | void 28 | RansacHomography::estimate (Correspondences2D2D const& matches, Result* result) 29 | { 30 | if (this->opts.verbose_output) 31 | { 32 | std::cout << "RANSAC-H: Running for " << this->opts.max_iterations 33 | << " iterations, threshold " << this->opts.threshold 34 | << "..." << std::endl; 35 | } 36 | 37 | std::vector inliers; 38 | inliers.reserve(matches.size()); 39 | for (int iteration = 0; iteration < this->opts.max_iterations; ++iteration) 40 | { 41 | HomographyMatrix homography; 42 | this->compute_homography(matches, &homography); 43 | this->evaluate_homography(matches, homography, &inliers); 44 | if (inliers.size() > result->inliers.size()) 45 | { 46 | if (this->opts.verbose_output) 47 | { 48 | std::cout << "RANSAC-H: Iteration " << iteration 49 | << ", inliers " << inliers.size() << " (" 50 | << (100.0 * inliers.size() / matches.size()) 51 | << "%)" << std::endl; 52 | } 53 | 54 | result->homography = homography; 55 | std::swap(result->inliers, inliers); 56 | inliers.reserve(matches.size()); 57 | } 58 | } 59 | } 60 | 61 | void 62 | RansacHomography::compute_homography (Correspondences2D2D const& matches, 63 | HomographyMatrix* homography) 64 | { 65 | if (matches.size() < 4) 66 | throw std::invalid_argument("At least 4 matches required"); 67 | 68 | /* 69 | * Draw 4 random numbers in the interval [0, matches.size() - 1] 70 | * without duplicates. This is done by keeping a set with drawn numbers. 71 | */ 72 | std::set result; 73 | while (result.size() < 4) 74 | result.insert(util::system::rand_int() % matches.size()); 75 | 76 | Correspondences2D2D four_correspondeces(4); 77 | std::set::const_iterator iter = result.begin(); 78 | for (std::size_t i = 0; i < 4; ++i, ++iter) 79 | four_correspondeces[i] = matches[*iter]; 80 | 81 | sfm::homography_dlt(four_correspondeces, homography); 82 | *homography /= (*homography)[8]; 83 | } 84 | 85 | void 86 | RansacHomography::evaluate_homography (Correspondences2D2D const& matches, 87 | HomographyMatrix const& homography, std::vector* inliers) 88 | { 89 | double const square_threshold = MATH_POW2(this->opts.threshold); 90 | inliers->resize(0); 91 | for (std::size_t i = 0; i < matches.size(); ++i) 92 | { 93 | Correspondence2D2D const& match = matches[i]; 94 | double error = sfm::symmetric_transfer_error(homography, match); 95 | if (error < square_threshold) 96 | inliers->push_back(i); 97 | } 98 | } 99 | 100 | SFM_NAMESPACE_END 101 | -------------------------------------------------------------------------------- /sfm/ransac_homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_RANSAC_HOMOGRAPHY_HEADER 11 | #define SFM_RANSAC_HOMOGRAPHY_HEADER 12 | 13 | #include 14 | 15 | #include "sfm/homography.h" 16 | 17 | SFM_NAMESPACE_BEGIN 18 | 19 | /** 20 | * RANSAC homography estimation from noisy 2D-2D image correspondences. 21 | * 22 | * The homography matrix for two views is to be determined from a set of image 23 | * correspondences contaminated with outliers. The algorithm randomly selects 4 24 | * image correspondences to estimate a homography matrix. Running for a number 25 | * of iterations, the homography matrix supporting the most matches returned. 26 | */ 27 | class RansacHomography 28 | { 29 | public: 30 | struct Options 31 | { 32 | Options (void); 33 | 34 | /** 35 | * The number of RANSAC iterations. Defaults to 1000. 36 | * Function compute_ransac_iterations() can be used to estimate the 37 | * required number of iterations for a certain RANSAC success rate. 38 | */ 39 | int max_iterations; 40 | 41 | /** 42 | * Threshold used to determine inliers. Defaults to 0.005. 43 | * This threshold assumes that the input points are normalized. 44 | */ 45 | double threshold; 46 | 47 | /** 48 | * Produce status messages on the console. 49 | */ 50 | bool verbose_output; 51 | }; 52 | 53 | struct Result 54 | { 55 | /** 56 | * The resulting homography matrix which led to the inliers. 57 | * This is NOT the re-computed matrix from the inliers. 58 | */ 59 | HomographyMatrix homography; 60 | 61 | /** 62 | * The indices of inliers in the correspondences which led to the 63 | * homography matrix. 64 | */ 65 | std::vector inliers; 66 | }; 67 | 68 | public: 69 | explicit RansacHomography (Options const& options); 70 | void estimate (Correspondences2D2D const& matches, Result* result); 71 | 72 | private: 73 | void compute_homography (Correspondences2D2D const& matches, 74 | HomographyMatrix* homography); 75 | void evaluate_homography (Correspondences2D2D const& matches, 76 | HomographyMatrix const& homography, std::vector* inliers); 77 | 78 | private: 79 | Options opts; 80 | }; 81 | 82 | /* ------------------------ Implementation ------------------------ */ 83 | 84 | inline 85 | RansacHomography::Options::Options (void) 86 | : max_iterations(1000) 87 | , threshold(0.005) 88 | , verbose_output(false) 89 | { 90 | } 91 | 92 | SFM_NAMESPACE_END 93 | 94 | #endif /* SFM_RANSAC_HOMOGRAPHY_HEADER */ 95 | -------------------------------------------------------------------------------- /sfm/ransac_pose_p3p.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "util/system.h" 15 | #include "math/matrix_tools.h" 16 | #include "sfm/ransac_pose_p3p.h" 17 | #include "sfm/pose_p3p.h" 18 | 19 | SFM_NAMESPACE_BEGIN 20 | 21 | RansacPoseP3P::RansacPoseP3P (Options const& options) 22 | : opts(options) 23 | { 24 | } 25 | 26 | void 27 | RansacPoseP3P::estimate (Correspondences2D3D const& corresp, 28 | math::Matrix const& k_matrix, Result* result) const 29 | { 30 | if (this->opts.verbose_output) 31 | { 32 | std::cout << "RANSAC-3: Running for " << this->opts.max_iterations 33 | << " iterations, threshold " << this->opts.threshold 34 | << "..." << std::endl; 35 | } 36 | 37 | /* Pre-compute inverse K matrix to compute directions from corresp. */ 38 | math::Matrix inv_k_matrix = math::matrix_inverse(k_matrix); 39 | std::atomic num_iterations; 40 | 41 | #pragma omp parallel 42 | { 43 | std::vector inliers; 44 | inliers.reserve(corresp.size()); 45 | #pragma omp for 46 | for (int i = 0; i < this->opts.max_iterations; ++i) 47 | { 48 | int iteration = i; 49 | if (this->opts.verbose_output) 50 | iteration = num_iterations++; 51 | 52 | /* Compute up to four poses [R|t] using P3P algorithm. */ 53 | PutativePoses poses; 54 | this->compute_p3p(corresp, inv_k_matrix, &poses); 55 | 56 | /* Check all putative solutions and count inliers. */ 57 | for (std::size_t j = 0; j < poses.size(); ++j) 58 | { 59 | this->find_inliers(corresp, k_matrix, poses[j], &inliers); 60 | #pragma omp critical 61 | if (inliers.size() > result->inliers.size()) 62 | { 63 | result->pose = poses[j]; 64 | std::swap(result->inliers, inliers); 65 | inliers.reserve(corresp.size()); 66 | 67 | if (this->opts.verbose_output) 68 | { 69 | std::cout << "RANSAC-3: Iteration " << iteration 70 | << ", inliers " << result->inliers.size() << " (" 71 | << (100.0 * result->inliers.size() / corresp.size()) 72 | << "%)" << std::endl; 73 | } 74 | } 75 | } 76 | } 77 | } 78 | } 79 | 80 | void 81 | RansacPoseP3P::compute_p3p (Correspondences2D3D const& corresp, 82 | math::Matrix const& inv_k_matrix, 83 | PutativePoses* poses) const 84 | { 85 | if (corresp.size() < 3) 86 | throw std::invalid_argument("At least 3 correspondences required"); 87 | 88 | /* Draw 3 unique random numbers. */ 89 | std::set result; 90 | while (result.size() < 3) 91 | result.insert(util::system::rand_int() % corresp.size()); 92 | 93 | std::set::const_iterator iter = result.begin(); 94 | Correspondence2D3D const& c1(corresp[*iter++]); 95 | Correspondence2D3D const& c2(corresp[*iter++]); 96 | Correspondence2D3D const& c3(corresp[*iter]); 97 | pose_p3p_kneip( 98 | math::Vec3d(c1.p3d), math::Vec3d(c2.p3d), math::Vec3d(c3.p3d), 99 | inv_k_matrix.mult(math::Vec3d(c1.p2d[0], c1.p2d[1], 1.0)), 100 | inv_k_matrix.mult(math::Vec3d(c2.p2d[0], c2.p2d[1], 1.0)), 101 | inv_k_matrix.mult(math::Vec3d(c3.p2d[0], c3.p2d[1], 1.0)), 102 | poses); 103 | } 104 | 105 | void 106 | RansacPoseP3P::find_inliers (Correspondences2D3D const& corresp, 107 | math::Matrix const& k_matrix, 108 | Pose const& pose, std::vector* inliers) const 109 | { 110 | inliers->resize(0); 111 | double const square_threshold = MATH_POW2(this->opts.threshold); 112 | for (std::size_t i = 0; i < corresp.size(); ++i) 113 | { 114 | Correspondence2D3D const& c = corresp[i]; 115 | math::Vec4d p3d(c.p3d[0], c.p3d[1], c.p3d[2], 1.0); 116 | math::Vec3d p2d = k_matrix * (pose * p3d); 117 | double square_error = MATH_POW2(p2d[0] / p2d[2] - c.p2d[0]) 118 | + MATH_POW2(p2d[1] / p2d[2] - c.p2d[1]); 119 | if (square_error < square_threshold) 120 | inliers->push_back(i); 121 | } 122 | } 123 | 124 | SFM_NAMESPACE_END 125 | -------------------------------------------------------------------------------- /sfm/ransac_pose_p3p.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_RANSAC_POSE_P3P_HEADER 11 | #define SFM_RANSAC_POSE_P3P_HEADER 12 | 13 | #include 14 | 15 | #include "math/matrix.h" 16 | #include "math/vector.h" 17 | #include "sfm/correspondence.h" 18 | #include "sfm/defines.h" 19 | 20 | SFM_NAMESPACE_BEGIN 21 | 22 | /** 23 | * RANSAC pose estimation from 2D-3D correspondences and known camera 24 | * calibration using the perspective 3-point (P3P) algorithm. 25 | * 26 | * The rotation and translation of a camera is determined from a set of 27 | * 2D image to 3D point correspondences contaminated with outliers. The 28 | * algorithm iteratively selects 3 random correspondences and returns the 29 | * result which led to the most inliers. 30 | * 31 | * The input 2D image coordinates, the input K-matrix and the threshold 32 | * in the options must be consistent. For example, if the 2D image coordinates 33 | * are NOT normalized but in pixel coordinates, the K-matrix must be in pixel 34 | * coordinates and the threshold must be in pixel, too. For this algorithm, 35 | * image coordinates do not need to be normalized for stability. 36 | */ 37 | class RansacPoseP3P 38 | { 39 | public: 40 | struct Options 41 | { 42 | Options (void); 43 | 44 | /** 45 | * The number of RANSAC iterations. Defaults to 1000. 46 | * Function compute_ransac_iterations() can be used to estimate the 47 | * required number of iterations for a certain RANSAC success rate. 48 | */ 49 | int max_iterations; 50 | 51 | /** 52 | * Threshold used to determine inliers. Defaults to 0.005. 53 | * This threshold assumes that the input points are normalized. 54 | */ 55 | double threshold; 56 | 57 | /** 58 | * Produce status messages on the console. 59 | */ 60 | bool verbose_output; 61 | }; 62 | 63 | struct Result 64 | { 65 | /** The pose [R|t] which led to the inliers. */ 66 | math::Matrix pose; 67 | 68 | /** The correspondence indices which led to the result. */ 69 | std::vector inliers; 70 | }; 71 | 72 | public: 73 | explicit RansacPoseP3P (Options const& options); 74 | 75 | void estimate (Correspondences2D3D const& corresp, 76 | math::Matrix const& k_matrix, 77 | Result* result) const; 78 | 79 | private: 80 | typedef math::Matrix Pose; 81 | typedef std::vector PutativePoses; 82 | 83 | private: 84 | void compute_p3p (Correspondences2D3D const& corresp, 85 | math::Matrix const& inv_k_matrix, 86 | PutativePoses* poses) const; 87 | 88 | void find_inliers (Correspondences2D3D const& corresp, 89 | math::Matrix const& k_matrix, 90 | Pose const& pose, std::vector* inliers) const; 91 | 92 | private: 93 | Options opts; 94 | }; 95 | 96 | /* ------------------------ Implementation ------------------------ */ 97 | 98 | inline 99 | RansacPoseP3P::Options::Options (void) 100 | : max_iterations(1000) 101 | , threshold(0.005) 102 | , verbose_output(false) 103 | { 104 | } 105 | 106 | SFM_NAMESPACE_END 107 | 108 | #endif /* SFM_RANSAC_POSE_P3P_HEADER */ 109 | -------------------------------------------------------------------------------- /sfm/triangulate.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #include 11 | 12 | #include "math/matrix_svd.h" 13 | #include "sfm/triangulate.h" 14 | 15 | SFM_NAMESPACE_BEGIN 16 | 17 | /* ---------------- Low-level triangulation solver ---------------- */ 18 | 19 | math::Vector 20 | triangulate_match (Correspondence2D2D const& match, 21 | CameraPose const& pose1, CameraPose const& pose2) 22 | { 23 | /* The algorithm is described in HZ 12.2, page 312. */ 24 | math::Matrix P1, P2; 25 | pose1.fill_p_matrix(&P1); 26 | pose2.fill_p_matrix(&P2); 27 | 28 | math::Matrix A; 29 | for (int i = 0; i < 4; ++i) 30 | { 31 | A(0, i) = match.p1[0] * P1(2, i) - P1(0, i); 32 | A(1, i) = match.p1[1] * P1(2, i) - P1(1, i); 33 | A(2, i) = match.p2[0] * P2(2, i) - P2(0, i); 34 | A(3, i) = match.p2[1] * P2(2, i) - P2(1, i); 35 | } 36 | 37 | math::Matrix V; 38 | math::matrix_svd(A, nullptr, nullptr, &V); 39 | math::Vector x = V.col(3); 40 | return math::Vector(x[0] / x[3], x[1] / x[3], x[2] / x[3]); 41 | } 42 | 43 | math::Vector 44 | triangulate_track (std::vector const& pos, 45 | std::vector const& poses) 46 | { 47 | if (pos.size() != poses.size() || pos.size() < 2) 48 | throw std::invalid_argument("Invalid number of positions/poses"); 49 | 50 | std::vector A(4 * 2 * poses.size(), 0.0); 51 | for (std::size_t i = 0; i < poses.size(); ++i) 52 | { 53 | CameraPose const& pose = *poses[i]; 54 | math::Vec2d p = pos[i]; 55 | math::Matrix p_mat; 56 | pose.fill_p_matrix(&p_mat); 57 | 58 | for (int j = 0; j < 4; ++j) 59 | { 60 | A[(2 * i + 0) * 4 + j] = p[0] * p_mat(2, j) - p_mat(0, j); 61 | A[(2 * i + 1) * 4 + j] = p[1] * p_mat(2, j) - p_mat(1, j); 62 | } 63 | } 64 | 65 | /* Compute SVD. */ 66 | math::Matrix mat_v; 67 | math::matrix_svd(&A[0], 2 * poses.size(), 4, 68 | nullptr, nullptr, mat_v.begin()); 69 | 70 | /* Consider the last column of V and extract 3D point. */ 71 | math::Vector x = mat_v.col(3); 72 | return math::Vector(x[0] / x[3], x[1] / x[3], x[2] / x[3]); 73 | } 74 | 75 | bool 76 | is_consistent_pose (Correspondence2D2D const& match, 77 | CameraPose const& pose1, CameraPose const& pose2) 78 | { 79 | math::Vector x = triangulate_match(match, pose1, pose2); 80 | math::Vector x1 = pose1.R * x + pose1.t; 81 | math::Vector x2 = pose2.R * x + pose2.t; 82 | return x1[2] > 0.0f && x2[2] > 0.0f; 83 | } 84 | 85 | /* --------------- Higher-level triangulation class --------------- */ 86 | 87 | bool 88 | Triangulate::triangulate (std::vector const& poses, 89 | std::vector const& positions, 90 | math::Vec3d* track_pos, Statistics* stats, 91 | std::vector* outliers) const 92 | { 93 | if (poses.size() < 2) 94 | throw std::invalid_argument("At least two poses required"); 95 | if (poses.size() != positions.size()) 96 | throw std::invalid_argument("Poses and positions size mismatch"); 97 | 98 | /* Check all possible pose pairs for successful triangulation */ 99 | std::vector best_outliers(positions.size()); 100 | math::Vec3f best_pos(0.0f); 101 | for (std::size_t p1 = 0; p1 < poses.size(); ++p1) 102 | for (std::size_t p2 = p1 + 1; p2 < poses.size(); ++p2) 103 | { 104 | /* Triangulate position from current pair */ 105 | std::vector pose_pair; 106 | std::vector position_pair; 107 | pose_pair.push_back(poses[p1]); 108 | pose_pair.push_back(poses[p2]); 109 | position_pair.push_back(positions[p1]); 110 | position_pair.push_back(positions[p2]); 111 | math::Vec3d tmp_pos = triangulate_track(position_pair, pose_pair); 112 | if (MATH_ISNAN(tmp_pos[0]) || MATH_ISINF(tmp_pos[0]) || 113 | MATH_ISNAN(tmp_pos[1]) || MATH_ISINF(tmp_pos[1]) || 114 | MATH_ISNAN(tmp_pos[2]) || MATH_ISINF(tmp_pos[2])) 115 | continue; 116 | 117 | /* Check if pair has small triangulation angle. */ 118 | if (this->opts.angle_threshold > 0.0) 119 | { 120 | math::Vec3d camera_pos; 121 | pose_pair[0]->fill_camera_pos(&camera_pos); 122 | math::Vec3d ray0 = (tmp_pos - camera_pos).normalized(); 123 | pose_pair[1]->fill_camera_pos(&camera_pos); 124 | math::Vec3d ray1 = (tmp_pos - camera_pos).normalized(); 125 | double const cos_angle = ray0.dot(ray1); 126 | if (cos_angle > this->cos_angle_thres) 127 | continue; 128 | } 129 | 130 | /* Chek error in all input poses and find outliers. */ 131 | std::vector tmp_outliers; 132 | for (std::size_t i = 0; i < poses.size(); ++i) 133 | { 134 | math::Vec3d x = poses[i]->R * tmp_pos + poses[i]->t; 135 | 136 | /* Reject track if it appears behind the camera. */ 137 | if (x[2] <= 0.0) 138 | { 139 | tmp_outliers.push_back(i); 140 | continue; 141 | } 142 | 143 | x = poses[i]->K * x; 144 | math::Vec2d x2d(x[0] / x[2], x[1] / x[2]); 145 | double error = (positions[i] - x2d).norm(); 146 | if (error > this->opts.error_threshold) 147 | tmp_outliers.push_back(i); 148 | } 149 | 150 | /* Select triangulation with lowest amount of outliers. */ 151 | if (tmp_outliers.size() < best_outliers.size()) 152 | { 153 | best_pos = tmp_pos; 154 | std::swap(best_outliers, tmp_outliers); 155 | } 156 | 157 | } 158 | 159 | /* If all pairs have small angles pos will be 0 here. */ 160 | if (best_pos.norm() == 0.0f) 161 | { 162 | if (stats != nullptr) 163 | stats->num_too_small_angle += 1; 164 | return false; 165 | } 166 | 167 | /* Check if required number of inliers is found. */ 168 | if (poses.size() < best_outliers.size() + this->opts.min_num_views) 169 | { 170 | if (stats != nullptr) 171 | stats->num_large_error += 1; 172 | return false; 173 | } 174 | 175 | /* Return final position and outliers. */ 176 | *track_pos = best_pos; 177 | if (stats != nullptr) 178 | stats->num_new_tracks += 1; 179 | if (outliers != nullptr) 180 | std::swap(*outliers, best_outliers); 181 | 182 | return true; 183 | } 184 | 185 | void 186 | Triangulate::print_statistics (Statistics const& stats, std::ostream& out) const 187 | { 188 | int const num_rejected = stats.num_large_error 189 | + stats.num_behind_camera 190 | + stats.num_too_small_angle; 191 | 192 | out << "Triangulated " << stats.num_new_tracks 193 | << " new tracks, rejected " << num_rejected 194 | << " bad tracks." << std::endl; 195 | if (stats.num_large_error > 0) 196 | out << " Rejected " << stats.num_large_error 197 | << " tracks with large error." << std::endl; 198 | if (stats.num_behind_camera > 0) 199 | out << " Rejected " << stats.num_behind_camera 200 | << " tracks behind cameras." << std::endl; 201 | if (stats.num_too_small_angle > 0) 202 | out << " Rejected " << stats.num_too_small_angle 203 | << " tracks with unstable angle." << std::endl; 204 | } 205 | 206 | SFM_NAMESPACE_END 207 | -------------------------------------------------------------------------------- /sfm/triangulate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015, Simon Fuhrmann 3 | * TU Darmstadt - Graphics, Capture and Massively Parallel Computing 4 | * All rights reserved. 5 | * 6 | * This software may be modified and distributed under the terms 7 | * of the BSD 3-Clause license. See the LICENSE.txt file for details. 8 | */ 9 | 10 | #ifndef SFM_TRIANGULATE_HEADER 11 | #define SFM_TRIANGULATE_HEADER 12 | 13 | #include 14 | #include 15 | 16 | #include "math/vector.h" 17 | #include "sfm/correspondence.h" 18 | #include "sfm/camera_pose.h" 19 | #include "sfm/defines.h" 20 | 21 | SFM_NAMESPACE_BEGIN 22 | 23 | /* ---------------- Low-level triangulation solver ---------------- */ 24 | 25 | /** 26 | * Given an image correspondence in two views and the corresponding poses, 27 | * this function triangulates the 3D point coordinate using the DLT algorithm. 28 | */ 29 | math::Vector 30 | triangulate_match (Correspondence2D2D const& match, 31 | CameraPose const& pose1, CameraPose const& pose2); 32 | 33 | /** 34 | * Given any number of 2D image positions and the corresponding camera poses, 35 | * this function triangulates the 3D point coordinate using the DLT algorithm. 36 | */ 37 | math::Vector 38 | triangulate_track (std::vector const& pos, 39 | std::vector const& poses); 40 | 41 | /** 42 | * Given a two-view pose configuration and a correspondence, this function 43 | * returns true if the triangulated point is in front of both cameras. 44 | */ 45 | bool 46 | is_consistent_pose (Correspondence2D2D const& match, 47 | CameraPose const& pose1, CameraPose const& pose2); 48 | 49 | /* --------------- Higher-level triangulation class --------------- */ 50 | 51 | /** 52 | * Triangulation routine that triangulates a track from camera poses and 53 | * 2D image positions while keeping triangulation statistics. In contrast 54 | * to the low-level functions, this implementation checks for triangulation 55 | * problems such as large reprojection error, tracks appearing behind the 56 | * camera, and unstable triangulation angles. 57 | */ 58 | class Triangulate 59 | { 60 | public: 61 | struct Options 62 | { 63 | Options (void); 64 | 65 | /** Threshold on reprojection error for outlier detection. */ 66 | double error_threshold; 67 | /** Threshold on the triangulation angle (in radians). */ 68 | double angle_threshold; 69 | /** Minimal number of views with small error (inliers). */ 70 | int min_num_views; 71 | }; 72 | 73 | struct Statistics 74 | { 75 | Statistics (void); 76 | 77 | /** The number of successfully triangulated tracks. */ 78 | int num_new_tracks; 79 | /** Number of tracks with too large reprojection error. */ 80 | int num_large_error; 81 | /** Number of tracks that appeared behind the camera. */ 82 | int num_behind_camera; 83 | /** Number of tracks with too small triangulation angle. */ 84 | int num_too_small_angle; 85 | }; 86 | 87 | public: 88 | explicit Triangulate (Options const& options); 89 | bool triangulate (std::vector const& poses, 90 | std::vector const& positions, 91 | math::Vec3d* track_pos, Statistics* stats = nullptr, 92 | std::vector* outliers = nullptr) const; 93 | void print_statistics (Statistics const& stats, std::ostream& out) const; 94 | 95 | private: 96 | Options const opts; 97 | double const cos_angle_thres; 98 | }; 99 | 100 | /* ------------------------ Implementation ------------------------ */ 101 | 102 | inline 103 | Triangulate::Options::Options (void) 104 | : error_threshold(0.01) 105 | , angle_threshold(MATH_DEG2RAD(1.0)) 106 | , min_num_views(2) 107 | { 108 | } 109 | 110 | inline 111 | Triangulate::Statistics::Statistics (void) 112 | : num_new_tracks(0) 113 | , num_large_error(0) 114 | , num_behind_camera(0) 115 | , num_too_small_angle(0) 116 | { 117 | } 118 | 119 | inline 120 | Triangulate::Triangulate (Options const& options) 121 | : opts(options) 122 | , cos_angle_thres(std::cos(options.angle_threshold)) 123 | { 124 | } 125 | 126 | SFM_NAMESPACE_END 127 | 128 | #endif // SFM_TRIANGULATE_HEADER 129 | --------------------------------------------------------------------------------