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

Linux Cross Reference
Tina4/src/vision/camera/par_proj.c

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

  1 /**@(#)
  2 **/
  3 #include <tina/sys.h>
  4 #include <tina/sysfuncs.h>
  5 #include <tina/math.h>
  6 #include <tina/mathfuncs.h>
  7 #include <tina/vision.h>
  8 #include <tina/visionfuncs.h>
  9 
 10 /* module to project from parallel camera disparity coords to word
 11  * coords and visa versa
 12  * 
 13  * f is focal length of parallel camera in pixel units I is inter-ocular
 14  * separation in units of the word coord */
 15 
 16 static float f = (float)1.0, I = (float)1.0;
 17 
 18 void    set_par_proj(double fnew, double Inew)
 19 {
 20     f = (float)fnew;
 21     I = (float)Inew;
 22 }
 23 
 24 void    par_proj_set(double fnew, double Inew)
 25 {
 26     f = (float)fnew;
 27     I = (float)Inew;
 28 }
 29 
 30 void    par_proj_get(float *fp, float *Ip)
 31 {
 32     *fp = f;
 33     *Ip = I;
 34 }
 35 
 36 void    par_proj_ray(Vec2 u, Vec3 * p, Vec3 * v)
 37 {
 38     *p = vec3_zero();           /* the origin of parallel coords */
 39 
 40     *v = vec3_of_vec2(u);
 41     vec3_z(*v) = f;
 42     *v = vec3_unit(*v);
 43 }
 44 
 45 Vec3    vec3_par_proj_3d(Vec3 p)
 46 {
 47     float   proj = -I / vec3_z(p);
 48 
 49     vec3_z(p) = f;
 50     return (vec3_times(proj, p));
 51 }
 52 
 53 Vec3    vec3_par_proj_disp(Vec3 p)
 54 {
 55     float   proj = f / vec3_z(p);
 56 
 57     vec3_z(p) = -I;
 58     return (vec3_times(proj, p));
 59 }
 60 
 61 void    vec3_pp3d_inplace(Vec3 * p)
 62 {
 63     float   proj;
 64 
 65     if (p == NULL)
 66         return;
 67 
 68     proj = -I / vec3_z(*p);
 69     vec3_z(*p) = f;
 70     *p = vec3_times(proj, *p);
 71 }
 72 
 73 void    vec3_ppdisp_inplace(Vec3 * p)
 74 {
 75     float   proj;
 76 
 77     if (p == NULL)
 78         return;
 79 
 80     proj = f / vec3_z(*p);
 81     vec3_z(*p) = -I;
 82     *p = vec3_times(proj, *p);
 83 }
 84 
 85 void    line3_par_proj_3d(Line3 * line)
 86 {
 87     if (line == NULL)
 88         return;
 89 
 90     line->p1 = vec3_par_proj_3d(line->p1);
 91     line->p2 = vec3_par_proj_3d(line->p2);
 92     line3_remake(line, THREEDIM);
 93 }
 94 
 95 void    line3_par_proj_disp(Line3 * line)
 96 {
 97     if (line == NULL)
 98         return;
 99 
100     line->p1 = vec3_par_proj_disp(line->p1);
101     line->p2 = vec3_par_proj_disp(line->p2);
102     line3_remake((Line3 *) line, DISPARITY);
103 }
104 
105 void    plane_par_proj_3d(Plane * plane)
106 {
107     float   D;                  /* PX + QY + RZ = D disparity coords */
108     float   d;                  /* px + qy + rz = d world coords */
109     Vec3    n = {Vec3_id};
110     float   mag;
111 
112     if (plane == NULL)
113         return;
114 
115     n = plane->n;
116 
117     /* first convert plane to algerbraic form n.x = n.p */
118     D = (float)vec3_dot(plane->p, plane->n);    /* |closest approach to origin| */
119 
120     /* derive 3D algebra from disp algebra Px + Qy - Dz/f = RI */
121     d = vec3_z(n) * I;
122     vec3_z(n) = -D / f;
123 
124     mag = (float)vec3_mod(n);
125     plane->n = vec3_times(1 / mag, n);  /* make unit vector */
126     plane->p = vec3_times(d / mag, plane->n);   /* point closest to
127                                                  * origin */
128     plane->type = THREEDIM;
129 }
130 

~ [ 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.