Ejemplo n.º 1
0
void renderScene(void) {

	//open file
	fstream file;
	stringstream filename;
	filename << "ProjectionData_"<< time(0)<<".dat";

	file.open(filename.str().c_str(),ios::out);


for(int q(100);q>=50;--q)
{
file << "FRAME NO. "<<q<<endl;
for(int fu(0);fu<2;++fu){
	/*if(_m[0]<1)
		_m[0] += 0.1;
	else
		_m[0] = 0.99;*/
/*
	if(_m[4]<1)
		_m[4] += 0.10;
	else
		_m[4] = 0.99;

	if(_m[8]<1)
		_m[8] += 0.10;
	else
		_m[8] = 0.99;*/
	int window_w = 1280;
	int window_h = 1024;

	cout << "render scene... "<<endl;

	TriangleIterator ti;
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glBegin(GL_TRIANGLES);

	//map<int, vector<Pnt3f> > _COLORS;
	map<int, int> _COLORS;

	for(ti = TEST->beginTriangles();ti != TEST->endTriangles();++ti)
	{
		int rgb_index = 0;
		int r(0),g(0),b(0);
		while(_COLORS[rgb_index] != NULL)
		{
			// create random color
			r = (int)rand() % 255;
			g = (int)rand() % 255;
			b = (int)rand() % 255;

			// calculate index
			rgb_index = 256 * g + 256 * 256 * r + b;
			//cout << rgb_index << endl;
		}

		// SAVE ALL _USED_ COLORS
		vector<Pnt3f> v;
		v.push_back(ti.getPosition(0));
		v.push_back(ti.getPosition(1));
		v.push_back(ti.getPosition(2));
		_COLORS[rgb_index] = ti.getIndex();

		//cout << "r " << r << " g "<<g << " b "<<b<<endl;

		// get points from triangle
		Pnt3f p1 = ti.getPosition(0);
		Pnt3f p2 = ti.getPosition(1);
		Pnt3f p3 = ti.getPosition(2);

		//set color and add vertices
		//glColor3f(r,g,b);
		glColor3ub(r,g,b);
		glVertex3f(p1[0],p1[1],p1[2]);
		glVertex3f(p2[0],p2[1],p2[2]);
		glVertex3f(p3[0],p3[1],p3[2]);

	}
	glEnd();

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glEnable(GL_POLYGON_OFFSET_FILL);
	glPolygonOffset(1.0, 1.0);

	glBegin(GL_TRIANGLES);
	// set background color
	glColor3f(0,0,0);
	for(ti = TEST->beginTriangles();ti != TEST->endTriangles();++ti)
	{
		Pnt3f p1 = ti.getPosition(0);
		Pnt3f p2 = ti.getPosition(1);
		Pnt3f p3 = ti.getPosition(2);

		glVertex3f(p1[0],p1[1],p1[2]);
		glVertex3f(p2[0],p2[1],p2[2]);
		glVertex3f(p3[0],p3[1],p3[2]);
	}
	glEnd();

	glDisable(GL_POLYGON_OFFSET_FILL);
	glDisable(GL_DEPTH_TEST);

	//glutSwapBuffers();

	map<int,std::vector<int> > color_map;

	// window size
	int size = window_w*window_h*3;
	// read pixels
	GLubyte *pixels = new GLubyte[size];
	glReadPixels(0 , 0 , window_w , window_h , GL_RGB , GL_UNSIGNED_BYTE , pixels);

	// init RGB and count&debug values
	int red,green,blue;
	int count(0);
	//iterate through pixels
	for(int u(0);u < size;u=u+3){
		// get pixels
		red = pixels[u];
		green = pixels[u+1];
		blue = pixels[u+2];
		// calc unique index
		int index = 256 * green + 256 * 256 * red + blue;
		// ignore black
		if(index == 0 )
			continue;

		// fill RGB vector
		vector<int> ct;
		ct.push_back(red);
		ct.push_back(green);
		ct.push_back(blue);

		// put in map
		color_map[index] = ct;
	}

	cout << "Colors seen in frame: "<< color_map.size()<<endl;

	map<int,vector<int> >::iterator mip;
	// for all _visible_ triangles
	int h(0);
	FaceIterator fit = TEST->beginFaces();
	float thresh = 0.95;
	int count_lines_drawn(0);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINES);

	vector<Pnt3f> cp;
	for(mip = color_map.begin();mip != color_map.end();mip++){
		int face_index = _COLORS[mip->first];
		fit.seek(face_index);

		int a = fit.getPositionIndex(0);
		int b = fit.getPositionIndex(1);
		int c = fit.getPositionIndex(2);

		FaceIterator nit;
		glBegin(GL_LINES);
		glColor3f(1.0,0,0);
		for(nit = TEST->beginFaces();nit != TEST->endFaces();++nit)
		{

			if(fit.getIndex() == nit.getIndex())
				continue;
			int a2 = nit.getPositionIndex(0);
			int b2 = nit.getPositionIndex(1);
			int c2 = nit.getPositionIndex(2);

			// a-b
			if(a == a2 || a == b2 || a == c2)
				if(b == a2 || b == b2 || b == c2){
					if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){
						count_lines_drawn++;
						//glVertex3f(fit.getPosition(0)[0],fit.getPosition(0)[1],fit.getPosition(0)[2]);
						//glVertex3f(fit.getPosition(1)[0],fit.getPosition(1)[1],fit.getPosition(1)[2]);
						_LINES.push_back(fit.getPosition(0));
						_LINES.push_back(fit.getPosition(1));
						//createControlPoints(_LINES.size()-2,fit.getPosition(0),fit.getPosition(1));
					}
					h++;
				}
			// a-c
			if(a == a2 || a == b2 || a == c2)
				if(c == a2 || c == b2 || c == c2){
					if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){
						count_lines_drawn++;
						//glVertex3f(fit.getPosition(0)[0],fit.getPosition(0)[1],fit.getPosition(0)[2]);
						//glVertex3f(fit.getPosition(2)[0],fit.getPosition(2)[1],fit.getPosition(2)[2]);
						_LINES.push_back(fit.getPosition(0));
						_LINES.push_back(fit.getPosition(2));
						//createControlPoints(_LINES.size()-2,fit.getPosition(0),fit.getPosition(2));
					}
					h++;
				}
			// c-b
			if(c == a2 || c == b2 || c == c2)
				if(b == a2 || b == b2 || b == c2){
					if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){
						count_lines_drawn++;
						//glVertex3f(fit.getPosition(1)[0],fit.getPosition(1)[1],fit.getPosition(1)[2]);
						//glVertex3f(fit.getPosition(2)[0],fit.getPosition(2)[1],fit.getPosition(2)[2]);
						_LINES.push_back(fit.getPosition(1));
						_LINES.push_back(fit.getPosition(2));
						//createControlPoints(_LINES.size()-2,fit.getPosition(1),fit.getPosition(2));
					}
					h++;
				}
		}

		glEnd();
	}

	//glutSwapBuffers();

	// DRAW _CONTROLPOINTS_

	//sorted lines by length in _LINES
	//sortLines();
	// create CP's now
	_HITPOINTS.clear();
	_CONTROLPOINTS.clear();
	_CP2D.clear();
	_LINES2D.clear();
	/*for(int i(0);i < _LINES.size()/10;i=i+2){
		createControlPoints(i,_LINES[i],_LINES[i+1]);
	}*/
