tracker.cpp

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