1 /**********************************************************************
2 * File: lmedsq.cpp (Formerly lms.c)
3 * Description: Code for the LMS class.
4 * Author: Ray Smith
5 * Created: Fri Aug 7 09:30:53 BST 1992
6 *
7 * (C) Copyright 1992, Hewlett-Packard Ltd.
8 ** Licensed under the Apache License, Version 2.0 (the "License");
9 ** you may not use this file except in compliance with the License.
10 ** You may obtain a copy of the License at
11 ** http://www.apache.org/licenses/LICENSE-2.0
12 ** Unless required by applicable law or agreed to in writing, software
13 ** distributed under the License is distributed on an "AS IS" BASIS,
14 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 ** See the License for the specific language governing permissions and
16 ** limitations under the License.
17 *
18 **********************************************************************/
19
20 #include "mfcpch.h"
21 #include <stdlib.h>
22 #include "statistc.h"
23 #include "memry.h"
24 #include "statistc.h"
25 #include "lmedsq.h"
26
27 #define EXTERN
28
29 EXTERN INT_VAR (lms_line_trials, 12, "Number of linew fits to do");
30 #define SEED1 0x1234 //default seeds
31 #define SEED2 0x5678
32 #define SEED3 0x9abc
33 #define LMS_MAX_FAILURES 3
34
35 #ifndef __UNIX__
nrand48(uinT16 * seeds)36 uinT32 nrand48( //get random number
37 uinT16 *seeds //seeds to use
38 ) {
39 static uinT32 seed = 0; //only seed
40
41 if (seed == 0) {
42 seed = seeds[0] ^ (seeds[1] << 8) ^ (seeds[2] << 16);
43 srand(seed);
44 }
45 //make 32 bit one
46 return rand () | (rand () << 16);
47 }
48 #endif
49
50 /**********************************************************************
51 * LMS::LMS
52 *
53 * Construct a LMS class, given the max no of samples to be given
54 **********************************************************************/
55
LMS(inT32 size)56 LMS::LMS ( //constructor
57 inT32 size //samplesize
58 ):samplesize (size) {
59 samplecount = 0;
60 a = 0;
61 m = 0.0f;
62 c = 0.0f;
63 samples = (FCOORD *) alloc_mem (size * sizeof (FCOORD));
64 errors = (float *) alloc_mem (size * sizeof (float));
65 line_error = 0.0f;
66 fitted = FALSE;
67 }
68
69
70 /**********************************************************************
71 * LMS::~LMS
72 *
73 * Destruct a LMS class.
74 **********************************************************************/
75
~LMS()76 LMS::~LMS ( //constructor
77 ) {
78 free_mem(samples);
79 free_mem(errors);
80 }
81
82
83 /**********************************************************************
84 * LMS::clear
85 *
86 * Clear samples from array.
87 **********************************************************************/
88
clear()89 void LMS::clear() { //clear sample
90 samplecount = 0;
91 fitted = FALSE;
92 }
93
94
95 /**********************************************************************
96 * LMS::add
97 *
98 * Add another sample. More than the constructed number will be ignored.
99 **********************************************************************/
100
add(FCOORD sample)101 void LMS::add( //add sample
102 FCOORD sample //sample coords
103 ) {
104 if (samplecount < samplesize)
105 //save it
106 samples[samplecount++] = sample;
107 fitted = FALSE;
108 }
109
110
111 /**********************************************************************
112 * LMS::fit
113 *
114 * Fit a line to the given sample points.
115 **********************************************************************/
116
fit(float & out_m,float & out_c)117 void LMS::fit( //fit sample
118 float &out_m, //output line
119 float &out_c) {
120 inT32 index; //of median
121 inT32 trials; //no of medians
122 float test_m, test_c; //candidate line
123 float test_error; //error of test line
124
125 switch (samplecount) {
126 case 0:
127 m = 0.0f; //no info
128 c = 0.0f;
129 line_error = 0.0f;
130 break;
131
132 case 1:
133 m = 0.0f;
134 c = samples[0].y (); //horiz thru pt
135 line_error = 0.0f;
136 break;
137
138 case 2:
139 if (samples[0].x () != samples[1].x ()) {
140 m = (samples[1].y () - samples[0].y ())
141 / (samples[1].x () - samples[0].x ());
142 c = samples[0].y () - m * samples[0].x ();
143 }
144 else {
145 m = 0.0f;
146 c = (samples[0].y () + samples[1].y ()) / 2;
147 }
148 line_error = 0.0f;
149 break;
150
151 default:
152 pick_line(m, c); //use pts at random
153 compute_errors(m, c); //from given line
154 index = choose_nth_item (samplecount / 2, errors, samplecount);
155 line_error = errors[index];
156 for (trials = 1; trials < lms_line_trials; trials++) {
157 //random again
158 pick_line(test_m, test_c);
159 compute_errors(test_m, test_c);
160 index = choose_nth_item (samplecount / 2, errors, samplecount);
161 test_error = errors[index];
162 if (test_error < line_error) {
163 //find least median
164 line_error = test_error;
165 m = test_m;
166 c = test_c;
167 }
168 }
169 }
170 fitted = TRUE;
171 out_m = m;
172 out_c = c;
173 a = 0;
174 }
175
176
177 /**********************************************************************
178 * LMS::fit_quadratic
179 *
180 * Fit a quadratic to the given sample points.
181 **********************************************************************/
182
fit_quadratic(float outlier_threshold,double & out_a,float & out_b,float & out_c)183 void LMS::fit_quadratic( //fit sample
184 float outlier_threshold, //min outlier size
185 double &out_a, //x squared
186 float &out_b, //output line
187 float &out_c) {
188 inT32 trials; //no of medians
189 double test_a;
190 float test_b, test_c; //candidate line
191 float test_error; //error of test line
192
193 if (samplecount < 3) {
194 out_a = 0;
195 fit(out_b, out_c);
196 return;
197 }
198 pick_quadratic(a, m, c);
199 line_error = compute_quadratic_errors (outlier_threshold, a, m, c);
200 for (trials = 1; trials < lms_line_trials * 2; trials++) {
201 pick_quadratic(test_a, test_b, test_c);
202 test_error = compute_quadratic_errors (outlier_threshold,
203 test_a, test_b, test_c);
204 if (test_error < line_error) {
205 line_error = test_error; //find least median
206 a = test_a;
207 m = test_b;
208 c = test_c;
209 }
210 }
211 fitted = TRUE;
212 out_a = a;
213 out_b = m;
214 out_c = c;
215 }
216
217
218 /**********************************************************************
219 * LMS::constrained_fit
220 *
221 * Fit a line to the given sample points.
222 * The line must have the given gradient.
223 **********************************************************************/
224
constrained_fit(float fixed_m,float & out_c)225 void LMS::constrained_fit( //fit sample
226 float fixed_m, //forced gradient
227 float &out_c) {
228 inT32 index; //of median
229 inT32 trials; //no of medians
230 float test_c; //candidate line
231 static uinT16 seeds[3] = { SEED1, SEED2, SEED3 };
232 //for nrand
233 float test_error; //error of test line
234
235 m = fixed_m;
236 switch (samplecount) {
237 case 0:
238 c = 0.0f;
239 line_error = 0.0f;
240 break;
241
242 case 1:
243 //horiz thru pt
244 c = samples[0].y () - m * samples[0].x ();
245 line_error = 0.0f;
246 break;
247
248 case 2:
249 c = (samples[0].y () + samples[1].y ()
250 - m * (samples[0].x () + samples[1].x ())) / 2;
251 line_error = m * samples[0].x () + c - samples[0].y ();
252 line_error *= line_error;
253 break;
254
255 default:
256 index = (inT32) nrand48 (seeds) % samplecount;
257 //compute line
258 c = samples[index].y () - m * samples[index].x ();
259 compute_errors(m, c); //from given line
260 index = choose_nth_item (samplecount / 2, errors, samplecount);
261 line_error = errors[index];
262 for (trials = 1; trials < lms_line_trials; trials++) {
263 index = (inT32) nrand48 (seeds) % samplecount;
264 test_c = samples[index].y () - m * samples[index].x ();
265 //compute line
266 compute_errors(m, test_c);
267 index = choose_nth_item (samplecount / 2, errors, samplecount);
268 test_error = errors[index];
269 if (test_error < line_error) {
270 //find least median
271 line_error = test_error;
272 c = test_c;
273 }
274 }
275 }
276 fitted = TRUE;
277 out_c = c;
278 a = 0;
279 }
280
281
282 /**********************************************************************
283 * LMS::pick_line
284 *
285 * Fit a line to a random pair of sample points.
286 **********************************************************************/
287
pick_line(float & line_m,float & line_c)288 void LMS::pick_line( //fit sample
289 float &line_m, //output gradient
290 float &line_c) {
291 inT16 trial_count; //no of attempts
292 static uinT16 seeds[3] = { SEED1, SEED2, SEED3 };
293 //for nrand
294 inT32 index1; //picked point
295 inT32 index2; //picked point
296
297 trial_count = 0;
298 do {
299 index1 = (inT32) nrand48 (seeds) % samplecount;
300 index2 = (inT32) nrand48 (seeds) % samplecount;
301 line_m = samples[index2].x () - samples[index1].x ();
302 trial_count++;
303 }
304 while (line_m == 0 && trial_count < LMS_MAX_FAILURES);
305 if (line_m == 0) {
306 line_c = (samples[index2].y () + samples[index1].y ()) / 2;
307 }
308 else {
309 line_m = (samples[index2].y () - samples[index1].y ()) / line_m;
310 line_c = samples[index1].y () - samples[index1].x () * line_m;
311 }
312 }
313
314
315 /**********************************************************************
316 * LMS::pick_quadratic
317 *
318 * Fit a quadratic to a random triplet of sample points.
319 **********************************************************************/
320
pick_quadratic(double & line_a,float & line_m,float & line_c)321 void LMS::pick_quadratic( //fit sample
322 double &line_a, //x suaread
323 float &line_m, //output gradient
324 float &line_c) {
325 inT16 trial_count; //no of attempts
326 static uinT16 seeds[3] = { SEED1, SEED2, SEED3 };
327 //for nrand
328 inT32 index1; //picked point
329 inT32 index2; //picked point
330 inT32 index3;
331 FCOORD x1x2; //vector
332 FCOORD x1x3;
333 FCOORD x3x2;
334 double bottom; //of a
335
336 trial_count = 0;
337 do {
338 if (trial_count >= LMS_MAX_FAILURES - 1) {
339 index1 = 0;
340 index2 = samplecount / 2;
341 index3 = samplecount - 1;
342 }
343 else {
344 index1 = (inT32) nrand48 (seeds) % samplecount;
345 index2 = (inT32) nrand48 (seeds) % samplecount;
346 index3 = (inT32) nrand48 (seeds) % samplecount;
347 }
348 x1x2 = samples[index2] - samples[index1];
349 x1x3 = samples[index3] - samples[index1];
350 x3x2 = samples[index2] - samples[index3];
351 bottom = x1x2.x () * x1x3.x () * x3x2.x ();
352 trial_count++;
353 }
354 while (bottom == 0 && trial_count < LMS_MAX_FAILURES);
355 if (bottom == 0) {
356 line_a = 0;
357 pick_line(line_m, line_c);
358 }
359 else {
360 line_a = x1x3 * x1x2 / bottom;
361 line_m = x1x2.y () - line_a * x1x2.x ()
362 * (samples[index2].x () + samples[index1].x ());
363 line_m /= x1x2.x ();
364 line_c = samples[index1].y () - samples[index1].x ()
365 * (samples[index1].x () * line_a + line_m);
366 }
367 }
368
369
370 /**********************************************************************
371 * LMS::compute_errors
372 *
373 * Compute the squared error from all the points.
374 **********************************************************************/
375
compute_errors(float line_m,float line_c)376 void LMS::compute_errors( //fit sample
377 float line_m, //input gradient
378 float line_c) {
379 inT32 index; //picked point
380
381 for (index = 0; index < samplecount; index++) {
382 errors[index] =
383 line_m * samples[index].x () + line_c - samples[index].y ();
384 errors[index] *= errors[index];
385 }
386 }
387
388
389 /**********************************************************************
390 * LMS::compute_quadratic_errors
391 *
392 * Compute the squared error from all the points.
393 **********************************************************************/
394
compute_quadratic_errors(float outlier_threshold,double line_a,float line_m,float line_c)395 float LMS::compute_quadratic_errors( //fit sample
396 float outlier_threshold, //min outlier
397 double line_a,
398 float line_m, //input gradient
399 float line_c) {
400 inT32 outlier_count; //total outliers
401 inT32 index; //picked point
402 inT32 error_count; //no in total
403 double total_error; //summed squares
404
405 total_error = 0;
406 outlier_count = 0;
407 error_count = 0;
408 for (index = 0; index < samplecount; index++) {
409 errors[error_count] = line_c + samples[index].x ()
410 * (line_m + samples[index].x () * line_a) - samples[index].y ();
411 errors[error_count] *= errors[error_count];
412 if (errors[error_count] > outlier_threshold) {
413 outlier_count++;
414 errors[samplecount - outlier_count] = errors[error_count];
415 }
416 else {
417 total_error += errors[error_count++];
418 }
419 }
420 if (outlier_count * 3 < error_count)
421 return total_error / error_count;
422 else {
423 index = choose_nth_item (outlier_count / 2,
424 errors + samplecount - outlier_count,
425 outlier_count);
426 //median outlier
427 return errors[samplecount - outlier_count + index];
428 }
429 }
430
431
432 /**********************************************************************
433 * LMS::plot
434 *
435 * Plot the fitted line of a LMS.
436 **********************************************************************/
437
438 #ifndef GRAPHICS_DISABLED
plot(ScrollView * win,ScrollView::Color colour)439 void LMS::plot( //plot fit
440 ScrollView* win, //window
441 ScrollView::Color colour //colour to draw in
442 ) {
443 if (fitted) {
444 win->Pen(colour);
445 win->SetCursor(samples[0].x (),
446 c + samples[0].x () * (m + samples[0].x () * a));
447 win->DrawTo(samples[samplecount - 1].x (),
448 c + samples[samplecount - 1].x () * (m +
449 samples[samplecount -
450 1].x () * a));
451 }
452 }
453 #endif
454