/*
  Aufruf dieser Funktion erfolgt bei Traversierung des Szenengraphen
  mittels OpenSG-Funktion traverse().
  Enthaelt ein Knoten verwertbare Geometrieinformation so tragen wir
  Zeiger auf seine Geometrie (OpenSG-Strukturen) im array gla_meshInfo_
  ein.
  Nebenbei bestimmen wir für die Geometrie auch noch die World-Space-
  Transformation (evtl. existiert eine OpenSG-Funktion um diese
  Information zu erhalten, der Autor hat keine in der OpenSG-API
  entdeckt).
*/
Action::ResultE enter(NodePtr &node)
{
    int             i, j, h;
    Pnt3f           v;
    int             numFaces, numFaceVertices, vId, size;

    MeshInfo        meshInfo;
    TinyMatrix      transf;
    FaceIterator    fit;
    int             numQuads;
    NamePtr         namePtr;
    char            name[255];

    namePtr = NamePtr::dcast(node->findAttachment(Name::getClassType().getGroupId()));
    if(namePtr == osg::NullFC)
        strcpy(name, "");
    else
    {
        strcpy(name, namePtr->getFieldPtr()->getValue().c_str());
    }

    SINFO << "Node name = '" << name << "'" << endl << endLog;

    GeometryPtr geo = GeometryPtr::dcast(node->getCore());

    if(geo != NullFC)
    {
        GeoPLengthsUI32Ptr  pLength = GeoPLengthsUI32Ptr::dcast(geo->getLengths());
        GeoPTypesUI8Ptr     pTypes = GeoPTypesUI8Ptr::dcast(geo->getTypes());

        /* pLength and pTypes should not be NullFC, however VRML Importer/Exporter
		  code is instable by now, so this can happen */
        if((pLength != NullFC) && (pTypes != NullFC))
        {
            GeoPLengthsUI32::StoredFieldType * pLengthField = pLength->getFieldPtr();
            GeoPTypesUI8::StoredFieldType * pTypeField = pTypes->getFieldPtr();

            size = pLengthField->size();

            for(h = 0; h < size; h++)
            {
                if(((*pTypeField)[h] == GL_TRIANGLES) ||
                   ((*pTypeField)[h] == GL_QUADS))
                {
                    /* may quads appear in GL_TRIANGLES ? */
                    /* check if all triangles have three vertices */
                    numQuads = 0;
                    fit = geo->beginFaces();
                    while(fit != geo->endFaces())
                    {
                        numFaceVertices = fit.getLength();
                        if(numFaceVertices == 4)
                            numQuads++;
                        if(numFaceVertices > 4)
                        {
                            SWARNING <<
                                "More than 4 vertices in face!" <<
                                endl <<
                                endLog;
                            return Action::Continue;

                            // exit(1);
                        }

                        ++fit;
                    }

                    if(numQuads > 0)
                    {
                        SWARNING << "Quad encountered" << endl << endLog;
                    }

                    if(gl_sga->nodeDepth_ > 0)
                    {
                        for(i = 0; i < gl_sga->nodeDepth_; i++)
                        {
                            meshInfo.transf = meshInfo.transf * gl_sga->transf_[i];
                        }
                    }
                    else
                        meshInfo.transf.identity();

                    /* access to vertices */
                    GeoPositions3fPtr   pPos = GeoPositions3fPtr::dcast(geo->getPositions());
                    GeoPositions3f::StoredFieldType * pPosField = pPos->getFieldPtr();

                    /* access to faces */
                    numFaces = 0;
                    fit = geo->beginFaces();
                    for(fit = geo->beginFaces(); fit != geo->endFaces(); ++fit)
                    {
                        numFaceVertices = fit.getLength();

                        for(j = 0; j < numFaceVertices; j++)
                        {
                            vId = fit.getPositionIndex(j);
                        }

                        numFaces++;
                    }                       /* for fit */

                    /* set other mesh attributes */
                    meshInfo.numQuads = numQuads;
                    meshInfo.geoPtr = geo;
                    meshInfo.vPtr = pPosField;
                    meshInfo.triangularFaces = (numQuads == 0);
                    meshInfo.numVertices = pPosField->size();
                    meshInfo.numFaces = numFaces;

                    gl_sga->meshInfo_.push_back(meshInfo);
                    gl_sga->numGeometryNodes_++;
                }
                else
                {
                    //			SWARNING << "Neither triangle nor quad. Field type = " <<
                    //				        (*pTypeField)[h] << endl << endLog;
                }
            }                               /* for h */
        }                                   /* if pLength!=NullFC */
    }
    else if(node->getCore()->getType().isDerivedFrom(Transform::getClassType()))
    {
        TransformPtr    t = TransformPtr::dcast(node->getCore());
        Matrix          ma;

        ma = t->getMatrix();

        SINFO <<
            "Node type derived from transform, skipping children" <<
            endl <<
            endLog;

        for(i = 0; i < 4; i++)
        {
            for(j = 0; j < 4; j++)
            {
                transf[i][j] = ma[j][i];    /* the matrix is stored as columns/rows ? */
            }
        }

        if(gl_sga->nodeDepth_ > gl_sga->maxNodeDepth_)
        {
            gl_sga->maxNodeDepth_ = gl_sga->nodeDepth_;
            gl_sga->transf_.push_back(transf);
        }
        else
        {
            gl_sga->transf_[gl_sga->nodeDepth_] = transf;
        }

        gl_sga->nodeDepth_++;
    }

    return Action::Continue;
}
Example #2
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();
}
/*
  Konvertierung des mit meshId bezeichneten IndexedFaceSets in ein Mesh
  vom Typ TriangleSet.
  Zu beachten:
  IndexedFaceSets, die Eckpunkte "shared", werden beim Parsen des
  Szenengraphen (Konstruktor) zu einem Teilmesh zusammengefasst
  (erhalten dieselbe ID).
  Bei der Konvertierung wird aus diesen ein Mesh generiert.
*/
int WmSceneGraphAccess::convertToTs(TriangleSet *ts, int *numQuadsFound,
                                    GEOMARK_BitArray **isQuadVertex, int meshId)
{
    int             i, j, k;
    int             offsetVertices, numVertices, numFaces, numFaceVertices;
    int             to, from;
    TinyVector      v;
    FaceIterator    fit;
    int             trVIds[3], quadVIds[4];
    GeometryPtr     geoPtr;
    BOOL            quadsPresent;
    int             numQuads;

    if(numMeshes_ < 1)
        return -1;

    offsetVertices = 0;
    *isQuadVertex = NULL;
    quadsPresent = FALSE;
    numQuads = 0;

    if(meshId == -1)
    {               /* generate single flat mesh */
        /* first pass */
        SINFO << "Generating single flat mesh" << endl << endLog;

        numVertices = 0;
        numFaces = 0;
        numQuads = 0;
        for(i = 0; i < numGeometryNodes_; i++)
        {
            if((i == 0) || (meshInfo_[i].meshId != meshInfo_[i - 1].meshId))
            {
                numVertices += meshInfo_[i].numVertices;
                SINFO <<
                    "mesh id = " <<
                    meshInfo_[i].meshId <<
                    endl <<
                    "# of vertices = " <<
                    meshInfo_[i].numVertices <<
                    endl <<
                    endLog;
            }

            numFaces += meshInfo_[i].numFaces;
            numQuads += meshInfo_[i].numQuads;
            if((!quadsPresent) && (meshInfo_[i].numQuads > 0))
                quadsPresent = TRUE;
        }

        if(quadsPresent)
        {
            *isQuadVertex = new GEOMARK_BitArray(numVertices);
            numFaces += numQuads;
        }

        SINFO << "# of faces = " << numFaces << endl << endLog;

        ts->init(numVertices, numFaces);

        /* second pass */
        numVertices = 0;
        numFaces = 0;

        for(i = 0; i < numGeometryNodes_; i++)
        {
            if((i > 0) && (meshInfo_[i].meshId != meshInfo_[i - 1].meshId))
            {
                offsetVertices += meshInfo_[i - 1].numVertices;
            }

            if((i == 0) || (meshInfo_[i].meshId != meshInfo_[i - 1].meshId))
            {
                /* set vertex data */
                for(j = 0; j < meshInfo_[i].vPtr->size(); j++)
                {
                    v[0] = (*meshInfo_[i].vPtr)[j][0];
                    v[1] = (*meshInfo_[i].vPtr)[j][1];
                    v[2] = (*meshInfo_[i].vPtr)[j][2];

                    /* transformation does matter for case of single flat mesh */
                    v = meshInfo_[i].transf * v;    /* transform into world space */

                    ts->setVertex(offsetVertices + j, v);
                }

                numVertices += meshInfo_[i].numVertices;
            }

            /* set face data */
            geoPtr = meshInfo_[i].geoPtr;

            for(fit = geoPtr->beginFaces(); fit != geoPtr->endFaces(); ++fit)
            {
                /* this should be always 3 */
                numFaceVertices = fit.getLength();
                if(numFaceVertices == 4)
                {
                    for(j = 0; j < 4; j++)
                    {
                        quadVIds[j] = fit.getPositionIndex(j);
                        (*isQuadVertex)->setBit(quadVIds[j], 1);
                    }

                    /* split quad into two triangles */
                    ts->setFace(numFaces, offsetVertices + quadVIds[0],
                                offsetVertices + quadVIds[1],
                                offsetVertices + quadVIds[2]);
                    numFaces++;
                    ts->setFace(numFaces, offsetVertices + quadVIds[2],
                                offsetVertices + quadVIds[3],
                                offsetVertices + quadVIds[0]);
                    numFaces++;
                }
                else
                {
                    if(numFaceVertices != 3)
                    {
                        printf("numFaceVertices=%d\n", numFaceVertices);

                        // exit(1);
                        return -1;
                    }

                    for(j = 0; j < 3; j++)
                    {
                        trVIds[j] = fit.getPositionIndex(j);
                    }

                    ts->setFace(numFaces, offsetVertices + trVIds[0],
                                offsetVertices + trVIds[1],
                                offsetVertices + trVIds[2]);
                    numFaces++;
                }
            }                               /* for fit */
        }                                   /* for i */

        ts->meshComplete();
    }
    else
    {
        /* generate triangleset for mesh meshId */
        from = 0;

        while((from < numGeometryNodes_) && (meshInfo_[from].meshId < meshId))
        {
            from++;
        }

        if((from < numGeometryNodes_) && (meshInfo_[from].meshId == meshId))
        {
            /* first pass: count vertices and faces  */
            numVertices = 0;
            numFaces = 0;
            offsetVertices = 0;             /* const 0 */
            i = from;
            numVertices = meshInfo_[i].numVertices;
            numQuads = 0;
            while((i < numGeometryNodes_) && (meshInfo_[i].meshId == meshId))
            {
                numFaces += meshInfo_[i].numFaces;
                numQuads += meshInfo_[i].numQuads;
                i++;
            }

            SINFO << "# of vertices = " << numVertices << endLog;
            SINFO << "# of quads = " << numQuads << endLog;
            SINFO << "# of faces = " << numFaces << endl << endLog;

            if(numQuads > 0)
            {
                *isQuadVertex = new GEOMARK_BitArray(numVertices);
                numFaces += numQuads;
            }

            ts->init(numVertices, numFaces);

            /* second pass */
            numVertices = 0;
            numFaces = 0;
            offsetVertices = 0;
            i = from;
            while((i < numGeometryNodes_) && (meshInfo_[i].meshId == meshId))
            {
                /* set vertex data */
                for(j = 0; j < meshInfo_[i].vPtr->size(); j++)
                {
                    v[0] = (*meshInfo_[i].vPtr)[j][0];
                    v[1] = (*meshInfo_[i].vPtr)[j][1];
                    v[2] = (*meshInfo_[i].vPtr)[j][2];

                    /* transformation does not matter for single IndexedFaceSets */
                    ts->setVertex(offsetVertices + j, v);
                }

                /* set face data */
                geoPtr = meshInfo_[i].geoPtr;

                for(fit = geoPtr->beginFaces(); fit != geoPtr->endFaces();
                    ++fit)
                {
                    numFaceVertices = fit.getLength();
                    if(numFaceVertices == 4)
                    {
                        SWARNING <<
                            "Quad found. Will be split" <<
                            endl <<
                            endLog;
                        for(j = 0; j < 4; j++)
                        {
                            quadVIds[j] = fit.getPositionIndex(j);
                            (*isQuadVertex)->setBit(quadVIds[j], 1);
                        }

                        /* split quad into two triangles */
                        ts->setFace(numFaces, offsetVertices + quadVIds[0],
                                    offsetVertices + quadVIds[1],
                                    offsetVertices + quadVIds[2]);
                        numFaces++;
                        ts->setFace(numFaces, offsetVertices + quadVIds[2],
                                    offsetVertices + quadVIds[3],
                                    offsetVertices + quadVIds[0]);
                        numFaces++;
                    }
                    else
                    {
                        if(numFaceVertices != 3)
                        {
                            SFATAL <<
                                "# of vertices for face = " <<
                                numFaceVertices <<
                                endl <<
                                endLog;
                            return 1;
                        }

                        for(j = 0; j < 3; j++)
                        {
                            trVIds[j] = fit.getPositionIndex(j);
                        }

                        ts->setFace(numFaces, offsetVertices + trVIds[0],
                                    offsetVertices + trVIds[1],
                                    offsetVertices + trVIds[2]);
                        numFaces++;
                    }
                }                           /* for fit */

                i++;
            }                               /* while i */

            SINFO << "# of faces = " << numFaces << endl << endLog;

            ts->meshComplete();
        }                                   /* if */
        else
        {
            SFATAL << "No mesh with id " << meshId << endl << endLog;
            return 1;
        }
    }                                       /* generate flat mesh from all nodes */

    *numQuadsFound = numQuads;

    return 0;
}