source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/vrippack-0.31/src/vrip/occFunc.cc @ 37

Last change on this file since 37 was 37, checked in by (none), 14 years ago

Added original make3d
File size: 15.0 KB
Line 
1/*
2
3Brian Curless
4
5Computer Graphics Laboratory
6Stanford University
7
8---------------------------------------------------------------------
9
10Copyright (1997) The Board of Trustees of the Leland Stanford Junior
11University. Except for commercial resale, lease, license or other
12commercial transactions, permission is hereby given to use, copy,
13modify this software for academic purposes only.  No part of this
14software or any derivatives thereof may be used in the production of
15computer models for resale or for use in a commercial
16product. STANFORD MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY KIND
17CONCERNING THIS SOFTWARE.  No support is implied or provided.
18
19*/
20
21
22#include <math.h>
23
24#include "occFunc.h"
25#include "vripGlobals.h"
26
27void
28initOccFunc()
29{
30    float factor = pow(2, MeshResolution-1);
31
32    MAX_DEPTH_DIFFERENCE = MaxEdgeLength;
33
34    D1 = OccupancyRampWidth/2;
35    D2 = -OccupancyRampWidth/2;
36
37    C1 = WeightPos1;
38    C2 = WeightPos2;
39
40    C3 = WeightPos3;
41    C4 = WeightPos4;
42    C5 = WeightPos5;
43
44    // Careful about choosing this number.  Should be something like
45    //  MAX_STRETCH = (C5-bufferDist)/C4;
46    MAX_STRETCH = 2.5;
47    //MAX_C5 = MAX_STRETCH*C5;
48
49    M_WEIGHT_1 = 1/(C2-C1);
50    B_WEIGHT_1 = -C1/(C2-C1);
51
52    M_WEIGHT_2 = 1/(C3-C4);
53    B_WEIGHT_2 = -C4/(C3-C4);
54
55    M_VALUE = 1/(D2-D1);
56    B_VALUE = -D1/(D2-D1);
57}
58
59#if 1
60
61void
62updateCell(float cellDepth, float rangeDepth, float sampleSpacing,
63           float rangeConfidence, OccElement *cell)
64{
65    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE;
66    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
67    double weight, value, totalWeight, currentValue;
68    float expansion;
69
70//    expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
71    expansion = 1;
72
73    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
74    NEW_M_VALUE = M_VALUE/expansion;
75    NEW_C3 = C3*expansion;
76    NEW_C4 = C4*expansion;
77    NEW_C5 = C5;
78    NEW_D1 = D1*expansion;
79    NEW_D2 = D2*expansion;
80
81    deltaZ = cellDepth - rangeDepth;
82    deltaZ *= sampleSpacing;
83
84    if (deltaZ > C1 || deltaZ < NEW_C5) {
85        weight = 0;
86        return;
87    }
88    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
89        weight = 0;
90    }
91    else if (deltaZ > NEW_C3 && deltaZ < C2) {
92        weight = rangeConfidence;
93    }
94    else if (deltaZ > C2) {
95        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
96    }
97    else {
98        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + B_WEIGHT_2);
99    }
100
101    weight = MAX(weight, MinWeight/255.0);
102
103    if (deltaZ > NEW_D1) {
104        value = 0;
105    }
106    else if (deltaZ < NEW_D2) {
107        value = 1;
108    }
109    else {
110        value = NEW_M_VALUE*deltaZ + B_VALUE;
111    }
112
113    totalWeight = double(cell->totalWeight);
114
115    weight *= UCHAR_MAX;
116    currentValue = double(cell->value);
117    value *= USHRT_MAX;
118
119    currentValue = currentValue*totalWeight + value*weight;
120    totalWeight += weight;
121    if (totalWeight != 0) {
122        currentValue /= totalWeight;
123    } else {
124        currentValue = value;
125    }
126   
127    cell->value = ushort(currentValue);
128    cell->totalWeight = ushort(totalWeight);
129   
130    return;
131}
132
133
134#else
135
136void
137updateCell(float cellDepth, float rangeDepth, float sampleSpacing,
138             float rangeConfidence, OccElement *cell)
139{
140    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE;
141    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
142    double weight, value, totalWeight, currentValue;
143    float expansion;
144
145//    expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
146    expansion = 1;
147
148    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
149    NEW_M_VALUE = M_VALUE/expansion;
150    NEW_C3 = C3*expansion;
151    NEW_C4 = C4*expansion;
152    NEW_C5 = C5;
153    NEW_D1 = D1*expansion;
154    NEW_D2 = D2*expansion;
155
156    deltaZ = cellDepth - rangeDepth;
157    deltaZ *= sampleSpacing;
158
159    if (deltaZ > C1 || deltaZ < NEW_C5) {
160        weight = 0;
161        return;
162    }
163    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
164        weight = 0;
165    }
166    else if (deltaZ > NEW_C3 && deltaZ < C2) {
167        weight = rangeConfidence;
168    }
169    else if (deltaZ > C2) {
170        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
171    }
172    else {
173        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + B_WEIGHT_2);
174    }
175
176    weight = MAX(weight, MinWeight/255.0);
177
178    if (deltaZ > NEW_D1) {
179        value = 0;
180    }
181    else if (deltaZ < NEW_D2) {
182        value = 1;
183    }
184    else {
185        value = NEW_M_VALUE*deltaZ + B_VALUE;
186    }
187
188
189    value = 4*(value-0.5)*(value-0.5);
190    totalWeight = double(cell->totalWeight);
191
192    weight *= UCHAR_MAX;
193    currentValue = double(cell->value);
194    value *= USHRT_MAX;
195
196    currentValue = currentValue*totalWeight + value*weight;
197    totalWeight += weight;
198    if (totalWeight != 0) {
199        currentValue /= totalWeight;
200    } else {
201        currentValue = value;
202    }
203   
204    cell->value = ushort(currentValue);
205    cell->totalWeight = ushort(totalWeight);
206   
207    return;
208}
209
210#endif
211
212
213// Bias to emptiness if the input cell has low confidence
214
215void
216updateCellBiasEdgesEmpty(float cellDepth, float rangeDepth, 
217                         float sampleSpacing,
218                         float rangeConfidence, 
219                         float edgeConfidence,
220                         OccElement *cell)
221{
222    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE, NEW_B_WEIGHT_2;
223    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
224    double weight, value, totalWeight, currentValue;
225    float expansion;
226    int isFalseData, wasFalseData;
227
228    //expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
229    expansion = 1;
230
231    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
232    NEW_M_VALUE = M_VALUE/expansion;
233    NEW_C3 = C3*expansion;
234    NEW_C4 = C4*expansion;
235    NEW_C5 = C5;
236    NEW_D1 = D1*expansion;
237    NEW_D2 = D2*expansion;
238
239
240    if (edgeConfidence < 0)
241        isFalseData = 1;
242    else
243        isFalseData = 0;
244
245//    edgeConfidence = edgeConfidence*edgeConfidence;
246    edgeConfidence = MAX(MinEdgeConfidence/255.0, edgeConfidence);
247
248    if (isFalseData)
249        edgeConfidence = 0;
250
251/*
252    if (isFalseData)
253        return;
254        */
255
256//    edgeConfidence = 10/256.0;
257//    edgeConfidence = 1;
258
259    NEW_C3 = edgeConfidence*NEW_D2 + (1-edgeConfidence)*NEW_D1;
260
261    NEW_C4 = edgeConfidence*(NEW_D2 - (NEW_D1 - NEW_D2)/2);
262
263/*
264    NEW_C3 = (0.5*edgeConfidence+0.5)*NEW_D2 +
265        (1-(0.5*edgeConfidence+0.5))*NEW_D1;
266    NEW_C4 = (0.5*edgeConfidence+0.5)*(NEW_D2 - (NEW_D1 - NEW_D2)/2);
267
268    NEW_C3 = rangeConfidence*NEW_D2 + (1-rangeConfidence)*NEW_D1;
269    NEW_C4 = rangeConfidence*(NEW_D2 - (NEW_D1 - NEW_D2)/2);
270    */
271
272    NEW_M_WEIGHT_2 = 1/(NEW_C3-NEW_C4);
273    NEW_B_WEIGHT_2 = -NEW_C4/(NEW_C3-NEW_C4);
274   
275
276    deltaZ = cellDepth - rangeDepth;
277    deltaZ *= sampleSpacing;
278
279    if (deltaZ > C1 || deltaZ < NEW_C5) {
280        weight = 0;
281        return;
282    }
283    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
284        weight = 0;
285    }
286    else if (deltaZ > NEW_C3 && deltaZ < C2) {
287        weight = rangeConfidence;
288    }
289    else if (deltaZ > C2) {
290        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
291    }
292    else {
293        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + NEW_B_WEIGHT_2);
294    }
295
296    if (!isFalseData)
297        weight = MAX(weight, MinWeight/255.0);
298
299
300    if (deltaZ > NEW_D1) {
301        value = 0;
302    }
303    else if (deltaZ < NEW_D2) {
304        value = 1;
305    }
306    else {
307        value = NEW_M_VALUE*deltaZ + B_VALUE;
308    }
309
310
311//    weight = MIN(1.0, weight+minWeight);
312/*
313    if (weight == 0)
314        return;
315        */
316
317    wasFalseData = cell->totalWeight & OccGridRLE::FALSE_DATA_BIT
318        || cell->totalWeight == 0;
319
320    totalWeight = double(cell->totalWeight & ~OccGridRLE::FALSE_DATA_BIT);
321
322    weight *= UCHAR_MAX;
323    currentValue = double(cell->value);
324    value *= USHRT_MAX;
325
326
327//    if (weight > 30 || !isEdge) {
328    if (weight > -1 || totalWeight > -1) {
329        if (totalWeight + weight != 0) {
330            currentValue = currentValue*totalWeight + value*weight;
331            totalWeight += weight;
332            currentValue /= totalWeight;
333        } else {
334            currentValue = MIN(value, currentValue);       
335        }
336//      isEdge = FALSE;
337    }
338    else {
339        if (totalWeight != 0) {
340            totalWeight = MAX(totalWeight, weight);
341
342            // Min rule
343            //currentValue = MIN(currentValue, value);
344
345            // Geometric mean
346            //currentValue = sqrt(value*currentValue);
347
348            // u-name-it: (1+x-y)/2*y + (1+y-x)/2*x; x,y between 0,1
349            currentValue /= USHRT_MAX;
350            value /= USHRT_MAX;
351            currentValue = ((1+value-currentValue)*currentValue/2 +
352                (1-value+currentValue)*value/2);
353            currentValue *= USHRT_MAX;
354
355        } else {
356            totalWeight = weight;
357            currentValue = value;
358        }
359    }
360   
361    cell->value = ushort(currentValue);
362
363    cell->totalWeight = ushort(totalWeight);
364
365    if (isFalseData && wasFalseData) {
366        cell->totalWeight = cell->totalWeight | OccGridRLE::FALSE_DATA_BIT;
367    }
368   
369    return;
370}
371
372
373void
374updateCellForCarving(float cellDepth, float rangeDepth, float sampleSpacing,
375                     float rangeConfidence, OccElement *cell)
376{
377    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE;
378    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
379    double weight, value, totalWeight, currentValue;
380    float expansion;
381
382    //expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
383    expansion = 1;
384
385    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
386    NEW_M_VALUE = M_VALUE/expansion;
387    NEW_C3 = C3*expansion;
388    NEW_C4 = C4*expansion;
389    NEW_C5 = C5;
390    NEW_D1 = D1*expansion;
391    NEW_D2 = D2*expansion;
392
393    deltaZ = cellDepth - rangeDepth;
394    deltaZ *= sampleSpacing;
395
396    if (deltaZ < NEW_C5) {
397        weight = 0;
398        return;
399    } else if (deltaZ > C1) {
400        weight = 0;
401    }
402    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
403
404        // To avoid overwriting good data??
405        //weight = 1/255.0;
406
407        weight = 0;
408    }
409    else if (deltaZ > NEW_C3 && deltaZ < C2) {
410        weight = rangeConfidence;
411    }
412    else if (deltaZ > C2) {
413        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
414    }
415    else {
416        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + B_WEIGHT_2);
417    }
418
419//    weight = MAX(weight, MinWeight/255.0);
420
421    if (deltaZ > NEW_D1) {
422        value = 0;
423    }
424    else if (deltaZ < NEW_D2) {
425        value = 1;
426    }
427    else {
428        value = NEW_M_VALUE*deltaZ + B_VALUE;
429    }
430
431    totalWeight = double(cell->totalWeight);
432    currentValue = double(cell->value);
433
434    if (weight == 0 && totalWeight == 0) {
435        currentValue /= USHRT_MAX;
436        value *= currentValue;
437        value *= USHRT_MAX;
438        cell->value = ushort(value);
439    } else {   
440        weight *= UCHAR_MAX;
441        value *= USHRT_MAX;
442
443        currentValue = currentValue*totalWeight + value*weight;
444        totalWeight += weight;
445        if (totalWeight != 0) {
446            currentValue /= totalWeight;
447        } else {
448            currentValue = value;
449        }
450   
451        cell->value = ushort(currentValue);
452        cell->totalWeight = ushort(totalWeight);
453    }
454    return;
455}
456
457
458void
459updateCell(float cellDepth, float rangeDepth, Vec3f &norm, 
460           float sampleSpacing, float rangeConfidence, OccNormElement *cell)
461{
462    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE;
463    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
464    double weight, value, totalWeight, currentValue;
465    float expansion;
466    Vec3f curNorm1, curNorm2, curNorm3, curNorm4;
467
468    //expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
469    expansion = 1;
470
471    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
472    NEW_M_VALUE = M_VALUE/expansion;
473    NEW_C3 = C3*expansion;
474    NEW_C4 = C4*expansion;
475    NEW_C5 = C5;
476    NEW_D1 = D1*expansion;
477    NEW_D2 = D2*expansion;
478
479    deltaZ = cellDepth - rangeDepth;
480    deltaZ *= sampleSpacing;
481
482    if (deltaZ > C1 || deltaZ < NEW_C5) {
483        weight = 0;
484        return;
485    }
486    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
487        weight = 0;
488    }
489    else if (deltaZ > NEW_C3 && deltaZ < C2) {
490        weight = rangeConfidence;
491    }
492    else if (deltaZ > C2) {
493        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
494    }
495    else {
496        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + B_WEIGHT_2);
497    }
498
499    weight = MAX(weight, MinWeight/255.0);
500
501    if (deltaZ > NEW_D1) {
502        value = 0;
503    }
504    else if (deltaZ < NEW_D2) {
505        value = 1;
506    }
507    else {
508        value = NEW_M_VALUE*deltaZ + B_VALUE;
509    }
510
511
512#if 0
513
514    totalWeight = double(cell->totalWeight);
515    currentValue = double(cell->value);
516
517    if (weight == 0 && totalWeight == 0) {
518        currentValue /= USHRT_MAX;
519        value *= currentValue;
520        value *= USHRT_MAX;
521        cell->value = ushort(value);
522    } else {   
523        weight *= UCHAR_MAX;
524        value *= USHRT_MAX;
525
526        currentValue = currentValue*totalWeight + value*weight;
527        totalWeight += weight;
528        if (totalWeight != 0) {
529            currentValue /= totalWeight;
530        } else {
531            currentValue = value;
532        }
533   
534        cell->value = ushort(currentValue);
535        cell->totalWeight = ushort(totalWeight);
536    }
537    return;
538
539#else
540
541    Vec3f curNorm;
542    if (weight != 0) {
543        weight *= UCHAR_MAX;
544        currentValue = double(cell->value);
545        value *= USHRT_MAX;
546        totalWeight = double(cell->totalWeight);
547        if (totalWeight == 0) {
548            cell->totalWeight = ushort(weight);
549            cell->value = ushort(value);
550            cell->nx = (signed char)(126*norm.x);
551            cell->ny = (signed char)(126*norm.y);
552            cell->nz = (signed char)(126*norm.z);
553
554            return;
555        } 
556
557        curNorm.setValue(cell->nx, cell->ny, cell->nz);
558        if (curNorm.dot(norm) > 0) {
559            curNorm.normalize();
560            curNorm *= totalWeight;
561            norm *= weight;
562            curNorm += curNorm;
563            curNorm.normalize();
564            currentValue = currentValue*totalWeight + value*weight;
565            totalWeight += weight;
566            currentValue /= totalWeight;
567
568            cell->value = ushort(currentValue);
569            cell->totalWeight = ushort(totalWeight);
570            cell->nx = (signed char)(126*curNorm.x);
571            cell->ny = (signed char)(126*curNorm.y);
572            cell->nz = (signed char)(126*curNorm.z);
573        } else {
574            if (currentValue > value) {
575                cell->value = ushort(value);
576                cell->totalWeight = ushort(weight);
577                cell->nx = (signed char)(126*norm.x);
578                cell->ny = (signed char)(126*norm.y);
579                cell->nz = (signed char)(126*norm.z);
580            }
581        }
582    }
583
584#endif
585
586}
587
588void
589updateCellForCarving(float cellDepth, float rangeDepth, float sampleSpacing,
590                     float rangeConfidence, OccNormElement *cell)
591{
592    float deltaZ, NEW_M_WEIGHT_2, NEW_M_VALUE;
593    float NEW_C3, NEW_C4, NEW_C5, NEW_D1, NEW_D2;
594    double weight, value, totalWeight, currentValue;
595    float expansion;
596
597    //expansion = (1-MAX_STRETCH)*rangeConfidence + MAX_STRETCH;
598    expansion = 1;
599
600    NEW_M_WEIGHT_2 = M_WEIGHT_2/expansion;
601    NEW_M_VALUE = M_VALUE/expansion;
602    NEW_C3 = C3*expansion;
603    NEW_C4 = C4*expansion;
604    NEW_C5 = C5;
605    NEW_D1 = D1*expansion;
606    NEW_D2 = D2*expansion;
607
608    deltaZ = cellDepth - rangeDepth;
609    deltaZ *= sampleSpacing;
610
611    if (deltaZ < NEW_C5) {
612        weight = 0;
613        return;
614    } else if (deltaZ > C1) {
615        weight = 0;
616    }
617    else if (deltaZ > NEW_C5 && deltaZ < NEW_C4) {
618
619        // To avoid overwriting good data??
620        //weight = 1/255.0;
621
622        weight = 0;
623    }
624    else if (deltaZ > NEW_C3 && deltaZ < C2) {
625        weight = rangeConfidence;
626    }
627    else if (deltaZ > C2) {
628        weight = rangeConfidence*(M_WEIGHT_1*deltaZ + B_WEIGHT_1);
629    }
630    else {
631        weight = rangeConfidence*(NEW_M_WEIGHT_2*deltaZ + B_WEIGHT_2);
632    }
633
634    if (deltaZ > NEW_D1) {
635        value = 0;
636    }
637    else if (deltaZ < NEW_D2) {
638        value = 1;
639    }
640    else {
641        value = NEW_M_VALUE*deltaZ + B_VALUE;
642    }
643
644    totalWeight = double(cell->totalWeight);
645    currentValue = double(cell->value);
646
647    if (weight == 0 && totalWeight == 0) {
648        currentValue /= USHRT_MAX;
649        value *= currentValue;
650        value *= USHRT_MAX;
651        cell->value = ushort(value);
652    } else {   
653        weight *= UCHAR_MAX;
654        value *= USHRT_MAX;
655
656        currentValue = currentValue*totalWeight + value*weight;
657        totalWeight += weight;
658        if (totalWeight != 0) {
659            currentValue /= totalWeight;
660        } else {
661            currentValue = value;
662        }
663   
664        cell->value = ushort(currentValue);
665        cell->totalWeight = ushort(totalWeight);
666    }
667    return;
668}
669
670
Note: See TracBrowser for help on using the repository browser.