/*
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glBegin(GL_POINTS);
	glColor3f(0,1,0);

	// for each line
	for(int i(0);i < _CONTROLPOINTS.size(); ++i){
		// get CP-vector
		vector<Pnt3f> cps = _CONTROLPOINTS[i];
		// for every single controlpoint
		for(int j(0);j<cps.size();++j)
			glVertex3f(cps[i][0],cps[i][1],cps[i][2]);
	}
	glEnd();
	glutSwapBuffers();
*/
	cout << "Lines over Threshold :: "<< _CONTROLPOINTS.size() <<endl;
	cout << "Shared Edges :: " << h <<endl;
	cout << "Lines drawn :: " << count_lines_drawn << endl;
	cout << "Lines :: "<< _LINES.size()<<endl;

	//open cv
	stringstream stream;
	stream << "pics/undist/resize/";
	stream << q;
	stream << ".bmp";
	/*"pics/1_1.bmp"*/
	cout << "/*** PICTURE "<< stream.str().c_str() << " ***/"<<endl;

	cv::Mat init_Image2 = cvLoadImage(stream.str().c_str());
	cv::flip(init_Image2,init_Image2,0);

	// overwrite canny with ground truth
	//cv::Mat init_Image2 = cvLoadImage("ground_truth/canny/30c.bmp");

	cv::Mat gray;
	cv::Mat init_Image;// = init_Image2.clone();

	cv::cvtColor(init_Image2,gray,CV_RGB2GRAY);


	cv::Mat gaus;
	cv::GaussianBlur(gray,gaus,cv::Size(3,3),1);
	////cv::Sobel(gray,init_Image,init_Image.type(),1,0,3);
	cv::Canny(gaus,init_Image,5,10,3,true);

	vector<vector<cv::Point> > contours;
	vector<vector<cv::Point> > new_contours;
	cv::findContours(init_Image,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE,cv::Point());

	cv::Mat new_contour = cv::Mat(1024,1280,CV_8UC3);
	// for all contours
	for(int c(0);c<contours.size();++c){
		int size = (contours[c]).size();
		//cout << "size = "<< size <<endl;
		cv::Point start = (contours[c])[0];
		cv::Point end = (contours[c])[size];
		float length = sqrt((start.x-end.x)*(start.x-end.x)+(start.y-end.y)*(start.y-end.y));

		//check contours size
		if(contours[c].size()>20 /* && (size-10 < length < size+10)*/){

			for(int c1(0);c1<contours[c].size();++c1){
				cv::Point tmp = (contours[c])[c1];

				cv::line(new_contour,tmp,tmp,CV_RGB(255,255,255));
			}
		}
		//cout << start.x << " " << start.y<<endl;
		//cout << end.x << " " << end.y<<endl;
		//if(length > 20)
		//cout << "::"<<length<<endl;

	}
	//exit(0);
	cv::Mat cont;
	cv::cvtColor(new_contour,cont,CV_RGB2GRAY);
	init_Image = cont.clone();


	//cv::cvtColor(init_Image,init_Image2,CV_GRAY2RGB);
	//init_Image = init_Image2.clone();

	cv::imshow("gray",init_Image);

	// window size
	size = window_w*window_h*3;
	// read pixels
	GLubyte *pixels2 = new GLubyte[size];
	glReadPixels(0 , 0 , window_w , window_h , GL_RGB , GL_UNSIGNED_BYTE , pixels2);

	cv::Mat ogl = cv::Mat(window_h,window_w,CV_8UC3);
	ogl.data = pixels2;

	cv::flip(ogl,ogl,0);
	//cv::imwrite("test45.bmp",ogl);

	CvSize size2;
	size2.height=1024;
	size2.width=1280;
	cv::Mat result = cv::Mat(size2,CV_8UC3);//= (cv::Mat)cvCreateImage(size2,8,3);

	//MY 2D-POINTS
	GLdouble modelview[16], projection2[16];
	GLint viewport[4];

	glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
	glGetDoublev(GL_PROJECTION_MATRIX, projection2);
	glGetIntegerv(GL_VIEWPORT, viewport);

	/*double tx, ty, tz;
	for(int i(0); i < _LINES.size(); ++i)
	{
		Pnt3f tmp = _LINES[i];
		cout << "3d-point:" << tmp<<endl;
		gluProject(tmp[0], tmp[1], tmp[2],
			modelview, projection2, viewport,
			&tx, &ty, &tz);

		//cout <<"x: " <<tx << "y: "<<ty<<endl;
		Pnt3d res;
		res[0] = tx;
		res[1] = ty;
		res[2] = tz;
		cout <<"2d-point"<<res<<endl;
		_LINES2D.push_back(res);
	}*/

	int thres = (int)_LINES.size();//(int)(_LINES.size()/2)*0.15;
	sortLines3D(thres);
	_HITPOINTS = map<int,vector<Pnt2f> >();
	double tx1, ty1, tz1;
	double tx2, ty2, tz2;
	// draw 2D-Lines
	//define threshold for CP creation

	//cout << ">>>>>> "<<thres<<endl;
	cv::flip(init_Image,init_Image,0);
	int ok(0);
	for(int i(0);i<thres;i=i+2){
		ok = 0;

		// get 3D Line endings
		Pnt3d p1 = _LINES[i];
		Pnt3d p2 = _LINES[i+1];

		if(p1.dist(p2)<0.3){
			continue;
		}

		// get 2d points of line endings
		gluProject(p1[0], p1[1], p1[2],
			modelview, projection2, viewport,
			&tx1, &ty1, &tz1);

		gluProject(p2[0], p2[1], p2[2],
			modelview, projection2, viewport,
			&tx2, &ty2, &tz2);

		// create Normals
		Pnt2f lineStart, lineEnd;
		lineStart[0]=tx1;lineStart[1]=ty1;
		lineEnd[0]=tx2;lineEnd[1]=ty2;
		Pnt2f lineNormal = lineStart - lineEnd;
		float l = sqrt(lineNormal[0]*lineNormal[0]+lineNormal[1]*lineNormal[1]);
		Pnt2f normal1,normal2;
		//normalized Normals 1 & 2
		normal1[0]=-lineNormal[1]/l;normal1[1]=lineNormal[0]/l;
		normal2[0]=lineNormal[1]/l;normal2[1]=-lineNormal[0]/l;

		// draw line
		//cv::line(result,cv::Point(tx1,ty1),cv::Point(tx2,ty2),CV_RGB(255,0,0),1);

		//creating 2d controlpoints
		//float t = createControlPoints2D(i,p1,p2);
		//if(i <= thres){
			//vector<Pnt2d> cp = _CP2D[i];
			vector<Pnt3f> cp = _CONTROLPOINTS[i];
			Pnt2f old_hit = Pnt2f(-1,0);
			for(int j(0);j<cp.size();++j){
				//get single 3D-ControlPoint
				Pnt3d tmp = cp[j];
				//cout <<"i: "<<i<<" j: "<< j<<endl;
				// fixpoint check!
				if(i==0){
					if(_FIX[0] == -1){
						double x,y,z;
						gluProject(tmp[0], tmp[1], tmp[2],
							modelview, projection2, viewport,
							&x,&y,&z);
						//Pnt2d tmp2d;
						_FIX[0] = x;
						_FIX[1] = y;

					//	cout << "::: "<< (_HITPOINTS[0]).size()<<endl;
					}
					(_HITPOINTS[0]).push_back(_FIX);
					cv::circle(result,cv::Point(_FIX[0],_FIX[1]),3,CV_RGB(255,255,0),2);
					continue;
				}

				// project to 2D
				double x,y,z;
				gluProject(tmp[0], tmp[1], tmp[2],
					modelview, projection2, viewport,
					&x,&y,&z);
				Pnt2d tmp2d;
				tmp2d[0] = x;
				tmp2d[1] = y;
				// CONTROLPOINTS DRAWING
				//cv::circle(result,cv::Point(x,y),2,CV_RGB(100,100,100),2);

				//Pnt2f n1 = _NORMALS[i];
				//Pnt2f n2 = _NORMALS[i+1];
				Pnt2f hit = checkNormal(tmp2d,normal1,init_Image);
				Pnt2f hit1 = checkNormal(tmp2d,normal2,init_Image);

				//cout << "hit" << hit <<endl;

				bool outlier = isOutlier(tmp2d, hit,normal1,old_hit);
				bool outlier2= isOutlier(tmp2d,hit1,normal1,old_hit);
				old_hit = hit;

				//drawing
				if(outlier && outlier2){
					//ok++;
					(_HITPOINTS[i]).push_back(Pnt2f(-1,0));
					//cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3);
					//cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3);
					continue;
					}
				else {
					ok++;
					if(!outlier && !outlier2){
						if(tmp2d.dist(hit)<tmp2d.dist(hit1)){
							//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit[0],hit[1]),CV_RGB(0,255,0),1);
							(_HITPOINTS[i]).push_back(hit);
							old_hit = hit;
							//cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3);
						}
						else{
							//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit1[0],hit1[1]),CV_RGB(0,255,0),1);
							(_HITPOINTS[i]).push_back(hit1);
							old_hit = hit1;
							//cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3);
						}
					} else if(!outlier){
						(_HITPOINTS[i]).push_back(hit);
						//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit[0],hit[1]),CV_RGB(0,255,0),1);
						old_hit = hit;
						//cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3);
						}
						else {
						(_HITPOINTS[i]).push_back(hit1);
						//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit1[0],hit1[1]),CV_RGB(0,255,0),1);
						old_hit = hit1;
						//cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3);
						}
					//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(tmp2d[0]+normal1[0],tmp2d[1]+normal1[1]),CV_RGB(0,255,0));
					//cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(tmp2d[0]+normal2[0],tmp2d[1]+normal2[1]),CV_RGB(0,255,0));
				}

			}

			/*vector<Pnt2f> t_vec = _HITPOINTS[i];
			if(t_vec.size() < 5){
				_CONTROLPOINTS[i].clear();
				_HITPOINTS[i].clear();
			}*/
			cout << i<<"::: "<< (_HITPOINTS[i]).size()<<endl;
			int realHP(0);
			for(int g(0);g<_HITPOINTS[i].size();++g){
				if(_HITPOINTS[i][g] != -1)
					realHP++;
			}
			if((int)_CONTROLPOINTS[i].size()*0.2 > realHP)
			{
				_CONTROLPOINTS[i].clear();
				_HITPOINTS[i].clear();
			}
			//cout << "REAL HITPOINTS IN LINE "<< i << " -> "<<realHP<<endl;
			//cout << ":::"<<ok<<endl;
			// clear if not enough hitpoints on line
			if(ok < 3 && i != 0){
				//(_HITPOINTS[i]).clear();
				//cout << "clear hitlist"<< endl;
			}
	}
	int countCP = 0;
	//int thres = (int)(_LINES.size()/2)*0.15;
	for(int i(0);i<thres;i=i+2){
		//cout << "line : "<<i<<endl;
		vector<Pnt3f> tmp = _CONTROLPOINTS[i];
		vector<Pnt2f> tmp2 = _HITPOINTS[i];
		for(int j(0);j<tmp.size();++j){
			countCP++;
			Pnt2f hitp = tmp2[j];
			//cv::circle(result,cv::Point(hitp[0],hitp[1]),2,CV_RGB(255,255,0));
			//cout << "Point "<< tmp2[j]<<endl;
		//	cout << "3D-Point "<< tmp[j]<<endl;
		}
		//exit(1);
	}
	cout << "####CONTROLPOINTS>> "<< countCP<<endl;
	countCP = 0;

	cout << "#### LEV - MAR ###"<<endl;
	for(int v(0);v<12;++v){
		cout << " " << _m[v];
		if(v%3 == 2)
			cout <<endl;
	}
	cout << "================"<<endl;

	int ret;
	//for(int go(0);go <=25	; ++go)
	ret = dlevmar_dif(mapping,_m,NULL,12,_CP,1000,NULL,NULL,NULL,NULL,NULL);
	//ret = dlevmar_dif(mapping,_m,NULL,12,_CP,1000,NULL,NULL,NULL,NULL,NULL);

	for(int v(0);v<12;++v){
		cout << " " << _m[v];
		//file << " " << projection2[v];
		if(v%3 == 2){
			cout <<endl;
			//file <<endl;
		}
	}
	file << _m[0] << "," << _m[1] << "," << _m[2] << endl;
	file << _m[3] << "," << _m[4] << "," << _m[5] << endl;
	file << _m[6] << "," << _m[7] << "," << _m[8]<< endl;
	file << _m[9] << "," << _m[10] << "," << _m[11]<< endl;
	file << endl;

	cout <<endl;
	cout <<"iterated for: "<<ret<<endl;
	cout << "#### LEV - MAR  - ENDE###"<<endl;
	//cout << "== HIT POINTS USED " << _HITPOINTS.size()<<endl;
	drawModel(result,0,0,255);

	//convert gray canny image back to rgb

	//cv::cvtColor(init_Image,init_Image2,CV_GRAY2RGB);

	// generated error image
	//cv::add(original,result,result);
	cv::flip(init_Image2,init_Image2,0);
	cv::add(init_Image2,result,result);
	// flip like hell
	cv::flip(result,result,0);

	// save image
	cv::imshow("showing image",result);
	stringstream ss;
	ss << time(0) << ".bmp";
	cv::imwrite(ss.str(),result);
	_CONTROLPOINTS.clear();
	_HITPOINTS.clear();
	_LINES.clear();
}
file << "=====" <<endl;
}
file.close();
}
Ejemplo n.º 2
0
Action::ResultE OOCOSGFeeder::enter(NodePtr& node)
{
    GeometryPtr geo = GeometryPtr::dcast(node->getCore());
    
    if(geo == NullFC)
        return Action::Continue;
    
    GeoPositionsPtr pos = geo->getPositions();
    UInt32 pntindexbase;
    
    if(_poss.find(pos) != _poss.end())
    {
        pntindexbase = _poss[pos];
        pos = NullFC;
    }
    else
    {
        pntindexbase = _pntindexbase;
        _poss[pos]   = _pntindexbase;
        _pntindexbase += pos->getSize();
    }   
    
    UInt32 matind = MaterialPool::addMaterial(geo->getMaterial());

    if(pos != NullFC)
    {
        if(_pfunc != NULL)
        {
            Pnt3f p;
            UInt32 s = pos->getSize();
            
            for(UInt32 i = 0; i < s; ++i)
            {
                if(_npts != 0)
                {
                    ++_pntprog;
                    
                    Real32 prog = _pntprog / (Real32)_npts;
                    if(prog > _nextpntprog)
                    {
                        PLOG << _nextpntprog * 100 << "%..";
                        _nextpntprog += 0.1;
                    }
                }
                
                pos->getValue(p, i);
                
                _pfunc(_rec, p);
            }            
        }
    }
    
    if(_tfunc != NULL)
    {
        TriangleIterator it, end = geo->endTriangles();

        for(it = geo->beginTriangles(); 
            it != end; 
            ++it)
        {
            if(_ntris != 0)
            {
                ++_triprog;

                Real32 prog = _triprog / (Real32)_ntris;
                if(prog > _nexttriprog)
                {
                    PLOG << _nexttriprog * 100 << "%..";
                    _nexttriprog += 0.1;
                }
            }
                
            _tfunc(_rec, it.getPositionIndex(0) + pntindexbase, 
                         it.getPositionIndex(1) + pntindexbase, 
                         it.getPositionIndex(2) + pntindexbase,
                         matind);
        }
    }
    
    return Action::Continue;
}