tracker.cpp
00001 00002 /*************************************************************************** 00003 * camera_tracker.cpp - Implementation of the camera tracker 00004 * 00005 * Generated: Thu Jul 14 22:18:14 2005 00006 * Copyright 2005 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <core/exception.h> 00025 00026 #include <fvutils/camera/tracker.h> 00027 #include <utils/system/console_colors.h> 00028 #include <utils/math/angle.h> 00029 00030 #include <models/relative_position/relativepositionmodel.h> 00031 00032 #include <cmath> 00033 00034 /** @class CameraTracker tracker.h <fvutils/camera/tracker.h> 00035 * Camera Tracker. 00036 * Utility class that allows for tracking and object or a world point 00037 * by using a camera pan/tilt unit. It is NOT meant to track an object 00038 * in a scene! 00039 * 00040 * The camera tracker will try to keep the desired object or point in the middle 00041 * of the image. Given a relative position model or a world point and robot pose 00042 * information and initial information the camera tracker returns pan/tilt angles 00043 * that are required to have the object in the center of the image. The using 00044 * application can then fulfill this desired angles if this lies within the 00045 * physical constraints of the pan/tilt unit. 00046 * 00047 * @author Tim Niemueller 00048 */ 00049 00050 /** Model mode, track by a relative world model. */ 00051 const unsigned int CameraTracker::MODE_MODEL = 0; 00052 /** World point mode, track a world point */ 00053 const unsigned int CameraTracker::MODE_WORLD = 1; 00054 00055 00056 /** Constructor. 00057 * @param relative_position_model Relative position model to use if in model tracking 00058 * mode. 00059 * @param camera_height height above ground of the camera, objects are assumed to lie 00060 * on the ground plane. 00061 * @param camera_ori_deg The angle between the forward position and the actual position 00062 * of the camera on the robot in degrees, clock-wise positive. 00063 */ 00064 CameraTracker::CameraTracker(RelativePositionModel *relative_position_model, 00065 float camera_height, 00066 float camera_ori_deg 00067 ) 00068 { 00069 rpm = relative_position_model; 00070 mode = MODE_MODEL; 00071 this->camera_height = camera_height; 00072 this->camera_orientation = fawkes::deg2rad( camera_ori_deg ); 00073 } 00074 00075 00076 /** Destructor. */ 00077 CameraTracker::~CameraTracker() 00078 { 00079 } 00080 00081 00082 /** Calculate values. 00083 * Based on the set data like robot position, world point and relative position model 00084 * this calculates the new desired values for pan and tilt. 00085 */ 00086 void 00087 CameraTracker::calc() 00088 { 00089 if (mode == MODE_MODEL) { 00090 new_pan = rpm->get_bearing() - camera_orientation; 00091 new_tilt = rpm->get_slope(); 00092 } else if (mode == MODE_WORLD) { 00093 00094 float w_r_x = world_x - robot_x; 00095 float w_r_y = world_y - robot_y; 00096 00097 float distance = sqrt( w_r_x * w_r_x + w_r_y * w_r_y ); 00098 00099 //cout << msg_prefix << " world_x=" << world_x << " world_y=" << world_y 00100 // << " robot_x=" << robot_x << " robot_y=" << robot_y << endl; 00101 //cout << msg_prefix << " w_r_x=" << w_r_x << " w_r_y=" << w_r_y 00102 // << " dist=" << distance << endl; 00103 00104 /* atan2f magic 00105 * tan alpha = opposite leg / adjacent leg 00106 * => alpha = atan( opposite leg / adjacent leg ) 00107 * 00108 * atan2f now takes y = length(opposite leg) and x = length(adjacent leg) 00109 * and calculates the angle alpha. It's exactle the same as the above 00110 * 00111 * So here we want to calculate the bearing to the world point 00112 * So we have a right triangle, where w_r_y is the length of the adjacent 00113 * leg and w_r_x is the length of the opposite leg. So to calculate the 00114 * bearing / new pan we call atan2f(w_r_x, w_r_y). 00115 * For the new tilt we need the distance. This gives us a right triangle 00116 * with distance being the opposite leg and the height of the camera on 00117 * the robot being the adjacent leg. So slope / new tilt is 00118 * atan2f(distance, camera_height). 00119 */ 00120 00121 // Calculate bearing to point 00122 new_pan = atan2f( w_r_y, w_r_x ); 00123 new_pan = fawkes::normalize_mirror_rad( new_pan - robot_ori - camera_orientation); 00124 new_tilt = atan2f( camera_height, distance ); 00125 } 00126 } 00127 00128 00129 /** Get the new pan value. 00130 * @return new optimal pan value 00131 */ 00132 float 00133 CameraTracker::get_new_pan() 00134 { 00135 return new_pan; 00136 } 00137 00138 00139 /** Get the new tilt value. 00140 * @return new optimal tilt value 00141 */ 00142 float 00143 CameraTracker::get_new_tilt() 00144 { 00145 return new_tilt; 00146 } 00147 00148 00149 /** Set tracking mode. 00150 * @param mode new tracking mode 00151 * @exception Exception thrown, if mode is neither MODE_WORLD nor MODE_MODEL 00152 */ 00153 void 00154 CameraTracker::set_mode(unsigned int mode) 00155 { 00156 if ( (mode == MODE_WORLD) || (mode == MODE_MODEL)) { 00157 this->mode = mode; 00158 } else { 00159 throw fawkes::Exception("CameraTracker: Invalid mode, not setting mode"); 00160 } 00161 } 00162 00163 00164 /** Set relative position model. 00165 * Switch the relative position model. 00166 * @param rpm new relative position model 00167 */ 00168 void 00169 CameraTracker::set_relative_position_model(RelativePositionModel *rpm) 00170 { 00171 this->rpm = rpm; 00172 } 00173 00174 00175 /** Set robot position. 00176 * Set the current robot position. 00177 * @param x new x coordinate in robot system 00178 * @param y new y coordinate in robot system 00179 * @param ori new orientation 00180 */ 00181 void 00182 CameraTracker::set_robot_position(float x, float y, float ori) 00183 { 00184 robot_x = x; 00185 robot_y = y; 00186 robot_ori = ori; 00187 } 00188 00189 00190 /** Set world point. 00191 * World point to track for the robot. The world point is given in a robot-relative 00192 * coordinate system on the ground plane. X-axis is pointing forward, Y-axis to 00193 * the right (right-handed coordinate system). 00194 * @param x x coordinate to track 00195 * @param y y coordinate to track 00196 */ 00197 void 00198 CameraTracker::set_world_point(float x, float y) 00199 { 00200 world_x = x; 00201 world_y = y; 00202 } 00203
