1 /**************************************************************************** 2 * 3 * ViSP, open source Visual Servoing Platform software. 4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved. 5 * 6 * This software is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation; either version 2 of the License, or 9 * (at your option) any later version. 10 * See the file LICENSE.txt at the root directory of this source 11 * distribution for additional information about the GNU GPL. 12 * 13 * For using ViSP with software that can not be combined with the GNU 14 * GPL, please contact Inria about acquiring a ViSP Professional 15 * Edition License. 16 * 17 * See http://visp.inria.fr for more information. 18 * 19 * This software was developed at: 20 * Inria Rennes - Bretagne Atlantique 21 * Campus Universitaire de Beaulieu 22 * 35042 Rennes Cedex 23 * France 24 * 25 * If you have questions regarding the use of this file, please contact 26 * Inria at visp@inria.fr 27 * 28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 30 * 31 * Description: 32 * Homography transformation. 33 * 34 * Authors: 35 * Muriel Pressigout 36 * Fabien Spindler 37 * 38 *****************************************************************************/ 39 40 /*! 41 \file vpHomography.h 42 43 This file defines an homography transformation. This class aims to provide 44 some tools for homography computation. 45 */ 46 47 #ifndef vpHomography_hh 48 #define vpHomography_hh 49 50 #include <list> 51 #include <vector> 52 53 #include <visp3/core/vpCameraParameters.h> 54 #include <visp3/core/vpHomogeneousMatrix.h> 55 #include <visp3/core/vpImagePoint.h> 56 #include <visp3/core/vpMatrix.h> 57 #include <visp3/core/vpPlane.h> 58 #include <visp3/core/vpPoint.h> 59 60 /*! 61 62 \class vpHomography 63 \ingroup group_vision_homography 64 65 \brief Implementation of an homography and operations on homographies. 66 67 This class aims to compute the homography wrt. two images \cite Marchand16a. 68 69 The vpHomography class is derived from vpArray2D<double>. 70 71 These two images are both described by a set of points. The 2 sets (one per 72 image) are sets of corresponding points : for a point in a image, there is 73 the corresponding point (image of the same 3D point) in the other image 74 points set. These 2 sets are the only data needed to compute the 75 homography. One method used is the one introduced by Ezio Malis during his 76 PhD \cite TheseMalis. A normalization is carried out on this points in order 77 to improve the conditioning of the problem, what leads to improve the 78 stability of the result. 79 80 Store and compute the homography such that 81 \f[ 82 ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p} 83 \f] 84 85 with 86 \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd} 87 { ^b{\bf n}^T} 88 \f] 89 90 The \ref tutorial-homography explains how to use this class. 91 92 The example below shows also how to manipulate this class to first 93 compute a ground truth homography from camera poses, project pixel 94 coordinates points using an homography and lastly estimate an 95 homography from a subset of 4 matched points in frame a and frame b 96 respectively. 97 98 \code 99 #include <visp3/core/vpHomogeneousMatrix.h> 100 #include <visp3/core/vpMath.h> 101 #include <visp3/core/vpMeterPixelConversion.h> 102 #include <visp3/vision/vpHomography.h> 103 104 int main() 105 { 106 // Initialize in the object frame the coordinates in meters of 4 points that 107 // belong to a planar object 108 vpPoint Po[4]; 109 Po[0].setWorldCoordinates(-0.1, -0.1, 0); 110 Po[1].setWorldCoordinates( 0.2, -0.1, 0); 111 Po[2].setWorldCoordinates( 0.1, 0.1, 0); 112 Po[3].setWorldCoordinates(-0.1, 0.3, 0); 113 114 // Initialize the pose between camera frame a and object frame o 115 vpHomogeneousMatrix aMo(0, 0, 1, 0, 0, 0); // Camera is 1 meter far 116 117 // Initialize the pose between camera frame a and camera frame 118 // b. These two frames correspond for example to two successive 119 // camera positions 120 vpHomogeneousMatrix aMb(0.2, 0.1, 0, 0, 0, vpMath::rad(2)); 121 122 // Compute the pose between camera frame b and object frame 123 vpHomogeneousMatrix bMo = aMb.inverse() * aMo; 124 125 // Initialize camera intrinsic parameters 126 vpCameraParameters cam; 127 128 // Compute the coordinates in pixels of the 4 object points in the 129 // camera frame a 130 vpPoint Pa[4]; 131 std::vector<double> xa(4), ya(4); // Coordinates in pixels of the points in frame a 132 for(int i=0 ; i < 4 ; i++) { 133 Pa[i] = Po[i]; Pa[i].project(aMo); // Project the points from object frame to camera frame a 134 vpMeterPixelConversion::convertPoint(cam, 135 Pa[i].get_x(), Pa[i].get_y(), 136 xa[i], ya[i]); 137 } 138 139 // Compute the coordinates in pixels of the 4 object points in the 140 // camera frame b 141 vpPoint Pb[4]; 142 std::vector<double> xb(4), yb(4); // Coordinates in pixels of the points in frame b 143 for(int i=0 ; i < 4 ; i++) { 144 Pb[i] = Po[i]; Pb[i].project(bMo); // Project the points from object frame to camera frame a 145 } 146 147 // Compute equation of the 3D plane containing the points in camera frame b 148 vpPlane bP(Pb[0], Pb[1], Pb[2]); 149 150 // Compute the corresponding ground truth homography 151 vpHomography aHb(aMb, bP); 152 153 std::cout << "Ground truth homography aHb: \n" << aHb<< std::endl; 154 155 // Compute the coordinates of the points in frame b using the ground 156 // truth homography and the coordinates of the points in frame a 157 vpHomography bHa = aHb.inverse(); 158 for(int i = 0; i < 4 ; i++){ 159 double inv_z = 1. / (bHa[2][0] * xa[i] + bHa[2][1] * ya[i] + bHa[2][2]); 160 161 xb[i] = (bHa[0][0] * xa[i] + bHa[0][1] * ya[i] + bHa[0][2]) * inv_z; 162 yb[i] = (bHa[1][0] * xa[i] + bHa[1][1] * ya[i] + bHa[1][2]) * inv_z; 163 } 164 165 // Estimate the homography from 4 points coordinates expressed in pixels 166 vpHomography::DLT(xb, yb, xa, ya, aHb, true); 167 aHb /= aHb[2][2]; // Apply a scale factor to have aHb[2][2] = 1 168 169 std::cout << "Estimated homography aHb: \n" << aHb<< std::endl; 170 } 171 \endcode 172 173 */ 174 class VISP_EXPORT vpHomography : public vpArray2D<double> 175 { 176 private: 177 static const double sing_threshold; // = 0.0001; 178 static const double threshold_rotation; 179 static const double threshold_displacement; 180 vpHomogeneousMatrix aMb; 181 // bool isplanar; 182 //! reference plane coordinates expressed in Rb 183 vpPlane bP; 184 185 private: 186 //! build the homography from aMb and Rb 187 void build(); 188 189 //! insert a rotation matrix 190 void insert(const vpHomogeneousMatrix &aRb); 191 //! insert a rotation matrix 192 void insert(const vpRotationMatrix &aRb); 193 //! insert a theta u vector (transformation into a rotation matrix) 194 void insert(const vpThetaUVector &tu); 195 //! insert a translation vector 196 void insert(const vpTranslationVector &atb); 197 //! insert a translation vector 198 void insert(const vpPlane &bP); 199 200 static void initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x); 201 202 public: 203 vpHomography(); 204 vpHomography(const vpHomography &H); 205 //! Construction from Translation and rotation and a plane 206 vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP); 207 //! Construction from Translation and rotation and a plane 208 vpHomography(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP); 209 //! Construction from Translation and rotation and a plane 210 vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP); 211 //! Construction from Translation and rotation and a plane 212 vpHomography(const vpPoseVector &arb, const vpPlane &bP); ~vpHomography()213 virtual ~vpHomography(){}; 214 215 //! Construction from Translation and rotation and a plane 216 void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP); 217 //! Construction from Translation and rotation and a plane 218 void buildFrom(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP); 219 //! Construction from Translation and rotation and a plane 220 void buildFrom(const vpPoseVector &arb, const vpPlane &bP); 221 //! Construction from homogeneous matrix and a plane 222 void buildFrom(const vpHomogeneousMatrix &aMb, const vpPlane &bP); 223 224 vpHomography collineation2homography(const vpCameraParameters &cam) const; 225 226 vpMatrix convert() const; 227 228 void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); 229 230 void computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); 231 232 double det() const; 233 void eye(); 234 235 vpHomography homography2collineation(const vpCameraParameters &cam) const; 236 237 //! invert the homography 238 vpHomography inverse(double sv_threshold = 1e-16, unsigned int *rank=NULL) const; 239 //! invert the homography 240 void inverse(vpHomography &Hi) const; 241 242 //! Load an homography from a file 243 void load(std::ifstream &f); 244 245 // Multiplication by an homography 246 vpHomography operator*(const vpHomography &H) const; 247 // Multiplication by a scalar 248 vpHomography operator*(const double &v) const; 249 vpColVector operator*(const vpColVector &b) const; 250 // Multiplication by a point 251 vpPoint operator*(const vpPoint &H) const; 252 253 // Division by a scalar 254 vpHomography operator/(const double &v) const; 255 vpHomography &operator/=(double v); 256 vpHomography &operator=(const vpHomography &H); 257 vpHomography &operator=(const vpMatrix &H); 258 259 vpImagePoint projection(const vpImagePoint &p); 260 261 /*! 262 This function is not applicable to an homography that is always a 263 3-by-3 matrix. 264 \exception vpException::fatalError When this function is called. 265 */ 266 void resize(unsigned int nrows, unsigned int ncols, bool flagNullify = true) 267 { 268 (void)nrows; 269 (void)ncols; 270 (void)flagNullify; 271 throw(vpException(vpException::fatalError, "Cannot resize an homography matrix")); 272 }; 273 274 void save(std::ofstream &f) const; 275 276 static void DLT(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa, 277 const std::vector<double> &ya, vpHomography &aHb, bool normalization = true); 278 279 static void HLM(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa, 280 const std::vector<double> &ya, bool isplanar, vpHomography &aHb); 281 282 static bool ransac(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa, 283 const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers, double &residual, 284 unsigned int nbInliersConsensus, double threshold, bool normalization = true); 285 286 static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa); 287 static vpPoint project(const vpHomography &bHa, const vpPoint &Pa); 288 289 static void robust(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa, 290 const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inlier, double &residual, 291 double weights_threshold = 0.4, unsigned int niter = 4, bool normalization = true); 292 293 #ifndef DOXYGEN_SHOULD_SKIP_THIS 294 //! build the homography from aMb and Rb 295 static void build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP); 296 297 static void computeDisplacement(const vpHomography &aHb, const vpColVector &nd, vpRotationMatrix &aRb, 298 vpTranslationVector &atb, vpColVector &n); 299 300 static void computeDisplacement(const vpHomography &aHb, vpRotationMatrix &aRb, vpTranslationVector &atb, 301 vpColVector &n); 302 303 static void computeDisplacement(const vpHomography &H, double x, double y, 304 std::list<vpRotationMatrix> &vR, std::list<vpTranslationVector> &vT, 305 std::list<vpColVector> &vN); 306 static double computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane &oN, 307 vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust); 308 static double computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane *oN, 309 vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust); 310 static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d); 311 // VVS 312 static double computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1, 313 int userobust); 314 static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M); 315 static bool degenerateConfiguration(vpColVector &x, unsigned int *ind); 316 static bool degenerateConfiguration(vpColVector &x, unsigned int *ind, double threshold_area); 317 static bool degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb, 318 const std::vector<double> &xa, const std::vector<double> &ya); 319 static void HartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, double &xg, 320 double &yg, double &coef); 321 static void HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y, std::vector<double> &xn, 322 std::vector<double> &yn, double &xg, double &yg, double &coef); 323 static void HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1, 324 double xg2, double yg2, double coef2); 325 326 #endif // DOXYGEN_SHOULD_SKIP_THIS 327 328 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 329 /*! 330 \deprecated You should rather use eye(). 331 */ 332 //@{ 333 void setIdentity(); 334 //@} 335 #endif 336 }; 337 338 #endif 339