1 2 // Copyright (c) 2010 libmv authors. 3 // 4 // Permission is hereby granted, free of charge, to any person obtaining a copy 5 // of this software and associated documentation files (the "Software"), to 6 // deal in the Software without restriction, including without limitation the 7 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 8 // sell copies of the Software, and to permit persons to whom the Software is 9 // furnished to do so, subject to the following conditions: 10 // 11 // The above copyright notice and this permission notice shall be included in 12 // all copies or substantial portions of the Software. 13 // 14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 20 // IN THE SOFTWARE. 21 22 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library. 23 24 // Copyright (c) 2012, 2013 Pierre MOULON. 25 26 // This Source Code Form is subject to the terms of the Mozilla Public 27 // License, v. 2.0. If a copy of the MPL was not distributed with this 28 // file, You can obtain one at http://mozilla.org/MPL/2.0/. 29 30 // 31 // 32 // Five point relative pose computation using Groebner basis. 33 // We follow the algorithm of [1] and apply some optimization hints of [2]. 34 // 35 // [1] H. Stewénius, C. Engels and D. Nistér, "Recent Developments on Direct 36 // Relative Orientation", ISPRS 2006 37 // 38 // [2] D. Nistér, "An Efficient Solution to the Five-Point Relative Pose", 39 // PAMI 2004 40 41 42 #ifndef OPENMVG_MULTIVIEW_SOLVER_ESSENTIAL_FIVE_POINT_HPP 43 #define OPENMVG_MULTIVIEW_SOLVER_ESSENTIAL_FIVE_POINT_HPP 44 45 #include "openMVG/numeric/eigen_alias_definition.hpp" 46 47 namespace openMVG { 48 49 /** 50 * @brief Computes the relative pose of two calibrated cameras from 5 correspondences. 51 * 52 * \param x1 Corresponding bearing vectors in the first image. One per column. 53 * \param x2 Corresponding bearing vectors in the second image. One per column. 54 * \param E A list of at most 10 candidate essential matrix solutions. 55 */ 56 void FivePointsRelativePose( const Mat3X &x1, const Mat3X &x2, 57 std::vector<Mat3> *E ); 58 59 /** 60 * @brief Compute the nullspace of the linear constraints given by the matches. 61 * @param x1 Corresponding bearing vectors in first camera 62 * @param x2 Corresponding bearing vectors in second camera 63 * @return Nullspace that maps x1 points to x2 points 64 */ 65 Mat FivePointsNullspaceBasis( const Mat3X &x1, const Mat3X &x2 ); 66 67 /** 68 * @brief Multiply two polynomials of degree 1. 69 * @param a Polynomial a1 + a2 x + a3 y + a4 z 70 * @param b Polynomial b1 + b2 x + b3 y + b4 z 71 * @return Product of a and b : 72 * res = a1 b1 + 73 (a1b2 + b1a2) x + 74 (a1b3 + b1a3) y + 75 (a1b4 + b1a4) z + 76 (a2b3 + b2a3) xy + 77 (a2b4 + b2a4) xz + 78 (a3b4 + b3a4) yz + 79 a2b2 x^2 + 80 a3b3 y^2 + 81 a4b4 z^2 82 * @note Ordering is defined as follow: 83 * [xxx xxy xyy yyy xxz xyz yyz xzz yzz zzz xx xy yy xz yz zz x y z 1] 84 */ 85 Vec o1( const Vec &a, const Vec &b ); 86 87 /** 88 * @brief Multiply two polynomials of degree 2 89 * @param a Polynomial a1 + a2 x + a3 y + a4 z + a5 x^2 + a6 y^2 + a7 z^2 90 * @param b Polynomial b1 + b2 x + b3 y + b4 z + b5 x^2 + b6 y^2 + b7 z^2 91 * Product of a and b 92 * @note Ordering is defined as follow : 93 * [xxx xxy xyy yyy xxz xyz yyz xzz yzz zzz xx xy yy xz yz zz x y z 1] 94 */ 95 Vec o2( const Vec &a, const Vec &b ); 96 97 /** 98 * Builds the polynomial constraint matrix M. 99 * @param E_basis Basis essential matrix 100 * @return polynomial constraint associated to the essential matrix 101 */ 102 Mat FivePointsPolynomialConstraints( const Mat &E_basis ); 103 104 // In the following code, polynomials are expressed as vectors containing 105 // their coeficients in the basis of monomials: 106 // 107 // [xxx xxy xyy yyy xxz xyz yyz xzz yzz zzz xx xy yy xz yz zz x y z 1] 108 // 109 // Note that there is an error in Stewenius' paper. In equation (9) they 110 // propose to use the basis: 111 // 112 // [xxx xxy xxz xyy xyz xzz yyy yyz yzz zzz xx xy xz yy yz zz x y z 1] 113 // 114 // But this is not the basis used in the rest of the paper, neither in 115 // the code they provide. I (pau) have spend 4 hours debugging and 116 // reverse engineering their code to find the problem. :( 117 enum 118 { 119 coef_xxx, 120 coef_xxy, 121 coef_xyy, 122 coef_yyy, 123 coef_xxz, 124 coef_xyz, 125 coef_yyz, 126 coef_xzz, 127 coef_yzz, 128 coef_zzz, 129 coef_xx, 130 coef_xy, 131 coef_yy, 132 coef_xz, 133 coef_yz, 134 coef_zz, 135 coef_x, 136 coef_y, 137 coef_z, 138 coef_1 139 }; 140 141 } // namespace openMVG 142 143 #endif // OPENMVG_MULTIVIEW_SOLVER_ESSENTIAL_FIVE_POINT_HPP 144