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(); }