C++ cvmGet函数代码示例

Point2D getOptFlow(IplImage* currentFrame,Point2D p,IplImage* preFrame)
{
	Point2D temp;
	double b[2];
	b[0]=0;b[1]=0;
	
	double M11=0,M12=0,M22=0;
	for(int i = -OPTICAL_FLOW_POINT_AREA/2; i < OPTICAL_FLOW_POINT_AREA/2; i++)
	{
		for (int j = -OPTICAL_FLOW_POINT_AREA/2;j < OPTICAL_FLOW_POINT_AREA/2;j++)
		{
			temp = partial(currentFrame,Point2D(p.row+i,p.col+j));
			M11 += temp.dcol*temp.dcol;
			M12 += temp.dcol*temp.drow;
			M22 += temp.drow*temp.drow;
			b[0] += temp.dcol*(pixval8U(currentFrame,p.row+i,p.col+j)-pixval8U(preFrame,p.row+i,p.col+j));
			b[1] += temp.drow*(pixval8U(currentFrame,p.row+i,p.col+j)-pixval8U(preFrame,p.row+i,p.col+j));
		}
	}
	double a[] = {M11,M12,M12,M22};
	CvMat M=cvMat(2, 2, CV_64FC1, a);
	CvMat *Mi = cvCloneMat(&M);
	cvInvert(&M,Mi,CV_SVD);
	temp.col=0;
	temp.row=0;
	b[0] = -b[0];
	b[1] = -b[1];
	CvMat Mb = cvMat(2,1,CV_64FC1,b);
	CvMat *Mr = cvCloneMat(&Mb);
	cvMatMul( Mi, &Mb, Mr);
	double vy = (cvmGet(Mr,1,0));
	double vx = (cvmGet(Mr,0,0));
	
	return (Point2D(vy,vx));
}

/** Computes the optical center of camera from camera projection matrix (http://en.wikipedia.org/wiki/Camera_matrix )
* @param pM : Camera projection matrix (3x4).
* @return : Optical center of camera.
*/
Point3D Utility::getOpticalCenter( CvMat* pM )
{
	CvMat *A = cvCreateMat(3, 3, CV_64FC1);
	CvMat *Ainv = cvCreateMat(3, 3, CV_64FC1);
	CvMat *b = cvCreateMat(3, 1, CV_64FC1);
	for(int i=0; i<3; ++i)
	{
		for(int j=0; j<3; ++j)
			cvmSet(A, i, j, cvmGet(pM,i,j));
		cvmSet(b, i, 0, cvmGet(pM, i,3));
	}
	cvInvert(A, Ainv);
	CvMat *oc = cvCreateMat(3, 1, CV_64FC1);
	cvMatMul(Ainv, b, oc);
	Point3D toRet;
	toRet.x = -1 * cvmGet(oc, 0, 0);				//NULL SPACE OF MATRIX pM
	toRet.y = -1 * cvmGet(oc, 1, 0);
	toRet.z = -1 * cvmGet(oc, 2, 0);

	cvReleaseMat(&A);
	cvReleaseMat(&Ainv);
	cvReleaseMat(&b);
	cvReleaseMat(&oc);
	return toRet;
}

void mCallBack(int event,int x,int y, int flags, void * userdata)
{
	if(event == CV_EVENT_LBUTTONDOWN)
	{
		printf("x:%d y:%d\n",x,y);
		if(lb_type==TYPE_REC||lb_type==TYPE_PNT||lb_type==TYPE_SQR)
		{
			point_val[cur_point_ptr][0] = x;
			point_val[cur_point_ptr][1] = y;
			if(cur_point_ptr==1&&lb_type==TYPE_SQR)
			{
				cur_point_ptr = -1;    
			}
			nextPoint();
		}
		if(lb_type==TYPE_SP)
		{
			sp_val[(int)cvmGet(spmat,y,x)-1] = cur_point_ptr;
		}
		showImage();
	}
	if(event == CV_EVENT_RBUTTONDOWN)
	{
		if(lb_type==TYPE_SP)
		{
			sp_val[(int)cvmGet(spmat,y,x)-1] = -1;
		}
		showImage();
	}
}

	void PM(CvMat *m, char *filename) {
		if (filename) {
			FILE *f = fopen(filename, "w");
			CvSize size = cvGetSize(m);
			char buf[50];
			for (int i = 0; i < size.height; i++) {
				fprintf(f, "{");
				for (int j = 0; j < size.width; j++) {
					sprintf(buf, "%f", cvmGet(m, i, j));
					fprintf(f, "%15s, ", buf);
				}
				fprintf(f, "},");
				fprintf(f, "\n");
			}
			fclose(f);
		}
		else {
			CvSize size = cvGetSize(m);
			for (int r = 0; r < size.height; r++) {
				for (int c = 0; c < size.width; c++)
					cout << cvmGet(m, r, c) << ' ';
				cout << endl;
			}
		}
	}

