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.
