/*
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;
}
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;
}
