void CV_BaseHistTest::get_hist_params( int /*test_case_idx*/ ) { CvRNG* rng = ts->get_rng(); int i, max_dim_size, max_ni_dim_size = 31; double hist_size; cdims = cvTsRandInt(rng) % max_cdims + 1; hist_size = exp(cvTsRandReal(rng)*max_log_size*CV_LOG2); max_dim_size = cvRound(pow(hist_size,1./cdims)); total_size = 1; uniform = cvTsRandInt(rng) % 2; hist_type = cvTsRandInt(rng) % 2 ? CV_HIST_SPARSE : CV_HIST_ARRAY; for( i = 0; i < cdims; i++ ) { dims[i] = cvTsRandInt(rng) % (max_dim_size + 2) + 2; if( !uniform ) dims[i] = MIN(dims[i], max_ni_dim_size); total_size *= dims[i]; } img_type = cvTsRandInt(rng) % 2 ? CV_32F : CV_8U; img_size.width = cvRound( exp(cvRandReal(rng) * img_max_log_size*CV_LOG2) ); img_size.height = cvRound( exp(cvRandReal(rng) * img_max_log_size*CV_LOG2) ); low = cvTsMinVal(img_type); high = cvTsMaxVal(img_type); range_delta = (cvTsRandInt(rng) % 2)*(high-low)*0.05; }
main( int argc, char* argv[] ) { // Choose a negative floating point number. Take its absolute value, // round it, and then take its ceiling and floor. double a = -1.23; printf( "CV_IABS(a) = %d\n", CV_IABS(a) ); printf( "cvRound(a) = %d\n", cvRound(a) ); printf( "cvCeil(a) = %d\n", cvCeil(a) ); printf( "cvFloor(a) = %d\n", cvFloor(a) ); // Generate some random numbers. CvRNG rngState = cvRNG(-1); for (int i = 0; i < 10; i++) { printf( "%u %f\n", cvRandInt( &rngState ), cvRandReal( &rngState ) ); } // Create a floating point CvPoint2D32f and convert it to an integer // CvPoint. CvPoint2D32f point_float1 = cvPoint2D32f(1.0, 2.0); CvPoint point_int1 = cvPointFrom32f( point_float1 ); // Convert a CvPoint to a CvPoint2D32f. CvPoint point_int2 = cvPoint(3, 4); CvPoint2D32f point_float2 = cvPointTo32f( point_int2 ); }
void CvANN_MLP::init_weights() { int i, j, k; for( i = 1; i < layer_sizes->cols; i++ ) { int n1 = layer_sizes->data.i[i-1]; int n2 = layer_sizes->data.i[i]; double val = 0, G = n2 > 2 ? 0.7*pow((double)n1,1./(n2-1)) : 1.; double* w = weights[i]; // initialize weights using Nguyen-Widrow algorithm for( j = 0; j < n2; j++ ) { double s = 0; for( k = 0; k <= n1; k++ ) { val = cvRandReal(&rng)*2-1.; w[k*n2 + j] = val; s += val; } if( i < layer_sizes->cols - 1 ) { s = 1./(s - val); for( k = 0; k <= n1; k++ ) w[k*n2 + j] *= s; w[n1*n2 + j] *= G*(-1+j*2./n2); } } } }
/*! \fn int Select() * \brief This select un number corresponding to a gaussian index * according to the gaussian weights * \return an integer that is a gaussian index */ int GaussianMixture::Select() { if(curnb==1) return 0; /* si les gaussiennes sont equiprobables alors on retourne un entier tire aleatoirement entre 0 et le nombre de gaussiennes */ if(areEquiprob) return cvRandInt(&rng_state) % curnb; else { /* Choix de l'index en fonction des poids de chaque gaussiennes */ int id; double RandVal; /* Tir aleatoire uniforme d'un reel entre 0 et 1 */ RandVal = cvRandReal(&rng_state); /* Choix de l'etat en fonction des probas de transition */ id=0; /* Choix en fonction des poids */ while((id < curnb) && (RandVal > coeffCumul[id]) ) id++; return id; } }
int main (int argc, char **argv) { int i, j; int nrow = 3; int ncol = 3; CvMat *src, *dst, *mul; double det; CvRNG rng = cvRNG (time (NULL)); /* 乱数の初期化 */ // (1) 行列のメモリ確保 src = cvCreateMat (nrow, ncol, CV_32FC1); dst = cvCreateMat (ncol, nrow, CV_32FC1); mul = cvCreateMat (nrow, nrow, CV_32FC1); // (2) 行列srcに乱数を代入 printf ("src\n"); cvmSet (src, 0, 0, 1); for (i = 0; i < src->rows; i++) { for (j = 0; j < src->cols; j++) { cvmSet (src, i, j, cvRandReal (&rng)); printf ("% lf\t", cvmGet (src, i, j)); } printf ("\n"); } // (3) 行列srcの逆行列を求めて,行列dstに代入 det = cvInvert (src, dst, CV_SVD); // (4) 行列srcの行列式を表示 printf ("det(src)=%lf\n", det); // (5) 行列dstの表示 printf ("dst\n"); for (i = 0; i < dst->rows; i++) { for (j = 0; j < dst->cols; j++) { printf ("% lf\t", cvmGet (dst, i, j)); } printf ("\n"); } // (6) 行列srcとdstの積を計算して確認 cvMatMul (src, dst, mul); printf ("mul\n"); for (i = 0; i < mul->rows; i++) { for (j = 0; j < mul->cols; j++) { printf ("% lf\t", cvmGet (mul, i, j)); } printf ("\n"); } // (7) 行列のメモリを開放 cvReleaseMat (&src); cvReleaseMat (&dst); cvReleaseMat (&mul); return 0; }
void CvArrTest::get_test_array_types_and_sizes( int /*test_case_idx*/, CvSize** sizes, int** types ) { CvRNG* rng = ts->get_rng(); CvSize size; double val; int i, j; val = cvRandReal(rng) * (max_log_array_size - min_log_array_size) + min_log_array_size; size.width = cvRound( exp(val*CV_LOG2) ); val = cvRandReal(rng) * (max_log_array_size - min_log_array_size) + min_log_array_size; size.height = cvRound( exp(val*CV_LOG2) ); for( i = 0; i < max_arr; i++ ) { int count = test_array[i].size(); for( j = 0; j < count; j++ ) { sizes[i][j] = size; types[i][j] = CV_8UC1; } } }
/** * Creates a matrix for training but feeds it from images taken of folder * ./training; Only JPG images are taken into account and all shapes in those * images are clasified according to the first letter of the picture. */ void train() { training = true; mostrar = (flags & SH_T) != 0; listFiles(DIR_TR,training_image); fillMatrix(); //printMatrix(t_data); //printMatrix(t_resp); CvMat *vartype = cvCreateMat( t_data->cols + 1, 1, CV_8U ); unsigned char *vtype = vartype->data.ptr; //Tipos de variables de entrada al árbol vtype[0]=CV_VAR_NUMERICAL; vtype[1]=CV_VAR_NUMERICAL; vtype[2]=CV_VAR_NUMERICAL; vtype[3]=CV_VAR_NUMERICAL; vtype[4]=CV_VAR_CATEGORICAL; //Tipo de la salida del árbol if ((flags & F_CHK) != 0) { trainMask = cvCreateMat(t_data->rows, 1, CV_8U); unsigned char *x = trainMask->data.ptr; CvRNG seed = cvRNG(time(0)); for (int i=0; i<t_data->rows; i++,x++) { double p = cvRandReal(&seed); if (p < probTrain) { *x = 1; } else { *x=0; } } } ptree = new CvDTree; ptree->train(t_data,CV_ROW_SAMPLE,t_resp,0,trainMask,vartype,0,CvDTreeParams()); }
int DoubleSidesModelFitting::FullFit(const vector<CvPoint> & list_left, const vector<CvPoint> & list_right, int horizon, double& belief) { int max_counter = 0; //double max_weight = 0; for(int cycle_counter = 0; cycle_counter < DSMF_RANSAC_MAX_TIMES; ++cycle_counter) { int det_times = 0; //Randomly find a sample of points that are good for solving the equation. do { int rndind[4]; rndind[0] = cvFloor(cvRandReal(&rng) * total_length); do { rndind[1] = cvFloor(cvRandReal(&rng) * total_length); } while(rndind[1] == rndind[0]); do { rndind[2] = cvFloor(cvRandReal(&rng) * total_length); } while(rndind[2] == rndind[0] || rndind[2] == rndind[1]); do { rndind[3] = cvFloor(cvRandReal(&rng) * total_length); } while(rndind[3] == rndind[2] || rndind[3] == rndind[1] || rndind[3] == rndind[0]); for(int i = 0; i < 4; i++) { if(rndind[i] < static_cast<int>(list_left.size())) { const CvPoint & p = list_left.at(rndind[i]); cvmSet(matA, i, 0, 1.0 / (p.y - horizon)); cvmSet(matA, i, 1, p.y - horizon); cvmSet(matA, i, 2, 0.0); cvmSet(matA, i, 3, 1.0); cvmSet(matB, i, 0, p.x); } else { const CvPoint & p = list_right.at(rndind[i] - list_left.size()); cvmSet(matA, i, 0, 1.0 / (p.y - horizon)); cvmSet(matA, i, 1, 0.0); cvmSet(matA, i, 2, p.y - horizon); cvmSet(matA, i, 3, 1.0); cvmSet(matB, i, 0, p.x); } } } while(fabs(cvDet(matA)) < DSMF_RANSAC_DET_THRESHOLD && ++det_times < DSMF_RANSAC_MAX_DET_TIMES); if(det_times >= DSMF_RANSAC_MAX_DET_TIMES) continue; //Get a model as candidate. if(cvSolve(matA, matB, matX) == 0) continue; //Calculate the difference between the model and the actual points. cvGEMM(matFullA, matX, 1, matFullB, -1, matFullDiff); //Check whether the candidate is a good fit. int counter = 0; for(int i = 0; i < (int)total_length; i++) { if(fabs(cvmGet(matFullDiff, i, 0)) < DSMF_RANSAC_TOL) { counter++; } } //Check whether the model is the best one ever. If so, remember the points that are close to it. if(counter > max_counter) { cvCopy(matX, matOptX); max_counter = counter; filtered_index.clear(); for(int i = 0; i < (int)total_length; i++) { if(fabs(cvmGet(matFullDiff, i, 0)) < DSMF_RANSAC_TOL_STRICT) { filtered_index.push_back(i); } } } } //Check whether the best found model is a good fit (acceptable). if(filtered_index.size() > max(4, DSMF_RANSAC_WEIGHT_ACPT_RATE * max_counter)) { belief = (double)filtered_index.size() / max_counter; FilteredFit(); return DSMF_SUCCESS; } else { return DSMF_FAIL_NO_ACCEPTED_RESULT; } return DSMF_SUCCESS; }
float randfloat(const float min, const float max ){ return (float)cvRandReal( &rng_state )*(max+min) + min; }
float randfloat( ){ return (float)cvRandReal( &rng_state ); }
void CV_SolvePolyTest::run( int ) { CvRNG rng = cvRNG(); int fig = 100; double range = 50; double err_eps = 1e-4; for (int idx = 0, max_idx = 1000, progress = 0; idx < max_idx; ++idx) { progress = update_progress(progress, idx-1, max_idx, 0); int n = cvRandInt(&rng) % 13 + 1; std::vector<complex_type> r(n), ar(n), c(n + 1, 0); std::vector<double> a(n + 1), u(n * 2), ar1(n), ar2(n); int rr_odds = 3; // odds that we get a real root for (int j = 0; j < n;) { if (cvRandInt(&rng) % rr_odds == 0 || j == n - 1) r[j++] = cvRandReal(&rng) * range; else { r[j] = complex_type(cvRandReal(&rng) * range, cvRandReal(&rng) * range + 1); r[j + 1] = std::conj(r[j]); j += 2; } } for (int j = 0, k = 1 << n, jj, kk; j < k; ++j) { int p = 0; complex_type v(1); for (jj = 0, kk = 1; jj < n && !(j & kk); ++jj, ++p, kk <<= 1) ; for (; jj < n; ++jj, kk <<= 1) { if (j & kk) v *= -r[jj]; else ++p; } c[p] += v; } bool pass = false; double div = 0, s = 0; int cubic_case = idx & 1; for (int maxiter = 100; !pass && maxiter < 10000; maxiter *= 2, cubic_case = (cubic_case + 1) % 2) { for (int j = 0; j < n + 1; ++j) a[j] = c[j].real(); CvMat amat, umat; cvInitMatHeader(&amat, n + 1, 1, CV_64FC1, &a[0]); cvInitMatHeader(&umat, n, 1, CV_64FC2, &u[0]); cvSolvePoly(&amat, &umat, maxiter, fig); for (int j = 0; j < n; ++j) ar[j] = complex_type(u[j * 2], u[j * 2 + 1]); sort(r.begin(), r.end(), pred_complex()); sort(ar.begin(), ar.end(), pred_complex()); pass = true; if( n == 3 ) { ar2.resize(n); cv::Mat _umat2(3, 1, CV_64F, &ar2[0]), umat2 = _umat2; cvFlip(&amat, &amat, 0); int nr2; if( cubic_case == 0 ) nr2 = cv::solveCubic(cv::Mat(&amat),umat2); else nr2 = cv::solveCubic(cv::Mat_<float>(cv::Mat(&amat)), umat2); cvFlip(&amat, &amat, 0); if(nr2 > 0) sort(ar2.begin(), ar2.begin()+nr2, pred_double()); ar2.resize(nr2); int nr1 = 0; for(int j = 0; j < n; j++) if( fabs(r[j].imag()) < DBL_EPSILON ) ar1[nr1++] = r[j].real(); pass = pass && nr1 == nr2; if( nr2 > 0 ) { div = s = 0; for(int j = 0; j < nr1; j++) { s += fabs(ar1[j]); div += fabs(ar1[j] - ar2[j]); } div /= s; pass = pass && div < err_eps; } } div = s = 0; for (int j = 0; j < n; ++j) { s += fabs(r[j].real()) + fabs(r[j].imag()); div += sqrt(pow(r[j].real() - ar[j].real(), 2) + pow(r[j].imag() - ar[j].imag(), 2)); } div /= s; pass = pass && div < err_eps; } if (!pass) { ts->set_failed_test_info(CvTS::FAIL_INVALID_OUTPUT); ts->printf( CvTS::LOG, "too big diff = %g\n", div ); for (size_t j=0;j<ar2.size();++j) ts->printf( CvTS::LOG, "ar2[%d]=%g\n", j, ar2[j]); ts->printf(CvTS::LOG, "\n"); for (size_t j=0;j<r.size();++j) ts->printf( CvTS::LOG, "r[%d]=(%g, %g)\n", j, r[j].real(), r[j].imag()); ts->printf( CvTS::LOG, "\n" ); for (size_t j=0;j<ar.size();++j) ts->printf( CvTS::LOG, "ar[%d]=(%g, %g)\n", j, ar[j].real(), ar[j].imag()); break; } } }
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); }
/** * Main function * Controls the robot using the keyboard keys and outputs posture and velocity * related information. */ int main(int argc, char** argv) { // Open file to store robot poses outfile.open("Postures.txt"); // Create a window to show the map cv::namedWindow("Mapa", 0); // // Create map related variables // map.create(ceil(MAP_HEIGHT/MAP_RESOLUTION), ceil(MAP_WIDTH/MAP_RESOLUTION), CV_8UC1); // Set the initial map cell values to "undefined" map.setTo(127); // // Create robot related objects // // Linear and angular velocities for the robot (initially stopped) double lin_vel=0, ang_vel=0; double last_ang_vel = DEG2RAD(15); // Navigation variables bool avoid, new_rotation = false; double stop_front_dist, min_front_dist; // Random number generator (used for direction choice) CvRNG rnggstate = cvRNG(0xffffffff); // Init ROS ros::init(argc, argv, "tp4"); // ROS variables/objects ros::NodeHandle nh; // Node handle ros::Publisher vel_pub; // Velocity commands publisher geometry_msgs::Twist vel_cmd; // Velocity commands std::cout << "Random navigation with obstacle avoidance and map generation\n" << "---------------------------" << std::endl; // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", min_front_dist, 1.0); n_private.param("stop_front_dist", stop_front_dist, 0.6); /// Setup subscribers // Odometry ros::Subscriber sub_odom = nh.subscribe("/robot_0/odom", 1, odomCallback); // Laser scans ros::Subscriber sub_laser = nh.subscribe("/robot_0/base_scan", 1, laserCallback); // Point clouds ros::Subscriber sub_pcl = nh.subscribe("cloud_filtered", 1, pointCloudCallback); /// Setup publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/robot_0/cmd_vel", 1); // Infinite loop ros::Rate cycle(10.0); // Rate when no key is being pressed while(ros::ok()) { // Get data from the robot and print it if available ros::spinOnce(); // Only change navigation controls if laser was updated if( laser_updated == false ) continue; // show pose estimated from odometry std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3) << "Robot estimated pose = " << robot_pose.x << " [m], " << robot_pose.y << " [m], " << RAD2DEG(robot_pose.theta) << " [º]\n"; // Show estimated velocity std::cout << "Robot estimated velocity = " << true_lin_vel << " [m/s], " << RAD2DEG(true_ang_vel) << " [º/s]\n"; // Check for obstacles near the front of the robot avoid = false; if( closest_front_obstacle < min_front_dist ) { if( closest_front_obstacle < stop_front_dist ) { avoid = true; lin_vel = -0.100; } else { avoid = true; lin_vel = 0; } } else { lin_vel = 0.8; ang_vel = 0; new_rotation = false; } // Rotate to avoid obstacles if(avoid) { if( new_rotation == false ) { float rnd_point = cvRandReal(&rnggstate ); if( rnd_point >= 0.9 ) { last_ang_vel = -last_ang_vel; } } ang_vel = last_ang_vel; new_rotation = true; } // Limit maximum velocities // (not needed here) // lin_vel = clipValue(lin_vel, -MAX_LIN_VEL, MAX_LIN_VEL); // ang_vel = clipValue(ang_vel, -MAX_ANG_VEL, MAX_ANG_VEL); // Show desired velocity std::cout << "Robot desired velocity = " << lin_vel << " [m/s], " << RAD2DEG(lin_vel) << " [º/s]" << std::endl; // Send velocity commands vel_cmd.angular.z = ang_vel; vel_cmd.linear.x = lin_vel; vel_pub.publish(vel_cmd); // Terminate loop if Escape key is pressed if( cv::waitKey(10) == 27 ) break; // Proceed at desired framerate cycle.sleep(); } // If we are quitting, stop the robot vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Store map cv::imwrite("mapa.png", map); // Close file outfile.close(); return 1; }
void CV_POSITTest::run( int start_from ) { int code = CvTS::OK; /* fixed parameters output */ /*float rot[3][3]={ 0.49010f, 0.85057f, 0.19063f, -0.56948f, 0.14671f, 0.80880f, 0.65997f, -0.50495f, 0.55629f }; float trans[3] = { 0.0f, 0.0f, 40.02637f }; */ /* Some variables */ int i, counter; CvTermCriteria criteria; CvPoint3D32f* obj_points; CvPoint2D32f* img_points; CvPOSITObject* object; float angleX, angleY, angleZ; CvRNG* rng = ts->get_rng(); int progress = 0; CvMat* true_rotationX = cvCreateMat( 3, 3, CV_32F ); CvMat* true_rotationY = cvCreateMat( 3, 3, CV_32F ); CvMat* true_rotationZ = cvCreateMat( 3, 3, CV_32F ); CvMat* tmp_matrix = cvCreateMat( 3, 3, CV_32F ); CvMat* true_rotation = cvCreateMat( 3, 3, CV_32F ); CvMat* rotation = cvCreateMat( 3, 3, CV_32F ); CvMat* translation = cvCreateMat( 3, 1, CV_32F ); CvMat* true_translation = cvCreateMat( 3, 1, CV_32F ); const float flFocalLength = 760.f; const float flEpsilon = 0.1f; /* Initilization */ criteria.type = CV_TERMCRIT_EPS|CV_TERMCRIT_ITER; criteria.epsilon = flEpsilon; criteria.max_iter = 10000; /* Allocating source arrays; */ obj_points = (CvPoint3D32f*)cvAlloc( 8 * sizeof(CvPoint3D32f) ); img_points = (CvPoint2D32f*)cvAlloc( 8 * sizeof(CvPoint2D32f) ); /* Fill points arrays with values */ /* cube model with edge size 10 */ obj_points[0].x = 0; obj_points[0].y = 0; obj_points[0].z = 0; obj_points[1].x = 10; obj_points[1].y = 0; obj_points[1].z = 0; obj_points[2].x = 10; obj_points[2].y = 10; obj_points[2].z = 0; obj_points[3].x = 0; obj_points[3].y = 10; obj_points[3].z = 0; obj_points[4].x = 0; obj_points[4].y = 0; obj_points[4].z = 10; obj_points[5].x = 10; obj_points[5].y = 0; obj_points[5].z = 10; obj_points[6].x = 10; obj_points[6].y = 10; obj_points[6].z = 10; obj_points[7].x = 0; obj_points[7].y = 10; obj_points[7].z = 10; /* Loop for test some random object positions */ for( counter = start_from; counter < test_case_count; counter++ ) { ts->update_context( this, counter, true ); progress = update_progress( progress, counter, test_case_count, 0 ); /* set all rotation matrix to zero */ cvZero( true_rotationX ); cvZero( true_rotationY ); cvZero( true_rotationZ ); /* fill random rotation matrix */ angleX = (float)(cvTsRandReal(rng)*2*CV_PI); angleY = (float)(cvTsRandReal(rng)*2*CV_PI); angleZ = (float)(cvTsRandReal(rng)*2*CV_PI); true_rotationX->data.fl[0 *3+ 0] = 1; true_rotationX->data.fl[1 *3+ 1] = (float)cos(angleX); true_rotationX->data.fl[2 *3+ 2] = true_rotationX->data.fl[1 *3+ 1]; true_rotationX->data.fl[1 *3+ 2] = -(float)sin(angleX); true_rotationX->data.fl[2 *3+ 1] = -true_rotationX->data.fl[1 *3+ 2]; true_rotationY->data.fl[1 *3+ 1] = 1; true_rotationY->data.fl[0 *3+ 0] = (float)cos(angleY); true_rotationY->data.fl[2 *3+ 2] = true_rotationY->data.fl[0 *3+ 0]; true_rotationY->data.fl[0 *3+ 2] = -(float)sin(angleY); true_rotationY->data.fl[2 *3+ 0] = -true_rotationY->data.fl[0 *3+ 2]; true_rotationZ->data.fl[2 *3+ 2] = 1; true_rotationZ->data.fl[0 *3+ 0] = (float)cos(angleZ); true_rotationZ->data.fl[1 *3+ 1] = true_rotationZ->data.fl[0 *3+ 0]; true_rotationZ->data.fl[0 *3+ 1] = -(float)sin(angleZ); true_rotationZ->data.fl[1 *3+ 0] = -true_rotationZ->data.fl[0 *3+ 1]; cvMatMul( true_rotationX, true_rotationY, tmp_matrix); cvMatMul( tmp_matrix, true_rotationZ, true_rotation); /* fill translation vector */ true_translation->data.fl[2] = (float)(cvRandReal(rng)*(2*flFocalLength-40) + 40); true_translation->data.fl[0] = (float)((cvRandReal(rng)*2-1)*true_translation->data.fl[2]); true_translation->data.fl[1] = (float)((cvRandReal(rng)*2-1)*true_translation->data.fl[2]); /* calculate perspective projection */ for ( i = 0; i < 8; i++ ) { float vec[3]; CvMat Vec = cvMat( 3, 1, CV_MAT32F, vec ); CvMat Obj_point = cvMat( 3, 1, CV_MAT32F, &obj_points[i].x ); cvMatMul( true_rotation, &Obj_point, &Vec ); vec[0] += true_translation->data.fl[0]; vec[1] += true_translation->data.fl[1]; vec[2] += true_translation->data.fl[2]; img_points[i].x = flFocalLength * vec[0] / vec[2]; img_points[i].y = flFocalLength * vec[1] / vec[2]; } /*img_points[0].x = 0 ; img_points[0].y = 0; img_points[1].x = 80; img_points[1].y = -93; img_points[2].x = 245;img_points[2].y = -77; img_points[3].x = 185;img_points[3].y = 32; img_points[4].x = 32; img_points[4].y = 135; img_points[5].x = 99; img_points[5].y = 35; img_points[6].x = 247; img_points[6].y = 62; img_points[7].x = 195; img_points[7].y = 179; */ object = cvCreatePOSITObject( obj_points, 8 ); cvPOSIT( object, img_points, flFocalLength, criteria, rotation->data.fl, translation->data.fl ); cvReleasePOSITObject( &object ); code = cvTsCmpEps2( ts, rotation, true_rotation, flEpsilon, false, "rotation matrix" ); if( code < 0 ) goto _exit_; code = cvTsCmpEps2( ts, translation, true_translation, flEpsilon, false, "translation vector" ); if( code < 0 ) goto _exit_; } _exit_: cvFree( &obj_points ); cvFree( &img_points ); cvReleaseMat( &true_rotationX ); cvReleaseMat( &true_rotationY ); cvReleaseMat( &true_rotationZ ); cvReleaseMat( &tmp_matrix ); cvReleaseMat( &true_rotation ); cvReleaseMat( &rotation ); cvReleaseMat( &translation ); cvReleaseMat( &true_translation ); if( code < 0 ) ts->set_failed_test_info( code ); }
int main(int argc, const char * argv[]) { IplImage* img = cvCreateImage( cvSize( 500, 500 ), 8, 3 ); CvRNG rng = cvRNG(-1); cvNamedWindow( "fitline", 1 ); for(;;) { char key; int i, count = cvRandInt(&rng) % 100 + 1, outliers = count/5; float a = cvRandReal(&rng) * 200; float b = cvRandReal(&rng) * 40; float angle = cvRandReal(&rng) * CV_PI; float cos_a = cos(angle), sin_a = sin(angle); CvPoint pt1, pt2; CvPoint* points = (CvPoint*)malloc( count * sizeof(points[0])); CvMat pointMat = cvMat( 1, count, CV_32SC2, points ); float line[4]; float d, t; b = MIN(a*0.3, b); // generate some points that are close to the line for( i = 0; i < count - outliers; i++ ) { float x = (cvRandReal(&rng)*2-1)*a; float y = (cvRandReal(&rng)*2-1)*b; points[i].x = cvRound(x*cos_a - y*sin_a + img->width/2); points[i].y = cvRound(x*sin_a + y*cos_a + img->height/2); } // generate "completely off" points for( ; i < count; i++ ) { points[i].x = cvRandInt(&rng) % img->width; points[i].y = cvRandInt(&rng) % img->height; } // find the optimal line cvFitLine( &pointMat, CV_DIST_L1, 1, 0.001, 0.001, line ); cvZero( img ); // draw the points for( i = 0; i < count; i++ ) { cvCircle( img, points[i], 2, i < count - outliers ? CV_RGB(255, 0, 0) : CV_RGB(255,255,0), CV_FILLED, CV_AA, 0 ); } d = sqrt((double)line[0]*line[0] + (double)line[1]*line[1]); line[0] /= d; line[1] /= d; t = (float)(img->width + img->height); pt1.x = cvRound(line[2] - line[0]*t); pt1.y = cvRound(line[3] - line[1]*t); pt2.x = cvRound(line[2] + line[0]*t); pt2.y = cvRound(line[3] + line[1]*t); cvLine( img, pt1, pt2, CV_RGB(0,255,0), 3, CV_AA, 0 ); cvShowImage( "fitline", img ); key = (char) cvWaitKey(0); if( key == 27 ) break; free( points ); } cvDestroyWindow( "fitline" ); return 0; }
void CV_SolvePolyTest::run( int start_from ) { CvRNG rng = cvRNG(); int fig = 100; double range = 50; for (int idx = 0, max_idx = 1000, progress = 0; idx < max_idx; ++idx) { int n = cvRandInt(&rng) % 13 + 1; std::vector<complex_type> r(n), ar(n), c(n + 1, 0); std::vector<double> a(n + 1), u(n * 2); int rr_odds = 3; // odds that we get a real root for (int j = 0; j < n;) { if (cvRandInt(&rng) % rr_odds == 0 || j == n - 1) r[j++] = cvRandReal(&rng) * range; else { r[j] = complex_type(cvRandReal(&rng) * range, cvRandReal(&rng) * range + 1); r[j + 1] = std::conj(r[j]); j += 2; } } for (int j = 0, k = 1 << n, jj, kk; j < k; ++j) { int p = 0; complex_type v(1); for (jj = 0, kk = 1; jj < n && !(j & kk); ++jj, ++p, kk <<= 1); for (; jj < n; ++jj, kk <<= 1) if (j & kk) v *= -r[jj]; else ++p; c[p] += v; } bool pass = false; double div; for (int maxiter = 10; !pass && maxiter < 10000; maxiter *= 2) { for (int j = 0; j < n + 1; ++j) a[j] = c[j].real(); CvMat amat, umat; cvInitMatHeader(&amat, n + 1, 1, CV_64FC1, &a[0]); cvInitMatHeader(&umat, n, 1, CV_64FC2, &u[0]); cvSolvePoly(&amat, &umat, maxiter, fig); for (int j = 0; j < n; ++j) ar[j] = complex_type(u[j * 2], u[j * 2 + 1]); sort(r.begin(), r.end(), pred_complex()); sort(ar.begin(), ar.end(), pred_complex()); div = 0; double s = 0; for (int j = 0; j < n; ++j) { s += r[j].real() + fabs(r[j].imag()); div += pow(r[j].real() - ar[j].real(), 2) + pow(r[j].imag() - ar[j].imag(), 2); } div /= s; pass = div < 1e-2; } if (!pass) { ts->set_failed_test_info(CvTS::FAIL_INVALID_OUTPUT); std::cerr<<std::endl; for (unsigned int j=0; j<r.size(); ++j) std::cout << "r[" << j << "] = " << r[j] << std::endl; std::cout << std::endl; for (unsigned int j=0; j<ar.size(); ++j) std::cout << "ar[" << j << "] = " << ar[j] << std::endl; } progress = update_progress(progress, idx-1, max_idx, 0); } }
/* _lib7_OpenCV_cvRandReal : Random_Number_Generator -> Float * */ Val _lib7_OpenCV_cvRandReal (Task *task, Val arg) { #if HAVE_OPENCV_CV_H && HAVE_LIBCV CvRNG* rng = GET_VECTOR_DATACHUNK_AS( CvRNG*, arg ); double random_float64 = cvRandReal( rng ); return make_float64(task, random_float64 ); #else extern char* no_opencv_support_in_runtime; return RAISE_ERROR__MAY_HEAPCLEAN(task, no_opencv_support_in_runtime, NULL); #endif } // Code by Jeff Prothero: Copyright (c) 2010-2012, // released under Gnu Public Licence version 3.