bica::ShapeList
SingleObjectModel::getGrDebugRel()
{
	pthread_mutex_lock(&(jpdaf->mutex));

	shapeListRel.clear();

	// Ball
	bica::Point3DPtr p(new bica::Point3D);
	p->x = estimate.getPositionInRelativeCoordinates().x;
	p->y = estimate.getPositionInRelativeCoordinates().y;;
	p->z = 0.0f;

	bica::EllipsePtr estEllipse(new bica::Ellipse);
	estEllipse->center = p;
	estEllipse->width = 4 * sqrt(cvmGet(jpdaf->objects[0]->error_cov_post, 0, 0));
	estEllipse->length = 4 * sqrt(cvmGet(jpdaf->objects[0]->error_cov_post, 1, 1));
	estEllipse->angle =  toDegrees(atan2(cvmGet(jpdaf->objects[0]->state_post, 1, 0),
			cvmGet(jpdaf->objects[0]->state_post, 0, 0)));
	estEllipse->z = 0.0f;
	estEllipse->color = bica::ORANGE;
	estEllipse->filled = true;
	estEllipse->opacity = 125;
	estEllipse->accKey = "o";
	estEllipse->label = "Ball";
	shapeListRel.push_back(estEllipse);

	pthread_mutex_unlock(&(jpdaf->mutex));

	return shapeListRel;
}

int LDARec::ActionRecognition(double* input){//Input: 1*6 vector


	vector <double> dist;
	dist.resize(cnum);

	ydata[0] = input[0];
	ydata[1] = input[1];
	ydata[2] = input[2];
	ydata[3] = input[3];		
	ydata[4] = input[4];
	ydata[5] = input[5];	
	//Compute one-dimensionl result

	//First: only compare once!

	int count = 0;


	for(int i=0;i<cnum;++i){		
		for(int j=i+1;j<cnum;++j){

			double x1d = 0;

			for(int l=0;l<Ws.cols;++l)
				x1d += cvmGet(&y, 0, l) * cvmGet(&Ws, count, l);
		    
			
		    double d1 = fabs(x1d - cvmGet(&mean1d, count, 0));
			double d2 = fabs(x1d - cvmGet(&mean1d, count, 1));//i-th with j-th

			if(d2>d1){//
				dist[i] +=1;
				dist[j] -=1;
			}
			else{
				dist[i] -=1;
				dist[j] +=1;
			}

			++count;

		}



	}

	int midx = 0;
	double mval = dist[0];

	for(int i=0;i<dist.size();++i){
		if(dist[i]>mval)
			midx = i,mval = dist[i];
	}

	return midx;

}

void Res_Func2(CvMat *vectX,CvMat *res)
{
    double x = cvmGet(vectX,0,0);
    double y = cvmGet(vectX,1,0);
    cvmSet(res,0,0,(x-2)*(x-2)+(y+3)*(y+3));
    cvmSet(res,1,0,x+y);

    return;
}

void display_CvMat(CvMat * arrayToDisplay, int arraySize)
{
    int i;
    printf("\n\n\tCvMat Coordinates (x;y)\n\n");
    for(i=0;i<arraySize;i++)
    {
        printf("(%.1f;%.1f)\t",cvmGet(arrayToDisplay,i,0),cvmGet(arrayToDisplay,i,1));
    }
}

