back_projection.cpp

00001 /***************************************************************************
00002  *  back_projection.cpp - Projective camera back projection model
00003  *
00004  *  Created:  Mon Apr 20 21:59:00 2009
00005  *  Copyright 2009 Christof Rath <christof.rath@gmail.com>
00006  *
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include <models/relative_position/back_projection.h>
00023 
00024 #include <cmath>
00025 
00026 namespace firevision {
00027 #if 0 /* just to make Emacs auto-indent happy */
00028 }
00029 #endif
00030 
00031 /** @class BackProjectionPositionModel <models/relative_position/back_projection.h>
00032  * This model uses a ProjectiveCam to back project points in the image to
00033  * the world by the ground plane assumption.
00034  *
00035  * @author Christof Rath
00036  */
00037 
00038 /** Constructor
00039  * @param projective_cam the projective camera object
00040  * @param ball_circumference the circumference in case it's a ball
00041  */
00042 BackProjectionPositionModel::BackProjectionPositionModel(ProjectiveCam &projective_cam,
00043                                                          float ball_circumference):
00044   __projective_cam(projective_cam),
00045   __ball_circumference(ball_circumference)
00046 {
00047   __ball_radius   = __ball_circumference / ( 2.0 * M_PI );
00048 
00049   __world_x        = __world_y = __bearing = __slope = __distance = 0.f;
00050   __cirt_center.x = __cirt_center.y = 0.0f;
00051   __pos_valid     = false;
00052 }
00053 
00054 
00055 void
00056 BackProjectionPositionModel::set_radius(float r)
00057 {
00058   __ball_radius = r;
00059 }
00060 
00061 
00062 void
00063 BackProjectionPositionModel::set_center(float x, float y)
00064 {
00065   __cirt_center.x = x;
00066   __cirt_center.y = y;
00067 }
00068 
00069 
00070 void
00071 BackProjectionPositionModel::set_cam_rotation(float pan, float tilt, float roll)
00072 {
00073   __cam_position.roll  = roll;
00074   __cam_position.pitch = -tilt; //tilt down-wards negative, cam pitch down-wards positive
00075   __cam_position.yaw   = pan;
00076 }
00077 
00078 void BackProjectionPositionModel::get_pan_tilt(float *pan, float *tilt) const
00079 {
00080   float roll;
00081   get_cam_rotation(*pan, *tilt, roll);
00082 }
00083 
00084 void BackProjectionPositionModel::get_cam_rotation(float &pan, float &tilt, float &roll) const
00085 {
00086   pan  = __cam_position.yaw;
00087   tilt = -__cam_position.pitch; //tilt down-wards negative, cam pitch down-wards positive
00088   roll = __cam_position.roll;
00089 }
00090 
00091 void BackProjectionPositionModel::set_cam_translation(float height, float rel_x, float rel_y)
00092 {
00093   __cam_position.x = rel_x;
00094   __cam_position.y = rel_y;
00095   __cam_position.z = height;
00096 }
00097 
00098 void BackProjectionPositionModel::get_cam_translation(float & height, float & rel_x, float & rel_y) const
00099 {
00100   height = __cam_position.z;
00101   rel_x  = __cam_position.x;
00102   rel_y  = __cam_position.y;
00103 }
00104 
00105 
00106 
00107 void BackProjectionPositionModel::calc()
00108 {
00109   __projective_cam.set_location(
00110       __cam_position.roll,
00111       __cam_position.pitch,
00112       __cam_position.yaw,
00113       __cam_position.z,
00114       __cam_position.x,
00115       __cam_position.y);
00116 
00117   try {
00118     fawkes::cart_coord_2d_t wld_pt = __projective_cam.get_GPA_world_coord(__cirt_center);
00119 
00120     __world_x   = wld_pt.x;
00121     __world_y   = wld_pt.y;
00122     __pos_valid = true;
00123   }
00124   catch (AboveHorizonException &e) {
00125     __world_x   = 0.f;
00126     __world_y   = 0.f;
00127     __pos_valid = false;
00128   }
00129 
00130 #ifdef OLD_COORD_SYS
00131   /* Bearing shall be clockwise positive. */
00132   __bearing = -atan2f(_ball_y, __world_x);
00133 #else
00134   __bearing = atan2f(__world_y, __world_x);
00135 #endif
00136 
00137   __distance = sqrt(__world_x * __world_x + __world_y * __world_y);
00138   __slope    = -atan2f(__cam_position.z, __distance); //slope is down-wards negative
00139 }
00140 
00141 void BackProjectionPositionModel::reset()
00142 {
00143   __pos_valid = false;
00144 }
00145 
00146 } // end namespace firevision

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1