void ReadInCovMatrix(char* filename,int numFeatures,CvMat * mat)
{
fprintf(stderr,"1\n");
FILE *file=NULL;
int i,j;
double element;
double *dp = &element;
if((file=fopen(filename,"r"))==NULL)fprintf(stderr,"Error: can't open file\n");
fprintf(stderr,"2\n");
//float element_3_2 = 7.7;
for(j=0;j<numFeatures;j++)
{
for(i=0;i<numFeatures-1;i++)
{
fscanf(file,"%lf ",dp);
*( (double*)CV_MAT_ELEM_PTR( *mat, j,i ) ) = element;
}
fscanf(file,"%lf\n",dp);
*( (double*)CV_MAT_ELEM_PTR( *mat, j,i ) ) = element;
}
for(j=0;j<numFeatures;j++)
{
for(i=0;i<numFeatures;i++)
{
fprintf(stderr,"%lf ",CV_MAT_ELEM( *mat, double, j, i ));
}
fprintf(stderr, "\n");
}
fclose(file);
}
int main(){
CvMat * mat = cvCreateMat(2,2,CV_32FC3);
float arr_1_1[] = {1.0 , 2.0 , 3.0};
*((float **)CV_MAT_ELEM_PTR(*mat , 1 , 1)) = arr_1_1;
float * arr = *((float **)CV_MAT_ELEM_PTR( *mat , 1 , 1));
/*printf("%f",scalar);*/
printf("%f\t%f\t%f\n",arr[0],arr[1],arr[2]);
CvScalar scalar = cvGet2D(mat , 1 , 1);
printf("%f\t%f\t%f\n",scalar.val[0],scalar.val[1],scalar.val[2]);
}
////////////////////////////create high pass filter//////////////////////////////////////////////
// for using gaussian high pass filter
void CLightSet::lpfilter(CvMat *matD,CvMat *matH,float D0,float rH,float rL,float c)
{
if (D0 < 0)
{
//cout<<"ERROR! D0 MUST BE POSITIVE"<<endl;
return ;
}
int w = matD->rows;
int h = matD->cols;
for (int u = 0; u < w; ++u)
for (int v =0 ;v < h; ++v)
{
float Elem_D,h;
Elem_D = CV_MAT_ELEM(*matD,float,u,v);
h = exp(-(c*Elem_D*Elem_D)/(2*D0*D0));
h = (rH - rL)*(1 - h) + rL;
*( (float *)CV_MAT_ELEM_PTR(*matH,u,v) ) = h;
}
}
int main(int argc, const char * argv[]) {
CvMat *cmatrix = cvCreateMat(5,5,CV_32FC1);
float element_3_2 = 7.7;
*((float*)CV_MAT_ELEM_PTR( *cmatrix, 3,2) ) = element_3_2;
cvmSet(cmatrix,4,4,0.5000);
cvSetReal2D(cmatrix,3,3,0.5000);
CvFileStorage* fs1 = cvOpenFileStorage("cfg.xml", 0, CV_STORAGE_WRITE);
cvWriteInt( fs1, "frame_count", 10 );
cvStartWriteStruct( fs1, "frame_size", CV_NODE_SEQ );
cvWriteInt( fs1 , 0, 320 );
cvWriteInt( fs1 , 0, 200 );
cvEndWriteStruct( fs1 );
cvWrite( fs1, "color_cvt_matrix", cmatrix );
cvReleaseFileStorage( &fs1 );
CvFileStorage* fs2 = cvOpenFileStorage("cfg.xml", 0, CV_STORAGE_READ);
int frame_count = cvReadIntByName( fs2 , 0, "frame_count");
CvSeq* s = cvGetFileNodeByName( fs2,0,"frame_size" )->data.seq;
int frame_width = cvReadInt( (CvFileNode*)cvGetSeqElem(s,0) );
int frame_height = cvReadInt( (CvFileNode*)cvGetSeqElem(s,1) );
CvMat* color_cvt_matrix = (CvMat*) cvReadByName( fs2, 0 , "color_cvt_matrix");
printf("color_cvt_matrix: width=%d, height=%d\n",color_cvt_matrix->width, color_cvt_matrix->height );
printf("frame_count=%d, frame_width=%d, frame_height=%d\n",frame_count,frame_width,frame_height);
cvReleaseFileStorage( &fs2 );
return 0;
}
////////////////////////compute the meshgrid arrays needed for LPF//////////////////////////////////////////////////
// CDM compute meshgrid frequency matrices (ok!)
// see Gonzalez Digital image processing using matlab page93 function dftuv
void CLightSet::CDM(int M,int N,CvMat *mat)
{
int width = mat->rows;
int height = mat->cols;
if (M != width && N != height)
{
//cout<<"ERROR! THE SIZE DOES NOT MATCH WITH MAT"<<endl;
return;
}
if (cvGetElemType(mat) < CV_32F)
{
//cout<<"ERROR! THE TYPE DOES NOT MATCH WITH MAT"<<endl;
return;
}
CvMat *U,*V;
U = cvCreateMat(M,N,CV_32FC1);
V = cvCreateMat(M,N,CV_32FC1);
for (int u = 0; u < M; ++u)
for (int v =0 ;v < N; ++v)
{
float tm1,tm2;
tm1 = (float)((u > cvRound(M/2))?u-M:u);
tm2 = (float)((v > cvRound(N/2))?v-N:v);
*( (float *)CV_MAT_ELEM_PTR(*U,u,v) ) = tm1;
*( (float *)CV_MAT_ELEM_PTR(*V,u,v) ) = tm2;
}
for ( int u = 0; u < M; ++u)
for (int v =0 ;v < N; ++v)
{
float t1,t2;
t1 = CV_MAT_ELEM(*U,float,u,v);
t2 = CV_MAT_ELEM(*V,float,u,v);
*( (float *)CV_MAT_ELEM_PTR(*mat,u,v) ) = sqrt(t1*t1 + t2*t2);
}
}
int Recognize(double * Features,CvArr** eigenVects,CvArr** eigenVals,CvArr** Means,int numGestures,int numFeatures,int use)
{
int i,j,z;
double d=10000;
double d2,tmp;
int choice=0;
//double mahal=0;
CvArr* input=cvCreateMat(1,numFeatures,CV_64FC1);
CvArr* tempSub=cvCreateMat(1,numFeatures,CV_64FC1);
CvArr* tempEV=cvCreateMat(1,numFeatures,CV_64FC1);
for(i=0;i<numFeatures;i++)
{
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)input), 0,i ) ) = Features[i];
}
for(i=0;i<numGestures;i++)
{
for(j=0;j<use;j++)
{
cvSub(input,Means[i],tempSub,NULL);
//cvTranspose(tempSub,tempTran);
for(z=0;z<numFeatures;z++)
{
//tempEV=eigenVects[i]
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tempEV), 0,z ) )=CV_MAT_ELEM( *((CvMat *)eigenVects[i]), double, j, z );
}
tmp=(double)(cvDotProduct(tempSub,tempEV));//ut(x-u)/lambda
d2+=(tmp*tmp)/(double)(CV_MAT_ELEM( *((CvMat *)eigenVals[i]), double, 0, j ));
}
d2=sqrt(d2);
fprintf(stderr,"for %d the distance is %lf\n",i,d2);
if (d2<d)
{
//fprintf(stderr,"for %d the distance is %lf\n",i,d2);
d=d2;
choice=i;
}
d2=0;
}
return choice;
}
int main(int argc, char** argv)
{
CvMat *mat = cvCreateMat(5,5,CV_32FC1);
float element_3_2 = 7.7;
*((float*)CV_MAT_ELEM_PTR( *mat, 3,2) ) = element_3_2;
cvmSet(mat,4,4,0.5000);
cvSetReal2D(mat,3,3,0.5000);
float s = sum(mat);
printf("%f\n",s);
return 0;
}
CvMat cvmat_remove_column(const CvMat*mat, int column)
{
assert(column<mat->cols && column>=0);
assert(mat->cols > 1);
CvMat new_mat = cvMat(mat->rows, mat->cols-1, mat->type, malloc(mat->rows*mat->cols*8));
int t;
int size = CV_ELEM_SIZE(mat->type);
for(t=0;t<mat->rows;t++) {
int pos = 0;
int s;
for(s=0;s<mat->cols;s++) {
if(s!=column) {
memcpy(CV_MAT_ELEM_PTR(new_mat, t, pos), CV_MAT_ELEM_PTR((*mat), t, s), size);
pos++;
}
}
}
return new_mat;
}
/**
* Trying to figure out how this works based on
* http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html#findextrinsiccameraparams2
*/
void test_cvFindExtrinsicCameraParams2()
{
// The array of object points in the object coordinate space, 3xN or Nx3 1-channel, or 1xN or Nx1 3-channel, where N is the number of points.
CvMat* object_points = cvCreateMat( 3, 3, CV_32FC1);
// The array of corresponding image points, 2xN or Nx2 1-channel or 1xN or Nx1 2-channel, where N is the number of points.
CvMat* image_points = cvCreateMat( 3, 3, CV_32FC1);
// camera and lens model
CvMat* intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1);
CvMat* distortion_coeffs = NULL; // If it is NULL, all of the distortion coefficients are set to 0
// output
CvMat* rotation_vector = cvCreateMat( 3, 1, CV_32F);
CvMat* translation_vector = cvCreateMat( 3, 1, CV_32F);
// Using the RGB camera intrinsics calculated at http://nicolas.burrus.name/index.php/Research/KinectCalibration
float fx_rgb = 5.2921508098293293e+02;
float fy_rgb = 5.2556393630057437e+02;
float cx_rgb = 3.2894272028759258e+02;
float cy_rgb = 2.6748068171871557e+02;
// Camera intrinsic matrix:
// [ fx, 0, cx ]
// K = [ 0, fx, cy ]
// [ 0, 0, 1 ]
*( (float*)CV_MAT_ELEM_PTR( *intrinsic_matrix, 0, 0 ) ) = fx_rgb;
*( (float*)CV_MAT_ELEM_PTR( *intrinsic_matrix, 1, 1 ) ) = fy_rgb;
*( (float*)CV_MAT_ELEM_PTR( *intrinsic_matrix, 0, 2 ) ) = cx_rgb;
*( (float*)CV_MAT_ELEM_PTR( *intrinsic_matrix, 1, 2 ) ) = cy_rgb;
*( (float*)CV_MAT_ELEM_PTR( *intrinsic_matrix, 2, 2 ) ) = 1.0;
cvFindExtrinsicCameraParams2( object_points, image_points, intrinsic_matrix, distortion_coeffs, rotation_vector, translation_vector, 0);
// release the data
cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
cvReleaseMat(&intrinsic_matrix);
cvReleaseMat(&distortion_coeffs);
cvReleaseMat(&rotation_vector);
cvReleaseMat(&translation_vector);
}
// experimenting with http://opencv.willowgarage.com/documentation/operations_on_arrays.html#cvSolve
void test_cvSolve()
{
// coefficient matrix
CvMat* matA = cvCreateMat( 2, 2, CV_32FC1 );
cvSetZero(matA);
*( (float*)CV_MAT_ELEM_PTR( *matA, 0, 0 ) ) = 3;
*( (float*)CV_MAT_ELEM_PTR( *matA, 0, 1 ) ) = -1;
*( (float*)CV_MAT_ELEM_PTR( *matA, 1, 0 ) ) = -5;
*( (float*)CV_MAT_ELEM_PTR( *matA, 1, 1 ) ) = 4;
CvMat* matB = cvCreateMat( 2, 1, CV_32FC1 );
cvSetZero(matB);
*( (float*)CV_MAT_ELEM_PTR( *matB, 0, 0 ) ) = 7;
*( (float*)CV_MAT_ELEM_PTR( *matB, 1, 0 ) ) = -2;
CvMat* matX = cvCreateMat( 2, 1, CV_32FC1 );
cvSetZero(matX);
int return_code = cvSolve( matA, matB, matX, CV_LU);
float x = CV_MAT_ELEM( *matX, float, 0, 0 );
float y = CV_MAT_ELEM( *matX, float, 1, 0 );
if (!return_code)
printf("Solution (x,y) = (%f, %f)\n", x, y);
else
printf("No solution found");
cvReleaseMat(&matA);
cvReleaseMat(&matB);
cvReleaseMat(&matX);
}
void ReadInMeans(char* filename,int numFeatures,CvMat ** Means,int numGestures)
{
fprintf(stderr,"1\n");
FILE *file=NULL;
int i,j;
double element;
double *dp = &element;
if((file=fopen(filename,"r"))==NULL)fprintf(stderr,"Error: can't open file\n");
fprintf(stderr,"2\n");
//float element_3_2 = 7.7;
for(j=0;j<numGestures;j++)
{
Means[j]=cvCreateMat(1,numFeatures,CV_64FC1);
for(i=0;i<numFeatures-1;i++)
{
fscanf(file,"%lf ",dp);
//fprintf(stderr,"j is :%d, and i is %d\n",j,i);
*( (double*)CV_MAT_ELEM_PTR( *(Means[j]), 0,i ) ) = element;
}
fscanf(file,"%lf",dp);
*( (double*)CV_MAT_ELEM_PTR( *(Means[j]), 0,i ) ) = element;
}
for(j=0;j<numGestures;j++)
{
for(i=0;i<numFeatures-1;i++)
{
fprintf(stderr,"%lf ",CV_MAT_ELEM( *Means[j], double, 0, i ));
}
fprintf(stderr,"%lf ",CV_MAT_ELEM( *Means[j], double, 0, i ));
}
}
int main()
{
// 创建矩阵，行列类型
CvMat* mat = cvCreateMat( 5, 5, CV_32FC1 );
// 为矩阵设置一个数值
float element_3_2 = 7.7;
*( (float*)CV_MAT_ELEM_PTR( *mat, 3, 2 ) ) = element_3_2;
// below from example ch3_ex3_8.txt
cvmSet( mat, 2, 2, 0.5000 );
cvSetReal2D( mat, 3, 3, 0.3300 );
printf("Exercise 3_5, matrix created and accessed [3,2]=%f, [2,2]=%f, [3,3]=%f\n",CV_MAT_ELEM( *mat, float, 3, 2 ),CV_MAT_ELEM( *mat, float, 2, 2 ),CV_MAT_ELEM( *mat, float, 3, 3 ));
}
int main(int argc, char** argv) {
CvMat *cmatrix = cvCreateMat(5, 5, CV_32FC1);
float element_3_2 = 7.7;
*((float*) CV_MAT_ELEM_PTR( *cmatrix, 3,2)) = element_3_2;
cvmSet(cmatrix, 4, 4, 0.5000);
cvSetReal2D(cmatrix, 3, 3, 0.5000);
printf("Example 3_17, writing cfg.xml\n");
CvFileStorage* fs = cvOpenFileStorage("cfg.xml", 0, CV_STORAGE_WRITE);
cvWriteInt(fs, "frame_count", 10);
cvStartWriteStruct(fs, "frame_size", CV_NODE_SEQ);
cvWriteInt(fs, 0, 320);
cvWriteInt(fs, 0, 200);
cvEndWriteStruct(fs);
cvWrite(fs, "color_cvt_matrix", cmatrix);
cvReleaseFileStorage(&fs);
return 0;
}
void train(CvArr **Means,CvArr **eigenVects,CvArr ** eigenVals,int numGestures,int numTraining,int numFeatures, int indx[]) {
int two=2;
char num[] ="01-01";
double *Features;
char *fo = ".png";
char filename[] = "../../../TrainingData/01-01.png";
int i,j,k;
double avg;
CvScalar t;
CvArr *tmpCov = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
// CvArr *tmpEigVec = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
//1CvArr **tmp= (CvArr **)malloc(sizeof(CvArr *)*numFeatures);
CvArr **tmp= (CvArr **)malloc(sizeof(CvArr *)*numTraining);
for(k=0; k<numTraining; k++) //1numFeatures
tmp[k] = cvCreateMat(1,numFeatures,CV_64FC1); //1tmp[k] = cvCreateMat(1,numTraining,CV_64FC1);
IplImage* src;
for(i=0; i< numGestures; i++) {
Means[i] = cvCreateMat(1,numFeatures,CV_64FC1);
for(j=0;j<numTraining; j++) {
filename[22] = '\0';
num[0] = '0'+indx[i]/10 ;
num[1] = '0'+indx[i]%10;
if((j+1)>9) {
num[3] = '0'+(j+1)/10;
num[4] = '0'+(j+1)%10;
num[5] = '\0';
}
else {
num[3] = '0'+j+1;
num[4] = '\0';
}
strcat(filename,num);
strcat(filename,fo);
//fprintf(stderr,"i=%d j=%d %s \n",i,j,filename);
src = cvLoadImage( filename,CV_LOAD_IMAGE_GRAYSCALE );
Features=computeFDFeatures(src,numFeatures);
//fprintf(stderr,"got contour\n");
for(k=0; k < numFeatures; k++)
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmp[j]),0,k ) ) = Features[k]; //1*((CvMat *)tmp[k]),0,j
//fprintf(stderr,"copied values\n");
free(Features);
//cvReleaseImage( &src );
}
/*for(k=0;k<numFeatures;k++) {
avg=0;
for(j=0;j<numGestures;j++)
avg = avg+CV_MAT_ELEM( *((CvMat*)tmp[k]), double, 0, j );
avg=avg/(double)numGestures;
//fprintf(stderr,"%lf\n",t.val[0]);
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)Means[i]),0,k ) ) = avg;
}*/
// print the feature vectors
/*for(k=0;k<numTraining;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmp[k]),0,j ) ));
fprintf(stderr,";\n",i);
}
fprintf(stderr,"covs now\n");*/
//for(i=0;i<numGestures;i++) {
cvCalcCovarMatrix( tmp, numTraining, tmpCov, Means[i],CV_COVAR_SCALE|CV_COVAR_NORMAL); //Means[i] , |2
fprintf(stderr,"%d\n",i);
for(k=0;k<numFeatures;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmpCov),k,j ) ));
fprintf(stderr,";\n",i);
}
eigenVects[i]=cvCreateMat(numFeatures,numFeatures,CV_64FC1);
eigenVals[i]=cvCreateMat(1,numFeatures,CV_64FC1);
cvEigenVV(tmpCov,eigenVects[i],eigenVals[i],0);
fprintf(stderr,"Eigenvalues:\n");
for(k=0;k<numFeatures;k++)
{
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)eigenVals[i]),0,k ) ));
}
fprintf(stderr,";\n",i);
for(k=0;k<numFeatures;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)eigenVects[i]),k,j ) ));
fprintf(stderr,";\n",i);
}
//invCovMat[i] = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
//cvInvert(tmpCov,invCovMat[i],CV_SVD);
//}
}
//fprintf(stderr,"found averages\n");
/*for(i=0;i<numGestures;i++) {
fprintf(stderr,"i=%d ",i);
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)Means[i]),0,j ) ));
fprintf(stderr,"\n",i);
}*/
}
/*
* Calculate 3D information
*/
void dc_viewer::calculate3DData()
{
//TODO: points
//X = (u - 320) * depth_md_[k] * pixel_size_ * 0.001 / F_;
//Y = (v - 240) * depth_md_[k] * pixel_size_ * 0.001 / F_;
//Z = depth_md_[k] * 0.001; // from mm in meters!
int depthHeight = this->depthImage->height;
int depthWidth = this->depthImage->width;
int y,x;
int offsetX = this->boundingBox.x();
int offsetY = this->boundingBox.y();
double pxlSize = pixelSize;
double fcsLength = focusLength;
double X,Y,Z,XYZ;
char *data = this->depthImage->imageData;
uchar tmpDepth = 0;
int channels = this->depthImage->nChannels;
int numOfValidPoints = 0;
this->meanX = 0;
this->meanY = 0;
this->meanZ = 0;
//point cloud
for ( y=0; y<depthHeight; y++, data += depthImage->widthStep) {
for( x=0; x<depthWidth; x++) {
tmpDepth = uchar(data[x*channels]);
X = double(x+offsetX - 320) * depthLU[tmpDepth] * pxlSize * 0.001 / fcsLength;
Y = double(y+offsetY - 240) * depthLU[tmpDepth] * pxlSize * 0.001 / fcsLength;
Z = depthLU[tmpDepth] * 0.001; // from mm in meters!
*((float*) CV_MAT_ELEM_PTR(*(this->pointData),x+y*depthWidth,0)) = X;
*((float*) CV_MAT_ELEM_PTR(*(this->pointData),x+y*depthWidth,1)) = Y;
*((float*) CV_MAT_ELEM_PTR(*(this->pointData),x+y*depthWidth,2)) = Z;
if (tmpDepth != 0) {
meanX += X;
meanY += Y;
meanZ += Z;
minX = minX<X? minX:X;
minY = minY<Y? minY:Y;
minZ = minZ<Z? minZ:Z;
maxX = maxX>X? maxX:X;
maxY = maxY>Y? maxY:Y;
maxZ = maxZ>Z? maxZ:Z;
numOfValidPoints++;
}
}
}
meanX /= double(numOfValidPoints);
meanY /= double(numOfValidPoints);
meanZ /= double(numOfValidPoints);
//normal
float buffer[NORMAL_NEIGHBORHOOD_SIZE_SQUARE_BY_3] = {0};
float bufferZ[NORMAL_NEIGHBORHOOD_SIZE_SQUARE] = {0};
CvMat *tmpMatLeft;
CvMat *tmpMatRight;
CvMat *tmpResult;
int normNum;
data = this->depthImage->imageData;
for ( y=0; y<depthHeight; y++, data += depthImage->widthStep) {
for( x=0; x<depthWidth; x++) {
tmpDepth = uchar(data[x*channels]);
normNum = 0;
if (tmpDepth) {
//get neighborhood
for ( int ii=-NORMAL_NEIGHBORHOOD_SIZE_HALF; ii<NORMAL_NEIGHBORHOOD_SIZE_HALF+1; ii++) {
for ( int jj=-NORMAL_NEIGHBORHOOD_SIZE_HALF; jj<NORMAL_NEIGHBORHOOD_SIZE_HALF+1; jj++) {
if (isInBoundingBox(x+ii,y+jj)) {
tmpDepth = uchar(data[(x+ii)*channels+jj*depthImage->widthStep]);
if (tmpDepth) {
buffer[normNum*3] = CV_MAT_ELEM(*(this->pointData),float,(x+ii)+(y+jj)*depthWidth,0);
buffer[normNum*3+1] = CV_MAT_ELEM(*(this->pointData),float,(x+ii)+(y+jj)*depthWidth,1);
buffer[normNum*3+2] = 1;
bufferZ[normNum] = CV_MAT_ELEM(*(this->pointData),float,(x+ii)+(y+jj)*depthWidth,2);
normNum++;
}
}
}
}
//calculate normal
if (normNum >= NORMNUM_THRESHOLD) {
tmpMatLeft = cvCreateMat(normNum,3,CV_32FC1);
tmpResult = cvCreateMat(3,1,CV_32FC1);
tmpMatRight = cvCreateMat(normNum,1,CV_32FC1);
cvSetData(tmpMatLeft, buffer, tmpMatLeft->step);
cvSetData(tmpMatRight, bufferZ, tmpMatRight->step);
cvSolve(tmpMatLeft, tmpMatRight, tmpResult, CV_SVD);
X = CV_MAT_ELEM(*(tmpResult),float,0,0);
Y = CV_MAT_ELEM(*(tmpResult),float,1,0);
Z = -1;
XYZ = sqrt(X*X+Y*Y+Z*Z);
X /= XYZ;
Y /= XYZ;
Z /= XYZ;
*((float*) CV_MAT_ELEM_PTR(*(this->normalData),x+y*depthWidth,0)) = X;
*((float*) CV_MAT_ELEM_PTR(*(this->normalData),x+y*depthWidth,1)) = Y;
*((float*) CV_MAT_ELEM_PTR(*(this->normalData),x+y*depthWidth,2)) = Z;
cvReleaseMat(&tmpMatLeft);
cvReleaseMat(&tmpResult);
cvReleaseMat(&tmpMatRight);
}
//calculate residue
}
}
int main(int argc, char* argv[])
{
int height,width,step,channels; //parameters of the image we are working on
int i,j,k,t1min=0,t1max=0,t2min=0,t2max=0,t3min=0,t3max=0; // other variables used
CvMat* threshold_matrix = cvCreateMat(2,3,CV_32FC1);
CvFileStorage* temp = cvOpenFileStorage("threshold_matrix.xml",NULL,CV_STORAGE_READ);
// Load the previous values of the threshold if they exist
if(temp)
{
threshold_matrix = (CvMat*)cvLoad("threshold_matrix.xml");
t1min =(int) CV_MAT_ELEM(*threshold_matrix,float,0,0) ;t2min =(int) CV_MAT_ELEM(*threshold_matrix,float,0,1) ;t3min =(int) CV_MAT_ELEM(*threshold_matrix,float,0,2);
t1max =(int) CV_MAT_ELEM(*threshold_matrix,float,1,0) ;t2max =(int) CV_MAT_ELEM(*threshold_matrix,float,1,1) ;t3max =(int) CV_MAT_ELEM(*threshold_matrix,float,1,2) ;
}
// Open capture device. 0 is /dev/video0, 1 is /dev/video1, etc.
CvCapture* capture = cvCaptureFromCAM( 1 );
if( !capture )
{
fprintf( stderr, "ERROR: capture is NULL \n" );
getchar();
return -1;
}
// grab an image from the capture
IplImage* frame = cvQueryFrame( capture );
// Create a window in which the captured images will be presented
cvNamedWindow( "Camera", CV_WINDOW_AUTOSIZE );
cvNamedWindow( "HSV", CV_WINDOW_AUTOSIZE );
cvNamedWindow( "F1", CV_WINDOW_AUTOSIZE );
cvNamedWindow( "F2", CV_WINDOW_AUTOSIZE );
cvNamedWindow( "F3", CV_WINDOW_AUTOSIZE );
//cvNamedWindow( "EdgeDetection", CV_WINDOW_AUTOSIZE );
/// Create Trackbars
char TrackbarName1[50]="t1min";
char TrackbarName2[50]="t1max";
char TrackbarName3[50]="t2min";
char TrackbarName4[50]="t2max";
char TrackbarName5[50]="t3min";
char TrackbarName6[50]="t3max";
cvCreateTrackbar( TrackbarName1, "F1", &t1min, 260 , NULL );
cvCreateTrackbar( TrackbarName2, "F1", &t1max, 260, NULL );
cvCreateTrackbar( TrackbarName3, "F2", &t2min, 260 , NULL );
cvCreateTrackbar( TrackbarName4, "F2", &t2max, 260, NULL );
cvCreateTrackbar( TrackbarName5, "F3", &t3min, 260 , NULL );
cvCreateTrackbar( TrackbarName6, "F3", &t3max, 260, NULL );
// Load threshold from the slider bars in these 2 parameters
CvScalar hsv_min = cvScalar(t1min, t2min, t3min, 0);
CvScalar hsv_max = cvScalar(t1max, t2max ,t3max, 0);
// get the image data
height = frame->height;
width = frame->width;
step = frame->widthStep;
// capture size -
CvSize size = cvSize(width,height);
// Initialize different images that are going to be used in the program
IplImage* hsv_frame = cvCreateImage(size, IPL_DEPTH_8U, 3); // image converted to HSV plane
IplImage* thresholded = cvCreateImage(size, IPL_DEPTH_8U, 1); // final thresholded image
IplImage* thresholded1 = cvCreateImage(size, IPL_DEPTH_8U, 1); // Component image threshold
IplImage* thresholded2 = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* thresholded3 = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* filtered = cvCreateImage(size, IPL_DEPTH_8U, 1); //smoothed image
while( 1 )
{
// Load threshold from the slider bars in these 2 parameters
hsv_min = cvScalar(t1min, t2min, t3min, 0);
hsv_max = cvScalar(t1max, t2max ,t3max, 0);
// Get one frame
frame = cvQueryFrame( capture );
if( !frame )
{
fprintf( stderr, "ERROR: frame is null...\n" );
getchar();
break;
}
// Covert color space to HSV as it is much easier to filter colors in the HSV color-space.
cvCvtColor(frame, hsv_frame, CV_BGR2HSV);
// Filter out colors which are out of range.
cvInRangeS(hsv_frame, hsv_min, hsv_max, thresholded);
// the below lines of code is for visual purpose only remove after calibration
//--------------FROM HERE-----------------------------------
//Split image into its 3 one dimensional images
cvSplit( hsv_frame,thresholded1, thresholded2, thresholded3, NULL );
// Filter out colors which are out of range.
cvInRangeS(thresholded1,cvScalar(t1min,0,0,0) ,cvScalar(t1max,0,0,0) ,thresholded1);
cvInRangeS(thresholded2,cvScalar(t2min,0,0,0) ,cvScalar(t2max,0,0,0) ,thresholded2);
cvInRangeS(thresholded3,cvScalar(t3min,0,0,0) ,cvScalar(t3max,0,0,0) ,thresholded3);
//-------------REMOVE OR COMMENT AFTER CALIBRATION TILL HERE ------------------
// Memory for hough circles
CvMemStorage* storage = cvCreateMemStorage(0);
// hough detector works better with some smoothing of the image
cvSmooth( thresholded, thresholded, CV_GAUSSIAN, 9, 9 );
//hough transform to detect circle
CvSeq* circles = cvHoughCircles(thresholded, storage, CV_HOUGH_GRADIENT, 2,
thresholded->height/4, 100, 50, 10, 400);
for (int i = 0; i < circles->total; i++)
{ //get the parameters of circles detected
float* p = (float*)cvGetSeqElem( circles, i );
printf("Ball! x=%f y=%f r=%f\n\r",p[0],p[1],p[2] );
// draw a circle with the centre and the radius obtained from the hough transform
cvCircle( frame, cvPoint(cvRound(p[0]),cvRound(p[1])), //plot centre
2, CV_RGB(255,255,255),-1, 8, 0 );
cvCircle( frame, cvPoint(cvRound(p[0]),cvRound(p[1])), //plot circle
cvRound(p[2]), CV_RGB(0,255,0), 2, 8, 0 );
}
/* for testing purpose you can show all the images but when done with calibration
only show frame to keep the screen clean */
cvShowImage( "Camera", frame ); // Original stream with detected ball overlay
cvShowImage( "HSV", hsv_frame); // Original stream in the HSV color space
cvShowImage( "After Color Filtering", thresholded ); // The stream after color filtering
cvShowImage( "F1", thresholded1 ); // individual filters
cvShowImage( "F2", thresholded2 );
cvShowImage( "F3", thresholded3 );
//cvShowImage( "filtered", thresholded );
cvReleaseMemStorage(&storage);
//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
//remove higher bits using AND operator
if( (cvWaitKey(10) & 255) == 27 ) break;
}
//Save the threshold values before exiting
*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 0, 0 ) ) = t1min ;*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 0, 1 ) ) = t2min ;*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 0, 2 ) ) = t3min ;
*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 1, 0 ) ) = t1max ;*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 1, 1 ) ) = t2max ;*((float*)CV_MAT_ELEM_PTR( *threshold_matrix, 1, 2 ) ) = t3max ;
cvSave("threshold_matrix.xml",threshold_matrix);
// Release the capture device housekeeping
cvReleaseCapture( &capture );
cvDestroyWindow( "mywindow" );
return 0;
}
/**
* Test using the laser range finder from a point defined as 0,0,0.
*/
void test_laser_solve3D( )
{
// % satellite positions
CvMat* svpos = cvCreateMat( 3, 3, CV_32FC1 );
cvSetZero(svpos);
CvMat* svrange = cvCreateMat( 3, 1, CV_32FC1 );
cvSetZero(svrange);
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 0 ) ) = 2.438;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 0 ) ) = 1.535;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 0 ) ) = 2.895;
*( (float*)CV_MAT_ELEM_PTR( *svrange, 0, 0 ) ) = 4.04;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 1 ) ) = 0.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 1 ) ) = 1.535;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 1 ) ) = 2.895;
*( (float*)CV_MAT_ELEM_PTR( *svrange, 1, 0 ) ) = 3.258;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 2 ) ) = 0.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 2 ) ) = 3.364;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 2 ) ) = 2.905;
*( (float*)CV_MAT_ELEM_PTR( *svrange, 2, 0 ) ) = 4.426;
solve3D( svrange, svpos);
// clean-up
cvReleaseMat(&svpos);
cvReleaseMat(&svrange);
}
int main()
{
// Create an OpenCV Matrix containing some fixed data.
//
float vals[] = { 0.866025, -0.500000, 0.500000, 0.866025};
CvMat rotmat;
// cvInitMatHeader Method (mat, rows, cols, type, data, step)
cvInitMatHeader(
&rotmat, // mat
2, // rows
2, // cols
CV_32FC1, // type
vals // data
);
printf("Ex 3_3 matrix initialized\n");
// Matrix created (init all 0 ) and accessed [3,2]
// cvCreateMat(int rows, int cols, int type)
CvMat* mat = cvCreateMat( 5, 5, CV_32FC1 );
// CV_MAT_ELEM( matrix, elemtype, row, col )
float element_3_2 = CV_MAT_ELEM( *mat, float, 3, 2 );
printf("Exercise 3_4, matrix created and accessed [3,2]=%f\n",element_3_2);
// cvCreateMat(int rows, int cols, int type)
CvMat* matTwo = cvCreateMat( 5, 5, CV_32FC1 );
// ////////////////////////////////////////////////
// Good to Know But Should Not Use ( Not Efficient )
// ////////////////////////////////////////////////
// Different Method of Setting Value in cvMat:
// Method 1
float Element_3_2 = 7.7;
*( (float*)CV_MAT_ELEM_PTR( *matTwo, 3, 2 ) ) = Element_3_2;
// Different Method of Setting Value in cvMat:
// Method 2
cvmSet( matTwo, 2, 2, 0.5000 );
// Different Method of Setting Value in cvMat:
// Method 3
cvSetReal2D( matTwo, 3, 3, 0.3300 );
printf("Exercise 3_5, matrix created and accessed [3,2]=%f, [2,2]=%f, [3,3]=%f\n",CV_MAT_ELEM( *mat, float, 3, 2 ),CV_MAT_ELEM( *mat, float, 2, 2 ),CV_MAT_ELEM( *mat, float, 3, 3 ));
// ////////////////////////////////////////////////
// Access Mat Value (Sum Up All Val in cvMat)
// ////////////////////////////////////////////////
float s = 0.0f;
for( int row=0; row<mat->height; row++ ) {
float* ptr = mat->data.fl + row * mat->step/4;
for( int col=0; col<mat->width; col++ ) {
s += *ptr++;
}
}
printf("Sum of cvMat is %f\n", s);
}
void test_solve3D( )
{
// int i = 0;
// user[3][1] = [x, y, z]'
CvMat* user = cvCreateMat( 3, 1, CV_32FC1 );
cvSetZero(user);
double svpos_i_norm = 0;
// user = [-1, 1, 2.5]'
*( (float*)CV_MAT_ELEM_PTR( *user, 0, 0 ) ) = 1.0;
*( (float*)CV_MAT_ELEM_PTR( *user, 1, 0 ) ) = 2.0;
*( (float*)CV_MAT_ELEM_PTR( *user, 2, 0 ) ) = -3.0;
// % satellite positions
// svpos[3][3] = [[x1,y1,z1]',[x2,y2,z2]',[x3,y3,z3]']]
CvMat* svpos = cvCreateMat( 3, 3, CV_32FC1 );
cvSetZero(svpos);
// temp vector for each satellite for use in norm calculations
CvMat* svpos_temp = cvCreateMat( 3, 1, CV_32FC1 );
cvSetZero(svpos_temp);
CvMat* svrange = cvCreateMat( 3, 1, CV_32FC1 );
cvSetZero(svrange);
// svpos(:, 1) = [2, 3, 4]';
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 0 ) ) = 2.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 0 ) ) = 3.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 0 ) ) = 4.0;
// svrange(1,1) = norm(user - svpos(:, 1))+.05*randn;
get_vector_column( svpos, svpos_temp, 0);
svpos_i_norm = cvNorm(user, svpos_temp, CV_L2, NULL);
*( (float*)CV_MAT_ELEM_PTR( *svrange, 0, 0 ) ) = svpos_i_norm;
// svpos(:, 2) = [3, 4, 3]';
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 1 ) ) = 3.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 1 ) ) = 4.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 1 ) ) = 3.0;
get_vector_column( svpos, svpos_temp, 1);
svpos_i_norm = cvNorm(user, svpos_temp, CV_L2, NULL);
*( (float*)CV_MAT_ELEM_PTR( *svrange, 1, 0 ) ) = svpos_i_norm;
// svrange(2,1) = norm(user - svpos(:, 2))+.05*randn;
// svpos(:, 3) = [1, 3, 6]';
*( (float*)CV_MAT_ELEM_PTR( *svpos, 0, 2 ) ) = 1.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 1, 2 ) ) = 3.0;
*( (float*)CV_MAT_ELEM_PTR( *svpos, 2, 2 ) ) = 6.0;
// svrange(3,1) = norm(user - svpos(:, 3))+.05*randn;
get_vector_column( svpos, svpos_temp, 2);
svpos_i_norm = cvNorm(user, svpos_temp, CV_L2, NULL);
*( (float*)CV_MAT_ELEM_PTR( *svrange, 2, 0 ) ) = svpos_i_norm;
// svpos(:, 4) = [-1, 2, 3]';
// svrange(4,1) = norm(user - svpos(:, 4))+.05*randn;
// svpos(:, 5) = [-2, 1.5, 3]';
// svrange(5,1) = sqrt((user(1) - svpos(1, 5))^2 + (user(2) - svpos(2, 5))^2 ...
// + (user(3) - svpos(3, 5))^2 );
// xyz = solve3D(svrange, svpos)
solve3D( svrange, svpos);
// clean-up
cvReleaseMat(&user);
cvReleaseMat(&svpos);
cvReleaseMat(&svpos_temp);
cvReleaseMat(&svrange);
}
