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
32namespace 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. */
51const unsigned int CameraTracker::MODE_MODEL = 0;
52/** World point mode, track a world point */
53const 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 */
82void
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 */
126float
128{
129 return new_pan;
130}
131
132/** Get the new tilt value.
133 * @return new optimal tilt value
134 */
135float
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 */
145void
146CameraTracker::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 */
159void
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 */
171void
172CameraTracker::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 */
186void
188{
189 world_x = x;
190 world_y = y;
191}
192
193} // end namespace firevision
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_relative_position_model(RelativePositionModel *rpm)
Set relative position model.
Definition: tracker.cpp:160
float get_new_tilt()
Get the new tilt value.
Definition: tracker.cpp:136
void set_world_point(float x, float y)
Set world point.
Definition: tracker.cpp:187
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 calc()
Calculate values.
Definition: tracker.cpp:83
void set_mode(unsigned int mode)
Set tracking mode.
Definition: tracker.cpp:146
float get_new_pan()
Get the new pan value.
Definition: tracker.cpp:127
CameraTracker(RelativePositionModel *relative_position_model, float camera_height, float camera_ori_deg)
Constructor.
Definition: tracker.cpp:63
static const unsigned int MODE_WORLD
World point mode, track a world point.
Definition: tracker.h:51
~CameraTracker()
Destructor.
Definition: tracker.cpp:74
Relative Position Model Interface.
virtual float get_bearing() const =0
Get bearing (horizontal angle) to object.
virtual float get_slope() const =0
Get slope (vertical angle) to object.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72