Esempio n. 1
0
void GLScenePane::drawPoints() {
	//draw current map points
	for (size_t i = 0; i < curMapPoints.size(); i++) {
		const MapPoint* p = curMapPoints[i];
		drawCurMapPoint(p, m_scale, m_pointSize, true); //!MyApp::bStop);
	}
	glPointSize(3.0 * m_pointSize);
	//draw active map points
	glBegin(GL_POINTS);
	for (size_t i = 0; i < actMapPoints.size(); i++) {
		const MapPoint* p = actMapPoints[i];
		if (p->isCertainStatic()) {
			double r = p->color[0] / 255.0;
			double g = p->color[1] / 255.0;
			double b = p->color[2] / 255.0;
			glColor3d(r, g, b);
			glVertex3d(p->x, p->y, p->z);
		}
	}
	glEnd();
	glPointSize(2.0 * m_pointSize);
	//	//draw inactive map points
	//	for (size_t i = 0; i < iactMapPoints.size(); i++) {
	//		const MapPoint* p = iactMapPoints[i];
	//		if (p->type == TYPE_MAPPOINT_STATIC) {
	//			//glColor3f(PT_COLOR[0], PT_COLOR[1], PT_COLOR[2]);
	//			glColor3f(0.35f, 0.35f, 0.35f);
	//			glTranslatef(p->x, p->y, p->z);
	//			//glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? m_range * 0.018 : m_range * 0.018 * 5, 12, 12);
	//			glutSolidSphere(0.004 * m_range * p->numVisCam, 12, 12);
	//			//			glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? 0.004 * m_range * p->numVisCam : m_range * 0.004 * 5, 12,
	//			//					12);
	//			glTranslatef(-p->x, -p->y, -p->z);
	//		}
	//	}
	//glDisable(GL_LIGHT1);
	//glDisable(GL_LIGHTING);

	//draw inactive map points
	glBegin(GL_POINTS);
	for (size_t i = 0; i < iactMapPoints.size(); i++) {
		const MapPoint* p = iactMapPoints[i];
		if (p->isGlobalStatic()) {
			double r = p->color[0] / 255.0;
			double g = p->color[1] / 255.0;
			double b = p->color[2] / 255.0;
			glColor3d(r, g, b);

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

	//draw dynamic trajectories
	if (m_drawDynTrj) {
//		glPointSize(0.5);
//		glBegin(GL_POINTS);
//		int tf = dynMapPoints.size();
//		int f0 = tf - m_nTrjLen < 0 ? 0 : tf - m_nTrjLen;
//		int numFrms = tf - f0;
//		double dalpha = 0.3 / (numFrms + 1.0);
//		for (int f = f0; f < tf; f++) {
//			glColor4d(0, 0, 1, 0.0 + (f - f0) * dalpha);
//			for (int i = 0; i < dynMapPoints[f].m; i++) {
//				glVertex3d(dynMapPoints[f].data[3 * i], dynMapPoints[f].data[3 * i + 1], dynMapPoints[f].data[3 * i + 2]);
//			}
//		}
//		glEnd();

		for (size_t n = 0; n < dynTracks.size(); n++) {
			glLineWidth(0.5);
			glBegin(GL_LINE_STRIP);

			double alpha = 1.0 / dynTracks[n].size();
			for (size_t i = 0; i < dynTracks[n].size(); i++) {
				glColor4d(0, 0, 1, 1.0 - i * alpha);
				glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
						dynTracks[n][i].z);
			}
			glEnd();
		}
	}
}
Esempio n. 2
0
void GLScenePane::drawPoints() {
	//draw current map points
	for (size_t i = 0; i < curMapPoints.size(); i++) {
		const MapPoint* p = curMapPoints[i];
		drawCurMapPoint(p, m_scale, m_pointSize, true); //!MyApp::bStop);
	}
	glPointSize(8.0 * m_pointSize);
	//draw active map points
//	glBegin(GL_POINTS);
//	for (size_t i = 0; i < actMapPoints.size(); i++) {
//		const MapPoint* p = actMapPoints[i];
//		if (p->isCertainStatic()) {
//			double r = p->color[0] / 255.0;
//			double g = p->color[1] / 255.0;
//			double b = p->color[2] / 255.0;
//			glColor3d(0, 1, 0);
//			glVertex3d(p->x, p->y, p->z);
//		}
//	}
//	glEnd();
	glPointSize(2.0 * m_pointSize);
	//	//draw inactive map points
	//	for (size_t i = 0; i < iactMapPoints.size(); i++) {
	//		const MapPoint* p = iactMapPoints[i];
	//		if (p->type == TYPE_MAPPOINT_STATIC) {
	//			//glColor3f(PT_COLOR[0], PT_COLOR[1], PT_COLOR[2]);
	//			glColor3f(0.35f, 0.35f, 0.35f);
	//			glTranslatef(p->x, p->y, p->z);
	//			//glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? m_range * 0.018 : m_range * 0.018 * 5, 12, 12);
	//			glutSolidSphere(0.004 * m_range * p->numVisCam, 12, 12);
	//			//			glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? 0.004 * m_range * p->numVisCam : m_range * 0.004 * 5, 12,
	//			//					12);
	//			glTranslatef(-p->x, -p->y, -p->z);
	//		}
	//	}
	//glDisable(GL_LIGHT1);
	//glDisable(GL_LIGHTING);

	//draw inactive map points
//	glBegin(GL_POINTS);
//	for (size_t i = 0; i < iactMapPoints.size(); i++) {
//		const MapPoint* p = iactMapPoints[i];
//		if (p->isGlobalStatic()) {
//			double r = p->color[0] / 255.0;
//			double g = p->color[1] / 255.0;
//			double b = p->color[2] / 255.0;
//			glColor3d(r, g, b);
//
//			glVertex3d(p->x, p->y, p->z);
//		}
//	}
//	glEnd();

	if (mDynObjPresent){
		glPointSize(30.0 * m_pointSize);
		glBegin(GL_POINTS);
		glColor3d(1.0, 0, 0);
		glVertex3d(mDynObjPos[0], mDynObjPos[1], mDynObjPos[2]);
		glEnd();

//		glLineWidth(60);
//		glBegin(GL_LINE_STRIP);
//		glPointSize(10.0 * m_pointSize);
//		glBegin(GL_POINTS);
//		double alpha = 1.0 / mDynObjPosVec.size();
//		for (size_t i = 0; i < mDynObjPosVec.size(); i = i + 3) {
//			glColor3d(1, 0, 0);
//			glVertex3d(mDynObjPosVec[i],mDynObjPosVec[i+1],mDynObjPosVec[i+2]);
//		}
//		glEnd();
	}

	//draw dynamic trajectories
	if (m_drawDynTrj) {
//		glPointSize(0.5);
//		glBegin(GL_POINTS);
//		int tf = dynMapPoints.size();
//		int f0 = tf - m_nTrjLen < 0 ? 0 : tf - m_nTrjLen;
//		int numFrms = tf - f0;
//		double dalpha = 0.3 / (numFrms + 1.0);
//		for (int f = f0; f < tf; f++) {
//			glColor4d(0, 0, 1, 0.0 + (f - f0) * dalpha);
//			for (int i = 0; i < dynMapPoints[f].m; i++) {
//				glVertex3d(dynMapPoints[f].data[3 * i], dynMapPoints[f].data[3 * i + 1], dynMapPoints[f].data[3 * i + 2]);
//			}
//		}
//		glEnd();


		// Simple Clustering
//		vector<cv::Point3f> dynVel;
//		dynVel.clear();
//		for (size_t n = 0; n < dynTracks.size(); n++) {
//			dynVel.push_back(cv::Point3f(dynTracks[n][0].x - dynTracks[n][1].x,
//					dynTracks[n][0].y - dynTracks[n][1].y,
//					dynTracks[n][0].z - dynTracks[n][1].z));
//		}
//
//		vector<double> numLike;
//		vector<vector<int> > idLike;
//		int maxId = -1;
//		int maxNum = 0;
//
//		for (int i = 0; i < dynVel.size(); i++){
//			vector<int> ids;
//			for (int j = 0; j < dynVel.size(); j++){
////				if(j != i){
//					double lengthDiff = 0;
//					double lengthI = sqrt(pow(dynVel[i].x,2) + pow(dynVel[i].y,2) + pow(dynVel[i].z,2));
//					double lengthJ = sqrt(pow(dynVel[j].x,2) + pow(dynVel[j].y,2) + pow(dynVel[j].z,2));
//					lengthDiff = abs(lengthI - lengthJ);
//					double dotVec = dynVel[i].dot(dynVel[j]);
//					double theta = acos(dotVec / (lengthI * lengthJ)) * 180 / 3.1415926;
//					if (theta < 30){
//						ids.push_back(j);
//					}
////				}
//			}
//			idLike.push_back(ids);
//			if (ids.size() > maxNum){
//				maxId = i;
//				maxNum = ids.size();
//			}
//		}

//		for (size_t n = 0; n < dynTracks.size(); n++) {
//			glLineWidth(0.5);
//			glBegin(GL_LINE_STRIP);
//
//			double alpha = 1.0 / dynTracks[n].size();
//			for (size_t i = 0; i < dynTracks[n].size(); i++) {
//				glColor4d(0, 0, 1, 1.0 - i * alpha);
//				glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
//						dynTracks[n][i].z);
//			}
//			glEnd();
//		}
//
//		if (maxId > -1){
////			for (size_t ii = 0; ii < idLike[maxId].size(); ii++) {
////				int n = idLike[maxId][ii];
////				glLineWidth(1.5);
////				glBegin(GL_LINE_STRIP);
////
////				double alpha = 1.0 / dynTracks[n].size();
////				for (size_t i = 0; i < dynTracks[n].size(); i++) {
////					glColor4d(1, 0, 0, 1.0 - i * alpha);
////					glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
////							dynTracks[n][i].z);
////				}
////				glEnd();
////			}
//			for (size_t ii = 0; ii < idLike[maxId].size(); ii++){
//				int n = idLike[maxId][ii];
//				glPointSize(10.0 * m_pointSize);
//				glBegin(GL_POINTS);
//				glColor3d(1.0, 0, 0);
//				glVertex3d(dynTracks[n][0].x, dynTracks[n][0].y,
//						dynTracks[n][0].z);
//				glEnd();
//			}
//		}
	}
}