[37] | 1 | /* |
---|
| 2 | |
---|
| 3 | Brian Curless |
---|
| 4 | |
---|
| 5 | Computer Graphics Laboratory |
---|
| 6 | Stanford University |
---|
| 7 | |
---|
| 8 | --------------------------------------------------------------------- |
---|
| 9 | |
---|
| 10 | Copyright (1997) The Board of Trustees of the Leland Stanford Junior |
---|
| 11 | University. Except for commercial resale, lease, license or other |
---|
| 12 | commercial transactions, permission is hereby given to use, copy, |
---|
| 13 | modify this software for academic purposes only. No part of this |
---|
| 14 | software or any derivatives thereof may be used in the production of |
---|
| 15 | 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 "resample.h" |
---|
| 23 | #include "vripGlobals.h" |
---|
| 24 | |
---|
| 25 | |
---|
| 26 | void |
---|
| 27 | resample(DepthMap *depthMap, float xOff, float yOff, |
---|
| 28 | float *depth, float *confidence) |
---|
| 29 | { |
---|
| 30 | int ix, iy, offset, xdim; |
---|
| 31 | float dx, dy, z1, z2, z3, z4, c1, c2, c3, c4; |
---|
| 32 | DepthElement *buf; |
---|
| 33 | |
---|
| 34 | xdim = depthMap->xdim; |
---|
| 35 | |
---|
| 36 | ix = int(xOff); |
---|
| 37 | iy = int(yOff); |
---|
| 38 | |
---|
| 39 | offset = ix + iy*xdim; |
---|
| 40 | buf = depthMap->elems + offset; |
---|
| 41 | |
---|
| 42 | z1 = buf->z; |
---|
| 43 | c1 = buf->conf; |
---|
| 44 | |
---|
| 45 | z2 = (buf+1)->z; |
---|
| 46 | c2 = (buf+1)->conf; |
---|
| 47 | |
---|
| 48 | z3 = (buf+xdim)->z; |
---|
| 49 | c3 = (buf+xdim)->conf; |
---|
| 50 | |
---|
| 51 | z4 = (buf+xdim+1)->z; |
---|
| 52 | c4 = (buf+xdim+1)->conf; |
---|
| 53 | |
---|
| 54 | if (!IS_VALID_DEPTH(z1) || !IS_VALID_DEPTH(z2) || |
---|
| 55 | !IS_VALID_DEPTH(z3) || !IS_VALID_DEPTH(z4)) { |
---|
| 56 | *depth = -FAR_AWAY_DEPTH; |
---|
| 57 | *confidence = 0; |
---|
| 58 | return; |
---|
| 59 | } |
---|
| 60 | |
---|
| 61 | float maxZ = MAX(MAX(MAX(z1,z2),z3),z4); |
---|
| 62 | float minZ = MIN(MIN(MIN(z1,z2),z3),z4); |
---|
| 63 | |
---|
| 64 | if (maxZ - minZ > MAX_DEPTH_DIFFERENCE) { |
---|
| 65 | *depth = -FAR_AWAY_DEPTH; |
---|
| 66 | *confidence = 0; |
---|
| 67 | return; |
---|
| 68 | } |
---|
| 69 | |
---|
| 70 | dx = xOff - ix; |
---|
| 71 | dy = yOff - iy; |
---|
| 72 | |
---|
| 73 | *depth = ((1-dx)*(1-dy)*z1 + (dx)*(1-dy)*z2 |
---|
| 74 | + (1-dx)*(dy)*z3 + (dx)*(dy)*z4); |
---|
| 75 | |
---|
| 76 | *confidence = ((1-dx)*(1-dy)*c1 + (dx)*(1-dy)*c2 |
---|
| 77 | + (1-dx)*(dy)*c3 + (dx)*(dy)*c4); |
---|
| 78 | |
---|
| 79 | if (*confidence < 0) |
---|
| 80 | *confidence = 0; |
---|
| 81 | } |
---|
| 82 | |
---|
| 83 | |
---|
| 84 | void |
---|
| 85 | resampleBetter(DepthMap *depthMap, float xOff, float yOff, |
---|
| 86 | float *depth, float *confidence) |
---|
| 87 | { |
---|
| 88 | int ix, iy, offset, xdim; |
---|
| 89 | float dx, dy, z1, z2, z3, z4, c1, c2, c3, c4; |
---|
| 90 | float alpha, beta, gamma; |
---|
| 91 | DepthElement *buf; |
---|
| 92 | |
---|
| 93 | xdim = depthMap->xdim; |
---|
| 94 | |
---|
| 95 | ix = int(xOff); |
---|
| 96 | iy = int(yOff); |
---|
| 97 | |
---|
| 98 | offset = ix + iy*xdim; |
---|
| 99 | uchar tag = depthMap->tags[offset]; |
---|
| 100 | if (!tag) { |
---|
| 101 | *depth = -FAR_AWAY_DEPTH; |
---|
| 102 | *confidence = 0; |
---|
| 103 | return; |
---|
| 104 | } |
---|
| 105 | |
---|
| 106 | buf = depthMap->elems + offset; |
---|
| 107 | |
---|
| 108 | z1 = buf->z; |
---|
| 109 | c1 = buf->conf; |
---|
| 110 | |
---|
| 111 | z2 = (buf+1)->z; |
---|
| 112 | c2 = (buf+1)->conf; |
---|
| 113 | |
---|
| 114 | z3 = (buf+xdim)->z; |
---|
| 115 | c3 = (buf+xdim)->conf; |
---|
| 116 | |
---|
| 117 | z4 = (buf+xdim+1)->z; |
---|
| 118 | c4 = (buf+xdim+1)->conf; |
---|
| 119 | |
---|
| 120 | #if 0 |
---|
| 121 | if (!(((DepthMap::TRIANGLE_ONE & tag) && (DepthMap::TRIANGLE_TWO & tag)) || |
---|
| 122 | ((DepthMap::TRIANGLE_THREE & tag) && (DepthMap::TRIANGLE_FOUR & tag)))) { |
---|
| 123 | *depth = -FAR_AWAY_DEPTH; |
---|
| 124 | *confidence = 0; |
---|
| 125 | return; |
---|
| 126 | } |
---|
| 127 | |
---|
| 128 | dx = xOff - ix; |
---|
| 129 | dy = yOff - iy; |
---|
| 130 | |
---|
| 131 | *depth = ((1-dx)*(1-dy)*z1 + (dx)*(1-dy)*z2 |
---|
| 132 | + (1-dx)*(dy)*z3 + (dx)*(dy)*z4); |
---|
| 133 | |
---|
| 134 | *confidence = ((1-dx)*(1-dy)*c1 + (dx)*(1-dy)*c2 |
---|
| 135 | + (1-dx)*(dy)*c3 + (dx)*(dy)*c4); |
---|
| 136 | |
---|
| 137 | if (*confidence < 0) |
---|
| 138 | *confidence = 0; |
---|
| 139 | |
---|
| 140 | #else |
---|
| 141 | |
---|
| 142 | dx = xOff - ix; |
---|
| 143 | dy = yOff - iy; |
---|
| 144 | |
---|
| 145 | if (dx > dy) { |
---|
| 146 | if (DepthMap::TRIANGLE_THREE & tag) { |
---|
| 147 | alpha = dy; |
---|
| 148 | beta = 1-dx; |
---|
| 149 | gamma = 1 - alpha - beta; |
---|
| 150 | *depth = alpha*z4 + beta*z1 + gamma*z2; |
---|
| 151 | *confidence = alpha*c4 + beta*c1 + gamma*c2; |
---|
| 152 | return; |
---|
| 153 | } |
---|
| 154 | } else if (DepthMap::TRIANGLE_FOUR & tag) { |
---|
| 155 | alpha = 1-dy; |
---|
| 156 | beta = dx; |
---|
| 157 | gamma = 1 - alpha - beta; |
---|
| 158 | *depth = alpha*z1 + beta*z4 + gamma*z3; |
---|
| 159 | *confidence = alpha*c1 + beta*c4 + gamma*c3; |
---|
| 160 | return; |
---|
| 161 | } |
---|
| 162 | |
---|
| 163 | if (1-dx > dy) { |
---|
| 164 | if (DepthMap::TRIANGLE_ONE & tag) { |
---|
| 165 | alpha = dy; |
---|
| 166 | beta = dx; |
---|
| 167 | gamma = 1 - alpha - beta; |
---|
| 168 | *depth = alpha*z3 + beta*z2 + gamma*z1; |
---|
| 169 | *confidence = alpha*c3 + beta*c2 + gamma*c1; |
---|
| 170 | return; |
---|
| 171 | } |
---|
| 172 | } else if (DepthMap::TRIANGLE_TWO & tag) { |
---|
| 173 | alpha = 1-dy; |
---|
| 174 | beta = 1-dx; |
---|
| 175 | gamma = 1 - alpha - beta; |
---|
| 176 | *depth = alpha*z2 + beta*z3 + gamma*z4; |
---|
| 177 | *confidence = alpha*c2 + beta*c3 + gamma*c4; |
---|
| 178 | return; |
---|
| 179 | } |
---|
| 180 | |
---|
| 181 | *depth = -FAR_AWAY_DEPTH; |
---|
| 182 | *confidence = 0; |
---|
| 183 | return; |
---|
| 184 | |
---|
| 185 | #endif |
---|
| 186 | |
---|
| 187 | } |
---|
| 188 | |
---|
| 189 | |
---|
| 190 | void |
---|
| 191 | resampleWithEdge(DepthMap *depthMap, float xOff, float yOff, |
---|
| 192 | float *depth, float *confidence, float *edge) |
---|
| 193 | { |
---|
| 194 | int ix, iy, offset, xdim; |
---|
| 195 | float dx, dy, z1, z2, z3, z4, c1, c2, c3, c4; |
---|
| 196 | float e1, e2, e3, e4; |
---|
| 197 | DepthElement *buf; |
---|
| 198 | signed char *edgeBuf; |
---|
| 199 | |
---|
| 200 | xdim = depthMap->xdim; |
---|
| 201 | |
---|
| 202 | ix = int(xOff); |
---|
| 203 | iy = int(yOff); |
---|
| 204 | |
---|
| 205 | offset = ix + iy*xdim; |
---|
| 206 | buf = depthMap->elems + offset; |
---|
| 207 | edgeBuf = depthMap->edgeSteps + offset; |
---|
| 208 | |
---|
| 209 | z1 = buf->z; |
---|
| 210 | c1 = buf->conf; |
---|
| 211 | e1 = *edgeBuf; |
---|
| 212 | |
---|
| 213 | z2 = (buf+1)->z; |
---|
| 214 | c2 = (buf+1)->conf; |
---|
| 215 | e2 = *(edgeBuf+1); |
---|
| 216 | |
---|
| 217 | z3 = (buf+xdim)->z; |
---|
| 218 | c3 = (buf+xdim)->conf; |
---|
| 219 | e3 = *(edgeBuf+xdim); |
---|
| 220 | |
---|
| 221 | z4 = (buf+xdim+1)->z; |
---|
| 222 | c4 = (buf+xdim+1)->conf; |
---|
| 223 | e4 = *(edgeBuf+xdim+1); |
---|
| 224 | |
---|
| 225 | if (!IS_VALID_DEPTH(z1) || !IS_VALID_DEPTH(z2) || |
---|
| 226 | !IS_VALID_DEPTH(z3) || !IS_VALID_DEPTH(z4)) { |
---|
| 227 | *depth = -FAR_AWAY_DEPTH; |
---|
| 228 | *confidence = 0; |
---|
| 229 | *edge = 0; |
---|
| 230 | return; |
---|
| 231 | } |
---|
| 232 | |
---|
| 233 | float maxZ = MAX(MAX(MAX(z1,z2),z3),z4); |
---|
| 234 | float minZ = MIN(MIN(MIN(z1,z2),z3),z4); |
---|
| 235 | |
---|
| 236 | if (maxZ - minZ > MAX_DEPTH_DIFFERENCE) { |
---|
| 237 | *depth = -FAR_AWAY_DEPTH; |
---|
| 238 | *confidence = 0; |
---|
| 239 | *edge = 0; |
---|
| 240 | return; |
---|
| 241 | } |
---|
| 242 | |
---|
| 243 | dx = xOff - ix; |
---|
| 244 | dy = yOff - iy; |
---|
| 245 | |
---|
| 246 | *depth = ((1-dx)*(1-dy)*z1 + (dx)*(1-dy)*z2 |
---|
| 247 | + (1-dx)*(dy)*z3 + (dx)*(dy)*z4); |
---|
| 248 | |
---|
| 249 | *confidence = ((1-dx)*(1-dy)*c1 + (dx)*(1-dy)*c2 |
---|
| 250 | + (1-dx)*(dy)*c3 + (dx)*(dy)*c4); |
---|
| 251 | |
---|
| 252 | *edge = (((1-dx)*(1-dy)*e1 + (dx)*(1-dy)*e2 |
---|
| 253 | + (1-dx)*(dy)*e3 + (dx)*(dy)*e4))/128.0; |
---|
| 254 | |
---|
| 255 | if (*confidence < 0) |
---|
| 256 | *confidence = 0; |
---|
| 257 | } |
---|
| 258 | |
---|
| 259 | |
---|
| 260 | |
---|
| 261 | void |
---|
| 262 | resampleForCarving(DepthMap *depthMap, float xOff, float yOff, |
---|
| 263 | float *depth, float *confidence) |
---|
| 264 | { |
---|
| 265 | int ix, iy, offset, xdim; |
---|
| 266 | float dx, dy, z1, z2, z3, z4, c1, c2, c3, c4; |
---|
| 267 | DepthElement *buf; |
---|
| 268 | |
---|
| 269 | xdim = depthMap->xdim; |
---|
| 270 | |
---|
| 271 | ix = int(xOff); |
---|
| 272 | iy = int(yOff); |
---|
| 273 | |
---|
| 274 | offset = ix + iy*xdim; |
---|
| 275 | buf = depthMap->elems + offset; |
---|
| 276 | |
---|
| 277 | z1 = buf->z; |
---|
| 278 | c1 = buf->conf; |
---|
| 279 | |
---|
| 280 | z2 = (buf+1)->z; |
---|
| 281 | c2 = (buf+1)->conf; |
---|
| 282 | |
---|
| 283 | z3 = (buf+xdim)->z; |
---|
| 284 | c3 = (buf+xdim)->conf; |
---|
| 285 | |
---|
| 286 | z4 = (buf+xdim+1)->z; |
---|
| 287 | c4 = (buf+xdim+1)->conf; |
---|
| 288 | |
---|
| 289 | if (!IS_VALID_DEPTH(z1) || !IS_VALID_DEPTH(z2) || |
---|
| 290 | !IS_VALID_DEPTH(z3) || !IS_VALID_DEPTH(z4)) { |
---|
| 291 | *depth = -FAR_AWAY_DEPTH; |
---|
| 292 | *confidence = 0; |
---|
| 293 | return; |
---|
| 294 | } |
---|
| 295 | |
---|
| 296 | float maxZ = MAX(MAX(MAX(z1,z2),z3),z4); |
---|
| 297 | float minZ = MIN(MIN(MIN(z1,z2),z3),z4); |
---|
| 298 | float minC = MIN(MIN(MIN(c1,c2),c3),c4); |
---|
| 299 | |
---|
| 300 | dx = xOff - ix; |
---|
| 301 | dy = yOff - iy; |
---|
| 302 | |
---|
| 303 | *depth = ((1-dx)*(1-dy)*z1 + (dx)*(1-dy)*z2 |
---|
| 304 | + (1-dx)*(dy)*z3 + (dx)*(dy)*z4); |
---|
| 305 | |
---|
| 306 | *confidence = ((1-dx)*(1-dy)*c1 + (dx)*(1-dy)*c2 |
---|
| 307 | + (1-dx)*(dy)*c3 + (dx)*(dy)*c4); |
---|
| 308 | |
---|
| 309 | if (maxZ - minZ > MAX_DEPTH_DIFFERENCE || minC <= 0) { |
---|
| 310 | *confidence = 0; |
---|
| 311 | } |
---|
| 312 | } |
---|
| 313 | |
---|
| 314 | |
---|
| 315 | void |
---|
| 316 | resampleNorm(DepthMap *depthMap, float xOff, float yOff, |
---|
| 317 | float *depth, float *confidence, Vec3f &norm) |
---|
| 318 | { |
---|
| 319 | int ix, iy, offset, xdim; |
---|
| 320 | float dx, dy, z1, z2, z3, z4, c1, c2, c3, c4; |
---|
| 321 | float nx, ny, nz; |
---|
| 322 | float nx1, ny1, nz1; |
---|
| 323 | float nx2, ny2, nz2; |
---|
| 324 | float nx3, ny3, nz3; |
---|
| 325 | float nx4, ny4, nz4; |
---|
| 326 | DepthElement *buf; |
---|
| 327 | DepthMapNorm *normBuf; |
---|
| 328 | |
---|
| 329 | xdim = depthMap->xdim; |
---|
| 330 | |
---|
| 331 | ix = int(xOff); |
---|
| 332 | iy = int(yOff); |
---|
| 333 | |
---|
| 334 | offset = ix + iy*xdim; |
---|
| 335 | buf = depthMap->elems + offset; |
---|
| 336 | normBuf = depthMap->norms + offset; |
---|
| 337 | |
---|
| 338 | z1 = buf->z; |
---|
| 339 | z2 = (buf+1)->z; |
---|
| 340 | z3 = (buf+xdim)->z; |
---|
| 341 | z4 = (buf+xdim+1)->z; |
---|
| 342 | |
---|
| 343 | if (!IS_VALID_DEPTH(z1) || !IS_VALID_DEPTH(z2) || |
---|
| 344 | !IS_VALID_DEPTH(z3) || !IS_VALID_DEPTH(z4)) { |
---|
| 345 | *depth = -FAR_AWAY_DEPTH; |
---|
| 346 | *confidence = 0; |
---|
| 347 | norm.setValue(0,0,0); |
---|
| 348 | return; |
---|
| 349 | } |
---|
| 350 | |
---|
| 351 | float maxZ = MAX(MAX(MAX(z1,z2),z3),z4); |
---|
| 352 | float minZ = MIN(MIN(MIN(z1,z2),z3),z4); |
---|
| 353 | |
---|
| 354 | if (maxZ - minZ > MAX_DEPTH_DIFFERENCE) { |
---|
| 355 | *depth = -FAR_AWAY_DEPTH; |
---|
| 356 | *confidence = 0; |
---|
| 357 | norm.setValue(0,0,0); |
---|
| 358 | return; |
---|
| 359 | } |
---|
| 360 | |
---|
| 361 | c1 = buf->conf; |
---|
| 362 | c2 = (buf+1)->conf; |
---|
| 363 | c3 = (buf+xdim)->conf; |
---|
| 364 | c4 = (buf+xdim+1)->conf; |
---|
| 365 | |
---|
| 366 | nx1 = normBuf->nx; |
---|
| 367 | ny1 = normBuf->ny; |
---|
| 368 | nz1 = normBuf->nz; |
---|
| 369 | |
---|
| 370 | nx2 = (normBuf+1)->nx; |
---|
| 371 | ny2 = (normBuf+1)->ny; |
---|
| 372 | nz2 = (normBuf+1)->nz; |
---|
| 373 | |
---|
| 374 | nx3 = (normBuf+xdim)->nx; |
---|
| 375 | ny3 = (normBuf+xdim)->ny; |
---|
| 376 | nz3 = (normBuf+xdim)->nz; |
---|
| 377 | |
---|
| 378 | nx4 = (normBuf+xdim+1)->nx; |
---|
| 379 | ny4 = (normBuf+xdim+1)->ny; |
---|
| 380 | nz4 = (normBuf+xdim+1)->nz; |
---|
| 381 | |
---|
| 382 | dx = xOff - ix; |
---|
| 383 | dy = yOff - iy; |
---|
| 384 | |
---|
| 385 | *depth = ((1-dx)*(1-dy)*z1 + (dx)*(1-dy)*z2 |
---|
| 386 | + (1-dx)*(dy)*z3 + (dx)*(dy)*z4); |
---|
| 387 | |
---|
| 388 | *confidence = ((1-dx)*(1-dy)*c1 + (dx)*(1-dy)*c2 |
---|
| 389 | + (1-dx)*(dy)*c3 + (dx)*(dy)*c4); |
---|
| 390 | |
---|
| 391 | nx = ((1-dx)*(1-dy)*nx1 + (dx)*(1-dy)*nx2 |
---|
| 392 | + (1-dx)*(dy)*nx3 + (dx)*(dy)*nx4); |
---|
| 393 | |
---|
| 394 | ny = ((1-dx)*(1-dy)*ny1 + (dx)*(1-dy)*ny2 |
---|
| 395 | + (1-dx)*(dy)*ny3 + (dx)*(dy)*ny4); |
---|
| 396 | |
---|
| 397 | nz = ((1-dx)*(1-dy)*nz1 + (dx)*(1-dy)*nz2 |
---|
| 398 | + (1-dx)*(dy)*nz3 + (dx)*(dy)*nz4); |
---|
| 399 | |
---|
| 400 | norm.setValue(nx, ny, nz); |
---|
| 401 | norm.normalize(); |
---|
| 402 | |
---|
| 403 | } |
---|
| 404 | |
---|
| 405 | |
---|
| 406 | |
---|