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 "back_projection.h"
00023
00024 #include <cmath>
00025
00026 
00027 /** @class BackProjectionPositionModel back_projection.h
00028  * This model uses a ProjectiveCam to back project points in the image to
00029  * the world by the ground plane assumption
00030  *
00031  * @author Christof Rath
00032  */
00033 
00034 /** Constructor
00035  * @param projective_cam the projective camera object
00036  * @param ball_circumference the circumference in case it's a ball
00037  */
00038 BackProjectionPositionModel::BackProjectionPositionModel(ProjectiveCam &projective_cam,
00039                                                          float ball_circumference):
00040   __projective_cam(projective_cam),
00041   __ball_circumference(ball_circumference)
00042 {
00043   __ball_radius   = __ball_circumference / ( 2.0 * M_PI );
00044
00045   __world_x        = __world_y = __bearing = __slope = __distance = 0.f;
00046   __cirt_center.x = __cirt_center.y = 0.0f;
00047   __pos_valid     = false;
00048 }
00049
00050
00051 void
00052 BackProjectionPositionModel::set_radius(float r)
00053 {
00054   __ball_radius = r;
00055 }
00056
00057
00058 void
00059 BackProjectionPositionModel::set_center(float x, float y)
00060 {
00061   __cirt_center.x = x;
00062   __cirt_center.y = y;
00063 }
00064
00065
00066 void
00067 BackProjectionPositionModel::set_cam_rotation(float pan, float tilt, float roll)
00068 {
00069   __cam_position.roll  = roll;
00070   __cam_position.pitch = -tilt; //tilt down-wards negative, cam pitch down-wards positive
00071   __cam_position.yaw   = pan;
00072 }
00073
00074 void BackProjectionPositionModel::get_pan_tilt(float *pan, float *tilt) const
00075 {
00076   float roll;
00077   get_cam_rotation(*pan, *tilt, roll);
00078 }
00079
00080 void BackProjectionPositionModel::get_cam_rotation(float &pan, float &tilt, float &roll) const
00081 {
00082   pan  = __cam_position.yaw;
00083   tilt = -__cam_position.pitch; //tilt down-wards negative, cam pitch down-wards positive
00084   roll = __cam_position.roll;
00085 }
00086
00087 void BackProjectionPositionModel::set_cam_translation(float height, float rel_x, float rel_y)
00088 {
00089   __cam_position.x = rel_x;
00090   __cam_position.y = rel_y;
00091   __cam_position.z = height;
00092 }
00093
00094 void BackProjectionPositionModel::get_cam_translation(float & height, float & rel_x, float & rel_y) const
00095 {
00096   height = __cam_position.z;
00097   rel_x  = __cam_position.x;
00098   rel_y  = __cam_position.y;
00099 }
00100
00101
00102
00103 void BackProjectionPositionModel::calc()
00104 {
00105   __projective_cam.set_location(
00106       __cam_position.roll,
00107       __cam_position.pitch,
00108       __cam_position.yaw,
00109       __cam_position.z,
00110       __cam_position.x,
00111       __cam_position.y);
00112
00113   try {
00114     fawkes::cart_coord_2d_t wld_pt = __projective_cam.get_GPA_world_coord(__cirt_center);
00115
00116     __world_x   = wld_pt.x;
00117     __world_y   = wld_pt.y;
00118     __pos_valid = true;
00119   }
00120   catch (AboveHorizonException &e) {
00121     __world_x   = 0.f;
00122     __world_y   = 0.f;
00123     __pos_valid = false;
00124   }
00125
00126 #ifdef OLD_COORD_SYS
00127   /* Bearing shall be clockwise positive. */
00128   __bearing = -atan2f(_ball_y, __world_x);
00129 #else
00130   __bearing = atan2f(__world_y, __world_x);
00131 #endif
00132 
00133   __distance = sqrt(__world_x * __world_x + __world_y * __world_y);
00134   __slope    = -atan2f(__cam_position.z, __distance); //slope is down-wards negative
00135 }
00136
00137 void BackProjectionPositionModel::reset()
00138 {
00139   __pos_valid = false;
00140 }