Changeset 42715 for branches/2dbias/psLib/src/math/psVectorSmooth.c
- Timestamp:
- Aug 6, 2024, 5:35:16 PM (2 years ago)
- File:
-
- 1 edited
-
branches/2dbias/psLib/src/math/psVectorSmooth.c (modified) (5 diffs)
Legend:
- Unmodified
- Added
- Removed
-
branches/2dbias/psLib/src/math/psVectorSmooth.c
r31660 r42715 1 1 #ifdef HAVE_CONFIG_H 2 # 2 #include "config.h" 3 3 #endif 4 4 … … 15 15 #include "psAssert.h" 16 16 #include "psConstants.h" 17 #include "psStats.h" 17 18 18 19 #include "psVectorSmooth.h" … … 21 22 const psVector *input, 22 23 double sigma, 23 double Nsigma 24 ) 24 double Nsigma) 25 25 { 26 26 PS_ASSERT_VECTOR_NON_NULL(input, NULL); … … 28 28 PS_ASSERT_FLOAT_LARGER_THAN(Nsigma, 0.0, NULL); 29 29 30 if (output == input) { 30 if (output == input) 31 { 31 32 psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Cannot smooth vector in-place."); 32 33 return NULL; … … 34 35 35 36 // relevant terms 36 long Nrange = sigma*Nsigma + 0.5; // Extent of smoothing 37 long Npixel = 2*Nrange + 1; // Total size of smoothing kernel 38 long Nbin = input->n; // Number of elements 39 double factor = -0.5/(sigma*sigma); // Factor for Gaussian 40 41 if (Nbin < Npixel) { 42 // cannot smooth narrow vector 43 return NULL; 44 } 45 46 #define VECTOR_SMOOTH_CASE(TYPE) \ 47 case PS_TYPE_##TYPE: { \ 48 output = psVectorRecycle(output, Nbin, PS_TYPE_##TYPE); \ 49 /* generate normalized gaussian */ \ 50 psVector *gaussnorm = psVectorAlloc(Npixel, PS_TYPE_##TYPE); \ 51 double sum = 0.0; \ 52 for (long i = -Nrange; i < Nrange + 1; i++) { \ 53 gaussnorm->data.TYPE[i+Nrange] = exp(factor*i*i); \ 54 sum += gaussnorm->data.TYPE[i+Nrange]; \ 55 } \ 56 for (long i = -Nrange; i < Nrange + 1; i++) { \ 57 gaussnorm->data.TYPE[i+Nrange] /= sum; \ 58 } \ 59 ps##TYPE *gauss = &gaussnorm->data.TYPE[Nrange]; \ 60 \ 61 /* smooth vector */ \ 62 psVector *temp = psVectorAlloc(Nbin, PS_TYPE_##TYPE); \ 63 ps##TYPE *vi = input->data.TYPE; \ 64 ps##TYPE *vo = temp->data.TYPE; \ 65 /* smooth first Nrange pixels, with renorm */ \ 37 long Nrange = sigma * Nsigma + 0.5; // Extent of smoothing 38 long Npixel = 2 * Nrange + 1; // Total size of smoothing kernel 39 long Nbin = input->n; // Number of elements 40 double factor = -0.5 / (sigma * sigma); // Factor for Gaussian 41 42 if (Nbin < Npixel) 43 { 44 // cannot smooth narrow vector 45 return NULL; 46 } 47 48 #define VECTOR_SMOOTH_CASE(TYPE) \ 49 case PS_TYPE_##TYPE: \ 50 { \ 51 output = psVectorRecycle(output, Nbin, PS_TYPE_##TYPE); \ 52 /* generate normalized gaussian */ \ 53 psVector *gaussnorm = psVectorAlloc(Npixel, PS_TYPE_##TYPE); \ 54 double sum = 0.0; \ 55 for (long i = -Nrange; i < Nrange + 1; i++) \ 56 { \ 57 gaussnorm->data.TYPE[i + Nrange] = exp(factor * i * i); \ 58 sum += gaussnorm->data.TYPE[i + Nrange]; \ 59 } \ 60 for (long i = -Nrange; i < Nrange + 1; i++) \ 61 { \ 62 gaussnorm->data.TYPE[i + Nrange] /= sum; \ 63 } \ 64 ps##TYPE *gauss = &gaussnorm->data.TYPE[Nrange]; \ 65 \ 66 /* smooth vector */ \ 67 psVector *temp = psVectorAlloc(Nbin, PS_TYPE_##TYPE); \ 68 ps##TYPE *vi = input->data.TYPE; \ 69 ps##TYPE *vo = temp->data.TYPE; \ 70 /* smooth first Nrange pixels, with renorm */ \ 66 71 /* XXX need to check that this does not run over end for narrow vectors */ \ 67 for (long i = 0; i < Nrange; i++, vi++, vo++) { \ 68 ps##TYPE *vr = vi - i; \ 69 ps##TYPE *vg = gauss - i; \ 70 double g = 0; \ 71 double s = 0; \ 72 for (int n = -i; n < Nrange + 1; n++, vr++, vg++) { \ 73 s += *vg * *vr; \ 74 g += *vg; \ 75 } \ 76 *vo = s / g; \ 77 } \ 78 /* smooth middle pixels */ \ 79 for (long i = Nrange; i < Nbin - Nrange; i++, vi++, vo++) { \ 80 ps##TYPE *vr = vi - Nrange; \ 81 ps##TYPE *vg = gauss - Nrange; \ 82 double s = 0; \ 83 for (int n = -Nrange; n < Nrange + 1; n++, vr++, vg++) { \ 84 s += *vg * *vr; \ 85 } \ 86 *vo = s; \ 87 } \ 88 /* smooth last Nrange pixels, with renorm */ \ 89 /* XXX does this miss the last column? */ \ 90 for (long i = Nbin - Nrange; i < Nbin; i++, vi++, vo++) { \ 91 ps##TYPE *vr = vi - Nrange; \ 92 ps##TYPE *vg = gauss - Nrange; \ 93 double g = 0; \ 94 double s = 0; \ 95 for (int n = -Nrange; n < Nbin - i - 1; n++, vr++, vg++) { \ 96 s += *vg * *vr; \ 97 g += *vg; \ 98 } \ 99 *vo = s / g; \ 100 } \ 101 memcpy(output->data.TYPE, temp->data.TYPE, Nbin*sizeof(ps##TYPE)); \ 102 psFree(temp); \ 103 psFree(gaussnorm); \ 104 break; \ 105 } 106 107 switch (input->type.type) { 72 for (long i = 0; i < Nrange; i++, vi++, vo++) \ 73 { \ 74 ps##TYPE *vr = vi - i; \ 75 ps##TYPE *vg = gauss - i; \ 76 double g = 0; \ 77 double s = 0; \ 78 for (int n = -i; n < Nrange + 1; n++, vr++, vg++) \ 79 { \ 80 s += *vg * *vr; \ 81 g += *vg; \ 82 } \ 83 *vo = s / g; \ 84 } \ 85 /* smooth middle pixels */ \ 86 for (long i = Nrange; i < Nbin - Nrange; i++, vi++, vo++) \ 87 { \ 88 ps##TYPE *vr = vi - Nrange; \ 89 ps##TYPE *vg = gauss - Nrange; \ 90 double s = 0; \ 91 for (int n = -Nrange; n < Nrange + 1; n++, vr++, vg++) \ 92 { \ 93 s += *vg * *vr; \ 94 } \ 95 *vo = s; \ 96 } \ 97 /* smooth last Nrange pixels, with renorm */ \ 98 /* XXX does this miss the last column? */ \ 99 for (long i = Nbin - Nrange; i < Nbin; i++, vi++, vo++) \ 100 { \ 101 ps##TYPE *vr = vi - Nrange; \ 102 ps##TYPE *vg = gauss - Nrange; \ 103 double g = 0; \ 104 double s = 0; \ 105 for (int n = -Nrange; n < Nbin - i - 1; n++, vr++, vg++) \ 106 { \ 107 s += *vg * *vr; \ 108 g += *vg; \ 109 } \ 110 *vo = s / g; \ 111 } \ 112 memcpy(output->data.TYPE, temp->data.TYPE, Nbin * sizeof(ps##TYPE)); \ 113 psFree(temp); \ 114 psFree(gaussnorm); \ 115 break; \ 116 } 117 118 switch (input->type.type) 119 { 108 120 VECTOR_SMOOTH_CASE(F32); 109 121 VECTOR_SMOOTH_CASE(F64); 110 default: { 111 char* typeStr; 112 PS_TYPE_NAME(typeStr, input->type.type); 113 psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Type %s is not valid.", typeStr); 114 return NULL; 115 } 122 default: 123 { 124 char *typeStr; 125 PS_TYPE_NAME(typeStr, input->type.type); 126 psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Type %s is not valid.", typeStr); 127 return NULL; 128 } 116 129 } 117 130 return output; 118 131 } 119 132 120 121 122 133 psVector *psVectorBoxcar(psVector *output, 123 134 const psVector *input, 124 int size 125 )135 int size, 136 bool robust) 126 137 { 127 138 PS_ASSERT_VECTOR_NON_NULL(input, NULL); 128 139 PS_ASSERT_INT_POSITIVE(size, NULL); 129 140 130 if (output == input) { 141 if (output == input) 142 { 131 143 psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Cannot smooth vector in-place."); 132 144 return false; 133 145 } 134 146 135 long num = input->n; 147 long num = input->n; // Number of elements 136 148 output = psVectorRecycle(output, num, input->type.type); 137 psVector *nums = psVectorAlloc(num, PS_TYPE_U32); // Number of elements in each bin138 psU32 *numsData = nums->data.U32; // Dereferenced version139 140 149 psVectorInit(output, 0.0); 141 psVectorInit(nums, 0); 142 143 144 #define VECTOR_BOXCAR_CASE(TYPE) \ 145 case PS_TYPE_##TYPE: { \ 146 /* Dereference data */ \ 147 ps##TYPE *outputData = output->data.TYPE; \ 148 ps##TYPE *inputData = input->data.TYPE; \ 149 /* Smooth the vector */ \ 150 for (long i = 0; i < num; i++) { \ 151 for (long j = PS_MAX(0, i - size); j < PS_MIN(num, i + size + 1); j++) { \ 152 outputData[j] += inputData[i]; \ 153 numsData[j]++; \ 154 } \ 155 } \ 156 /* Normalisation */ \ 157 for (long i = 0; i < num; i++) { \ 158 outputData[i] /= numsData[i]; \ 159 } \ 160 break; \ 161 } 162 163 switch (input->type.type) { 150 151 #define VECTOR_BOXCAR_CASE(TYPE) \ 152 case PS_TYPE_##TYPE: \ 153 { \ 154 /* Dereference data */ \ 155 ps##TYPE *outputData = output->data.TYPE; \ 156 ps##TYPE *inputData = input->data.TYPE; \ 157 /* Smooth the vector */ \ 158 for (long i = 0; i < num; i++) \ 159 { \ 160 /* sample within the window size */ \ 161 int windowSize = PS_MIN(num, i + size + 1) - PS_MAX(0, i - size); \ 162 psVector *sample = psVectorAlloc(windowSize, PS_TYPE_##TYPE); \ 163 ps##TYPE *sampleData = sample->data.TYPE; \ 164 /* extract sample from inputData */ \ 165 for (long j = PS_MAX(0, i - size); j < PS_MIN(num, i + size + 1); j++) \ 166 { \ 167 sampleData[j - PS_MAX(0, i - size)] = inputData[j]; \ 168 } \ 169 /* find the desired statistic from the sample */ \ 170 psStatsOptions statistic; \ 171 psStats *stats; \ 172 if (robust) \ 173 { \ 174 statistic = PS_STAT_CLIPPED_MEAN; \ 175 stats = psStatsAlloc(statistic); \ 176 } \ 177 else \ 178 { \ 179 statistic = PS_STAT_SAMPLE_MEDIAN; \ 180 stats = psStatsAlloc(statistic); \ 181 } \ 182 if (!psVectorStats(stats, sample, NULL, NULL, 0)) \ 183 { \ 184 psError(PS_ERR_UNKNOWN, false, "failure to measure stats"); \ 185 psFree(sample); \ 186 psFree(stats); \ 187 return NULL; \ 188 } \ 189 outputData[i] = psStatsGetValue(stats, statistic); \ 190 psFree(sample); \ 191 psFree(stats); \ 192 } \ 193 break; \ 194 } 195 196 switch (input->type.type) 197 { 164 198 VECTOR_BOXCAR_CASE(F32); 165 199 VECTOR_BOXCAR_CASE(F64); 166 default: { 167 char* typeStr; 168 PS_TYPE_NAME(typeStr, input->type.type); 169 psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Type %s is not valid.", typeStr); 170 psFree(nums); 171 return NULL; 172 } 173 } 174 psFree(nums); 200 default: 201 { 202 char *typeStr; 203 PS_TYPE_NAME(typeStr, input->type.type); 204 psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Type %s is not valid.", typeStr); 205 return NULL; 206 } 207 } 175 208 return output; 176 209 }
Note:
See TracChangeset
for help on using the changeset viewer.
