TransformErrFunction::TransformErrFunction(TransformSet * transformSet, int alignTo, int latestImageId, int latestNOnly) : transformSet_(transformSet), reprojErrVal_(-HUGE), alignTo_(alignTo), RECalculated_(false), scaleCost_(1.0), minIdToRefine_(latestImageId - latestNOnly) { //Choose some points to project (4 is best I think--necessary+sufficient) double pointSquareSideLen = 400; //roughly 4 corners of an image pointVec_.push_back(cvPoint2D64f(0, 0)); pointVec_.push_back(cvPoint2D64f(pointSquareSideLen, 0)); pointVec_.push_back(cvPoint2D64f(0, pointSquareSideLen)); pointVec_.push_back(cvPoint2D64f(pointSquareSideLen, pointSquareSideLen)); //Add all the parameters to the param vector for(TransformSet::iterator ppTrans = transformSet_->begin(); ppTrans != transformSet_->end(); ppTrans++) { int j = ppTrans->first.im1Id(); int k = ppTrans->first.im2Id(); if(k == alignTo_ && j != alignTo_) //Refine all transform T_j0 : j!=0 { if(minIdToRefine_ < j) { Transform * Tj0 = ppTrans->second->transform(); Tj0->addParams(&indexedParamLocations_); } } } //Set scaleCost_ so that costs are around 1 and we get sensible well-conditioned derivatives evaluateReprojErr(); if(reprojErrVal_ > 0.0001) scaleCost_ = 1.0 / reprojErrVal_; RECalculated_ = false; }
/** * OptimizePair: * Input: * cam1 - the first camera (already optimized) * cam2 - the second camera with its intrinsic matrix initialized * dR - an initial relative 3x3 camera rotation matrix * set1 - SIFT features in the first image * set2 - SIFT features in the second image * aryInlier - the homography iniliers * * Ouput: * cam2 - update cam2's optimized focal length and pose */ void OptimizeSingle( const CCamera& cam1, CCamera& cam2, double* dR, const CFeatureArray& set1, const CFeatureArray& set2, const MatchArray& aryInlier ) { // Step 1. Initialize the camera pose of cam2 // cam2's relative rotation to cam1 CvMat matR = cvMat( 3, 3, CV_64F, dR ); // cam1's absolute rotation double dRod1[3]; CvMat matRod1 = cvMat( 3, 1, CV_64F, dRod1 ); cam1.GetPose( dRod1 ); double dRot1[9]; CvMat matRot1 = cvMat( 3, 3, CV_64F, dRot1 ); cvRodrigues2( &matRod1, &matRot1 ); // compose R and Rot1 to get cam2's initial absolute rotation cvMatMul( &matR, &matRot1, &matR ); double dRod2[3]; CvMat matRod2 = cvMat( 3, 1, CV_64F, dRod2 ); cvRodrigues2( &matR, &matRod2 ); cam2.SetPose( dRod2 ); // Step 2. Now we can perform bundle adjustment for cam2 CBundleAdjust ba( 1, BA_ITER ); ba.SetCamera( &cam2, 0 ); // set points for( int i=0; i<aryInlier.size(); ++i ) { const CFeature* ft1 = set1[ aryInlier[i].first ]; const CFeature* ft2 = set2[ aryInlier[i].second ]; double dir[3]; cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) ); // the 3d position CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius ); ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft2->x, ft2->y ) ); } ba.DoMotion(); ba.GetAdjustedCamera( &cam2, 0 ); }
void find_ellipse(CvPoint *points, int n_points, CvPoint2D64f *center, CvPoint2D64f *axes) { double *mean = (double*)malloc(2 * sizeof(double)); double *covariance = (double*)malloc(4 * sizeof(double)); get_mean_and_covariance_of_points_locations(points, n_points, mean, covariance); *center = cvPoint2D64f(mean[0], mean[1]); *axes = cvPoint2D64f(sqrt(covariance[0]), sqrt(covariance[3])); free(mean); free(covariance); }
/** * OptimizePair: * Input: * cam1 - the first camera with its intrinsic matrix and pose initialized ([R|T]=[I|0]) * cam2 - the second camera with its intrinsic matrix initialized * dR - an initial relative 3x3 camera rotation matrix * set1 - SIFT features in the first image * set2 - SIFT features in the second image * aryInlier - the homography iniliers * * Ouput: * cam1 - update cam1's optimized folcal length * cam2 - update cam2's optimized focal length and pose */ void OptimizePair( CCamera& cam1, CCamera& cam2, double* dR, const CFeatureArray& set1, const CFeatureArray& set2, const MatchArray& aryInlier ) { CBundleAdjust ba( 2, BA_ITER ); // Step 1. To perform bundle adjustment, we initialize cam1 and cam2 // as [K][I|0} and [K][R|0] respectively and optimize R using // bundle adjustment. double dRod[3]; CvMat matRod = cvMat( 3, 1, CV_64F, dRod ); CvMat matR = cvMat( 3, 3, CV_64F, dR ); cvRodrigues2( &matR, &matRod ); cam2.SetPose( dRod ); // Set cameras ba.SetCamera( &cam1, 0 ); ba.SetCamera( &cam2, 1 ); // Step 2. We still need to create a set of 3D points. From each homography inlier, // a 3D point can be initialized by locating it on the ray that goes through // its projection. for( int i=0; i<aryInlier.size(); ++i ) { const CFeature* ft1 = set1[ aryInlier[i].first ]; const CFeature* ft2 = set2[ aryInlier[i].second ]; double dir[3]; cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) ); // the initialized 3d position CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius ); // set the 3d point and its projections in both images ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft1->x, ft1->y ) ); ba.SetPointProjection( pt3, 1, cvPoint2D64f( ft2->x, ft2->y ) ); } // perform bundle adjustment ba.DoMotionAndStructure(); // retrieve the optimized cameras ba.GetAdjustedCamera( &cam1, 0 ); ba.GetAdjustedCamera( &cam2, 1 ); }
void GetHomographyInliers( MatchArray& aryInlier, const MatchArray& aryMatch, const CFeatureArray& set1, const CFeatureArray& set2, const CHomography& h**o, float tol ) { float SQR_TOL = tol*tol; aryInlier.resize( aryMatch.size() ); int k=0; for( int i=0; i<aryMatch.size(); ++i ) { const CFeature* ft1 = set1[ aryMatch[i].first ]; const CFeature* ft2 = set2[ aryMatch[i].second ]; CvPoint2D64f pt = h**o * cvPoint2D64f( ft1->x, ft1->y ); double dx = pt.x -ft2->x; double dy = pt.y -ft2->y; if( dx*dx +dy*dy < SQR_TOL ) // a homography inlier aryInlier[k++] = aryMatch[i]; } aryInlier.resize(k); }
void drawMallet(cv::Mat &f, CvPoint2D64f malletXY, double R, CvScalar color) { CvPoint center = convH2CV(malletXY, cvPoint2D64f(HKY_W,HKY_H), scale, cvPoint(ofsX,ofsY)); int r_ = static_cast<int>((R/2.0) / scale); cv::circle(f, center, r_, color, -1); }
CvPoint2D64f convCV2H(const CvPoint &pos, const CvPoint2D64f &hkyWH, double scaleCV2H, const CvPoint &drawOfsXY) { CvPoint2D64f ret = cvPoint2D64f(convCV2H_x(pos.x, hkyWH.x, scaleCV2H, drawOfsXY.x), convCV2H_y(pos.y, hkyWH.y, scaleCV2H, drawOfsXY.y)); return ret; }
SimilarityTransform::SimilarityTransform() { //**this = cvMat(ROWS, COLS, CV_64FC1, data_); //cvSetIdentity(this); scale_ = 1.0; theta_ = 0.0; translation_ = cvPoint2D64f(0,0); }
RTC::ReturnCode_t AHViewer::onExecute(RTC::UniqueId ec_id) { m_malletXY_inIn.read(); m_armXY_inIn.read(); m_puckXY_inIn.read(); CvPoint2D64f pMlt = cvPoint2D64f(m_malletXY_in.data[0], m_malletXY_in.data[1]); CvPoint2D64f rMlt = cvPoint2D64f(m_armXY_in.data[0], m_armXY_in.data[1]); CvPoint2D64f puck = cvPoint2D64f(m_puckXY_in.data[0], m_puckXY_in.data[1]); frame = cv::Scalar(255,255,255); drawHockeyBoard(frame); drawMallet(frame, pMlt, PLAYER_MLT_R, cv::Scalar(255,128,0)); drawMallet(frame, rMlt, ARM_MLT_R, cv::Scalar(255,0,0)); drawMallet(frame, puck, PUCK_R, cv::Scalar(0,0,255)); cv::imshow(winName, frame); cv::waitKey(10); return RTC::RTC_OK; }
/* Performs a perspective transformation on a single point. That is, for a point (x, y) and a 3 x 3 matrix T this function returns the point (u, v), where [x' y' w']^T = T * [x y 1]^T, and (u, v) = (x'/w', y'/w'). Note that affine transforms are a subset of perspective transforms. @param pt a 2D point @param T a perspective transformation matrix @return Returns the point (u, v) as above. */ CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T ) { CvMat XY, UV; double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 }; CvPoint2D64f rslt; cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP ); cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP ); cvMatMul( T, &XY, &UV ); rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] ); return rslt; }
/*计算点pt经透视变换后的点,即给定一点pt和透视变换矩阵T,计算变换后的点 给定点(x,y),变换矩阵M,计算[x',y',w']^T = M * [x,y,1]^T(^T表示转置), 则变换后的点是(u,v) = (x'/w', y'/w') 注意:仿射变换是透视变换的特例 参数: pt:一个二维点 T:透视变换矩阵 返回值:pt经透视变换后的点 */ CvPoint2D64f persp_xform_pt(CvPoint2D64f pt, CvMat* T) { CvMat XY, UV; //XY:点pt对应的3*1列向量,UV:pt变换后的点对应的3*1列向量 double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 }; //对应的数据 CvPoint2D64f rslt; //结果 //初始化矩阵头 cvInitMatHeader(&XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP); cvInitMatHeader(&UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP); cvMatMul(T, &XY, &UV); //计算矩阵乘法,T*XY,结果放在UV中 rslt = cvPoint2D64f(uv[0] / uv[2], uv[1] / uv[2]); //得到转换后的点 return rslt; }
CvPoint2D64f AffineTransform::applyToPoint(CvPoint2D64f p) const { /* double pointData[COLS], newPointData[ROWS]; CvMat pointAsMat = cvMat(COLS, 1, CV_64FC1, pointData); pointData[0] = p.x; pointData[1] = p.y; pointData[2] = 1.0; CvMat transPointAsMat = cvMat(ROWS, 1, CV_64FC1, newPointData); cvMatMul(*this, &pointAsMat, &transPointAsMat); return cvPoint2D64f(newPointData[0], newPointData[1]);*/ double x = data_[0]*p.x + data_[1]*p.y + data_[2]; double y = data_[3]*p.x + data_[4]*p.y + data_[5]; return cvPoint2D64f(x, y); }
CvPoint2D64f PerspectiveTransform::applyToPoint(CvPoint2D64f p) const { /* double pointData[COLS], newPointData[ROWS]; CvMat pointAsMat = cvMat(COLS, 1, CV_64FC1, pointData); pointData[0] = p.x; pointData[1] = p.y; pointData[2] = 1.0; CvMat transPointAsMat = cvMat(ROWS, 1, CV_64FC1, newPointData); cvMatMul((CvMat *)this, &pointAsMat, &transPointAsMat); double dHomogScale = 1.0/newPointData[2]; return cvPoint2D64f(dHomogScale*newPointData[0], dHomogScale*newPointData[1]); */ double x = data_[0]*p.x + data_[1]*p.y + data_[2]; double y = data_[3]*p.x + data_[4]*p.y + data_[5]; double t = data_[6]*p.x + data_[7]*p.y + data_[8]; double t_inv = 1.0/t; return cvPoint2D64f(t_inv*x, t_inv*y); }
SimilarityTransform * AffineTransform::getSimilarityTransform() const { double subMatData[4]; CvMat subMat = cvMat(2, 2, CV_64FC1, subMatData); subMatData[0] = data_[0]; subMatData[1] = data_[1]; subMatData[2] = data_[3]; subMatData[3] = data_[4]; double scale = sqrt(cvDet(&subMat)); //double theta = acos((*(const Transform *)this)(0,0)/scale); //Todo: do a LS fit? double theta = asin((*(const Transform *)this)(0,1)/scale); //Todo: do a LS fit? CvPoint2D64f translation = cvPoint2D64f((*(const Transform *)this)(0,2), (*(const Transform *)this)(1,2)); SimilarityTransform * pm = new SimilarityTransform(theta, scale, translation); //for(int x=0; x<2; x++) // for(int y=0; y<2; y++) // m(x,y) = scale_ * cvmGet(*this, x, y); return pm; }
void faceDbCreator(const char filePath[50],const char coordsFilename[100], const int startFile,const int endFile, const int noIterations,const int border){ /**Number of Feature Points used in aligning images.**/ const int noFeaturePoints = 4; const int initialSize = 38; int i,j,k,iteration; /**No of files from DB added for alignment**/ int noFiles = 0; double xr = 0; double yr = 0; int x,y; char filePathCopy[100]; /**Corrds of the standards face with respect to initialSize**/ CvMat *stdCoords = cvCreateMat(noFeaturePoints*2,1, CV_64FC1); double stdCoordsData[] = {5+border,6+border,32+border, 6+border,18+border,15+border, 18+border,25+border}; stdCoords->data.db = stdCoordsData; /**Average Coords of the faces aligned so far**/ double avgData[noFeaturePoints*2]; CvMat *avgMat = cvCreateMat(noFeaturePoints*2,1, CV_64FC1); avgMat->data.db = avgData; /**Coords to which other coordinates are aligned to**/ double testData[noFeaturePoints*2]; CvMat *testMat = cvCreateMat(noFeaturePoints*2,1, CV_64FC1); testMat->data.db = testData; cvCopy(stdCoords,testMat); double tempCoords[noFeaturePoints*2]; /**Coords of all the image in the database**/ CvMat* coords[endFile-startFile+1]; double coordsData[endFile-startFile+1][noFeaturePoints*8]; /**Face DB image file names**/ char fileNames[endFile-startFile+1][100]; char tempFileName[100]; char tempStr[50]; IplImage *img = NULL; IplImage *dst = NULL; FILE* coordsFile = fopen(coordsFilename,"r+"); FILE* t = NULL; if (coordsFile){ for (i=-startFile+1;i<=endFile-startFile;++i){ if(!feof(coordsFile)){ fscanf(coordsFile,"%s %lf %lf %lf %lf %lf %lf %lf %lf",&tempStr, &tempCoords[0],&tempCoords[1],&tempCoords[2], &tempCoords[3],&tempCoords[4],&tempCoords[5], &tempCoords[6],&tempCoords[7]); /**Skip the coords upto startImage**/ if (i>=0){ strcpy(tempFileName,filePath); strcat(tempFileName,tempStr); /**Check whether the file exists**/ if (t=fopen(tempFileName,"r")){ fclose(t); strcpy(fileNames[noFiles],tempFileName); coords[noFiles] = cvCreateMat(noFeaturePoints*2,4, CV_64FC1); faceDbCreatorFillData(coordsData[noFiles],tempCoords,noFeaturePoints); coords[noFiles]->data.db = coordsData[noFiles]; ++noFiles; } } } else{ noFiles = i-1; break; } } fclose(coordsFile); if (!noFiles){ printf("Face DB Creator Error: No File To Process\n"); exit(EXIT_FAILURE); } } else { printf("Face DB Creator Error: Could Not Open Coords File\n"); exit(EXIT_FAILURE); } /**PsuedoInverse**/ CvMat *temp2 = cvCreateMat(4,1,CV_64FC1); double tempData2[4]; temp2->data.db = tempData2; for (iteration=0;iteration<noIterations;++iteration){ cvSetZero(avgMat); for (i=0;i<noFiles;++i){ pseudoInverse(coords[i],testMat,temp2); for (j=0;j<noFeaturePoints;++j){ xr = coordsData[i][j*8]*temp2->data.db[0] -coordsData[i][j*8+4]* temp2->data.db[1]+temp2->data.db[2]; yr = coordsData[i][j*8]*temp2->data.db[1] +coordsData[i][j*8+4]* temp2->data.db[0]+temp2->data.db[3]; coordsData[i][j*8] = xr; coordsData[i][j*8+5] = xr; coordsData[i][j*8+1] = -yr; coordsData[i][j*8+4] = yr; avgData[j*2] += xr; avgData[j*2+1] += yr; } img = cvLoadImage(fileNames[i], CV_LOAD_IMAGE_GRAYSCALE); dst = cvCreateImage(cvSize(initialSize+ 2*border,initialSize+2*border), img->depth,img->nChannels); cvSetZero(dst); double a = temp2->data.db[0]; double b = temp2->data.db[1]; double det = a*a+b*b; double tx = temp2->data.db[2]; double ty = temp2->data.db[3]; /**Transform the image**/ for (j=0;j<dst->height;++j){ for (k=0;k<dst->width;++k){ xr = ((k-tx)*a+(j-ty)*b)/det; yr = ((k-tx)*-b+(j-ty)*a)/det; if ((int)xr>=0 && (int)xr <img->width && (int)yr>=0 && (int)yr<img->height){ *((unsigned char*)(dst->imageData)+j*dst->widthStep+k)= *((unsigned char*)(img->imageData)+ (int)yr*img->widthStep+(int)xr); } } } cvSaveImage(fileNames[i],dst); cvReleaseImage(&img); cvReleaseImage(&dst); } /**Averge of the transformation performed so far**/ for (j=0;j<noFeaturePoints*2;++j){ avgData[j] /= endFile-startFile+1; } /**Perform transformation on the average data**/ CvMat* tempMat = cvCreateMat(noFeaturePoints*2,4, CV_64FC1); double tempMatData[noFeaturePoints*8]; tempMat->data.db = tempMatData; faceDbCreatorFillData(tempMatData,avgData,noFeaturePoints); pseudoInverse(tempMat,stdCoords,temp2); for (j=0;j<noFeaturePoints;++j){ testData[j*2] = avgData[j*2]*temp2->data.db[0]- avgData[j*2+1]*temp2->data.db[1]+ temp2->data.db[2]; testData[j*2+1] = avgData[j*2]*temp2->data.db[1]+ avgData[j*2+1]*temp2->data.db[0]+ temp2->data.db[3]; } cvReleaseMat(&tempMat); } IplImage *img8U,*img64F; CvRect *cropArea; IplImage *finalImage32F = cvCreateImage(cvSize(CROPPED_WIDTH, CROPPED_HEIGHT),IPL_DEPTH_32F,1); IplImage *finalImage8U = cvCreateImage(cvSize(CROPPED_WIDTH, CROPPED_HEIGHT),IPL_DEPTH_8U,1); IplImage *transformImage64F; IplImage *transformImage32F; IplImage *croppedImage32F = cvCreateImage(cvSize(initialSize, initialSize),IPL_DEPTH_32F,1); IplImage *croppedImage64F = cvCreateImage(cvSize(initialSize, initialSize),IPL_DEPTH_64F,1); IplImage* mask = cvCreateImage(cvGetSize (croppedImage64F),IPL_DEPTH_8U,1); maskGenerator(mask); /**Random transformations**/ double scale = 0; double rotate = 0; double translateX = 0; double translateY = 0; tempStr[0] = '_'; tempStr[4] = '.'; tempStr[5] = 'j'; tempStr[6] = 'p'; tempStr[7] = 'g'; tempStr[8] = '\0'; /**Random Number Generator**/ CvRNG rg; for (i=0;i<noFiles;++i){ img8U = cvLoadImage(fileNames[i], CV_LOAD_IMAGE_GRAYSCALE); img64F = cvCreateImage(cvGetSize(img8U), IPL_DEPTH_64F,1); cvConvertScale(img8U,img64F); cvReleaseImage(&img8U); remove(fileNames[i]); xr = coordsData[i][0]-stdCoordsData[0]+ border; yr = coordsData[i][4]-stdCoordsData[1]+ border; cvSetImageROI(img64F,cvRect(cvRound(xr),cvRound(yr),initialSize, initialSize)); cvCopy(img64F,croppedImage64F); /**Creating variations for each image**/ for (j=0;j<NO_VARIATIONS;++j){ lightingCorrection(croppedImage64F,mask); rg = cvRNG(time(0)*1000*(i+20)*(j+30)); cvConvertScale(croppedImage64F,croppedImage32F); cvResize(croppedImage32F,finalImage32F); cvConvertScale(finalImage32F,finalImage8U); tempStr[1] = (j/100)%10+48; tempStr[2] = (j/10)%10+48;tempStr[3]=j%10+48; strncpy(tempFileName,fileNames[i],strlen(fileNames[i])-4); tempFileName[strlen(fileNames[i])-4] ='\0'; strcat(tempFileName,tempStr); cvSaveImage(tempFileName,finalImage8U); switch (cvRandInt(&rg)%3){ /**Scaling**/ case 0: if (cvRandInt(&rg)%2) scale = cvRandReal(&rg)*MAX_SCALE* initialSize/CROPPED_WIDTH; else scale = cvRandReal(&rg)*MIN_SCALE* initialSize/CROPPED_HEIGHT; transformImage64F = cvCreateImage( cvSize(cvRound(initialSize-2*scale), cvRound(initialSize-2*scale)), IPL_DEPTH_64F,1); transformImage32F = cvCreateImage( cvSize(cvRound(initialSize-2*scale), cvRound(initialSize-2*scale)), IPL_DEPTH_32F,1); cvSetImageROI(img64F,cvRect(cvRound(xr+scale),cvRound(yr+scale), cvRound(initialSize-2*scale),cvRound(initialSize-2*scale))); cvCopy(img64F,transformImage64F); cvConvertScale(transformImage64F,transformImage32F); cvResize(transformImage32F,croppedImage32F); cvConvertScale(croppedImage32F,croppedImage64F); cvReleaseImage(&transformImage64F); cvReleaseImage(&transformImage32F); break; /**Rotation**/ case 1: if (cvRandInt(&rg)%2) rotate = cvRandReal(&rg)*MAX_ROTATE; else rotate = cvRandReal(&rg)*MIN_ROTATE; cvResetImageROI(img64F); transformImage64F = cvCreateImage(cvGetSize(img64F), IPL_DEPTH_64F,1); transformRotate(img64F,transformImage64F, &cvPoint2D64f(xr+initialSize/2,yr+initialSize/2),rotate*M_PI/180); cvSetImageROI(transformImage64F, cvRect(xr,yr,initialSize,initialSize)); cvCopy(transformImage64F,croppedImage64F); cvReleaseImage(&transformImage64F); break; default: /**Translation**/ if (cvRandInt(&rg)%2){ if (cvRandInt(&rg)%2){ translateX = cvRandReal(&rg)*MAX_TRANSLATE* initialSize/CROPPED_WIDTH; translateY = cvRandReal(&rg)*MAX_TRANSLATE* initialSize/CROPPED_HEIGHT; } else{ translateX = cvRandReal(&rg)*MIN_TRANSLATE* initialSize/CROPPED_WIDTH; translateY = cvRandReal(&rg)*MIN_TRANSLATE* initialSize/CROPPED_HEIGHT; } } else{ if (cvRandInt(&rg)%2){ translateX = cvRandReal(&rg)*MAX_TRANSLATE* initialSize/CROPPED_WIDTH; translateY = cvRandReal(&rg)*MIN_TRANSLATE* initialSize/CROPPED_HEIGHT; } else{ translateX = cvRandReal(&rg)*MIN_TRANSLATE* initialSize/CROPPED_WIDTH; translateY = cvRandReal(&rg)*MAX_TRANSLATE* initialSize/CROPPED_HEIGHT; } } cvSetImageROI(img64F,cvRect(cvRound(xr+translateX), cvRound(yr+translateY),initialSize,initialSize)); cvCopy(img64F,croppedImage64F); } } cvReleaseImage(&img64F); cvReleaseMat(&coords[i]); } cvReleaseImage(&finalImage8U); cvReleaseImage(&finalImage32F); cvReleaseImage(&croppedImage32F); cvReleaseImage(&croppedImage64F); cvReleaseMat(&stdCoords); cvReleaseMat(&testMat); cvReleaseMat(&avgMat); cvReleaseMat(&temp2); }
CvPoint2D64f PerspectiveTransform::translation() const { double dHomoScale = 1. / cvmGet(*this, 2,2); return cvPoint2D64f(cvmGet(*this, 0, 2)*dHomoScale, cvmGet(*this, 1, 2)*dHomoScale); };
void drawHockeyBoard(cv::Mat &f) { { // 外枠 + 中央線 CvPoint from = convH2CV(cvPoint2D64f(-HKY_W/2.0, -HKY_H/2.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); CvPoint to = convH2CV(cvPoint2D64f(HKY_W/2.0, HKY_H/2.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); cv::rectangle(f, from, to, cv::Scalar(255,0,255)); from = convH2CV(cvPoint2D64f(-HKY_W/2.0, 0.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); to = convH2CV(cvPoint2D64f(HKY_W/2.0, 0.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); cv::line(f, from, to, cv::Scalar(255,0,255)); } { // 赤線 for (int i=0; i<6; i++) { // 横方向の線 CvPoint from = convH2CV(cvPoint2D64f(-HKY_W/2.0, HKY_H/2.0 - 400.0*i), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); CvPoint to = convH2CV(cvPoint2D64f(HKY_W/2.0, HKY_H/2.0 - 400.0*i), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); cv::line(f, from, to, cv::Scalar(0,0,255), 1); } for (int i=-2; i<=2; i++) { // 縦方向の線 CvPoint from = convH2CV(cvPoint2D64f(300.0*i, -HKY_H/2.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); CvPoint to = convH2CV(cvPoint2D64f(300.0*i, HKY_H/2.0), cvPoint2D64f(HKY_W, HKY_H), scale, cvPoint(ofsX,ofsY)); cv::line(f, from, to, cv::Scalar(0,0,255), 1); } } }
//!Estimate transform from a set of points void SimilarityTransform::estimateFromPoints(const CvMat * points1, const CvMat * points2) { //const CvMat * temp; //CV_SWAP(points1, points2, temp); /* AffineTransform * pAT = getAffineTransform(); pAT->estimateFromPoints(points1, points2); delete pAT;*/ //Umeyama's algorithm: //Find mean and s.d. int numPoints = points1->cols; double meanP1Data[2]; CvMat meanP1 = cvMat(2, 1, CV_64FC1, meanP1Data); double meanP2Data[2]; CvMat meanP2 = cvMat(2, 1, CV_64FC1, meanP2Data); cvSetZero(&meanP1); cvSetZero(&meanP2); for(int i = 0; i<numPoints; i++) { meanP1Data[0] += cvmGet(points1, 0, i); meanP1Data[1] += cvmGet(points1, 1, i); meanP2Data[0] += cvmGet(points2, 0, i); meanP2Data[1] += cvmGet(points2, 1, i); } double numPoints_inv = 1.0/numPoints; meanP1Data[0] *= numPoints_inv; meanP1Data[1] *= numPoints_inv; meanP2Data[0] *= numPoints_inv; meanP2Data[1] *= numPoints_inv; //Now calculate variance double varP1, varP2; varP1 = 0; varP2 = 0; double SIGMAData[4]; CvMat SIGMA = cvMat(2, 2, CV_64FC1, SIGMAData); cvSetZero(&SIGMA); for(int i = 0; i<numPoints; i++) { double x1 = cvmGet(points1, 0, i) - meanP1Data[0]; double y1 = cvmGet(points1, 1, i) - meanP1Data[1]; double x2 = cvmGet(points2, 0, i) - meanP2Data[0]; double y2 = cvmGet(points2, 1, i) - meanP2Data[1]; varP1 += sqr(x1) + sqr(y1); varP2 += sqr(x2) + sqr(y2); SIGMAData[0] += x1*x2; SIGMAData[1] += x1*y2; SIGMAData[2] += y1*x2; SIGMAData[3] += y1*y2; } varP1 *= numPoints_inv; varP2 *= numPoints_inv; SIGMAData[0] *= numPoints_inv; SIGMAData[1] *= numPoints_inv; SIGMAData[2] *= numPoints_inv; SIGMAData[3] *= numPoints_inv; double DData[4]; CvMat D = cvMat(2, 2, CV_64FC1, DData); cvSetZero(&D); double UData[4]; CvMat U = cvMat(2, 2, CV_64FC1, UData); cvSetZero(&U); double VData[4]; CvMat V = cvMat(2, 2, CV_64FC1, VData); cvSetZero(&V); double RotationData[4]; CvMat Rotation = cvMat(2, 2, CV_64FC1, RotationData); cvSetZero(&Rotation); cvSVD(&SIGMA, &D, &U, &V); cvGEMM(&U, &V, 1, 0, 0, &Rotation, CV_GEMM_B_T); // theta_ = acos(RotationData[0]); theta_ = asin(RotationData[1]); scale_ = (1.0/varP1) * cvTrace(&D).val[0]; double transData[2]; CvMat trans = cvMat(2, 1, CV_64FC1, transData); cvSetZero(&trans); cvGEMM( &Rotation, &meanP1, -scale_, &meanP2, 1, &trans); translation_ = cvPoint2D64f(transData[0], transData[1]); //Applying this to p1 gives us p2 }
CvPoint2D64f AffineTransform::translation() const { return cvPoint2D64f(cvmGet(*this, 0, 2), cvmGet(*this, 1, 2)); };