~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/geomstat/state/state_tina.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)
  2 **/
  3 /**
  4 state_tina.c:
  5 adding Tina primitives to a state
  6 **/
  7 
  8 #include <stdio.h>
  9 #include <math.h>
 10 #include <tina/sys.h>
 11 #include <tina/math.h>
 12 #include <tina/mathfuncs.h>
 13 #include <tina/sysfuncs.h>
 14 #include <tina/vision.h>
 15 #include <tina/visionfuncs.h>
 16 
 17 double  cam_pix(), cam_f();
 18 Vec3    left_origin();
 19 Vec3    right_origin();
 20 
 21 void    state_set_cov_scalar(State * state, int i, double err)
 22 {
 23     Matrix *sii;
 24     double  var = err * err;
 25     int     df = geom_df(SCALAR);
 26 
 27     sii = matrix_alloc(df, df, matrix_lower, double_v);
 28     matrix_putf(var, sii, 0, 0);
 29     matrix_putp((void *) sii, state->s, i, i);
 30 }
 31 
 32 void    state_set_cov_point2(State * state, int i, double poserr)
 33 {
 34     Matrix *sii;
 35     double  pvar = poserr * poserr;
 36     int     df = geom_df(POINT2);
 37 
 38     sii = matrix_alloc(df, df, matrix_lower, double_v);
 39     matrix_putf(pvar, sii, 0, 0);
 40     matrix_putf(pvar, sii, 1, 1);
 41     matrix_putp((void *) sii, state->s, i, i);
 42 }
 43 
 44 void    state_set_cov_line2(State * state, int i, double poserr, double angerr)
 45 {
 46     Matrix *sii;
 47     double  pvar = poserr * poserr;
 48     double  avar = angerr * angerr;
 49     int     df = geom_df(LINE2);
 50 
 51     sii = matrix_alloc(df, df, matrix_lower, double_v);
 52     matrix_putf(pvar, sii, 0, 0);
 53     matrix_putf(avar, sii, 1, 1);
 54     matrix_putp((void *) sii, state->s, i, i);
 55 }
 56 
 57 void    state_set_cov_point3(State * state, int i, double poserr)
 58 {
 59     Matrix *sii;
 60     double  pvar = poserr * poserr;
 61     int     df = geom_df(POINT3);
 62 
 63     sii = matrix_alloc(df, df, matrix_lower, double_v);
 64     matrix_putf(pvar, sii, 0, 0);
 65     matrix_putf(pvar, sii, 1, 1);
 66     matrix_putf(pvar, sii, 2, 2);
 67     matrix_putp((void *) sii, state->s, i, i);
 68 }
 69 
 70 void    state_set_cov_line3(State * state, int i, double poserr, double angerr)
 71 {
 72     Matrix *sii;
 73     double  pvar = poserr * poserr;
 74     double  avar = angerr * angerr;
 75     int     df = geom_df(LINE3);
 76 
 77     sii = matrix_alloc(df, df, matrix_lower, double_v);
 78     matrix_putf(pvar, sii, 0, 0);
 79     matrix_putf(pvar, sii, 1, 1);
 80     matrix_putf(avar, sii, 2, 2);
 81     matrix_putf(avar, sii, 3, 3);
 82     matrix_putp((void *) sii, state->s, i, i);
 83 }
 84 
 85 void    state_set_cov_plane(State * state, int i, double poserr, double angerr)
 86 {
 87     Matrix *sii;
 88     double  pvar = poserr * poserr;
 89     double  avar = angerr * angerr;
 90     int     df = geom_df(PLANE);
 91 
 92     sii = matrix_alloc(df, df, matrix_lower, double_v);
 93     matrix_putf(pvar, sii, 0, 0);
 94     matrix_putf(avar, sii, 1, 1);
 95     matrix_putf(avar, sii, 2, 2);
 96     matrix_putp((void *) sii, state->s, i, i);
 97 }
 98 
 99 void    state_set_cov_transf3(State * state, int i, double poserr, double angerr)
