Fawkes API  Fawkes Development Version
tracker.cpp
1 
2 /***************************************************************************
3  * camera_tracker.cpp - Implementation of the camera tracker
4  *
5  * Created: Thu Jul 14 22:18:14 2005
6  * Copyright 2005-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <core/exception.h>
25 
26 #include <fvutils/camera/tracker.h>
27 #include <utils/system/console_colors.h>
28 #include <utils/math/angle.h>
29 
30 #include <fvmodels/relative_position/relativepositionmodel.h>
31 
32 #include <cmath>
33 
34 namespace firevision {
35 #if 0 /* just to make Emacs auto-indent happy */
36 }
37 #endif
38 
39 /** @class CameraTracker <fvutils/camera/tracker.h>
40  * Camera Tracker.
41  * Utility class that allows for tracking and object or a world point
42  * by using a camera pan/tilt unit. It is NOT meant to track an object
43  * in a scene!
44  *
45  * The camera tracker will try to keep the desired object or point in the middle
46  * of the image. Given a relative position model or a world point and robot pose
47  * information and initial information the camera tracker returns pan/tilt angles
48  * that are required to have the object in the center of the image. The using
49  * application can then fulfill this desired angles if this lies within the
50  * physical constraints of the pan/tilt unit.
51  *
52  * @author Tim Niemueller
53  */
54 
55 /** Model mode, track by a relative world model. */
56 const unsigned int CameraTracker::MODE_MODEL = 0;
57 /** World point mode, track a world point */
58 const unsigned int CameraTracker::MODE_WORLD = 1;
59 
60 
61 /** Constructor.
62  * @param relative_position_model Relative position model to use if in model tracking
63  * mode.
64  * @param camera_height height above ground of the camera, objects are assumed to lie
65  * on the ground plane.
66  * @param camera_ori_deg The angle between the forward position and the actual position
67  * of the camera on the robot in degrees, clock-wise positive.
68  */
70  float camera_height,
71  float camera_ori_deg
72  )
73 {
74  rpm = relative_position_model;
75  mode = MODE_MODEL;
76  this->camera_height = camera_height;
77  this->camera_orientation = fawkes::deg2rad( camera_ori_deg );
78 }
79 
80 
81 /** Destructor. */
83 {
84 }
85 
86 
87 /** Calculate values.
88  * Based on the set data like robot position, world point and relative position model
89  * this calculates the new desired values for pan and tilt.
90  */
91 void
93 {
94  if (mode == MODE_MODEL) {
95  new_pan = rpm->get_bearing() - camera_orientation;
96  new_tilt = rpm->get_slope();
97  } else if (mode == MODE_WORLD) {
98 
99  float w_r_x = world_x - robot_x;
100  float w_r_y = world_y - robot_y;
101 
102  float distance = sqrt( w_r_x * w_r_x + w_r_y * w_r_y );
103 
104  //cout << msg_prefix << " world_x=" << world_x << " world_y=" << world_y
105  // << " robot_x=" << robot_x << " robot_y=" << robot_y << endl;
106  //cout << msg_prefix << " w_r_x=" << w_r_x << " w_r_y=" << w_r_y
107  // << " dist=" << distance << endl;
108 
109  /* atan2f magic
110  * tan alpha = opposite leg / adjacent leg
111  * => alpha = atan( opposite leg / adjacent leg )
112  *
113  * atan2f now takes y = length(opposite leg) and x = length(adjacent leg)
114  * and calculates the angle alpha. It's exactle the same as the above
115  *
116  * So here we want to calculate the bearing to the world point
117  * So we have a right triangle, where w_r_y is the length of the adjacent
118  * leg and w_r_x is the length of the opposite leg. So to calculate the
119  * bearing / new pan we call atan2f(w_r_x, w_r_y).
120  * For the new tilt we need the distance. This gives us a right triangle
121  * with distance being the opposite leg and the height of the camera on
122  * the robot being the adjacent leg. So slope / new tilt is
123  * atan2f(distance, camera_height).
124  */
125 
126  // Calculate bearing to point
127  new_pan = atan2f( w_r_y, w_r_x );
128  new_pan = fawkes::normalize_mirror_rad( new_pan - robot_ori - camera_orientation);
129  new_tilt = atan2f( camera_height, distance );
130  }
131 }
132 
133 
134 /** Get the new pan value.
135  * @return new optimal pan value
136  */
137 float
139 {
140  return new_pan;
141 }
142 
143 
144 /** Get the new tilt value.
145  * @return new optimal tilt value
146  */
147 float
149 {
150  return new_tilt;
151 }
152 
153 
154 /** Set tracking mode.
155  * @param mode new tracking mode
156  * @exception Exception thrown, if mode is neither MODE_WORLD nor MODE_MODEL
157  */
158 void
159 CameraTracker::set_mode(unsigned int mode)
160 {
161  if ( (mode == MODE_WORLD) || (mode == MODE_MODEL)) {
162  this->mode = mode;
163  } else {
164  throw fawkes::Exception("CameraTracker: Invalid mode, not setting mode");
165  }
166 }
167 
168 
169 /** Set relative position model.
170  * Switch the relative position model.
171  * @param rpm new relative position model
172  */
173 void
175 {
176  this->rpm = rpm;
177 }
178 
179 
180 /** Set robot position.
181  * Set the current robot position.
182  * @param x new x coordinate in robot system
183  * @param y new y coordinate in robot system
184  * @param ori new orientation
185  */
186 void
187 CameraTracker::set_robot_position(float x, float y, float ori)
188 {
189  robot_x = x;
190  robot_y = y;
191  robot_ori = ori;
192 }
193 
194 
195 /** Set world point.
196  * World point to track for the robot. The world point is given in a robot-relative
197  * coordinate system on the ground plane. X-axis is pointing forward, Y-axis to
198  * the right (right-handed coordinate system).
199  * @param x x coordinate to track
200  * @param y y coordinate to track
201  */
202 void
204 {
205  world_x = x;
206  world_y = y;
207 }
208 
209 
210 } // end namespace firevision
CameraTracker(RelativePositionModel *relative_position_model, float camera_height, float camera_ori_deg)
Constructor.
Definition: tracker.cpp:69
float get_new_pan()
Get the new pan value.
Definition: tracker.cpp:138
virtual float get_slope() const =0
Get slope (vertical angle) to object.
static const unsigned int MODE_MODEL
Model mode, track by a relative world model.
Definition: tracker.h:53
void set_robot_position(float x, float y, float ori)
Set robot position.
Definition: tracker.cpp:187
void set_world_point(float x, float y)
Set world point.
Definition: tracker.cpp:203
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
Relative Position Model Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
float get_new_tilt()
Get the new tilt value.
Definition: tracker.cpp:148
static const unsigned int MODE_WORLD
World point mode, track a world point.
Definition: tracker.h:54
void set_mode(unsigned int mode)
Set tracking mode.
Definition: tracker.cpp:159
void calc()
Calculate values.
Definition: tracker.cpp:92
~CameraTracker()
Destructor.
Definition: tracker.cpp:82
virtual float get_bearing() const =0
Get bearing (horizontal angle) to object.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
void set_relative_position_model(RelativePositionModel *rpm)
Set relative position model.
Definition: tracker.cpp:174