[37] | 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 | */ |
---|