cvgeometry.h
Go to the documentation of this file.
00001 
00011 #ifndef CV_GEOMETRY_H
00012 #define CV_GEOMETRY_H
00013 
00014 #include <stdio.h>
00015 //#include <opencv/cv.h>
00016 #include <opencv2/core/core.hpp>
00017 
00018 
00026 inline void cvDistort (const double *pIntrinsicDistort, const double *pDistortion, const double *pSrc, double *pDes ) {
00027     //std::cout << "k1: " << pDistortion[0] << ", k2: " << pDistortion[1] << ",p1: " << pDistortion[2] << ", p2: " << pDistortion[3] << std::endl;
00028     double dx = ( pSrc[0] - pIntrinsicDistort[2] ) / pIntrinsicDistort[0];
00029     // relative y position in distorted image to the center
00030     double dy = ( pSrc[1] - pIntrinsicDistort[5] ) / pIntrinsicDistort[4];
00031     // relative distance of the distorted point to the center
00032     double dxdy = dx*dy;
00033     double dx2 = dx*dx;
00034     double dy2 = dy*dy;
00035     double r = sqrt ( dx2 + dy2 );
00036     double r2 = r*r;
00037     double r4 = r2*r2;
00038 
00039     //x = dx*(1 + k1r^2 + k2r^4) + 2*p1*dx*dy + p2(r^2+2*dx^2)
00040     double Tx1 = dx * ( 1.0 + pDistortion[0] * r2 + pDistortion[1] * r4 );
00041     double Tx2 = 2 * pDistortion[2] * dxdy;
00042     double Tx3 = pDistortion[3] * ( r2 + 2 * dx2 );
00043 
00044     //y = dy*(1 + k1r^2 + k2r^4) + p1(r^2+2*dy^2) + 2*p2*dx*dy
00045     double Ty1 = dy * ( 1.0 + pDistortion[0] * r2 + pDistortion[1] * r4 );
00046     double Ty2 = pDistortion[2] * ( r2 + 2 * dy2 );
00047     double Ty3 = 2 * pDistortion[3] * dxdy;
00048 
00049     pDes[0] = ( Tx1 + Tx2 + Tx3 ) * pIntrinsicDistort[0] + pIntrinsicDistort[2];
00050     pDes[1] = ( Ty1 + Ty2 + Ty3 ) * pIntrinsicDistort[4] + pIntrinsicDistort[5];
00051 
00052 }
00053 
00054 
00062 inline void cvDistort (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, const CvPoint2D64f *pSrc, CvPoint2D64f *pDes ) {
00063     cvDistort (pIntrinsicDistort->data.db, pDistortion->data.db, (const double *) pSrc, (double *) pDes );
00064 }
00065 
00073 inline void cvDistort (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, const CvPoint *pSrc, CvPoint *pDes ) {
00074     double src[] = {static_cast<double>(pSrc->x), static_cast<double>(pSrc->y)}, des[2];
00075     cvDistort (pIntrinsicDistort->data.db, pDistortion->data.db, (const double *) &src, (double *) &des );
00076     pDes->x = cvRound(des[0]), pDes->y = cvRound(des[1]);
00077 }
00078 
00088 inline void cvInitUndistortMapExact (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, CvMat *pMapX,  CvMat *pMapY) {
00089     CvPoint2D64f undistort, distort;
00090                 if((cvGetElemType(pMapX) == CV_32FC1) && (cvGetElemType(pMapY) == CV_32FC1)){
00091         float *pX = pMapX->data.fl, *pY = pMapY->data.fl;
00092                         for ( undistort.y = 0; undistort.y < pMapX->height; undistort.y++) {
00093                                         for ( undistort.x = 0; undistort.x < pMapX->width; undistort.x++) {
00094                                                         cvDistort(pIntrinsicDistort, pDistortion, &undistort, &distort);
00095                                                         *pX++ = (float) distort.x, *pY++ = (float) distort.y;
00096                                         }
00097                         }
00098                 } else if((cvGetElemType(pMapX) == CV_64FC1) && (cvGetElemType(pMapY) == CV_64FC1)){
00099         double *pX = pMapX->data.db, *pY = pMapY->data.db;
00100                         for ( undistort.y = 0; undistort.y < pMapX->height; undistort.y++) {
00101                                         for ( undistort.x = 0; undistort.x < pMapX->width; undistort.x++) {
00102                                                         cvDistort(pIntrinsicDistort, pDistortion, &undistort, &distort);
00103                                                         *pX++ = distort.x, *pY++ = distort.y;
00104                                         }
00105                         }
00106                 }
00107 }
00108 
00118 inline void cvUndistortExact (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, const IplImage *pSrc,  IplImage *pDes) {
00119 
00120     CvMat *pMapX  = cvCreateMat ( pSrc->height,  pSrc->width, CV_32FC1 );
00121     CvMat *pMapY  = cvCreateMat ( pSrc->height,  pSrc->width, CV_32FC1 );
00122                 cvInitUndistortMapExact (pIntrinsicDistort, pDistortion, pMapX,  pMapY);
00123     cvRemap ( pSrc, pDes, pMapX, pMapY );
00124     cvReleaseMat ( &pMapX ), cvReleaseMat ( &pMapY );
00125 }
00126 
00134 inline bool cvUndistort (const double *pIntrinsicDistort, const double *pDistortion, const double *pSrc, double *pDes, double maxError = 1) {
00135     const int iMaxInterations = 100;
00136     double fError = (pIntrinsicDistort[2]+pIntrinsicDistort[5])*4;
00137     double cradient[] = {0, 0};
00138     pDes[0] = pSrc[0], pDes[1] = pSrc[1];
00139     double current[2];
00140     int i;
00141     for ( i = 0; ( ( i < iMaxInterations ) && ( fError > maxError ) ); i++ ) {
00142         pDes[0] += cradient[0];
00143         pDes[1] += cradient[1];
00144         cvDistort (pIntrinsicDistort, pDistortion, pDes, (double *) &current );
00145         cradient[0] = pSrc[0] - current[0];
00146         cradient[1] = pSrc[1] - current[1];
00147         fError = cradient[0] * cradient[0] + cradient[1] * cradient[1];
00148     }
00149     if ( i < iMaxInterations ) return true;
00150     else return false;
00151 }
00152 
00160 inline bool cvUndistort (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, const CvPoint2D64f *pSrc, CvPoint2D64f *pDes, double maxError = 1) {
00161     return cvUndistort (pIntrinsicDistort->data.db, pDistortion->data.db, (const double *) pSrc, (double *) pDes, maxError );
00162 }
00163 
00171 inline bool Undistort (const CvMat *pIntrinsicDistort, const CvMat *pDistortion, const CvPoint *pSrc, CvPoint *pDes, double maxError = 1) {
00172     double src[] = {static_cast<double>(pSrc->x), static_cast<double>(pSrc->y)}, des[2];
00173     bool result = cvUndistort (pIntrinsicDistort->data.db, pDistortion->data.db, (const double *) src, (double *) &des, maxError );
00174     pDes->x = round(des[0]);
00175     pDes->y = round(des[1]);
00176     return result;
00177 }
00184 inline void cvProjectPoint(const double *pMintMext, const double *pSrc, double *pDes) {
00185     pDes[0] = pMintMext[0] * pSrc[0] + pMintMext[1] * pSrc[1] + pMintMext[2] * pSrc[2] + pMintMext[3];
00186     pDes[1] = pMintMext[4] * pSrc[0] + pMintMext[5] * pSrc[1] + pMintMext[6] * pSrc[2] + pMintMext[7];
00187     double n = pMintMext[8] * pSrc[0] + pMintMext[9] * pSrc[1] + pMintMext[10] * pSrc[2] + pMintMext[11];
00188     pDes[0] /= n;
00189     pDes[1] /= n;
00190 }
00191 
00198 inline void cvProjectPoint(const CvMat *pMintMext, const CvPoint2D64f *pSrc, CvPoint2D64f *pDes) {
00199     cvProjectPoint(pMintMext->data.db, (const double *) pSrc, (double *) pDes);
00200 }
00201 
00208 template <typename A, typename B> inline void cvProjectPoint(const CvMat *pMintMext, const A *pSrc, B *pDes) {
00209     double src[] = {pSrc->x, pSrc->y, pSrc->z}, des[2];
00210     cvProjectPoint(pMintMext->data.db, (const double *) src, (double *) des);
00211     pDes->x = cvRound(des[0]), pDes->y = cvRound(des[1]);
00212 }
00213 
00214 #endif //CV_GEOMETIRY_H


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25