Пример #1
0
void drawEllipseToData(ImgRGB& img, double x, double y, double var[4], uchar r,
		uchar g, uchar b) {

	double evec[4], eval[2];
	dgeevFor(2, var, evec, eval);

	//	int l1 = static_cast<int> (eval[0] + 0.5);
	//	int l2 = static_cast<int> (eval[1] + 0.5);
	double l1 = sqrt(eval[0]);
	double l2 = sqrt(eval[1]);

	double dx1 = l1 * evec[0];
	double dy1 = l1 * evec[1];
	double dx2 = l2 * evec[2];
	double dy2 = l2 * evec[3];

	cv::Mat cvOutImg(img.rows, img.cols, CV_8UC3, img.data);
	cv::line(cvOutImg, cv::Point2f(x - dx1, y - dy1),
			cv::Point2f(x + dx1, y + dy1), cv::Scalar(r, g, b), 1, CV_AA);
	cv::line(cvOutImg, cv::Point2f(x - dx2, y - dy2),
			cv::Point2f(x + dx2, y + dy2), cv::Scalar(r, g, b), 1, CV_AA);
	//get rotation angle
	double theta = atan2(evec[0], evec[1]);
	cv::ellipse(cvOutImg, cv::Point2f(x, y), cv::Size(l1, l2), theta, 0, 360,
			cv::Scalar(r, g, b), 1, CV_AA);
	//	double ivar[4];
	//	mat22Inv(var,ivar);

}
Пример #2
0
void drawCurMapPoint(const MapPoint* p, double range, double pointSize,
		bool drawCov) {
	//GLfloat LightAmbient[] = { 0.5f, 1.0f, 0.5f, 1.0f };
	GLfloat LightAmbient[] = { 0.5f, 0.5f, 0.5f, 1.0f };
	GLfloat LightAmbientInAct[] = { 0.5f, 0.5f, 0.5f, 1.0f };
	GLfloat LightDiffuse[] = { 0.5f, 0.5f, 0.5f, 1.0f };
	GLfloat LightPosition[] = { -range * 1.0f, -range * 1.0f, 0.0f, 1.0f };

	glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);
	glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbientInAct);
	glLightfv(GL_POSITION, GL_DIFFUSE, LightAmbientInAct);

	if( p->isFalse())
		return;
	
	glColor3f(0.2f, 0.8f, 0.2f);

	//for debug
	if (p->flag == FLAG_MAPPOINT_TEST4)
		glColor3f(1.0f, 0.0f, 0.0f);

//	glEnable(GL_LIGHTING);
//	glEnable(GL_LIGHT1);
//
//	glPushMatrix();
//	glTranslatef(p->x, p->y, p->z);
//	glutSolidSphere(0.1 * range * p->numVisCam, 12, 12);
//	glPopMatrix();
//
//	glDisable(GL_LIGHT1);
//	glDisable(GL_LIGHTING);


	if(p->isFalse())
		return;
	
	glPointSize(4.0 * pointSize);

	glBegin(GL_POINTS);
	glVertex3d(p->x, p->y, p->z);
	glEnd();

	if (drawCov && p->cov[0] > 0) {		
		glColor4f(0.2f, 0.8f, 0.2f, 0.2f);

//		//for debug
//		if (p->flag == FLAG_MAPPOINT_TEST4
//		)
//			glColor3f(1.0f, 0.0f, 0.0f);

		double evec[9];
		double eval[3];
		dgeevFor(3, p->cov, evec, eval);
		glPushMatrix();
		glTranslatef(p->x, p->y, p->z);
		double rotMat[16];
		memset(rotMat, 0, sizeof(double) * 16);
		memcpy(rotMat, evec, sizeof(double) * 3);
		memcpy(rotMat + 4, evec + 3, sizeof(double) * 3);
		memcpy(rotMat + 8, evec + 3, sizeof(double) * 3);
		rotMat[15] = 1;
		glMultMatrixd(rotMat);
		//assert(eval[0] > 0 && eval[1] > 0 && eval[2] > 0);
		glScaled(sqrt(eval[0]), sqrt(eval[1]), sqrt(eval[2]));
		glutSolidSphere(1.0, 12, 12);
		glPopMatrix();
	}
}