1 #include "edit_scan.h"
2 #include "wrap/gui/trackball.h"
3 #include "wrap/qt/trackball.h" //QT2VCG trackball function
4 #include "wrap/qt/to_string.h" //QT2VCG trackball function
5
6 #include <wrap/gl/pick.h>
7
8 #define EDITSCANHACK
9 #ifdef EDITSCANHACK
10 #include <stdio.h>
11
12 class Sample{
13 public:
14 Point3f p; // object space coordinate
15 Point3f n; // normal
16 Point3f v; // view direction
17 bool bg; // volume carving ray? (part of background)
18 };
19
20 vector<Sample> temp_samples;
21
write_to_ply()22 void write_to_ply(){
23 FILE* fid = fopen( "/tmp/output.ply", "w" );
24 if( fid == NULL ){
25 qDebug("The output cannot be opened!");
26 return;
27 }
28
29 fprintf(fid, "ply\n");
30 fprintf(fid, "format ascii 1.0\n");
31 fprintf(fid, "element vertex %d\n", (int) temp_samples.size() );
32 fprintf(fid, "property float x\n");
33 fprintf(fid, "property float y\n");
34 fprintf(fid, "property float z\n");
35 fprintf(fid, "property float nx\n");
36 fprintf(fid, "property float ny\n");
37 fprintf(fid, "property float nz\n");
38 fprintf(fid, "property float vx\n");
39 fprintf(fid, "property float vy\n");
40 fprintf(fid, "property float vz\n");
41 fprintf(fid, "end_header\n");
42 for(unsigned int i=0; i<temp_samples.size(); i++){
43 Point3f& p = temp_samples[i].p;
44 Point3f& n = temp_samples[i].n;
45 Point3f& v = temp_samples[i].v;
46 fprintf(fid, "%f %f %f %f %f %f %f %f %f\n", p[0],p[1],p[2],n[0],n[1],n[2],v[0],v[1],v[2]);
47 }
48 fclose(fid);
49 }
50 #endif
51
52
myGluProject(Point3f p)53 Point2f myGluProject( Point3f p ){
54 // retrieve matrixes from the pipeline
55 GLdouble mvMatrix_f[16]; glGetDoublev(GL_MODELVIEW_MATRIX, mvMatrix_f);
56 GLdouble prMatrix_f[16]; glGetDoublev(GL_PROJECTION_MATRIX, prMatrix_f);
57 GLint viewpSize[4]; glGetIntegerv(GL_VIEWPORT, viewpSize);
58 // project the point p on viewport obtaining point q
59 Point2d q; double discard;
60 gluProject(p[0], p[1], p[2],
61 (const GLdouble *) mvMatrix_f,
62 (const GLdouble *) prMatrix_f,
63 (const GLint *) viewpSize,
64 &q[0], &q[1], &discard );
65 Point2f retf( q[0], q[1] );
66 return retf;
67 }
myGluUnProject(Point2f p,float z)68 Point3f myGluUnProject( Point2f p, float z ){
69 // retrieve matrixes from the pipeline
70 GLdouble mvMatrix_f[16]; glGetDoublev(GL_MODELVIEW_MATRIX, mvMatrix_f);
71 GLdouble prMatrix_f[16]; glGetDoublev(GL_PROJECTION_MATRIX, prMatrix_f);
72 GLint viewpSize[4]; glGetIntegerv(GL_VIEWPORT, viewpSize);
73 // project the point p on viewport obtaining point q
74 Point3d q;
75 gluUnProject(p[0], p[1], z,
76 (const GLdouble *) mvMatrix_f,
77 (const GLdouble *) prMatrix_f,
78 (const GLint *) viewpSize,
79 &q[0], &q[1], &q[2] );
80 Point3f retf( q[0], q[1], q[2] );
81 return retf;
82 }
83
randn()84 double randn(){
85 static double N=100;
86 return (rand() % ((int)N)) / N -.5;
87 }
88
StartEdit(MeshDocument & md,GLArea * gla)89 bool VirtualScan::StartEdit(MeshDocument& md, GLArea* gla){
90 assert(md.mm() != NULL); // Model exists
91 this->md = &md;
92 this->gla = gla;
93 gla->fov = 5; // Orthographic
94 isScanning = false; // Trigger is off initially
95 timer = 0;
96
97 //--- Create a new model to store the scan cloud
98 cloud = md.addNewMesh("Scan cloud",false);
99
100 //--- Instantiate the UI, and connect events
101 widget = new Widget(gla->window());
102 connect(widget, SIGNAL(laser_parameter_updated()),
103 this, SLOT(laser_parameter_updated()));
104 connect(widget, SIGNAL(scan_requested()),
105 this, SLOT(scan_requested()));
106 connect(widget, SIGNAL(save_requested()),
107 this, SLOT(save_requested()));
108
109 //--- Compute initial beam
110 laser_parameter_updated();
111
112 return true;
113 }
114 // We need to refresh the laser scan completely!
laser_parameter_updated()115 void VirtualScan::laser_parameter_updated(){
116 //--- Retrieve values from GUI
117 int period = 1000 / widget->getSampfreq();
118 int numsamp = widget->getNumsample();
119
120 // Compute start and stop coordinates in image space
121 Point2f delta( widget->getScanwidth()*gla->height()/(3.0*100.0), 0 );
122 Point2f str( gla->width()/2, gla->height()/2 ); str-=delta;
123 Point2f sto( gla->width()/2, gla->height()/2 ); sto+=delta;
124 // float width = *md->mm()->cm.bbox.MaxDim()/100.0;
125 qDebug("period: %d, samples: %d", period, numsamp);
126 // qDebug() << toString(str) << " " << toString(sto);
127
128 //--- Create the geometry of the scanline
129 gla->update(); // since we use gluproject
130 sline = ScanLine( numsamp, str, sto);
131 // sline = ScanLineGeom( 10, .1 ); // hardcoded parameters (GUI independent)
132
133 //--- Create a timer which enables scanning periodically
134 if( timer!=0 ) delete timer;
135 timer = new QTimer(this);
136 connect(timer, SIGNAL(timeout()), this, SLOT(readytoscan()));
137 timer->start(period); // period of repetition in ms
138
139 //--- Update the laser rendering
140 gla->update();
141 }
EndEdit(MeshModel &,GLArea *)142 void VirtualScan::EndEdit(MeshModel&, GLArea* ){
143 delete cloud;
144 delete widget;
145 }
146
save_requested()147 void VirtualScan::save_requested(){
148 #ifdef EDITSCANHACK
149 //--- Output model of "samples" to Ply file
150 qDebug("Writing to ply...");
151 write_to_ply();
152 #else
153 //--- Create a new model to store the scan cloud
154 cloud = md->addNewMesh("Scan cloud",false);
155 #endif
156 }
157
158 // This is called only when mouse is pressed at first during a drag or a click is received
mousePressEvent(QMouseEvent * e,MeshModel &,GLArea * gla)159 void VirtualScan::mousePressEvent(QMouseEvent* e, MeshModel &, GLArea* gla){
160 this->laser_parameter_updated();
161 gla->trackball.MouseDown(e->x(),gla->height()-e->y(), QT2VCG(e->button(), e->modifiers() ) );
162 gla->update();
163 }
164 // This is called during the whole drag
mouseMoveEvent(QMouseEvent * e,MeshModel &,GLArea * gla)165 void VirtualScan::mouseMoveEvent(QMouseEvent* e, MeshModel &, GLArea* gla){
166 gla->trackball.MouseMove(e->x(),gla->height()-e->y());
167 gla->update();
168 }
mouseReleaseEvent(QMouseEvent * e,MeshModel &,GLArea * gla)169 void VirtualScan::mouseReleaseEvent(QMouseEvent* e, MeshModel &, GLArea* gla){
170 isScanning = false;
171 gla->trackball.MouseUp(e->x(),gla->height()-e->y(), QT2VCG(e->button(), e->modifiers() ) );
172 gla->update();
173 }
Decorate(MeshModel & mm,GLArea * gla)174 void VirtualScan::Decorate(MeshModel& mm, GLArea* gla){
175 // Scan the mesh only if we are triggering a scan and if the scanner is ready to suck in a new sample
176 // This needs to be done after the mesh rendering has been done, but before any the scanned cloud is drawn!!
177 if( isScanning && sampleReady ){
178 sampleReady = false;
179 isScanning = false;
180 scanpoints();
181 }
182
183 //--- Draw the scanned samples stored in the cloud
184 glDisable(GL_LIGHTING);
185 cloud->cm.C() = Color4b(255,200,200,255);
186 glColor4f(.4f,.4f,1.f,.6f);
187 cloud->glw.cdm = GLW::DMPoints;
188 cloud->glw.SetHintParamf(GLW::HNPPointSize,SCANPOINTSIZE);
189 cloud->Render(GLW::DMPoints, GLW::CMPerMesh, GLW::TMNone);
190 glEnable(GL_LIGHTING);
191 if(widget->getDrawLineFlag()){
192 //--- Shows the view directions of the scanned samples
193 // The "decorate plugin does nothing inside GLW, but does it directly
194 // put these before ->Render
195 // cloud->updateDataMask(MeshModel::MM_VERTNORMAL);
196 // cloud->glw.cnm = GLW::NMPerVert;
197 float LineLen = mm.cm.bbox.Diag()/20.0;
198 glEnable(GL_BLEND);
199 glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
200 glDisable(GL_LIGHTING);
201 glBegin(GL_LINES);
202 glColor4f(.4f,.4f,1.f,.6f);
203 for(CMeshO::VertexIterator vi=cloud->cm.vert.begin(); vi!=cloud->cm.vert.end(); ++vi){
204 glVertex((*vi).P());
205 glVertex((*vi).P()+(*vi).N()*LineLen);
206 }
207 glEnd();
208 }
209 //--- Draw the laser beam (just to help interfacing)
210 sline.render(gla);
211 }
212
ScanLine(int N,Point2f & srt,Point2f & end)213 ScanLine::ScanLine(int N, Point2f& srt, Point2f& end){
214
215
216 // qDebug() << "Scanpoint list: ";
217
218 #define RANGE
219 #ifdef LASER
220 float alpha=0;
221 double delta_01 = 1.0 / (N-1);
222 for( int i=0; i<N; i++, alpha+=delta_01 ){
223 Point2f curr = srt*(1-alpha) + end*alpha;
224 soff.push_back(curr);
225 // qDebug() << " - " << toString( curr );
226 Point2i currI( curr[0], curr[1] );
227 bbox.Add(currI);
228 }
229 #endif
230 #ifdef RANGE
231 double delta_01 = 1.0 / (N-1.0);
232 double halfedgel = ((srt-end).Norm()/2.0);
233 srt[1] = srt[1] - halfedgel;
234 end[1] = end[1] + halfedgel;
235
236 Point2f srt_x=srt, end_x=end;
237 Point2f srt_y=srt, end_y=end;
238 srt_x[1]=0; end_x[1]=0;
239 srt_y[0]=0; end_y[0]=0;
240 float alpha=0;
241 for( int i=0; i<N; i++, alpha+=delta_01 ){
242 float beta = 0;
243 for( int j=0; j<N; j++, beta+=delta_01 ){
244 Point2f curr = srt_x*(1-alpha) + end_x*alpha + srt_y*(1-beta) + end_y*beta;
245 soff.push_back(curr);
246 Point2i currI( curr[0], curr[1] );
247 bbox.Add(currI);
248 }
249 }
250 #endif
251
252 // Retrieve a block 2 pixel larger from buffer
253 bbox.Offset(2);
254 }
255
scanpoints()256 void VirtualScan::scanpoints(){
257 //--- Create samples
258 int X = sqrt( sline.soff.size() ); // TODO: move
259 int Y = sqrt( sline.soff.size() ); // TODO: move
260 vector<Sample> curr_samples( sline.soff.size() );
261
262 //--- Read the portion of depth buffer we are interested in
263 float* buffer = new float[ sline.bbox.Area() ];
264 glReadPixels(sline.bbox.min[0],sline.bbox.min[1],sline.bbox.Dim()[0],
265 sline.bbox.Dim()[1],GL_DEPTH_COMPONENT, GL_FLOAT, buffer);
266
267 //--- Extract samples
268 for( int x=0,I=0; x<X; x++ ){
269 for( int y=0; y<Y; y++,I=x+X*y ){
270 //--- Get current scan point offset
271 Point2f laser_xy = sline.soff[I];
272
273 //--- Convert xy location into the buffer offset
274 Point2i buf_off;
275 buf_off[0] = round( laser_xy[0]-sline.bbox.min[0] );
276 buf_off[1] = round( laser_xy[1]-sline.bbox.min[1] );
277
278 //--- Retrieve Z from depth buffer
279 double z = buffer[ buf_off[0] + sline.bbox.DimX()*buf_off[1] ];
280
281 //--- Initialize sample
282 Point3f temp = myGluUnProject( laser_xy, z+0.01 );
283 curr_samples[I].p = myGluUnProject( laser_xy, z ); // sample
284 curr_samples[I].v = (curr_samples[I].p-temp).normalized(); // view direction
285 curr_samples[I].n = Point3f(0,0,0); // normal
286 // Check against Z-Buffer limit set by glClearDepth
287 // http://www.opengl.org/sdk/docs/man/xhtml/glClearDepth.xml
288 // Out of Z-buffer implies rays at infinity!!
289 curr_samples[I].bg = (z==1); // isbackground?
290 }
291 }
292
293 //--- Compute gradients and discard invalid
294 // samples on outer layer are invalid by default
295 for( int x=1,I=0; x<X-1; x++ ){
296 for( int y=1; y<Y-1; y++,I=x+X*y){
297 Sample& curr = curr_samples[ x + X* (y) ];
298
299 //--- Extract samples for center differences
300 Point3f& px_next = curr_samples[(x+1) + X* (y) ].p;
301 Point3f& px_prev = curr_samples[(x-1) + X* (y) ].p;
302 Point3f& py_next = curr_samples[ (x) + X*(y+1)].p;
303 Point3f& py_prev = curr_samples[ (x) + X*(y-1)].p;
304
305 //--- Extract tangent vectors & normal
306 Point3f px_tan = px_next-px_prev;
307 Point3f py_tan = py_next-py_prev;
308 curr.n = -( px_tan ^ py_tan );
309 curr.n.Normalize();
310
311 //--- Regardless if BG or not
312 if( curr.n.dot(curr.v) < cos( 60/180.0*3.14159 ) )
313 continue;
314
315 //--- Restore NULL normal for BG samples
316 if( curr.bg )
317 curr.n = Point3f(0,0,0);
318
319 //--- Add scanned sample to the "viz" cloud
320 tri::Allocator<CMeshO>::AddVertices(cloud->cm,1);
321 cloud->cm.vert.back().P() = curr.p;
322 cloud->cm.vert.back().N() = curr.v;
323
324 //--- Add scanned sample to the "data" cloud
325 temp_samples.push_back( curr );
326 }
327 }
328
329 delete [] buffer;
330 }
331
render(GLArea * gla)332 void ScanLine::render(GLArea* gla){
333 #if 0
334 //--- DEBUG! why mv[4,3] = -1000?
335 GLdouble mv[16]; glGetDoublev(GL_MODELVIEW_MATRIX, mv);
336 for(int i=0; i<4; i++)
337 qDebug() << mv[4*i+0] << " " << mv[4*i+1] << " " << mv[4*i+2] << " " << mv[4*i+3];
338 qDebug();
339 #endif
340
341 // This draws directly in 2D image space
342 glDisable(GL_DEPTH_TEST);
343 glPointSize(SCANPOINTSIZE);
344 glColor(Color4f(255,0,0,255));
345 glMatrixMode(GL_PROJECTION);
346 glPushMatrix();
347 glLoadIdentity();
348 glOrtho(0, gla->width(), 0, gla->height(), -1, 1);
349 glMatrixMode(GL_MODELVIEW);
350 glPushMatrix();
351 glLoadIdentity();
352 glColor3f(1.0,0.0,0.0);
353 glBegin(GL_POINTS);
354 for(unsigned int i=0; i<soff.size(); i++)
355 glVertex(soff[i]);
356 // glVertex2f( gla->width()/2, gla->height()/2 );
357 glEnd();
358 // glRectf(100,100, 200, 200);
359 glPopMatrix(); // restore modelview
360 // The pop must be done in the projection mode
361 glMatrixMode(GL_PROJECTION);
362 glPopMatrix();
363 }
364
365 // Must be at the end of everything in CPP file or get segfault at plugin load
366 Q_EXPORT_PLUGIN(VirtualScan)
367
368