00001 /***************************************************************************
00002  *  projective_cam.cpp - Abstract class defining a projective camera model
00003  *
00004  *  Generated: Thu May 8 15:08 2008
00005  *  Copyright  2008  Christof Rath <c.rath@student.tugraz.at>
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. A runtime exception applies to
00013  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00021  */
00022
00023 #include "projective_cam.h"
00024
00025 #include <geometry/hom_point.h>
00026 #include <geometry/vector.h>
00027 #include <core/exceptions/software.h>
00028
00029 #include <cmath>
00030 #include <iostream>
00031
00032 using namespace fawkes;
00033 using std::cout;
00034 using std::endl;
00035
00036
00037 
00038 /** @class AboveHorizonException projective_cam.h <models/camera/projective_cam.h>
00039  * The point that should be calculated lies above the horizon
00040  * @ingroup Exceptions
00041  */
00042 /** Constructor
00043  * @param msg message, appended to exception, base message "PostureException"
00044  * @param img_pt the point in the image
00045  */
00046 AboveHorizonException::AboveHorizonException(const char *msg, const center_in_roi_t img_pt) throw()
00047   : fawkes::Exception("AboveHorizonException: %s (%0.1f, %0.1f)", msg, img_pt.x, img_pt.y)
00048 {
00049   __img_pt = img_pt;
00050 }
00051 
00052 /**
00053  * @return the point in the image that caused the exception
00054  */
00055 const center_in_roi_t &
00056 AboveHorizonException::get_img_pt() const
00057 {
00058   return __img_pt;
00059 }
00060
00061
00062
00063
00064 
00065 /** @class ProjectiveCam projective_cam.h <models/camera/projective_cam.h>
00066  * Abstract class for projective cameras
00067  * @author Christof Rath
00068  */
00069 
00070 /** Constructor.
00071  * @param cal Calibration matrix of the camera
00072  * @param loc Location of the camera (= translation + rotation)
00073  */
00074 ProjectiveCam::ProjectiveCam(const Calibration &cal, const HomTransform *loc) :
00075   __cal(cal)
00076 {
00077   __p       = NULL;
00078   __gpa_inv = NULL;
00079   __gpa_inv_data = new float[9];
00080
00081   if (loc) set_location(*loc);
00082 }
00083 
00084 /** Constructor.
00085  * @param cal Calibration matrix of the camera
00086  * @param roll of the camera
00087  * @param pitch of the camera
00088  * @param yaw of the camera
00089  * @param height of the camera
00090  * @param x of the camera (in front if yaw is zero)
00091  * @param y of the camera (left if yaw is zero)
00092  */
00093 ProjectiveCam::ProjectiveCam(const Calibration &cal,
00094                              float roll, float pitch, float yaw,
00095                              float height, float x, float y):
00096   __cal(cal)
00097 {
00098   __p            = NULL;
00099   __gpa_inv      = NULL;
00100   __gpa_inv_data = new float[9];
00101
00102   set_location(roll, pitch, yaw, height, x, y);
00103 }
00104 
00105 /** Copy Constructor
00106  * @param pc the ProjectiveCam to copy
00107  */
00108 ProjectiveCam::ProjectiveCam(const ProjectiveCam &pc):
00109   __cal(pc.__cal)
00110 {
00111   __p            = (pc.__p != NULL ? new Matrix(*pc.__p) : NULL);
00112   __gpa_inv_data = new float[9];
00113
00114   if (pc.__gpa_inv) {
00115     for (unsigned int i = 0; i < 9; ++i) {
00116       __gpa_inv_data[i] = pc.__gpa_inv_data[i];
00117     }
00118
00119     __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
00120   }
00121   else __gpa_inv = NULL;
00122 }
00123 
00124 /** Destructor.
00125  */
00126 ProjectiveCam::~ProjectiveCam()
00127 {
00128   delete   __p;
00129   delete   __gpa_inv;
00130   delete[] __gpa_inv_data;
00131 }
00132
00133 
00134 /** Sets a new location for the camera
00135  * @param roll of the camera
00136  * @param pitch of the camera
00137  * @param height of the camera
00138  * @param yaw of the camera
00139  * @param x of the camera (in front if yaw is zero)
00140  * @param y of the camera (left if yaw is zero)
00141  * @return a reference to the camera
00142  */
00143 ProjectiveCam&
00144 ProjectiveCam::set_location(float roll, float pitch, float yaw, float height, float x, float y)
00145 {
00146   HomTransform t;
00147
00148 //  cout << "roll: " << roll << " pitch: " << pitch << " height: " << height << " yaw: " << yaw << endl;
00149
00150   //Transformation of world frame into cam frame [rot_x(-pi/2)+rot_z(-pi/2)]:
00151   t.rotate_x(-M_PI_2);
00152   t.rotate_z(-M_PI_2);
00153
00154   t.rotate_y(pitch);
00155   t.rotate_x(-roll);
00156
00157   t.trans(-x, y, height);
00158   t.rotate_z(yaw);
00159
00160   return set_location(t);
00161 }
00162 
00163 /** Sets a new location for the camera
00164  * @param loc the new location (remember the transformation from world frame
00165  *            into cam frame [rot_x(-pi/2)+rot_z(-pi/2)] before the rest of the
00166  *            transformation)
00167  * @return a reference to the camera
00168  */
00169 ProjectiveCam&
00170 ProjectiveCam::set_location(const HomTransform& loc)
00171 {
00172   if (__p) {
00173     delete __gpa_inv;
00174     delete __p;
00175     __p = NULL;
00176   }
00177
00178   __p = new Matrix (__cal * loc.get_matrix().get_submatrix(0, 0, 3, 4));
00179
00180   __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
00181   __gpa_inv->overlay(0, 0, __p->get_submatrix(0, 0, 3, 2));
00182   __gpa_inv->overlay(0, 2, __p->get_submatrix(0, 3, 3, 1));
00183   __gpa_inv->invert();
00184
00185   return *this;
00186 }
00187 
00188 /** Returns a point in the world under the ground plane assumption.
00189  * @param img_p a point in the image (x-px, y-px)
00190  * @return a point in the world (x-meters, y-meters)
00191  */
00192 fawkes::cart_coord_2d_t
00193 ProjectiveCam::get_GPA_world_coord(const center_in_roi_t &img_p) const
00194 {
00195   Vector img_v(3);
00196   img_v.x(img_p.x);
00197   img_v.y(img_p.y);
00198   img_v.z(1);
00199
00200   Vector wld_v = *__gpa_inv * img_v;
00201
00202   wld_v /= wld_v.z();
00203
00204   if (wld_v.x() < 0) {
00205     throw AboveHorizonException("The given point is above the horizon!\n", img_p);
00206   }
00207
00208   return (fawkes::cart_coord_2d_t){ wld_v.x(), -wld_v.y() };
00209 }
00210 
00211 /** Returns an image point of a world point under the ground plane assumption.
00212  * @param wld_p a point on the ground (x-meters, y-meters)
00213  * @return a point in the image (x-px, y-px)
00214  */
00215 center_in_roi_t
00216 ProjectiveCam::get_GPA_image_coord(const fawkes::cart_coord_2d_t &wld_p) const
00217 {
00218   Vector wld_v(4);
00219   wld_v.x(wld_p.x);
00220   wld_v.y(wld_p.y);
00221   wld_v.z(0); //GPA
00222   wld_v.set(3, 1);
00223
00224   Vector img_v = *__p * wld_v;
00225   img_v /= img_v.z();
00226
00227   center_in_roi_t res;
00228   res.x = img_v.x();
00229   res.y = img_v.y();
00230
00231   return res;
00232 }
00233 
00234 /**
00235  * Calibration getter.
00236  * @return the calibration matrix
00237  */
00238 Calibration
00239 ProjectiveCam::get_cal() const
00240 {
00241   return Calibration(__cal);
00242 }
00243 
00244 /**
00245  * P matrix getter.
00246  * @return the P matrix
00247  */
00248 Matrix
00249 ProjectiveCam::get_p() const
00250 {
00251   return Matrix(*__p);
00252 }
00253 
00254 /** Returns the modified P matrix.
00255  * With the ground plane assumption the third column can be ignored.
00256  */
00257 Matrix
00258 ProjectiveCam::get_GPA_p() const
00259 {
00260   Matrix res(3, 3);
00261   res.overlay(0, 0, __p->get_submatrix(0, 0, 3, 2)); //first two columns
00262   res.overlay(0, 2, __p->get_submatrix(0, 3, 3, 1)); //fourth column
00263
00264   return res;
00265 }
00266 
00267 /** Prints the matrix P.
00268  * @param name Heading of the output
00269  * @param col_sep a string used to separate columns (defaults to '\\t')
00270  * @param row_sep a string used to separate rows (defaults to '\\n')
00271  */
00272 void
00273 ProjectiveCam::print_info (const char *name, const char *col_sep, const char *row_sep) const
00274 {
00275   __p->print_info(name ? name : "Projective Camera", col_sep, row_sep);
00276   __cal.print_info("Calibration Matrix", col_sep, row_sep);
00277 }
00278