1 /**********
2 *
3 * This file is part of the TINA Open Source Image Analysis Environment
4 * henceforth known as TINA
5 *
6 * TINA is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU Lesser General Public License as
8 * published by the Free Software Foundation.
9 *
10 * TINA is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU Lesser General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with TINA; if not, write to the Free Software Foundation, Inc.,
17 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 *
19 **********
20 *
21 * Program : TINA
22 * File : $Source: /home/tina/cvs/tina-libs/tina/geometry/geomCurve_con_klmn.c,v $
23 * Date : $Date: 2002/12/09 11:51:23 $
24 * Version : $Revision: 1.1.1.1 $
25 * CVS Id : $Id: geomCurve_con_klmn.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
26 *
27 * Author : Legacy TINA
28 *
29 * Notes : Bierman u-d form of scalar measurement filter
30 *
31 *********
32 */
33
34 #include "geomCurve_con_klmn.h"
35
36 #if HAVE_CONFIG_H
37 #include <config.h>
38 #endif
39
40 #include <math.h>
41 #include <tina/sys/sysDef.h>
42 #include <tina/sys/sysPro.h>
43 #include <tina/math/mathDef.h>
44 #include <tina/math/mathPro.h>
45 #include <tina/geometry/geom_CurveDef.h>
46
47
48 #define N 5
49
50 /* Renamed from kalman by Julian to avoid compiler warning: 'kalman
51 * static & extern' */
52 static double klmn(double *x, double (*u)[5], double *d, double z, double *h, double var)
53 {
54 int i, j;
55 double a[N], s[N], r[N], p[N], k[N], denom, sum, uij;
56
57 for (i = 0; i < N; i++)
58 {
59 sum = 0.0;
60 for (j = 0; j < i; j++)
61 sum += u[j][i] * h[j];
62 r[i] = sum + h[i];
63 }
64
65 for (i = 0; i < N; i++)
66 {
67 z += h[i] * x[i];
68 s[i] = d[i] * r[i];
69 }
70
71 a[0] = s[0] * r[0] + var;
72 a[0] = s[0] * r[0] + var;
73 d[0] *= var / a[0];
74 k[0] = s[0];
75
76 for (j = 1; j < N; j++)
77 {
78 a[j] = a[j - 1] + s[j] * r[j];
79 d[j] *= a[j - 1] / a[j];
80 k[j] = s[j];
81 p[j] = -r[j] / a[j - 1];
82 for (i = 0; i < j; i++)
83 {
84 uij = u[i][j];
85 u[i][j] += k[i] * p[j];
86 k[i] += uij * s[j];
87 }
88 }
89
90 denom = a[4];
91 denom = 1.0 / denom;
92
93 for (i = 0; i < N; i++)
94 x[i] -= z * k[i] * denom;
95
96 return (z * z * denom);
97 }
98
99 /**
100
101 naive least squares (var unused)
102 **/
103
104 double conic_nlsq(Conic * conic, Conic_stat * stats, Vec2 p, double var)
105 {
106 double z, h[5];
107 double a, b, c, d, e, f;
108 double px = vec2_x(p), py = vec2_y(p);
109
110 a = conic->a;
111 b = conic->b;
112 c = conic->c;
113 d = conic->d;
114 e = conic->e;
115 f = conic->f;
116
117 z = a * px * px + 2.0 * b * px * py + c * py * py +
118 2.0 * d * px + 2.0 * e * py + f;
119 h[0] = px * px - py * py;
120 h[1] = 2.0 * px * py;
121 h[2] = 2.0 * px;
122 h[3] = 2.0 * py;
123 h[4] = 1.0;
124 var = 1.0; /* var reset to unity */
125
126 return (klmn(stats->x, stats->u, stats->d, z, h, var));
127 }
128
129 /**
130 extended kalman filter
131 **/
132
133 double conic_ekf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
134 {
135 double z, h[5];
136 double a, b, c, d, e, f;
137 double px = vec2_x(p), py = vec2_y(p);
138 double fx, fy;
139
140 a = conic->a;
141 b = conic->b;
142 c = conic->c;
143 d = conic->d;
144 e = conic->e;
145 f = conic->f;
146
147 z = a * px * px + 2.0 * b * px * py + c * py * py +
148 2.0 * d * px + 2.0 * e * py + f;
149 h[0] = px * px - py * py;
150 h[1] = 2.0 * px * py;
151 h[2] = 2.0 * px;
152 h[3] = 2.0 * py;
153 h[4] = 1.0;
154 fx = 2.0 * (a * px + b * py + d);
155 fy = 2.0 * (b * px + c * py + e);
156 var *= (fx * fx + fy * fy);
157
158 return (klmn(stats->x, stats->u, stats->d, z, h, var));
159 }
160
161 /**
162 bias corrected kalman filter
163 **/
164
165 double conic_bckf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
166 {
167 double z, h[5];
168 double a, b, c, d, e, f;
169 double px = vec2_x(p), py = vec2_y(p);
170 double fx, fy, t;
171
172 a = conic->a;
173 b = conic->b;
174 c = conic->c;
175 d = conic->d;
176 e = conic->e;
177 f = conic->f;
178
179 z = a * px * px + 2.0 * b * px * py + c * py * py +
180 2.0 * d * px + 2.0 * e * py + f;
181 fx = 2.0 * (a * px + b * py + d);
182 fy = 2.0 * (b * px + c * py + e);
183 var *= (fx * fx + fy * fy);
184 t = 2.0 * z / (fx * fx + fy * fy);
185
186 h[0] = px * px - py * py;
187 h[1] = 2.0 * px * py;
188 h[2] = 2.0 * px;
189 h[3] = 2.0 * py;
190 h[4] = 1.0;
191
192 h[0] -= t * (px * fx - py * fy);
193 h[1] -= t * (py * fx + px * fy);
194 h[2] -= t * fx;
195 h[3] -= t * fy;
196
197 return (klmn(stats->x, stats->u, stats->d, z, h, var));
198 }
199
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.