CvMat* AbstractCamera::computeRtMatrix(double a, double b, double g, double tX, double tY, double tZ) {

	//--- Represent 3d rotation with euler angles

	double sinG = sin(g);
	double cosG = cos(g);
	CvMat* rZg = cvCreateMat(3, 3, CV_32FC1);
	cvSetZero(rZg);
	cvmSet(rZg, 0, 0, cosG);
	cvmSet(rZg, 0, 1, -sinG);
	cvmSet(rZg, 1, 0, sinG);
	cvmSet(rZg, 1, 1, cosG);
	cvmSet(rZg, 2, 2, 1.0f);

	double sinB = sin(b);
	double cosB = cos(b);
	CvMat* rXb = cvCreateMat(3, 3, CV_32FC1);
	cvSetZero(rXb);
	cvmSet(rXb, 0, 0, 1.0f);
	cvmSet(rXb, 1, 1, cosB);
	cvmSet(rXb, 1, 2, -sinB);
	cvmSet(rXb, 2, 1, sinB);
	cvmSet(rXb, 2, 2, cosB);

	double sinA = sin(a);
	double cosA = cos(a);
	CvMat* rZa = cvCreateMat(3, 3, CV_32FC1);
	cvSetZero(rZa);
	cvmSet(rZa, 0, 0, cosA);
	cvmSet(rZa, 0, 1, -sinA);
	cvmSet(rZa, 1, 0, sinA);
	cvmSet(rZa, 1, 1, cosA);
	cvmSet(rZa, 2, 2, 1.0f);

	CvMat* rotationMatrix = cvCreateMat(3, 3, CV_32FC1);
	cvMatMul(rZg, rXb, rotationMatrix);
	cvMatMul(rotationMatrix, rZa, rotationMatrix);

	cvReleaseMat(&rZg);
	cvReleaseMat(&rXb);
	cvReleaseMat(&rZa);

	//--- [R|T] ; First camera rotation and translation matrix
	CvMat* rtMatrix = cvCreateMat(3, 4, CV_32FC1);
	for (int i = 0; i < 3; i++) {
		cvmSet(rtMatrix, i, 0, cvmGet(rotationMatrix, i, 0));
		cvmSet(rtMatrix, i, 1, cvmGet(rotationMatrix, i, 1));
		cvmSet(rtMatrix, i, 2, cvmGet(rotationMatrix, i, 2));
	}
	cvmSet(rtMatrix, 0, 3, tX);
	cvmSet(rtMatrix, 1, 3, tY);
	cvmSet(rtMatrix, 2, 3, tZ);

	cvReleaseMat(&rotationMatrix);

	return rtMatrix;
}

void saveRenseMatrix(CvMat *matrixRense){
	sprintf(renseParameters, "%lf\n%lf\n%lf\n%lf\n",
		cvmGet(matrixRense,0,0), 
		cvmGet(matrixRense,1,0), 
		cvmGet(matrixRense,2,0),
		cvmGet(matrixRense,3,0)
	);
	
}

QString AbstractCamera::getDistorcionCoefficientsInfo() {

	float k1 = cvmGet(_distortionCoefficients, 0, 0);
	float k2 = cvmGet(_distortionCoefficients, 1, 0);
	float p1 = cvmGet(_distortionCoefficients, 2, 0);
	float p2 = cvmGet(_distortionCoefficients, 3, 0);

	return QString("\n\nDistorcion Coefficients:\nk1 = %1 k2 = %2\np1 = %3 p2 = %4").arg(k1).arg(k2).arg(p1).arg(p2);
}

double PerformanceMatrix::getConfidence(int sym){
	double conf = 0.0;
	double val = 0.0;
	for(size_t i = 0; i < PerformanceMatrix::size; i++){
		val += cvmGet(pm, i, sym);
	}
	conf = cvmGet(pm, sym, sym) / val;
	return conf;
}

void CDataOut::Cam()
{
  CamFile<< cvmGet(pEstimator->pDataCam->translation,0,0)<<" ";
  CamFile<< cvmGet(pEstimator->pDataCam->translation,1,0)<<" ";
  CamFile<< cvmGet(pEstimator->pDataCam->translation,2,0)<<" ";
  CamFile<<"65000 ";
  CamFile<<"65000 ";
  CamFile<<endl;
}

