1 // This file is part of OpenCV project. 2 // It is subject to the license terms in the LICENSE file found in the top-level directory 3 // of this distribution and at http://opencv.org/license.html 4 5 // This file is based on file issued with the following license: 6 7 /*============================================================================ 8 9 Copyright 2017 Toby Collins 10 Redistribution and use in source and binary forms, with or without 11 modification, are permitted provided that the following conditions are met: 12 13 1. Redistributions of source code must retain the above copyright notice, this 14 list of conditions and the following disclaimer. 15 16 2. Redistributions in binary form must reproduce the above copyright notice, this 17 list of conditions and the following disclaimer in the documentation 18 and/or other materials provided with the distribution. 19 20 3. Neither the name of the copyright holder nor the names of its contributors may 21 be used to endorse or promote products derived from this software without 22 specific prior written permission. 23 24 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 28 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE 33 USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 */ 35 36 #ifndef OPENCV_CALIB3D_IPPE_HPP 37 #define OPENCV_CALIB3D_IPPE_HPP 38 39 #include <opencv2/core.hpp> 40 41 namespace cv { 42 namespace IPPE { 43 44 class PoseSolver { 45 public: 46 /** 47 * @brief PoseSolver constructor 48 */ 49 PoseSolver(); 50 51 /** 52 * @brief Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors. 53 * The poses are sorted with the first having the lowest reprojection error. 54 * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 55 * 1xN/Nx1 3-channel (float or double) where N is the number of points 56 * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates. 57 * @param rvec1 First rotation solution (3x1 rotation vector) 58 * @param tvec1 First translation solution (3x1 vector) 59 * @param reprojErr1 Reprojection error of first solution 60 * @param rvec2 Second rotation solution (3x1 rotation vector) 61 * @param tvec2 Second translation solution (3x1 vector) 62 * @param reprojErr2 Reprojection error of second solution 63 */ 64 void solveGeneric(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1, 65 float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2); 66 67 /** 68 * @brief Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE. 69 * The poses are sorted so that the first one is the one with the lowest reprojection error. 70 * 71 * @param objectPoints Array of 4 coplanar object points defined in the following object coordinates: 72 * - point 0: [-squareLength / 2.0, squareLength / 2.0, 0] 73 * - point 1: [squareLength / 2.0, squareLength / 2.0, 0] 74 * - point 2: [squareLength / 2.0, -squareLength / 2.0, 0] 75 * - point 3: [-squareLength / 2.0, -squareLength / 2.0, 0] 76 * 1xN/Nx1 3-channel (float or double) where N is the number of points 77 * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates. 78 * @param rvec1 First rotation solution (3x1 rotation vector) 79 * @param tvec1 First translation solution (3x1 vector) 80 * @param reprojErr1 Reprojection error of first solution 81 * @param rvec2 Second rotation solution (3x1 rotation vector) 82 * @param tvec2 Second translation solution (3x1 vector) 83 * @param reprojErr2 Reprojection error of second solution 84 */ 85 void solveSquare(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1, 86 float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2); 87 88 private: 89 /** 90 * @brief Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates. 91 * These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. 92 * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double). 93 * @param normalizedImagePoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double). 94 * @param Ma First pose solution (unsorted) 95 * @param Mb Second pose solution (unsorted) 96 */ 97 void solveGeneric(InputArray objectPoints, InputArray normalizedImagePoints, OutputArray Ma, OutputArray Mb); 98 99 /** 100 * @brief Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates. 101 * These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. 102 * @param canonicalObjPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points 103 * @param normalizedInputPoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points 104 * @param H Homography mapping canonicalObjPoints to normalizedInputPoints. 105 * @param Ma 106 * @param Mb 107 */ 108 void solveCanonicalForm(InputArray canonicalObjPoints, InputArray normalizedInputPoints, const Matx33d& H, 109 OutputArray Ma, OutputArray Mb); 110 111 /** 112 * @brief Computes the translation solution for a given rotation solution 113 * @param objectPoints Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points 114 * @param normalizedImagePoints Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points 115 * @param R Rotation solution (3x1 rotation vector) 116 * @param t Translation solution (3x1 rotation vector) 117 */ 118 void computeTranslation(InputArray objectPoints, InputArray normalizedImgPoints, InputArray R, OutputArray t); 119 120 /** 121 * @brief Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane. 122 * For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this). 123 * For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates). 124 * The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy). 125 * @param j00 Homography jacobian coefficient at (ux,uy) 126 * @param j01 Homography jacobian coefficient at (ux,uy) 127 * @param j10 Homography jacobian coefficient at (ux,uy) 128 * @param j11 Homography jacobian coefficient at (ux,uy) 129 * @param p The x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) 130 * @param q The y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) 131 */ 132 void computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2); 133 134 /** 135 * @brief Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points). 136 * The source points are the four corners of a zero-centred squared defined by: 137 * - point 0: [-squareLength / 2.0, squareLength / 2.0] 138 * - point 1: [squareLength / 2.0, squareLength / 2.0] 139 * - point 2: [squareLength / 2.0, -squareLength / 2.0] 140 * - point 3: [-squareLength / 2.0, -squareLength / 2.0] 141 * 142 * @param targetPoints Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3. 143 * @param halfLength The square's half length (i.e. squareLength/2.0) 144 * @param H Homograhy mapping the source points to the target points, 3x3 single channel 145 */ 146 void homographyFromSquarePoints(InputArray targetPoints, double halfLength, OutputArray H); 147 148 /** 149 * @brief Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula 150 * @param R Input rotation matrix, 3x3 1-channel (double) 151 * @param r Output rotation vector, 3x1/1x3 1-channel (double) 152 */ 153 void rot2vec(InputArray R, OutputArray r); 154 155 /** 156 * @brief Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0 157 * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points 158 * @param canonicalObjectPoints Object points in canonical coordinates 1xN/Nx1 2-channel (double) 159 * @param MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double) 160 */ 161 void makeCanonicalObjectPoints(InputArray objectPoints, OutputArray canonicalObjectPoints, OutputArray MobjectPoints2Canonical); 162 163 /** 164 * @brief Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution. 165 * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points 166 * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. 167 * @param M Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double) 168 * @param err RMS reprojection error 169 */ 170 void evalReprojError(InputArray objectPoints, InputArray imagePoints, InputArray M, float& err); 171 172 /** 173 * @brief Sorts two pose solutions according to their RMS reprojection error (lowest first). 174 * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points 175 * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. 176 * @param Ma Pose matrix 1: 4x4 1-channel 177 * @param Mb Pose matrix 2: 4x4 1-channel 178 * @param M1 Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy. 179 * @param M2 Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy. 180 * @param err1 RMS reprojection error of _M1 181 * @param err2 RMS reprojection error of _M2 182 */ 183 void sortPosesByReprojError(InputArray objectPoints, InputArray imagePoints, InputArray Ma, InputArray Mb, OutputArray M1, OutputArray M2, float& err1, float& err2); 184 185 /** 186 * @brief Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1) 187 * @param a vector: 3x1 mat (double) 188 * @param Ra Rotation: 3x3 mat (double) 189 */ 190 void rotateVec2ZAxis(const Matx31d& a, Matx33d& Ra); 191 192 /** 193 * @brief Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. 194 * @param objectPoints Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points 195 * @param R Rotation Mat: 3x3 (double) 196 * @return Success (true) or failure (false) 197 */ 198 bool computeObjextSpaceR3Pts(InputArray objectPoints, Matx33d& R); 199 200 /** 201 * @brief computeObjextSpaceRSvD Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. 202 * @param objectPointsZeroMean Zero-meaned coplanar object points: 3xN matrix (double) where N>=3 203 * @param R Rotation Mat: 3x3 (double) 204 */ 205 void computeObjextSpaceRSvD(InputArray objectPointsZeroMean, OutputArray R); 206 207 /** 208 * @brief Generates the 4 object points of a square planar object 209 * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) 210 * @param objectPoints Set of 4 object points (1x4 3-channel double) 211 */ 212 void generateSquareObjectCorners3D(double squareLength, OutputArray objectPoints); 213 214 /** 215 * @brief Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points). 216 * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) 217 * @param objectPoints Set of 4 object points (1x4 2-channel double) 218 */ 219 void generateSquareObjectCorners2D(double squareLength, OutputArray objectPoints); 220 221 /** 222 * @brief Computes the average depth of an object given its pose in camera coordinates 223 * @param objectPoints: Object points defined in 3D object space 224 * @param rvec: Rotation component of pose 225 * @param tvec: Translation component of pose 226 * @return: average depth of the object 227 */ 228 double meanSceneDepth(InputArray objectPoints, InputArray rvec, InputArray tvec); 229 230 //! a small constant used to test 'small' values close to zero. 231 double IPPE_SMALL; 232 }; 233 } //namespace IPPE 234 235 namespace HomographyHO { 236 237 /** 238 * @brief Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method: 239 * Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England. 240 * This is not the author's implementation. 241 * @param srcPoints Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points 242 * @param targPoints Array of target points: 1xN/Nx1 2-channel (float or double) 243 * @param H Homography from source to target: 3x3 1-channel (double) 244 */ 245 void homographyHO(InputArray srcPoints, InputArray targPoints, Matx33d& H); 246 247 /** 248 * @brief Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision, 249 * Cambridge University Press, Cambridge, 2001 250 * @param Data Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points 251 * @param DataN Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points 252 * @param T Homogeneous transform from source to normalized: 3x3 1-channel (double) 253 * @param Ti Homogeneous transform from normalized to source: 3x3 1-channel (double) 254 */ 255 void normalizeDataIsotropic(InputArray Data, OutputArray DataN, OutputArray T, OutputArray Ti); 256 257 } 258 } //namespace cv 259 #endif 260