// Create CvMat header only
CV_IMPL CvMat*
cvCreateMatHeader( int height, int width, int type )
{
CvMat* arr = 0;
CV_FUNCNAME( "cvCreateMatHeader" );
__BEGIN__;
CV_CALL( arr = (CvMat*)cvAlloc( sizeof(*arr)));
CV_CALL( cvInitMatHeader( arr, height, width, type, 0,
icvAlign(width*cvPixSize(type),4) ));
__END__;
if( cvGetErrStatus() < 0 )
cvReleaseMatHeader( &arr );
return arr;
}
// deallocate the CvMat structure and underlying date
CV_IMPL void
cvReleaseMat( CvMat** array )
{
CV_FUNCNAME( "cvReleaseMat" );
__BEGIN__;
if( !array )
CV_ERROR_FROM_CODE( CV_HeaderIsNull );
if( *array )
{
CvMat* arr = *array;
if( !_CV_IS_ARR( arr ))
CV_ERROR_FROM_CODE( !arr ? CV_StsNullPtr : CV_StsBadFlag );
*array = 0;
CV_CALL( cvReleaseData( arr ));
CV_CALL( cvReleaseMatHeader( &arr ));
}
__END__;
}
void mexFunction(
int nargout,
mxArray *out[],
int nargin,
const mxArray *in[]
)
{
/* declare variables */
double *mx_r_in;
double *mx_r_out;
int output_dim = 3;
/* check arguments */
if (nargin != 1 || nargout > 1){
mexErrMsgTxt("Wrong Number of arguments.");
exit(1);
}
// Link input vars to pointers in C
mx_r_in = mxGetPr(in[0]);
int m = mxGetM(in[0]);
int n = mxGetN(in[0]);
// Input is a rotation matrix
if (m == 3 && n == 3){
output_dim = 1;
}
// Check input argument: avoid errors
if (!((m == 3 && n == 3) || (m == 1 && n == 3) || (m == 3 && n == 1))){
mexPrintf("HELP! ERROR! %d %d\n", m, n);
exit(1);
}
// Create OpenCV array for input variable
// If we want to use cvSetData, our matrices are actually the transposed
// versions of those that come from Matlab.
CvMat *r_in_T = cvCreateMatHeader(m, n, CV_64F);
cvSetData (r_in_T, mx_r_in, sizeof(double)*n);
// Transpose the matrix
CvMat *r_in = cvCreateMat(n, m, CV_64F);
cvT(r_in_T, r_in);
// Result
CvMat *r_out_T = cvCreateMat(output_dim, 3, CV_64F);
// Call cvRodrigues
cvRodrigues2(r_in, r_out_T);
// Allocate memory for the output var
out[0] = mxCreateNumericMatrix(3, output_dim, mxDOUBLE_CLASS, mxREAL);
mx_r_out = mxGetPr(out[0]);
CvMat* r_out = cvCreateMatHeader(3, output_dim, CV_64F);
cvSetData (r_out, mx_r_out, sizeof(double)*output_dim);
cvT(r_out_T, r_out);
// Free all array headers and return
cvReleaseMat(&r_in);
cvReleaseMatHeader(&r_in_T);
cvReleaseMatHeader(&r_out);
}
void BagOfFeatures::trainSVM_CV(int type, int kernel, double degree, double gamma, double coef0,
double C, double nu, double p, int termType, int iterations, double eps,
char* fileName)
{
int i, j, k, l = -1;
int totalData = 0;
int size, length = dictionary->rows;
float *dPtr;
//Get the total number of training data
for(i = 0; i < numClasses; i++)
totalData += data[i].getTrainSize();
//CvMat* trainData = cvCreateMat(totalData, dictionary->rows, CV_32FC1);
//CvMat* dataLabel = cvCreateMat(totalData, 1, CV_32FC1);
float** trainData = new float* [totalData];
float* dataLabel = new float [totalData];
for(i = 0; i < totalData; i++)
trainData[i] = new float [dictionary->rows];
// For each class
for(i = 0; i < numClasses; i++)
{
// Get the number of images
size = data[i].getTrainSize();
for(j = 0; j < size; j++)
{
l++;
//Attach the label to it
//dataLabel->data.fl[l] = (float)data[i].getLabel();
//dPtr = (float*)(trainData->data.ptr + l*trainData->step);
dataLabel[l] = (float)data[i].getLabel();
// Copy the histograms
for(k = 0; k < length; k++)
{
//dPtr[k] = trainObject[i].histogramSet.histogram[j][k];
trainData[l][k] = trainObject[i].histogramSet.histogram[j][k];
}
}
}
CvSVMParams SVMParam_CV;
SVMParam_CV.svm_type = type;
SVMParam_CV.kernel_type = kernel;
SVMParam_CV.degree = degree;
SVMParam_CV.gamma = gamma;
SVMParam_CV.coef0 = coef0;
SVMParam_CV.C = C;
SVMParam_CV.nu = nu;
SVMParam_CV.p = p;
SVMParam_CV.class_weights = NULL;
SVMParam_CV.term_crit = cvTermCriteria(termType, iterations, eps);
CvMat *dataHeader = cvCreateMatHeader(totalData, dictionary->rows, CV_32FC1);
CvMat *labelHeader = cvCreateMatHeader(totalData, 1, CV_32FC1);
cvInitMatHeader(dataHeader, totalData, dictionary->rows, CV_32FC1, trainData);
cvInitMatHeader(labelHeader, totalData, 1, CV_32FC1, dataLabel);
//Train the SVM
//CvSVM svm(trainData, dataLabel, 0, 0,
// CvSVMParams(CvSVM::C_SVC, CvSVM::LINEAR, 0, 0, 0, 2,
// 0, 0, 0, cvTermCriteria(CV_TERMCRIT_EPS,0, 0.01)));
//strcpy(classifierFile, fileName);
//if(SVMModel_CV != NULL)
// delete SVMModel_CV;
SVMModel_CV.clear();
SVMModel_CV.train_auto(dataHeader, labelHeader, 0, 0, SVMParam_CV, 10);
SVMModel_CV.save(classifierFile);
cvReleaseMatHeader(&dataHeader);
cvReleaseMatHeader(&labelHeader);
for(i = 0; i < totalData; i++)
delete [] trainData[i];
delete [] trainData;
delete [] dataLabel;
classifierType = CVSVM_CLASSIFIER;
}
void CDataOut::Disp_out(IplImage *framecopy)
{
char ndisp[100];
sprintf(ndisp,(resdir+"disp%d.txt").c_str(),iter++);
DispFile.open(ndisp);
int ii=0;
CvMat *temp,*temp2,*temp3;
temp=cvCreateMatHeader(6,6,CV_32FC1);
temp2=cvCreateMat(3,6,CV_32FC1);
temp3=cvCreateMat(3,3,CV_32FC1);
CvMat* vect=cvCreateMat (6,1,CV_32FC1);
CvMat* res6=cvCreateMat (6,1,CV_32FC1);
CvMat* vect2=cvCreateMat(1,6,CV_32FC1);
CvMat* proj = cvCreateMat(4,2,CV_32FC1); ///NOT RELEASED
CvMat* m = cvCreateMat(3,1,CV_32FC1);
for ( list<CElempunto*>::iterator It=pEstimator->pMap->bbdd.begin();
It != pEstimator->pMap->bbdd.end(); It++ )
{
if((*It)->state>2)
{
cvmSet(m,0,0,cos((*It)->theta)*sin((*It)->phi));
cvmSet(m,1,0,-sin((*It)->theta));
cvmSet(m,2,0,cos((*It)->theta)*cos((*It)->phi));
cvNormalize( m, m);
/* float xreal=(*It)->wx +cvmGet(m,0,0)/(*It)->rho;
float yreal=(*It)->wy +cvmGet(m,1,0)/(*It)->rho;
float zreal=(*It)->wz +cvmGet(m,2,0)/(*It)->rho;
*/
CvMat *pCovMat = pEstimator->getCovMat();
if (12+ii*6< pCovMat->width && 12+ii*6< pCovMat->height)
{
cvGetSubRect( pCovMat,temp,cvRect(12+ii*6,12+ii*6,6,6) );
for (int part=0; part<40; part++){
cvmSet(vect,0,0,randomVector(-.005,.005));
cvmSet(vect,1,0,randomVector(-.005,.005));
cvmSet(vect,2,0,randomVector(-.005,.005));
cvmSet(vect,3,0,randomVector(-.005,0.005));
cvmSet(vect,4,0,randomVector(-.005,0.005));
cvmSet(vect,5,0,randomVector(-1,1));
cvMatMul(temp,vect,res6);
cvmSet(m,0,0,cos(cvmGet(res6,3,0)+(*It)->theta)*sin(cvmGet(res6,4,0)+(*It)->phi));
cvmSet(m,1,0,-sin(cvmGet(res6,3,0)+(*It)->theta));
cvmSet(m,2,0,cos(cvmGet(res6,3,0)+(*It)->theta)*cos(cvmGet(res6,4,0)+(*It)->phi));
cvNormalize( m, m);
cvmSet (vect2,0,0,((cvmGet(res6,0,0)+(*It)->wx)+cvmGet(m,0,0)/(cvmGet(res6,5,0)+(*It)->rho)));
cvmSet(vect2,0,1,((cvmGet(res6,1,0)+(*It)->wy)+cvmGet(m,1,0)/(cvmGet(res6,5,0)+(*It)->rho)));
cvmSet(vect2,0,2,((cvmGet(res6,2,0)+(*It)->wz)+cvmGet(m,2,0)/(cvmGet(res6,5,0)+(*It)->rho)));
DispFile<<cvmGet(vect2,0,0)<<" ";
DispFile<<cvmGet(vect2,0,1)<<" ";
DispFile<<cvmGet(vect2,0,2)<<" ";
DispFile<<ii<<endl;
cvmSet(vect2,0,0,cvmGet(res6,0,0)+(*It)->wx);
cvmSet(vect2,0,1,cvmGet(res6,1,0)+(*It)->wy);
cvmSet(vect2,0,2,cvmGet(res6,2,0)+(*It)->wz);
cvmSet(vect2,0,3,cvmGet(res6,3,0)+(*It)->theta);
cvmSet(vect2,0,4,cvmGet(res6,4,0)+(*It)->phi);
cvmSet(vect2,0,5,cvmGet(res6,5,0)+(*It)->rho);
pModelCam->cvProject_1_pto(vect2,proj,NULL,NULL,NULL);
if (framecopy != NULL){
cvCircle (framecopy,cvPoint((int)cvmGet(proj,0,0),(int)cvmGet(proj,0,1)),1,cvScalar(0,0,255),1 );
}
}
}
}
ii++;
}
cvReleaseMatHeader(&temp);
cvReleaseMat(&temp2);
cvReleaseMat(&temp3);
cvReleaseMat(&vect);
cvReleaseMat(&vect2);
cvReleaseMat(&res6);
cvReleaseMat(&proj);
DispFile.close();
}
