Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
projective_cam.cpp
1 /***************************************************************************
2  * projective_cam.cpp - Abstract class defining a projective camera model
3  *
4  * Created: Thu May 08 15:08:00 2008
5  * Copyright 2008 Christof Rath <c.rath@student.tugraz.at>
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #include "projective_cam.h"
24 
25 #include <geometry/hom_point.h>
26 #include <geometry/vector.h>
27 #include <core/exceptions/software.h>
28 
29 #include <cmath>
30 #include <iostream>
31 
32 using namespace fawkes;
33 using std::cout;
34 using std::endl;
35 
36 namespace firevision {
37 #if 0 /* just to make Emacs auto-indent happy */
38 }
39 #endif
40 
41 /** @class AboveHorizonException <fvmodels/camera/projective_cam.h>
42  * The point that should be calculated lies above the horizon
43  * @ingroup Exceptions
44  */
45 /** Constructor
46  * @param msg message, appended to exception, base message "PostureException"
47  * @param img_pt the point in the image
48  */
49 AboveHorizonException::AboveHorizonException(const char *msg, const center_in_roi_t img_pt) throw()
50  : fawkes::Exception("AboveHorizonException: %s (%0.1f, %0.1f)", msg, img_pt.x, img_pt.y)
51 {
52  __img_pt = img_pt;
53 }
54 
55 /**
56  * @return the point in the image that caused the exception
57  */
58 const center_in_roi_t &
59 AboveHorizonException::get_img_pt() const
60 {
61  return __img_pt;
62 }
63 
64 
65 
66 
67 
68 /** @class ProjectiveCam projective_cam.h <fvmodels/camera/projective_cam.h>
69  * Abstract class for projective cameras
70  * @author Christof Rath
71  */
72 
73 /** Constructor.
74  * @param cal Calibration matrix of the camera
75  * @param loc Location of the camera (= translation + rotation)
76  */
77 ProjectiveCam::ProjectiveCam(const Calibration &cal, const HomTransform *loc) :
78  __cal(cal)
79 {
80  __p = NULL;
81  __gpa_inv = NULL;
82  __gpa_inv_data = new float[9];
83 
84  if (loc) set_location(*loc);
85 }
86 
87 /** Constructor.
88  * @param cal Calibration matrix of the camera
89  * @param roll of the camera
90  * @param pitch of the camera
91  * @param yaw of the camera
92  * @param height of the camera
93  * @param x of the camera (in front if yaw is zero)
94  * @param y of the camera (left if yaw is zero)
95  */
97  float roll, float pitch, float yaw,
98  float height, float x, float y):
99  __cal(cal)
100 {
101  __p = NULL;
102  __gpa_inv = NULL;
103  __gpa_inv_data = new float[9];
104 
105  set_location(roll, pitch, yaw, height, x, y);
106 }
107 
108 /** Copy Constructor
109  * @param pc the ProjectiveCam to copy
110  */
112  __cal(pc.__cal)
113 {
114  __p = (pc.__p != NULL ? new Matrix(*pc.__p) : NULL);
115  __gpa_inv_data = new float[9];
116 
117  if (pc.__gpa_inv) {
118  for (unsigned int i = 0; i < 9; ++i) {
119  __gpa_inv_data[i] = pc.__gpa_inv_data[i];
120  }
121 
122  __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
123  }
124  else __gpa_inv = NULL;
125 }
126 
127 /** Destructor.
128  */
130 {
131  delete __p;
132  delete __gpa_inv;
133  delete[] __gpa_inv_data;
134 }
135 
136 
137 /** Sets a new location for the camera
138  * @param roll of the camera
139  * @param pitch of the camera
140  * @param height of the camera
141  * @param yaw of the camera
142  * @param x of the camera (in front if yaw is zero)
143  * @param y of the camera (left if yaw is zero)
144  * @return a reference to the camera
145  */
147 ProjectiveCam::set_location(float roll, float pitch, float yaw, float height, float x, float y)
148 {
149  HomTransform t;
150 
151 // cout << "roll: " << roll << " pitch: " << pitch << " height: " << height << " yaw: " << yaw << endl;
152 
153  //Transformation of world frame into cam frame [rot_x(-pi/2)+rot_z(-pi/2)]:
154  t.rotate_x(-M_PI_2);
155  t.rotate_z(-M_PI_2);
156 
157  t.rotate_y(pitch);
158  t.rotate_x(-roll);
159 
160  t.trans(-x, y, height);
161  t.rotate_z(yaw);
162 
163  return set_location(t);
164 }
165 
166 /** Sets a new location for the camera
167  * @param loc the new location (remember the transformation from world frame
168  * into cam frame [rot_x(-pi/2)+rot_z(-pi/2)] before the rest of the
169  * transformation)
170  * @return a reference to the camera
171  */
174 {
175  if (__p) {
176  delete __gpa_inv;
177  delete __p;
178  __p = NULL;
179  }
180 
181  __p = new Matrix (__cal * loc.get_matrix().get_submatrix(0, 0, 3, 4));
182 
183  __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
184  __gpa_inv->overlay(0, 0, __p->get_submatrix(0, 0, 3, 2));
185  __gpa_inv->overlay(0, 2, __p->get_submatrix(0, 3, 3, 1));
186  __gpa_inv->invert();
187 
188  return *this;
189 }
190 
191 /** Returns a point in the world under the ground plane assumption.
192  * @param img_p a point in the image (x-px, y-px)
193  * @return a point in the world (x-meters, y-meters)
194  */
197 {
198  Vector img_v(3);
199  img_v.x(img_p.x);
200  img_v.y(img_p.y);
201  img_v.z(1);
202 
203  Vector wld_v = *__gpa_inv * img_v;
204 
205  wld_v /= wld_v.z();
206 
207  if (wld_v.x() < 0) {
208  throw AboveHorizonException("The given point is above the horizon!\n", img_p);
209  }
210 
211  return (fawkes::cart_coord_2d_t){ wld_v.x(), -wld_v.y() };
212 }
213 
214 /** Returns an image point of a world point under the ground plane assumption.
215  * @param wld_p a point on the ground (x-meters, y-meters)
216  * @return a point in the image (x-px, y-px)
217  */
220 {
221  Vector wld_v(4);
222  wld_v.x(wld_p.x);
223  wld_v.y(wld_p.y);
224  wld_v.z(0); //GPA
225  wld_v.set(3, 1);
226 
227  Vector img_v = *__p * wld_v;
228  img_v /= img_v.z();
229 
230  center_in_roi_t res;
231  res.x = img_v.x();
232  res.y = img_v.y();
233 
234  return res;
235 }
236 
237 /**
238  * Calibration getter.
239  * @return the calibration matrix
240  */
243 {
244  return Calibration(__cal);
245 }
246 
247 /**
248  * P matrix getter.
249  * @return the P matrix
250  */
251 Matrix
253 {
254  return Matrix(*__p);
255 }
256 
257 /** Returns the modified P matrix.
258  * With the ground plane assumption the third column can be ignored.
259  * @return modified P matrix
260  */
261 Matrix
263 {
264  Matrix res(3, 3);
265  res.overlay(0, 0, __p->get_submatrix(0, 0, 3, 2)); //first two columns
266  res.overlay(0, 2, __p->get_submatrix(0, 3, 3, 1)); //fourth column
267 
268  return res;
269 }
270 
271 /** Prints the matrix P.
272  * @param name Heading of the output
273  * @param col_sep a string used to separate columns (defaults to '\\t')
274  * @param row_sep a string used to separate rows (defaults to '\\n')
275  */
276 void
277 ProjectiveCam::print_info (const char *name, const char *col_sep, const char *row_sep) const
278 {
279  __p->print_info(name ? name : "Projective Camera", col_sep, row_sep);
280  __cal.print_info("Calibration Matrix", col_sep, row_sep);
281 }
282 
283 } // end namespace firevision
float z() const
Convenience getter to obtain the third element.
Definition: vector.cpp:248
virtual ProjectiveCam & set_location(const fawkes::HomTransform &loc)
Sets a new location for the camera.
Cartesian coordinates (2D).
Definition: types.h:40
Calibration get_cal() const
Calibration getter.
const Matrix & get_matrix() const
Returns a copy of the matrix.
The point that should be calculated lies above the horizon.
void trans(float dx, float dy, float dz=0.0)
Add translation to the transformation.
A simple column vector.
Definition: vector.h:31
A general matrix class.
Definition: matrix.h:33
float x() const
Convenience getter to obtain the first element.
Definition: vector.cpp:192
ProjectiveCam(const ProjectiveCam &pc)
Copy Constructor.
float y
y coordinate
Definition: types.h:42
fawkes::Matrix get_p() const
P matrix getter.
virtual ~ProjectiveCam()
Destructor.
A Calibration matrix for a finite camera.
Definition: calibration.h:35
void print_info(const char *name=0, const char *col_sep=0, const char *row_sep=0) const
Print matrix to standard out.
Definition: matrix.cpp:890
void rotate_z(float rad)
Add rotation around the z-axis.
virtual void print_info(const char *name=0, const char *col_sep=0, const char *row_sep=0) const
Prints the matrix P.
void overlay(unsigned int row, unsigned int col, const Matrix &m)
Overlays another matrix over this matrix.
Definition: matrix.cpp:445
float x
x in pixels
Definition: types.h:40
virtual center_in_roi_t get_GPA_image_coord(const fawkes::cart_coord_2d_t &wld_p) const
Returns an image point of a world point under the ground plane assumption.
This class describes a homogeneous transformation.
Definition: hom_transform.h:31
Abstract class for projective cameras.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual fawkes::cart_coord_2d_t get_GPA_world_coord(const center_in_roi_t &img_p) const
Returns a point in the world under the ground plane assumption.
Matrix & invert()
Inverts the matrix.
Definition: matrix.cpp:317
Matrix get_submatrix(unsigned int row, unsigned int col, unsigned int num_rows, unsigned int num_cols) const
Returns a submatrix of the matrix.
Definition: matrix.cpp:415
void rotate_y(float rad)
Add rotation around the y-axis.
fawkes::Matrix get_GPA_p() const
Returns the modified P matrix.
void rotate_x(float rad)
Add rotation around the x-axis.
Center in ROI.
Definition: types.h:39
float x
x coordinate
Definition: types.h:41
float y
y in pixels
Definition: types.h:41
void set(unsigned int d, float v)
Set a certain element.
Definition: vector.cpp:176
float y() const
Convenience getter to obtain the second element.
Definition: vector.cpp:220