projective_cam.cpp

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

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7