Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
back_projection.cpp
1 /***************************************************************************
2  * back_projection.cpp - Projective camera back projection model
3  *
4  * Created: Mon Apr 20 21:59:00 2009
5  * Copyright 2009 Christof Rath <christof.rath@gmail.com>
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include <fvmodels/relative_position/back_projection.h>
23 
24 #include <cmath>
25 
26 namespace firevision {
27 #if 0 /* just to make Emacs auto-indent happy */
28 }
29 #endif
30 
31 /** @class BackProjectionPositionModel <fvmodels/relative_position/back_projection.h>
32  * This model uses a ProjectiveCam to back project points in the image to
33  * the world by the ground plane assumption.
34  *
35  * @author Christof Rath
36  */
37 
38 /** Constructor
39  * @param projective_cam the projective camera object
40  * @param ball_circumference the circumference in case it's a ball
41  */
42 BackProjectionPositionModel::BackProjectionPositionModel(ProjectiveCam &projective_cam,
43  float ball_circumference):
44  __projective_cam(projective_cam),
45  __ball_circumference(ball_circumference)
46 {
47  __ball_radius = __ball_circumference / ( 2.0 * M_PI );
48 
49  __world_x = __world_y = __bearing = __slope = __distance = 0.f;
50  __cirt_center.x = __cirt_center.y = 0.0f;
51  __pos_valid = false;
52 }
53 
54 
55 void
57 {
58  __ball_radius = r;
59 }
60 
61 
62 void
64 {
65  __cirt_center.x = x;
66  __cirt_center.y = y;
67 }
68 
69 
70 void
71 BackProjectionPositionModel::set_cam_rotation(float pan, float tilt, float roll)
72 {
73  __cam_position.roll = roll;
74  __cam_position.pitch = -tilt; //tilt down-wards negative, cam pitch down-wards positive
75  __cam_position.yaw = pan;
76 }
77 
78 void BackProjectionPositionModel::get_pan_tilt(float *pan, float *tilt) const
79 {
80  float roll;
81  get_cam_rotation(*pan, *tilt, roll);
82 }
83 
84 void BackProjectionPositionModel::get_cam_rotation(float &pan, float &tilt, float &roll) const
85 {
86  pan = __cam_position.yaw;
87  tilt = -__cam_position.pitch; //tilt down-wards negative, cam pitch down-wards positive
88  roll = __cam_position.roll;
89 }
90 
91 void BackProjectionPositionModel::set_cam_translation(float height, float rel_x, float rel_y)
92 {
93  __cam_position.x = rel_x;
94  __cam_position.y = rel_y;
95  __cam_position.z = height;
96 }
97 
98 void BackProjectionPositionModel::get_cam_translation(float & height, float & rel_x, float & rel_y) const
99 {
100  height = __cam_position.z;
101  rel_x = __cam_position.x;
102  rel_y = __cam_position.y;
103 }
104 
105 
106 
108 {
109  __projective_cam.set_location(
110  __cam_position.roll,
111  __cam_position.pitch,
112  __cam_position.yaw,
113  __cam_position.z,
114  __cam_position.x,
115  __cam_position.y);
116 
117  try {
118  fawkes::cart_coord_2d_t wld_pt = __projective_cam.get_GPA_world_coord(__cirt_center);
119 
120  __world_x = wld_pt.x;
121  __world_y = wld_pt.y;
122  __pos_valid = true;
123  }
124  catch (AboveHorizonException &e) {
125  __world_x = 0.f;
126  __world_y = 0.f;
127  __pos_valid = false;
128  }
129 
130 #ifdef OLD_COORD_SYS
131  /* Bearing shall be clockwise positive. */
132  __bearing = -atan2f(_ball_y, __world_x);
133 #else
134  __bearing = atan2f(__world_y, __world_x);
135 #endif
136 
137  __distance = sqrt(__world_x * __world_x + __world_y * __world_y);
138  __slope = -atan2f(__cam_position.z, __distance); //slope is down-wards negative
139 }
140 
142 {
143  __pos_valid = false;
144 }
145 
146 } // end namespace firevision
virtual ProjectiveCam & set_location(const fawkes::HomTransform &loc)
Sets a new location for the camera.
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Cartesian coordinates (2D).
Definition: types.h:40
virtual void set_cam_rotation(float pan=0.f, float tilt=0.f, float roll=0.f)
Sets the camera orientation.
The point that should be calculated lies above the horizon.
virtual void get_cam_translation(float &height, float &rel_x, float &rel_y) const
Returns the current translation of the camera.
float y
y coordinate
Definition: types.h:42
virtual void get_cam_rotation(float &pan, float &tilt, float &roll) const
Returns the camera orientation.
float x
The x-coordinate of the point.
Definition: types.h:134
float x
x in pixels
Definition: types.h:40
float z
The z-coordinate of the point.
Definition: types.h:136
virtual void reset()
Reset all data.
Abstract class for projective cameras.
float pitch
The angle around the y-axis.
Definition: types.h:138
float y
The y-coordinate of the point.
Definition: types.h:135
virtual fawkes::cart_coord_2d_t get_GPA_world_coord(const center_in_roi_t &img_p) const
Returns a point in the world under the ground plane assumption.
virtual void calc()
Calculate position data.
virtual void set_radius(float r)
Set radius of a found circle.
float roll
The angle around the x-axis.
Definition: types.h:137
virtual void set_center(float x, float y)
Set center of a found circle.
virtual void set_cam_translation(float height, float rel_x=0.f, float rel_y=0.f)
Sets the current translation of the camera.
float yaw
The angle around the z-axis.
Definition: types.h:139
float x
x coordinate
Definition: types.h:41
float y
y in pixels
Definition: types.h:41