Fawkes API
Fawkes Development Version
|
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