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 #include <fvmodels/relative_position/relativepositionmodel.h>
26 #include <fvutils/camera/tracker.h>
27 #include <utils/math/angle.h>
28 #include <utils/system/console_colors.h>
29 
30 #include <cmath>
31 
32 namespace firevision {
33 
34 /** @class CameraTracker <fvutils/camera/tracker.h>
35  * Camera Tracker.
36  * Utility class that allows for tracking and object or a world point
37  * by using a camera pan/tilt unit. It is NOT meant to track an object
38  * in a scene!
39  *
40  * The camera tracker will try to keep the desired object or point in the middle
41  * of the image. Given a relative position model or a world point and robot pose
42  * information and initial information the camera tracker returns pan/tilt angles
43  * that are required to have the object in the center of the image. The using
44  * application can then fulfill this desired angles if this lies within the
45  * physical constraints of the pan/tilt unit.
46  *
47  * @author Tim Niemueller
48  */
49 
50 /** Model mode, track by a relative world model. */
51 const unsigned int CameraTracker::MODE_MODEL = 0;
52 /** World point mode, track a world point */
53 const unsigned int CameraTracker::MODE_WORLD = 1;
54 
55 /** Constructor.
56  * @param relative_position_model Relative position model to use if in model tracking
57  * mode.
58  * @param camera_height height above ground of the camera, objects are assumed to lie
59  * on the ground plane.
60  * @param camera_ori_deg The angle between the forward position and the actual position
61  * of the camera on the robot in degrees, clock-wise positive.
62  */
64  float camera_height,
65  float camera_ori_deg)
66 {
67  rpm = relative_position_model;
68  mode = MODE_MODEL;
69  this->camera_height = camera_height;
70  this->camera_orientation = fawkes::deg2rad(camera_ori_deg);
71 }
72 
73 /** Destructor. */
75 {
76 }
77 
78 /** Calculate values.
79  * Based on the set data like robot position, world point and relative position model
80  * this calculates the new desired values for pan and tilt.
81  */
82 void
84 {
85  if (mode == MODE_MODEL) {
86  new_pan = rpm->get_bearing() - camera_orientation;
87  new_tilt = rpm->get_slope();
88  } else if (mode == MODE_WORLD) {
89  float w_r_x = world_x - robot_x;
90  float w_r_y = world_y - robot_y;
91 
92  float distance = sqrt(w_r_x * w_r_x + w_r_y * w_r_y);
93 
94  //cout << msg_prefix << " world_x=" << world_x << " world_y=" << world_y
95  // << " robot_x=" << robot_x << " robot_y=" << robot_y << endl;
96  //cout << msg_prefix << " w_r_x=" << w_r_x << " w_r_y=" << w_r_y
97  // << " dist=" << distance << endl;
98 
99  /* atan2f magic
100  * tan alpha = opposite leg / adjacent leg
101  * => alpha = atan( opposite leg / adjacent leg )
102  *
103  * atan2f now takes y = length(opposite leg) and x = length(adjacent leg)
104  * and calculates the angle alpha. It's exactle the same as the above
105  *
106  * So here we want to calculate the bearing to the world point
107  * So we have a right triangle, where w_r_y is the length of the adjacent
108  * leg and w_r_x is the length of the opposite leg. So to calculate the
109  * bearing / new pan we call atan2f(w_r_x, w_r_y).
110  * For the new tilt we need the distance. This gives us a right triangle
111  * with distance being the opposite leg and the height of the camera on
112  * the robot being the adjacent leg. So slope / new tilt is
113  * atan2f(distance, camera_height).
114  */
115 
116  // Calculate bearing to point
117  new_pan = atan2f(w_r_y, w_r_x);
118  new_pan = fawkes::normalize_mirror_rad(new_pan - robot_ori - camera_orientation);
119  new_tilt = atan2f(camera_height, distance);
120  }
121 }
122 
123 /** Get the new pan value.
124  * @return new optimal pan value
125  */
126 float
128 {
129  return new_pan;
130 }
131 
132 /** Get the new tilt value.
133  * @return new optimal tilt value
134  */
135 float
137 {
138  return new_tilt;
139 }
140 
141 /** Set tracking mode.
142  * @param mode new tracking mode
143  * @exception Exception thrown, if mode is neither MODE_WORLD nor MODE_MODEL
144  */
145 void
146 CameraTracker::set_mode(unsigned int mode)
147 {
148  if ((mode == MODE_WORLD) || (mode == MODE_MODEL)) {
149  this->mode = mode;
150  } else {
151  throw fawkes::Exception("CameraTracker: Invalid mode, not setting mode");
152  }
153 }
154 
155 /** Set relative position model.
156  * Switch the relative position model.
157  * @param rpm new relative position model
158  */
159 void
161 {
162  this->rpm = rpm;
163 }
164 
165 /** Set robot position.
166  * Set the current robot position.
167  * @param x new x coordinate in robot system
168  * @param y new y coordinate in robot system
169  * @param ori new orientation
170  */
171 void
172 CameraTracker::set_robot_position(float x, float y, float ori)
173 {
174  robot_x = x;
175  robot_y = y;
176  robot_ori = ori;
177 }
178 
179 /** Set world point.
180  * World point to track for the robot. The world point is given in a robot-relative
181  * coordinate system on the ground plane. X-axis is pointing forward, Y-axis to
182  * the right (right-handed coordinate system).
183  * @param x x coordinate to track
184  * @param y y coordinate to track
185  */
186 void
188 {
189  world_x = x;
190  world_y = y;
191 }
192 
193 } // end namespace firevision
CameraTracker(RelativePositionModel *relative_position_model, float camera_height, float camera_ori_deg)
Constructor.
Definition: tracker.cpp:63
float get_new_pan()
Get the new pan value.
Definition: tracker.cpp:127
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:50
void set_robot_position(float x, float y, float ori)
Set robot position.
Definition: tracker.cpp:172
void set_world_point(float x, float y)
Set world point.
Definition: tracker.cpp:187
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
Relative Position Model Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:35
float get_new_tilt()
Get the new tilt value.
Definition: tracker.cpp:136
static const unsigned int MODE_WORLD
World point mode, track a world point.
Definition: tracker.h:51
void set_mode(unsigned int mode)
Set tracking mode.
Definition: tracker.cpp:146
void calc()
Calculate values.
Definition: tracker.cpp:83
~CameraTracker()
Destructor.
Definition: tracker.cpp:74
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:36
void set_relative_position_model(RelativePositionModel *rpm)
Set relative position model.
Definition: tracker.cpp:160