100 {
101     Matrix *sii;
102     double  pvar = poserr * poserr;
103     double  avar = angerr * angerr;
104     int     df = geom_df(TRANSF3);
105 
106     sii = matrix_alloc(df, df, matrix_lower, double_v);
107     matrix_putf(avar, sii, 0, 0);
108     matrix_putf(avar, sii, 1, 1);
109     matrix_putf(avar, sii, 2, 2);
110     matrix_putf(pvar, sii, 3, 3);
111     matrix_putf(pvar, sii, 4, 4);
112     matrix_putf(pvar, sii, 5, 5);
113     matrix_putp((void *) sii, state->s, i, i);
114 }
115 
116 void    state_set_infcov(State * state, int i)
117 {
118     double  big = 1.0;
119     double  lscale = state->lscale;
120 
121     switch (vector_get(state->type, i))
122     {
123     case POINT2:
124         {
125             double  poserr = lscale * big;
126 
127             state_set_cov_point2(state, i, poserr);
128             break;
129         }
130     case LINE2:
131         {
132             double  poserr = lscale * big;
133             double  angerr = big;
134 
135             state_set_cov_line2(state, i, poserr, angerr);
136             break;
137         }
138     case POINT3:
139         {
140             double  poserr = lscale * big;
141 
142             state_set_cov_point3(state, i, poserr);
143             break;
144         }
145     case LINE3:
146         {
147             double  poserr = lscale * big;
148             double  angerr = big;
149 
150             state_set_cov_line3(state, i, poserr, angerr);
151             break;
152         }
153     case PLANE:
154         {
155             double  poserr = lscale * big;
156             double  angerr = big;
157 
158             state_set_cov_plane(state, i, poserr, angerr);
159             break;
160         }
161     case TRANSF3:
162         {
163             double  poserr = lscale * big;
164             double  angerr = big;
165 
166             state_set_cov_transf3(state, i, poserr, angerr);
167             break;
168         }
169     }
170 }
171 
172 void    state_tinacov_line3(State * state, int i, Line3 * line)
173 {
174     int     iray, ipoint;
175     Line3  *ray;
176     Point3 *point;
177     Vec3    origin = {Vec3_id};
178     Vec3    v = {Vec3_id};
179     double  err = cam_pix() / cam_f();
180     double  state_on();
181 
182     state_set_infcov(state, i);
183 
184     point = point3_make(line->p1, (int) NULL);
185     ipoint = state_add(state, (void *) point, POINT3);
186     state_set_infcov(state, ipoint);
187 
188     origin = left_origin();
189     v = vec3_diff(line->p1, origin);
190     ray = line3_make(vec3_diff(origin, v), line->p1, (int) NULL);
191     iray = state_add(state, (void *) ray, LINE3);
192     state_set_cov_line3(state, iray, 0.0, err);
193 
194     /* BUGFIX chi2 unused */
195     /* double  chi2; chi2 = state_on(state, ipoint, iray); */
196     (void) state_on(state, ipoint, iray);
197     state_delete(state, iray);
198     geom_free((void *) ray, LINE3);
199 
200     origin = right_origin();
201     v = vec3_diff(line->p1, origin);
202     ray = line3_make(vec3_diff(origin, v), line->p1, (int) NULL);
203     iray = state_add(state, (void *) ray, LINE3);
204     state_set_cov_line3(state, iray, 0.0, err);
205 
206     /* BUGFIX */
207     /* WAS: chi2 = state_on(state, ipoint, iray); */
208     (void) state_on(state, ipoint, iray);
209     state_delete(state, iray);
210     geom_free((void *) ray, LINE3);
211 
212     /* BUGFIX */
213     /* WAS: chi2 = state_on(state, ipoint, iray); */
214     (void) state_on(state, ipoint, i);
215     state_delete(state, ipoint);
216     geom_free((void *) point, POINT3);
217 
218     point = point3_make(line->p2, (int) NULL);
219     ipoint = state_add(state, (void *) point, POINT3);
220     state_set_infcov(state, ipoint);
221 
222     origin = left_origin();
223     v = vec3_diff(line->p2, origin);
224     ray = line3_make(vec3_diff(origin, v), line->p2, (int) NULL);
225     iray = state_add(state, (void *) ray, LINE3);
226     state_set_cov_line3(state, iray, 0.0, err);
227 
228     /* BUGFIX */
229     /* WAS: chi2 = state_on(state, ipoint, iray); */
230     (void) state_on(state, ipoint, iray);
231     state_delete(state, iray);
232     geom_free((void *) ray, LINE3);
233 
234     origin = right_origin();
235     v = vec3_diff(line->p2, origin);
236     ray = line3_make(vec3_diff(origin, v), line->p2, (int) NULL);
237     iray = state_add(state, (void *) ray, LINE3);
238     state_set_cov_line3(state, iray, 0.0, err);
239 
240     /* BUGFIX */
241     /* WAS: chi2 = state_on(state, ipoint, iray); */
242     (void) state_on(state, ipoint, iray);
243     state_delete(state, iray);
244     geom_free((void *) ray, LINE3);
245 
246     /* BUGFIX chi2 unused */
247     /* WAS: chi2 = state_on(state, ipoint, i); */
248     (void) state_on(state, ipoint, i);
249     state_delete(state, ipoint);
250     geom_free((void *) point, POINT3);
251 
252     state_correct(state);
253 }
254 
255 int     state_add_tina(State * state, void *geom, int type)
256 {
257     int     i;
258 
259     i = state_add(state, geom, type);
260     switch (type)
261     {
262     case LINE3:
263         {
264             Line3  *line = (Line3 *) geom;
265 
266             state_tinacov_line3(state, i, line);
267             break;
268         }
269     }
270     return (i);
271 }
272 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.