Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * camera_tracker.cpp - Implementation of the camera tracker 00004 * 00005 * Created: Thu Jul 14 22:18:14 2005 00006 * Copyright 2005-2009 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 namespace firevision { 00035 #if 0 /* just to make Emacs auto-indent happy */ 00036 } 00037 #endif 00038 00039 /** @class CameraTracker <fvutils/camera/tracker.h> 00040 * Camera Tracker. 00041 * Utility class that allows for tracking and object or a world point 00042 * by using a camera pan/tilt unit. It is NOT meant to track an object 00043 * in a scene! 00044 * 00045 * The camera tracker will try to keep the desired object or point in the middle 00046 * of the image. Given a relative position model or a world point and robot pose 00047 * information and initial information the camera tracker returns pan/tilt angles 00048 * that are required to have the object in the center of the image. The using 00049 * application can then fulfill this desired angles if this lies within the 00050 * physical constraints of the pan/tilt unit. 00051 * 00052 * @author Tim Niemueller 00053 */ 00054 00055 /** Model mode, track by a relative world model. */ 00056 const unsigned int CameraTracker::MODE_MODEL = 0; 00057 /** World point mode, track a world point */ 00058 const unsigned int CameraTracker::MODE_WORLD = 1; 00059 00060 00061 /** Constructor. 00062 * @param relative_position_model Relative position model to use if in model tracking 00063 * mode. 00064 * @param camera_height height above ground of the camera, objects are assumed to lie 00065 * on the ground plane. 00066 * @param camera_ori_deg The angle between the forward position and the actual position 00067 * of the camera on the robot in degrees, clock-wise positive. 00068 */ 00069 CameraTracker::CameraTracker(RelativePositionModel *relative_position_model, 00070 float camera_height, 00071 float camera_ori_deg 00072 ) 00073 { 00074 rpm = relative_position_model; 00075 mode = MODE_MODEL; 00076 this->camera_height = camera_height; 00077 this->camera_orientation = fawkes::deg2rad( camera_ori_deg ); 00078 } 00079 00080 00081 /** Destructor. */ 00082 CameraTracker::~CameraTracker() 00083 { 00084 } 00085 00086 00087 /** Calculate values. 00088 * Based on the set data like robot position, world point and relative position model 00089 * this calculates the new desired values for pan and tilt. 00090 */ 00091 void 00092 CameraTracker::calc() 00093 { 00094 if (mode == MODE_MODEL) { 00095 new_pan = rpm->get_bearing() - camera_orientation; 00096 new_tilt = rpm->get_slope(); 00097 } else if (mode == MODE_WORLD) { 00098 00099 float w_r_x = world_x - robot_x; 00100 float w_r_y = world_y - robot_y; 00101 00102 float distance = sqrt( w_r_x * w_r_x + w_r_y * w_r_y ); 00103 00104 //cout << msg_prefix << " world_x=" << world_x << " world_y=" << world_y 00105 // << " robot_x=" << robot_x << " robot_y=" << robot_y << endl; 00106 //cout << msg_prefix << " w_r_x=" << w_r_x << " w_r_y=" << w_r_y 00107 // << " dist=" << distance << endl; 00108 00109 /* atan2f magic 00110 * tan alpha = opposite leg / adjacent leg 00111 * => alpha = atan( opposite leg / adjacent leg ) 00112 * 00113 * atan2f now takes y = length(opposite leg) and x = length(adjacent leg) 00114 * and calculates the angle alpha. It's exactle the same as the above 00115 * 00116 * So here we want to calculate the bearing to the world point 00117 * So we have a right triangle, where w_r_y is the length of the adjacent 00118 * leg and w_r_x is the length of the opposite leg. So to calculate the 00119 * bearing / new pan we call atan2f(w_r_x, w_r_y). 00120 * For the new tilt we need the distance. This gives us a right triangle 00121 * with distance being the opposite leg and the height of the camera on 00122 * the robot being the adjacent leg. So slope / new tilt is 00123 * atan2f(distance, camera_height). 00124 */ 00125 00126 // Calculate bearing to point 00127 new_pan = atan2f( w_r_y, w_r_x ); 00128 new_pan = fawkes::normalize_mirror_rad( new_pan - robot_ori - camera_orientation); 00129 new_tilt = atan2f( camera_height, distance ); 00130 } 00131 } 00132 00133 00134 /** Get the new pan value. 00135 * @return new optimal pan value 00136 */ 00137 float 00138 CameraTracker::get_new_pan() 00139 { 00140 return new_pan; 00141 } 00142 00143 00144 /** Get the new tilt value. 00145 * @return new optimal tilt value 00146 */ 00147 float 00148 CameraTracker::get_new_tilt() 00149 { 00150 return new_tilt; 00151 } 00152 00153 00154 /** Set tracking mode. 00155 * @param mode new tracking mode 00156 * @exception Exception thrown, if mode is neither MODE_WORLD nor MODE_MODEL 00157 */ 00158 void 00159 CameraTracker::set_mode(unsigned int mode) 00160 { 00161 if ( (mode == MODE_WORLD) || (mode == MODE_MODEL)) { 00162 this->mode = mode; 00163 } else { 00164 throw fawkes::Exception("CameraTracker: Invalid mode, not setting mode"); 00165 } 00166 } 00167 00168 00169 /** Set relative position model. 00170 * Switch the relative position model. 00171 * @param rpm new relative position model 00172 */ 00173 void 00174 CameraTracker::set_relative_position_model(RelativePositionModel *rpm) 00175 { 00176 this->rpm = rpm; 00177 } 00178 00179 00180 /** Set robot position. 00181 * Set the current robot position. 00182 * @param x new x coordinate in robot system 00183 * @param y new y coordinate in robot system 00184 * @param ori new orientation 00185 */ 00186 void 00187 CameraTracker::set_robot_position(float x, float y, float ori) 00188 { 00189 robot_x = x; 00190 robot_y = y; 00191 robot_ori = ori; 00192 } 00193 00194 00195 /** Set world point. 00196 * World point to track for the robot. The world point is given in a robot-relative 00197 * coordinate system on the ground plane. X-axis is pointing forward, Y-axis to 00198 * the right (right-handed coordinate system). 00199 * @param x x coordinate to track 00200 * @param y y coordinate to track 00201 */ 00202 void 00203 CameraTracker::set_world_point(float x, float y) 00204 { 00205 world_x = x; 00206 world_y = y; 00207 } 00208 00209 00210 } // end namespace firevision