int quasi_poisson_solver(IplImage *im_src, IplImage *im_dst, IplImage *im_mask, int channel, int *offset){
    int i, j, loop, neighbor, count_neighbors, ok;
    float error, sum_f, sum_vpq, fp;
    int naddr[NUM_NEIGHBOR][2]={{-1, 0}, {0, -1}, {0, 1}, {1, 0}};
    CvMat* im_new = cvCreateMat(im_dst->height, im_dst->width, CV_64F);
    for(i=0; i<im_dst->height; i++){
        for(j=0; j<im_dst->width; j++){
            cvmSet(im_new, i, j, double(uchar(im_dst->imageData[(i)*im_dst->widthStep + (j)*im_dst->nChannels + channel])));
        }
    }
    
    for(loop=0; loop<LOOP_MAX; loop++){
        if (loop%100==0) {
            printf("loop:%d\n",loop);
        }
        ok = 1;
        for(i=0; i<im_mask->height; i++){
            for(j=0; j<im_mask->width; j++){
                if(int((uchar)im_mask->imageData[i*im_mask->widthStep + j]) > 0){
                    sum_f=0.0;
                    sum_vpq=0.0;
                    count_neighbors=0;
                    for(neighbor=0; neighbor<NUM_NEIGHBOR; neighbor++){
                        if(i+offset[0]+naddr[neighbor][0] >= 0 && j+offset[1]+naddr[neighbor][1] >= 0 && i+offset[0]+naddr[neighbor][0] < im_dst->height && j+offset[1]+naddr[neighbor][1] < im_dst->width){
                            sum_f += cvmGet(im_new, i+offset[0]+naddr[neighbor][0], j+offset[1]+naddr[neighbor][1]);
                            sum_vpq += float((uchar)im_src->imageData[(i)*im_src->widthStep + (j)*im_src->nChannels + channel]) - float((uchar)im_src->imageData[(i+naddr[neighbor][0])*im_src->widthStep + (j+naddr[neighbor][1])*im_src->nChannels + channel]);
                            count_neighbors++;
                        }
                    }
                    fp = (sum_f + sum_vpq)/(float)count_neighbors;
                    error = fabs(fp - cvmGet(im_new, i+offset[0], j+offset[1]));
                    if(ok && error > EPS * (1+fabs(fp))){
                        ok = 0;
                    }
                    cvmSet(im_new, i+offset[0], j+offset[1], fp);
                }
            }
        }
        if(ok){
            break;
        }
    }
    
    for(i=0; i<im_dst->height; i++){
        for(j=0; j<im_dst->width; j++){
            if(cvmGet(im_new, i, j) > 255){
                cvmSet(im_new, i, j, 255.0);
            }
            else if(cvmGet(im_new, i, j) < 0){
                cvmSet(im_new, i, j, 0.0);
            }
            im_dst->imageData[(i)*im_dst->widthStep + (j)*im_dst->nChannels + channel] = (uchar)cvmGet(im_new, i, j);
        }
    }
    return 1;
}

int DoubleSidesModelFitting::SingleFitList(const vector<CvPoint>& list, int horizon)
{
	double belief;

	if (list.empty())
	{
		return DSMF_FAIL_TOO_SHORT;
	}

	if (list.size() < DSMF_RANSAC_MIN_POINTS)
	{
		return DSMF_FAIL_TOO_SHORT;
	}

	matFullA = cvCreateMat(list.size(), 3, CV_32FC1);
	matFullB = cvCreateMat(list.size(), 1, CV_32FC1);
	matFullDiff = cvCreateMat(list.size(), 1, CV_32FC1);

	for (size_t i = 0; i < list.size(); i++)
	{
		cvmSet(matFullA, i, 0, 1.0 / (list.at(i).y - horizon));
		cvmSet(matFullA, i, 1, (list.at(i).y - horizon));
		cvmSet(matFullA, i, 2, 1.0);
		cvmSet(matFullB, i, 0, list.at(i).x);
	}

	int result = SingleFullFit(list, horizon, belief);

	if(result == DSMF_SUCCESS)
	{
		curve = cvScalar(cvmGet(matOptX, 0, 0), cvmGet(matOptX, 1, 0), cvmGet(matOptX, 2, 0), belief);
	}
	else
	{
		curve = cvScalar(0);
	}

	if (matFullA)
	{
		cvReleaseMat(&matFullA);
		matFullA = NULL;
	}
	if (matFullB)
	{
		cvReleaseMat(&matFullB);
		matFullB = NULL;
	}
	if (matFullDiff)
	{
		cvReleaseMat(&matFullDiff);
		matFullDiff = NULL;
	}

	return result;
}