1 | /* |
---|
2 | |
---|
3 | Brian Curless |
---|
4 | |
---|
5 | Computer Graphics Laboratory |
---|
6 | Stanford University |
---|
7 | |
---|
8 | --------------------------------------------------------------------- |
---|
9 | |
---|
10 | Copyright (1997-2001) The Board of Trustees of the Leland Stanford |
---|
11 | Junior University. Except for commercial resale, lease, license or |
---|
12 | other commercial transactions, permission is hereby given to use, |
---|
13 | copy, modify this software for academic purposes only. No part of |
---|
14 | this software or any derivatives thereof may be used in the production |
---|
15 | of computer models for resale or for use in a commercial |
---|
16 | product. STANFORD MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY KIND |
---|
17 | CONCERNING THIS SOFTWARE. No support is implied or provided. |
---|
18 | |
---|
19 | */ |
---|
20 | |
---|
21 | |
---|
22 | #include "DepthMap.h" |
---|
23 | #include "defines.h" |
---|
24 | #include "linePersp.h" |
---|
25 | #include "perspective.h" |
---|
26 | #include "vripGlobals.h" |
---|
27 | #include <limits.h> |
---|
28 | #include <ply.h> |
---|
29 | #include <math.h> |
---|
30 | #include <stdlib.h> |
---|
31 | |
---|
32 | #ifdef linux |
---|
33 | #include <float.h> |
---|
34 | #endif |
---|
35 | |
---|
36 | static char *elem_names[] = { |
---|
37 | "vertex", "range_grid" |
---|
38 | }; |
---|
39 | |
---|
40 | static PlyProperty vert_props[] = { |
---|
41 | {"x", PLY_FLOAT, PLY_FLOAT, 0, 0, PLY_START_TYPE, PLY_START_TYPE, 0}, |
---|
42 | {"y", PLY_FLOAT, PLY_FLOAT, 0, 0, PLY_START_TYPE, PLY_START_TYPE, 0}, |
---|
43 | {"z", PLY_FLOAT, PLY_FLOAT, 0, 0, PLY_START_TYPE, PLY_START_TYPE, 0} |
---|
44 | }; |
---|
45 | |
---|
46 | static PlyProperty range_grid_props[] = { |
---|
47 | {"vertex_indices", PLY_INT, PLY_INT, 0, 1, PLY_UCHAR, PLY_UCHAR, 0}, |
---|
48 | }; |
---|
49 | |
---|
50 | struct PlyVertex { |
---|
51 | float x, y, z; |
---|
52 | }; |
---|
53 | |
---|
54 | struct RangeGridList { |
---|
55 | uchar nverts; |
---|
56 | int *verts; |
---|
57 | }; |
---|
58 | |
---|
59 | static PlyVertex *vert_dummy; |
---|
60 | #define voffset(field) ((char *) (&vert_dummy->field) - (char *) vert_dummy) |
---|
61 | RangeGridList *range_grid_dummy; |
---|
62 | #define roffset(field) ((char *) (&range_grid_dummy->field) - (char *) range_grid_dummy) |
---|
63 | |
---|
64 | |
---|
65 | DepthMap::DepthMap() |
---|
66 | { |
---|
67 | this->xdim = this->ydim = 0; |
---|
68 | this->origXdim = this->origYdim = 0; |
---|
69 | this->elems = NULL; |
---|
70 | this->norms = NULL; |
---|
71 | this->tags = NULL; |
---|
72 | this->treeElems = NULL; |
---|
73 | this->holeTreeElems = NULL; |
---|
74 | } |
---|
75 | |
---|
76 | |
---|
77 | DepthMap::DepthMap(int xd, int yd, int useNorms) |
---|
78 | { |
---|
79 | this->xdim = xd; |
---|
80 | this->ydim = yd; |
---|
81 | |
---|
82 | this->origXdim = xd; |
---|
83 | this->origYdim = yd; |
---|
84 | |
---|
85 | this->elems = new DepthElement[xd*yd]; |
---|
86 | if (this->elems == NULL) { |
---|
87 | this->xdim = 0; |
---|
88 | this->ydim = 0; |
---|
89 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
90 | } |
---|
91 | |
---|
92 | this->tags = new uchar[xd*yd]; |
---|
93 | if (this->tags == NULL) { |
---|
94 | this->xdim = 0; |
---|
95 | this->ydim = 0; |
---|
96 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
97 | } |
---|
98 | |
---|
99 | this->edgeSteps = new signed char[xd*yd]; |
---|
100 | if (this->edgeSteps == NULL) { |
---|
101 | this->xdim = 0; |
---|
102 | this->ydim = 0; |
---|
103 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
104 | } |
---|
105 | |
---|
106 | if (useNorms) { |
---|
107 | this->norms = new DepthMapNorm[xd*yd]; |
---|
108 | if (this->norms == NULL ) { |
---|
109 | this->xdim = 0; |
---|
110 | this->ydim = 0; |
---|
111 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
112 | } |
---|
113 | } |
---|
114 | } |
---|
115 | |
---|
116 | |
---|
117 | DepthMap::DepthMap(int xd, int yd, int useNorms, int granularity) |
---|
118 | { |
---|
119 | this->xdim = xd; |
---|
120 | this->ydim = yd; |
---|
121 | |
---|
122 | this->origXdim = xd; |
---|
123 | this->origYdim = yd; |
---|
124 | |
---|
125 | this->elems = new DepthElement[xd*yd]; |
---|
126 | if (this->elems == NULL) { |
---|
127 | this->xdim = 0; |
---|
128 | this->ydim = 0; |
---|
129 | printf("DepthMap::new - couldn't allocate memory\n"); |
---|
130 | } |
---|
131 | |
---|
132 | this->tags = new uchar[xd*yd]; |
---|
133 | if (this->tags == NULL) { |
---|
134 | this->xdim = 0; |
---|
135 | this->ydim = 0; |
---|
136 | printf("DepthMap::new - couldn't allocate memory\n"); |
---|
137 | } |
---|
138 | |
---|
139 | this->edgeSteps = new signed char[xd*yd]; |
---|
140 | if (this->edgeSteps == NULL) { |
---|
141 | this->xdim = 0; |
---|
142 | this->ydim = 0; |
---|
143 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
144 | } |
---|
145 | |
---|
146 | if (useNorms) { |
---|
147 | this->norms = new DepthMapNorm[xd*yd]; |
---|
148 | if (this->norms == NULL ) { |
---|
149 | this->xdim = 0; |
---|
150 | this->ydim = 0; |
---|
151 | fprintf(stderr, "DepthMap::new - couldn't allocate memory\n"); |
---|
152 | } |
---|
153 | } |
---|
154 | |
---|
155 | this->initTree(granularity); |
---|
156 | } |
---|
157 | |
---|
158 | |
---|
159 | void |
---|
160 | DepthMap::initTree(int granularity) |
---|
161 | { |
---|
162 | this->treeGranularity = granularity; |
---|
163 | int numLeaves = int(ceil(float(this->xdim)/granularity)); |
---|
164 | |
---|
165 | int temp = 1; |
---|
166 | int depth = 1; |
---|
167 | while (temp < numLeaves) { |
---|
168 | temp = temp << 1; |
---|
169 | depth++; |
---|
170 | } |
---|
171 | |
---|
172 | //this->treeDepth = int(ceil(log(numLeaves)/log(2)))+1; |
---|
173 | this->treeDepth = depth; |
---|
174 | this->origTreeDepth = depth; |
---|
175 | |
---|
176 | this->treeElems = new DepthTreeElement**[this->ydim]; |
---|
177 | this->holeTreeElems = new DepthTreeElement**[this->ydim]; |
---|
178 | for (int yy = 0; yy < this->ydim; yy++) { |
---|
179 | this->treeElems[yy] = new DepthTreeElement*[this->treeDepth]; |
---|
180 | this->holeTreeElems[yy] = new DepthTreeElement*[this->treeDepth]; |
---|
181 | int numElems = 1; |
---|
182 | for (int j = 0; j < this->treeDepth; j++) { |
---|
183 | this->treeElems[yy][j] = new DepthTreeElement[numElems]; |
---|
184 | this->holeTreeElems[yy][j] = new DepthTreeElement[numElems]; |
---|
185 | for (int k = 0; k < numElems; k++) { |
---|
186 | this->treeElems[yy][j][k].minZ = FLT_MAX; |
---|
187 | this->treeElems[yy][j][k].maxZ = -FLT_MAX; |
---|
188 | this->holeTreeElems[yy][j][k].minZ = FLT_MAX; |
---|
189 | this->holeTreeElems[yy][j][k].maxZ = -FLT_MAX; |
---|
190 | } |
---|
191 | numElems *= 2; |
---|
192 | } |
---|
193 | } |
---|
194 | |
---|
195 | this->leafIndices = new int[2*numLeaves]; |
---|
196 | this->runs = new int[4*numLeaves]; |
---|
197 | |
---|
198 | } |
---|
199 | |
---|
200 | void |
---|
201 | DepthMap:: updateNx() |
---|
202 | { |
---|
203 | for (int i = 0; i < this->xdim*this->ydim; i++) { |
---|
204 | this->norms[i].nx = (signed char)(127*this->elems[i].conf); |
---|
205 | } |
---|
206 | } |
---|
207 | |
---|
208 | void |
---|
209 | DepthMap:: updateNy() |
---|
210 | { |
---|
211 | for (int i = 0; i < this->xdim*this->ydim; i++) { |
---|
212 | this->norms[i].ny = (signed char)(127*this->elems[i].conf); |
---|
213 | } |
---|
214 | } |
---|
215 | |
---|
216 | void |
---|
217 | DepthMap:: updateNz() |
---|
218 | { |
---|
219 | for (int i = 0; i < this->xdim*this->ydim; i++) { |
---|
220 | this->norms[i].nz = (signed char)(127*this->elems[i].conf); |
---|
221 | } |
---|
222 | } |
---|
223 | |
---|
224 | void |
---|
225 | DepthMap:: updateEdgeSteps() |
---|
226 | { |
---|
227 | for (int i = 0; i < this->xdim*this->ydim; i++) { |
---|
228 | this->edgeSteps[i] = (signed char)(127*this->elems[i].conf+0.5); |
---|
229 | } |
---|
230 | } |
---|
231 | |
---|
232 | |
---|
233 | // |
---|
234 | // Nomenclature for triangles when tagging |
---|
235 | // |
---|
236 | // z3 ----- z4 |
---|
237 | // | /| |
---|
238 | // | / | |
---|
239 | // | 4 / | |
---|
240 | // | / | |
---|
241 | // | / | |
---|
242 | // | / 3 | |
---|
243 | // | / | |
---|
244 | // z1 ----- z2 |
---|
245 | // |
---|
246 | // |
---|
247 | // |
---|
248 | // z3 ----- z4 |
---|
249 | // |\ | |
---|
250 | // | \ 2 | |
---|
251 | // | \ | |
---|
252 | // | \ | |
---|
253 | // | \ | |
---|
254 | // | 1 \ | |
---|
255 | // | \ | |
---|
256 | // z1 ----- z2 |
---|
257 | // |
---|
258 | |
---|
259 | void |
---|
260 | DepthMap::tagCellsForResampling() |
---|
261 | { |
---|
262 | float z1, z2, z3, z4, minZ, maxZ; |
---|
263 | |
---|
264 | for (int yy = 0; yy < this->ydim-1; yy++) { |
---|
265 | for (int xx = 0; xx < this->xdim-1; xx++) { |
---|
266 | DepthElement *depthBuf = this->elems + xx + yy*xdim; |
---|
267 | uchar *tagBuf = this->tags + xx + yy*xdim; |
---|
268 | z1 = depthBuf->z; |
---|
269 | z2 = (depthBuf+1)->z; |
---|
270 | z3 = (depthBuf+xdim)->z; |
---|
271 | z4 = (depthBuf+xdim+1)->z; |
---|
272 | |
---|
273 | *tagBuf = 0; |
---|
274 | |
---|
275 | int valid_count = IS_VALID_DEPTH(z1) + IS_VALID_DEPTH(z2) + |
---|
276 | IS_VALID_DEPTH(z3) + IS_VALID_DEPTH(z4); |
---|
277 | |
---|
278 | if (valid_count < 3) { |
---|
279 | continue; |
---|
280 | } |
---|
281 | |
---|
282 | if (valid_count == 3) { |
---|
283 | if (!IS_VALID_DEPTH(z4)) { |
---|
284 | maxZ = MAX(MAX(z1,z2),z3); |
---|
285 | minZ = MIN(MIN(z1,z2),z3); |
---|
286 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
287 | *tagBuf = DepthMap::TRIANGLE_ONE; |
---|
288 | } |
---|
289 | } |
---|
290 | else if (!IS_VALID_DEPTH(z1)) { |
---|
291 | maxZ = MAX(MAX(z4,z2),z3); |
---|
292 | minZ = MIN(MIN(z4,z2),z3); |
---|
293 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
294 | *tagBuf = DepthMap::TRIANGLE_TWO; |
---|
295 | } |
---|
296 | } |
---|
297 | else if (!IS_VALID_DEPTH(z3)) { |
---|
298 | maxZ = MAX(MAX(z1,z2),z4); |
---|
299 | minZ = MIN(MIN(z1,z2),z4); |
---|
300 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
301 | *tagBuf = DepthMap::TRIANGLE_THREE; |
---|
302 | } |
---|
303 | } |
---|
304 | else if (!IS_VALID_DEPTH(z2)) { |
---|
305 | maxZ = MAX(MAX(z1,z3),z4); |
---|
306 | minZ = MIN(MIN(z1,z3),z4); |
---|
307 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
308 | *tagBuf = DepthMap::TRIANGLE_FOUR; |
---|
309 | } |
---|
310 | } |
---|
311 | } |
---|
312 | else { |
---|
313 | if (fabs(z1-z4) > fabs(z2-z3)) { |
---|
314 | maxZ = MAX(MAX(z1,z2),z3); |
---|
315 | minZ = MIN(MIN(z1,z2),z3); |
---|
316 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
317 | *tagBuf = DepthMap::TRIANGLE_ONE; |
---|
318 | } |
---|
319 | maxZ = MAX(MAX(z4,z2),z3); |
---|
320 | minZ = MIN(MIN(z4,z2),z3); |
---|
321 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
322 | *tagBuf |= DepthMap::TRIANGLE_TWO; |
---|
323 | } |
---|
324 | } else { |
---|
325 | maxZ = MAX(MAX(z1,z2),z4); |
---|
326 | minZ = MIN(MIN(z1,z2),z4); |
---|
327 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
328 | *tagBuf = DepthMap::TRIANGLE_THREE; |
---|
329 | } |
---|
330 | |
---|
331 | maxZ = MAX(MAX(z1,z3),z4); |
---|
332 | minZ = MIN(MIN(z1,z3),z4); |
---|
333 | if (maxZ - minZ < MAX_DEPTH_DIFFERENCE) { |
---|
334 | *tagBuf |= DepthMap::TRIANGLE_FOUR; |
---|
335 | } |
---|
336 | } |
---|
337 | } |
---|
338 | } |
---|
339 | } |
---|
340 | } |
---|
341 | |
---|
342 | |
---|
343 | void |
---|
344 | DepthMap:: normalize() |
---|
345 | { |
---|
346 | float nx, ny, nz, length; |
---|
347 | |
---|
348 | for (int i = 0; i < this->xdim*this->ydim; i++) { |
---|
349 | nx = (float)this->norms[i].nx; |
---|
350 | ny = (float)this->norms[i].ny; |
---|
351 | nz = (float)this->norms[i].nz; |
---|
352 | length = sqrt(nx*nx+ny*ny+nz*nz); |
---|
353 | if (length != 0) { |
---|
354 | nx /= length; |
---|
355 | ny /= length; |
---|
356 | nz /= length; |
---|
357 | } else { |
---|
358 | nx = ny = nz = 0; |
---|
359 | } |
---|
360 | |
---|
361 | this->norms[i].nx = (signed char)(nx); |
---|
362 | this->norms[i].ny = (signed char)(ny); |
---|
363 | this->norms[i].nz = (signed char)(nz); |
---|
364 | } |
---|
365 | } |
---|
366 | |
---|
367 | int |
---|
368 | DepthMap::reuse(int xd, int yd) |
---|
369 | { |
---|
370 | this->xdim = xd; |
---|
371 | this->ydim = yd; |
---|
372 | |
---|
373 | if (this->xdim > this->origXdim || this->ydim > this->origYdim) { |
---|
374 | return 0; |
---|
375 | } |
---|
376 | |
---|
377 | int numLeaves = int(ceil(float(this->xdim)/this->treeGranularity)); |
---|
378 | |
---|
379 | int temp = 1; |
---|
380 | int depth = 1; |
---|
381 | while (temp < numLeaves) { |
---|
382 | temp = temp << 1; |
---|
383 | depth++; |
---|
384 | } |
---|
385 | |
---|
386 | this->treeDepth = depth; |
---|
387 | |
---|
388 | for (int yy = 0; yy < this->ydim; yy++) { |
---|
389 | int numElems = 1; |
---|
390 | for (int j = 0; j < this->treeDepth; j++) { |
---|
391 | for (int k = 0; k < numElems; k++) { |
---|
392 | this->treeElems[yy][j][k].minZ = FLT_MAX; |
---|
393 | this->treeElems[yy][j][k].maxZ = -FLT_MAX; |
---|
394 | this->holeTreeElems[yy][j][k].minZ = FLT_MAX; |
---|
395 | this->holeTreeElems[yy][j][k].maxZ = -FLT_MAX; |
---|
396 | } |
---|
397 | numElems *= 2; |
---|
398 | } |
---|
399 | } |
---|
400 | } |
---|
401 | |
---|
402 | |
---|
403 | void |
---|
404 | DepthMap::fillTree(int numLines) |
---|
405 | { |
---|
406 | DepthTreeElement *leaf, *holeLeaf; |
---|
407 | int yoffmin, yoffmax, xoffmax; |
---|
408 | int yy; |
---|
409 | |
---|
410 | for (yy = 0; yy < this->ydim; yy++) { |
---|
411 | |
---|
412 | if (IS_EVEN(numLines)) { |
---|
413 | yoffmax = numLines/2; |
---|
414 | yoffmin = -numLines/2 + 1; |
---|
415 | } else { |
---|
416 | yoffmax = (numLines+1)/2 - 1; |
---|
417 | // Used to be "yoffmin = -yoffmin;" Fix indicated by Huber 4/5/01 |
---|
418 | yoffmin = -yoffmax; |
---|
419 | } |
---|
420 | |
---|
421 | if (yy + yoffmax >= this->ydim) { |
---|
422 | yoffmax = this->ydim - yy - 1; |
---|
423 | } |
---|
424 | |
---|
425 | if (yy + yoffmin < 0) { |
---|
426 | yoffmin = -yy; |
---|
427 | } |
---|
428 | |
---|
429 | for (int yoff = yoffmin; yoff <= yoffmax; yoff++) { |
---|
430 | leaf = this->treeElems[yy][this->treeDepth-1]; |
---|
431 | holeLeaf = this->holeTreeElems[yy][this->treeDepth-1]; |
---|
432 | for (int xx = 0; xx < this->xdim; |
---|
433 | xx+=this->treeGranularity, leaf++, holeLeaf++) { |
---|
434 | if (xx <= this->xdim-this->treeGranularity) |
---|
435 | xoffmax = this->treeGranularity; |
---|
436 | else |
---|
437 | xoffmax = xx % this->treeGranularity; |
---|
438 | |
---|
439 | for (int xoff = 0; xoff < xoffmax; xoff++) { |
---|
440 | float depth = this->elems[(yy+yoff)*this->xdim+ xx+xoff].z; |
---|
441 | if (IS_VALID_DEPTH(depth)) { |
---|
442 | leaf->minZ = MIN(depth, leaf->minZ); |
---|
443 | leaf->maxZ = MAX(depth, leaf->maxZ); |
---|
444 | |
---|
445 | holeLeaf->minZ = MIN(depth, holeLeaf->minZ); |
---|
446 | holeLeaf->maxZ = MAX(depth, holeLeaf->maxZ); |
---|
447 | } else { |
---|
448 | holeLeaf->minZ = -FLT_MAX; |
---|
449 | holeLeaf->maxZ = FLT_MAX; |
---|
450 | } |
---|
451 | } |
---|
452 | } |
---|
453 | } |
---|
454 | } |
---|
455 | |
---|
456 | for (yy = 0; yy < this->ydim; yy++) { |
---|
457 | holeLeaf = this->holeTreeElems[yy][this->treeDepth-1]; |
---|
458 | for (int xx = 0; xx < this->xdim; |
---|
459 | xx+=this->treeGranularity, holeLeaf++) { |
---|
460 | if (holeLeaf->minZ == -FLT_MAX) { |
---|
461 | holeLeaf->minZ = FLT_MAX; |
---|
462 | } |
---|
463 | } |
---|
464 | this->pullUpTree(yy, this->treeDepth - 1); |
---|
465 | } |
---|
466 | } |
---|
467 | |
---|
468 | |
---|
469 | void |
---|
470 | DepthMap::pullUpTree(int y, int level) |
---|
471 | { |
---|
472 | if (level != 0) { |
---|
473 | int numNodes = ROUND_INT(pow(2,level-1)); |
---|
474 | DepthTreeElement **tree = this->treeElems[y]; |
---|
475 | DepthTreeElement **holeTree = this->holeTreeElems[y]; |
---|
476 | for (int i = 0; i < numNodes; i++) { |
---|
477 | tree[level-1][i].minZ = |
---|
478 | MIN(tree[level][2*i].minZ, tree[level][2*i+1].minZ); |
---|
479 | tree[level-1][i].maxZ = |
---|
480 | MAX(tree[level][2*i].maxZ, tree[level][2*i+1].maxZ); |
---|
481 | |
---|
482 | holeTree[level-1][i].minZ = |
---|
483 | MIN(holeTree[level][2*i].minZ, holeTree[level][2*i+1].minZ); |
---|
484 | |
---|
485 | holeTree[level-1][i].maxZ = |
---|
486 | MAX(holeTree[level][2*i].maxZ, holeTree[level][2*i+1].maxZ); |
---|
487 | } |
---|
488 | this->pullUpTree(y, level - 1); |
---|
489 | } |
---|
490 | } |
---|
491 | |
---|
492 | |
---|
493 | int * |
---|
494 | DepthMap::getDepthRuns(float y, float zmin, float zmax, int *numRuns) |
---|
495 | { |
---|
496 | int numLeaves = 0; |
---|
497 | int yy = int(y); |
---|
498 | this->traverseTree(yy, 0, 0, zmin, zmax, leafIndices, &numLeaves); |
---|
499 | this->consolidateRuns(leafIndices, numLeaves, numRuns); |
---|
500 | return this->runs; |
---|
501 | } |
---|
502 | |
---|
503 | |
---|
504 | void |
---|
505 | DepthMap::traverseTree(int y, int level, int off, float zmin, float zmax, |
---|
506 | int *leafIndices, int *numLeaves) |
---|
507 | { |
---|
508 | DepthTreeElement *node = &this->treeElems[y][level][off]; |
---|
509 | if (level < this->treeDepth - 1) { |
---|
510 | if (zmax >= node->minZ && zmin <= node->maxZ) { |
---|
511 | this->traverseTree(y, level+1, 2*off, |
---|
512 | zmin, zmax, leafIndices, numLeaves); |
---|
513 | this->traverseTree(y, level+1, 2*off+1, |
---|
514 | zmin, zmax, leafIndices, numLeaves); |
---|
515 | } |
---|
516 | else { |
---|
517 | return; |
---|
518 | } |
---|
519 | } |
---|
520 | else { |
---|
521 | if (zmax >= node->minZ && zmin <= node->maxZ) { |
---|
522 | leafIndices[*numLeaves] = off; |
---|
523 | *numLeaves = *numLeaves + 1; |
---|
524 | return; |
---|
525 | } |
---|
526 | else |
---|
527 | return; |
---|
528 | } |
---|
529 | } |
---|
530 | |
---|
531 | |
---|
532 | int * |
---|
533 | DepthMap::getDepthRunsUpperBound(float y, float zmax, int *numRuns) |
---|
534 | { |
---|
535 | int numLeaves = 0; |
---|
536 | int yy = int(y); |
---|
537 | this->traverseTreeUpperBound(yy, 0, 0, zmax, leafIndices, &numLeaves); |
---|
538 | this->consolidateRuns(leafIndices, numLeaves, numRuns); |
---|
539 | return this->runs; |
---|
540 | } |
---|
541 | |
---|
542 | |
---|
543 | int * |
---|
544 | DepthMap::getDepthRunsEdges(float y, float zmax, int *numRuns) |
---|
545 | { |
---|
546 | int numLeaves = 0; |
---|
547 | int yy = int(y); |
---|
548 | this->traverseTreeEdges(yy, 0, 0, zmax, leafIndices, &numLeaves); |
---|
549 | this->consolidateRuns(leafIndices, numLeaves, numRuns); |
---|
550 | return this->runs; |
---|
551 | } |
---|
552 | |
---|
553 | |
---|
554 | void |
---|
555 | DepthMap::traverseTreeUpperBound(int y, int level, int off, float zmax, |
---|
556 | int *leafIndices, int *numLeaves) |
---|
557 | { |
---|
558 | |
---|
559 | DepthTreeElement *node = &this->holeTreeElems[y][level][off]; |
---|
560 | if (level < this->treeDepth - 1) { |
---|
561 | if (zmax > node->minZ) { |
---|
562 | this->traverseTreeUpperBound(y, level+1, 2*off, |
---|
563 | zmax, leafIndices, numLeaves); |
---|
564 | this->traverseTreeUpperBound(y, level+1, 2*off+1, |
---|
565 | zmax, leafIndices, numLeaves); |
---|
566 | } |
---|
567 | else { |
---|
568 | return; |
---|
569 | } |
---|
570 | } |
---|
571 | else { |
---|
572 | if (zmax > node->maxZ && node->minZ != FLT_MAX) { |
---|
573 | leafIndices[*numLeaves] = off; |
---|
574 | *numLeaves = *numLeaves + 1; |
---|
575 | return; |
---|
576 | } |
---|
577 | else |
---|
578 | return; |
---|
579 | } |
---|
580 | } |
---|
581 | |
---|
582 | |
---|
583 | void |
---|
584 | DepthMap::traverseTreeEdges(int y, int level, int off, float zmax, |
---|
585 | int *leafIndices, int *numLeaves) |
---|
586 | { |
---|
587 | DepthTreeElement *node = &this->treeElems[y][level][off]; |
---|
588 | if (level < this->treeDepth - 1) { |
---|
589 | if (zmax > node->minZ) { |
---|
590 | this->traverseTreeEdges(y, level+1, 2*off, |
---|
591 | zmax, leafIndices, numLeaves); |
---|
592 | this->traverseTreeEdges(y, level+1, 2*off+1, |
---|
593 | zmax, leafIndices, numLeaves); |
---|
594 | } |
---|
595 | else { |
---|
596 | return; |
---|
597 | } |
---|
598 | } |
---|
599 | else { |
---|
600 | DepthTreeElement *holeNode = &this->holeTreeElems[y][level][off]; |
---|
601 | if (zmax > node->maxZ && zmax < holeNode->maxZ) { |
---|
602 | leafIndices[*numLeaves] = off; |
---|
603 | *numLeaves = *numLeaves + 1; |
---|
604 | return; |
---|
605 | } |
---|
606 | else |
---|
607 | return; |
---|
608 | } |
---|
609 | } |
---|
610 | |
---|
611 | |
---|
612 | void |
---|
613 | DepthMap::consolidateRuns(int *leafIndices, int numLeaves, int *numRuns) |
---|
614 | { |
---|
615 | if (numLeaves == 0) { |
---|
616 | *numRuns = 0; |
---|
617 | return; |
---|
618 | } |
---|
619 | |
---|
620 | this->runs[0] = leafIndices[0]*this->treeGranularity; |
---|
621 | this->runs[1] = this->runs[0]+this->treeGranularity-1; |
---|
622 | *numRuns = 1; |
---|
623 | for (int i = 1; i < numLeaves; i++) { |
---|
624 | if (leafIndices[i] == leafIndices[i-1]+1) { |
---|
625 | this->runs[2*(*numRuns-1)+1] += this->treeGranularity; |
---|
626 | } |
---|
627 | else { |
---|
628 | this->runs[2*(*numRuns)] = leafIndices[i]*this->treeGranularity; |
---|
629 | this->runs[2*(*numRuns)+1] = this->runs[2*(*numRuns)] |
---|
630 | +this->treeGranularity-1; |
---|
631 | *numRuns = *numRuns + 1; |
---|
632 | } |
---|
633 | } |
---|
634 | |
---|
635 | if (this->runs[2*(*numRuns - 1)+1] > this->xdim - 1) |
---|
636 | this->runs[2*(*numRuns - 1)+1] = this->xdim - 1; |
---|
637 | } |
---|
638 | |
---|
639 | |
---|
640 | DepthMap::~DepthMap() |
---|
641 | { |
---|
642 | if (this->elems != NULL) { |
---|
643 | delete [] this->elems; |
---|
644 | this->elems = NULL; |
---|
645 | } |
---|
646 | |
---|
647 | if (this->treeElems != NULL) { |
---|
648 | for (int yy = 0; yy < this->origYdim; yy++) { |
---|
649 | for (int j = 0; j < this->origTreeDepth; j++) { |
---|
650 | delete [] this->treeElems[yy][j]; |
---|
651 | } |
---|
652 | delete [] this->treeElems[yy]; |
---|
653 | } |
---|
654 | delete [] this->treeElems; |
---|
655 | this->treeElems = NULL; |
---|
656 | delete [] this->leafIndices; |
---|
657 | delete [] this->runs; |
---|
658 | } |
---|
659 | |
---|
660 | if (this->holeTreeElems != NULL) { |
---|
661 | for (int yy = 0; yy < this->origYdim; yy++) { |
---|
662 | for (int j = 0; j < this->origTreeDepth; j++) { |
---|
663 | delete [] this->holeTreeElems[yy][j]; |
---|
664 | } |
---|
665 | delete [] this->holeTreeElems[yy]; |
---|
666 | } |
---|
667 | delete [] this->holeTreeElems; |
---|
668 | this->holeTreeElems = NULL; |
---|
669 | } |
---|
670 | } |
---|
671 | |
---|
672 | |
---|
673 | int |
---|
674 | DepthMap::writePly(const char *filename, float noiseLevel) |
---|
675 | { |
---|
676 | float version; |
---|
677 | int xx, yy, index; |
---|
678 | int numRangeGridPoints; |
---|
679 | float noiseOffset; |
---|
680 | |
---|
681 | numRangeGridPoints = this->xdim*this->ydim; |
---|
682 | int numVerts = this->xdim*this->ydim; |
---|
683 | |
---|
684 | PlyFile *ply = ply_open_for_writing(filename, 2, elem_names, |
---|
685 | PLY_BINARY_BE, &version); |
---|
686 | |
---|
687 | int nvp = 0; |
---|
688 | vert_props[nvp].offset = voffset(x); nvp++; |
---|
689 | vert_props[nvp].offset = voffset(y); nvp++; |
---|
690 | vert_props[nvp].offset = voffset(z); nvp++; |
---|
691 | |
---|
692 | int *grid = new int[numRangeGridPoints]; |
---|
693 | int *pGrid = grid; |
---|
694 | index = 0; |
---|
695 | DepthElement *buf = this->elems; |
---|
696 | float minZ = FLT_MAX; |
---|
697 | for (yy = 0; yy < this->ydim; yy++) { |
---|
698 | for (xx = 0; xx < this->xdim; xx++, buf++, pGrid++) { |
---|
699 | if (IS_VALID_DEPTH(buf->z)) { |
---|
700 | if (noiseLevel != 0) { |
---|
701 | noiseOffset = 2*(rand()/32767.0) - 1; |
---|
702 | noiseOffset = noiseOffset*noiseOffset*noiseLevel; |
---|
703 | buf->z += noiseOffset; |
---|
704 | } |
---|
705 | *pGrid = index; |
---|
706 | index++; |
---|
707 | } else { |
---|
708 | *pGrid = -1; |
---|
709 | } |
---|
710 | } |
---|
711 | } |
---|
712 | |
---|
713 | ply_describe_element (ply, "vertex", index, |
---|
714 | nvp, vert_props); |
---|
715 | |
---|
716 | range_grid_props[0].offset = roffset(verts); |
---|
717 | range_grid_props[0].count_offset = roffset(nverts); /* count offset */ |
---|
718 | ply_describe_element (ply, "range_grid", numRangeGridPoints, 1, |
---|
719 | range_grid_props); |
---|
720 | |
---|
721 | char objInfo[PATH_MAX]; |
---|
722 | |
---|
723 | sprintf(objInfo, "num_cols %d", this->xdim); |
---|
724 | ply_put_obj_info (ply, objInfo); |
---|
725 | |
---|
726 | sprintf(objInfo, "num_rows %d", this->ydim); |
---|
727 | ply_put_obj_info (ply, objInfo); |
---|
728 | |
---|
729 | sprintf(objInfo, "is_mesh 0"); |
---|
730 | ply_put_obj_info (ply, objInfo); |
---|
731 | |
---|
732 | ply_header_complete (ply); |
---|
733 | |
---|
734 | /* set up and write the vertex elements */ |
---|
735 | Vec3f vin, vout; |
---|
736 | PlyVertex vert; |
---|
737 | float res = this->resolution; |
---|
738 | float ypos = this->origin[1]; |
---|
739 | buf = this->elems; |
---|
740 | ply_put_element_setup (ply, "vertex"); |
---|
741 | for (yy = 0; yy < this->ydim; yy++, ypos+=res) { |
---|
742 | float xpos = this->origin[0]; |
---|
743 | for (xx = 0; xx < this->xdim; xx++, buf++, xpos+=res) { |
---|
744 | if (IS_VALID_DEPTH(buf->z)) { |
---|
745 | if (this->linePersp) { |
---|
746 | vin.setValue(xpos, ypos, buf->z); |
---|
747 | applyInvLinePersp(vin,vout); |
---|
748 | vert.x = vout.x; |
---|
749 | vert.y = vout.y; |
---|
750 | vert.z = vout.z; |
---|
751 | } |
---|
752 | else if (this->perspective) { |
---|
753 | vin.setValue(xpos, ypos, buf->z); |
---|
754 | applyInvPersp(vin,vout); |
---|
755 | vert.x = vout.x; |
---|
756 | vert.y = vout.y; |
---|
757 | vert.z = vout.z; |
---|
758 | } |
---|
759 | else { |
---|
760 | vert.x = xpos; |
---|
761 | vert.y = ypos; |
---|
762 | vert.z = buf->z; |
---|
763 | } |
---|
764 | ply_put_element (ply, (void *) &vert); |
---|
765 | } |
---|
766 | } |
---|
767 | } |
---|
768 | |
---|
769 | /* set up and write the vertex elements */ |
---|
770 | RangeGridList gridElem; |
---|
771 | gridElem.verts = new int[1]; |
---|
772 | pGrid = grid; |
---|
773 | ply_put_element_setup (ply, "range_grid"); |
---|
774 | for (yy = 0; yy < this->ydim; yy++) { |
---|
775 | for (xx = 0; xx < this->xdim; xx++, pGrid++) { |
---|
776 | if (*pGrid < 0) { |
---|
777 | gridElem.nverts = 0; |
---|
778 | } else { |
---|
779 | gridElem.nverts = 1; |
---|
780 | gridElem.verts[0] = *pGrid; |
---|
781 | } |
---|
782 | ply_put_element (ply, (void *) &gridElem); |
---|
783 | } |
---|
784 | } |
---|
785 | delete gridElem.verts; |
---|
786 | |
---|
787 | /* close the PLY file */ |
---|
788 | ply_close (ply); |
---|
789 | |
---|
790 | delete grid; |
---|
791 | |
---|
792 | return 1; |
---|
793 | } |
---|
794 | |
---|
795 | |
---|
796 | /* |
---|
797 | int |
---|
798 | DepthMap::writePly(char *filename) |
---|
799 | { |
---|
800 | float version; |
---|
801 | int yy; |
---|
802 | |
---|
803 | int numVerts = this->xdim*this->ydim; |
---|
804 | |
---|
805 | PlyFile *ply = ply_open_for_writing(filename, 1, elem_names, |
---|
806 | PLY_BINARY_BE, &version); |
---|
807 | |
---|
808 | int nvp = 0; |
---|
809 | vert_props[nvp].offset = voffset(x); nvp++; |
---|
810 | vert_props[nvp].offset = voffset(y); nvp++; |
---|
811 | vert_props[nvp].offset = voffset(z); nvp++; |
---|
812 | |
---|
813 | ply_describe_element (ply, "vertex", numVerts, |
---|
814 | nvp, vert_props); |
---|
815 | |
---|
816 | ply_header_complete (ply); |
---|
817 | |
---|
818 | // set up and write the vertex elements |
---|
819 | DepthElement *buf = this->elems; |
---|
820 | float minZ = FLT_MAX; |
---|
821 | for (yy = 0; yy < this->ydim; yy++) { |
---|
822 | for (int xx = 0; xx < this->xdim; xx++, buf++) { |
---|
823 | if (IS_VALID_DEPTH(buf->z)) { |
---|
824 | minZ = MIN(buf->z, minZ); |
---|
825 | } |
---|
826 | } |
---|
827 | } |
---|
828 | |
---|
829 | PlyVertex vert; |
---|
830 | float res = this->resolution; |
---|
831 | ply_put_element_setup (ply, "vertex"); |
---|
832 | float ypos = this->origin[1]; |
---|
833 | buf = this->elems; |
---|
834 | for (yy = 0; yy < this->ydim; yy++, ypos+=res) { |
---|
835 | float xpos = this->origin[0]; |
---|
836 | for (int xx = 0; xx < this->xdim; xx++, xpos+=res, buf++) { |
---|
837 | vert.x = xpos; |
---|
838 | vert.y = ypos; |
---|
839 | if (IS_VALID_DEPTH(buf->z)) { |
---|
840 | vert.z = buf->z; |
---|
841 | } |
---|
842 | else { |
---|
843 | vert.z = minZ; |
---|
844 | } |
---|
845 | |
---|
846 | ply_put_element (ply, (void *) &vert); |
---|
847 | } |
---|
848 | } |
---|
849 | |
---|
850 | // close the PLY file |
---|
851 | ply_close (ply); |
---|
852 | |
---|
853 | return 1; |
---|
854 | } |
---|
855 | |
---|
856 | */ |
---|