Fawkes API  Fawkes Development Version
position_to_pixel.cpp
1 /***************************************************************************
2  * globfromrel.cpp - Implementation of the global ball position model
3  *
4  * Created: Do Apr 03 16:45:22 2014
5  * Copyright 2014 Tobias Neumann
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 "position_to_pixel.h"
24 
25 #include <utils/math/coord.h>
26 #include <core/exceptions/software.h>
27 
28 namespace firevision {
29 /**
30  * @class PositionToPixel
31  * Compute a pixel position in the camera image from a cartesian world coordinate.
32  */
33 
34 /**
35  * Construct a PositionToPixel model with the required camera geometry
36  * @param tf The transform listener used by the calling code
37  * @param cam_frame Reference frame of the camera coordinate system
38  * @param cam_aperture_x Horizontal opening angle (rad)
39  * @param cam_aperture_y Vertical opening angle (rad)
40  * @param cam_width_x Horizontal pixel resolution
41  * @param cam_height_y Vertical pixel resolution
42  * @param cam_angle_y Vertical camera mounting angle
43  */
45  float cam_aperture_x, float cam_aperture_y,
46  unsigned int cam_width_x, unsigned int cam_height_y, float cam_angle_y )
47 {
48  tf_listener = tf;
49  cam_frame_ = cam_frame;
50 
51  cam_aperture_horizontal_ = cam_aperture_x;
52  cam_aperture_vertical_ = cam_aperture_y;
53  cam_resolution_x_ = cam_width_x;
54  cam_resolution_y_ = cam_height_y;
55 
56  cam_pixel_per_angle_horizontal_ = double(cam_resolution_x_) / cam_aperture_horizontal_;
57  cam_pixel_per_angle_vertical_ = double(cam_resolution_y_) / cam_aperture_vertical_;
58 
59  cam_angle_max_horizontal_ = cam_aperture_horizontal_ / 2.0;
60  cam_angle_min_horizontal_ = -1.0 * cam_angle_max_horizontal_;
61 
62  cam_angle_y_ = cam_angle_y;
63  cam_angle_max_vertical_ = cam_angle_y + cam_aperture_vertical_ / 2.0;
64  cam_angle_min_vertical_ = cam_angle_y - cam_aperture_vertical_ / 2.0;
65 }
66 
67 
68 /**
69  * @param position the 3dimential position (x, y, z) in the given frame
70  * @param frame the frame where the data are in
71  * @param time the timestamp of position
72  * @return the pixel in the camera (may be negative or > resolution!)
73  *
74  * @throws OutOfBoundsException
75  * ConnectivityException
76  * ExtrapolationException
77  * LookupException
78  */
81 {
82  fawkes::point_t pixel_in_cam_unchecked;
83 
85  coord_in.frame_id = frame;
86  coord_in.stamp = time;
87  coord_in.setX( position.x );
88  coord_in.setY( position.y );
89  coord_in.setZ( position.z );
90  fawkes::tf::Stamped<fawkes::tf::Point> coord_in_transformed;
91 
92  //transform frame to cam_frame_ at time
93  tf_listener->transform_point(cam_frame_, coord_in, coord_in_transformed);
94 
95  //calculate into polar
96  fawkes::polar_coord_3d_t polar_in;
97  fawkes::cart2polar3d( coord_in_transformed.getX(), coord_in_transformed.getY(), coord_in_transformed.getZ(),
98  (polar_in.phi), (polar_in.theta), (polar_in.r) );
99 
100  //calculate into pixel
101  fawkes::point_t pixel_in_cam_rel;
102  pixel_in_cam_rel.x = -1.0 * polar_in.phi * cam_pixel_per_angle_horizontal_;
103  pixel_in_cam_rel.y = (polar_in.theta - cam_angle_y_) * cam_pixel_per_angle_vertical_;
104 
105  pixel_in_cam_unchecked.x = pixel_in_cam_rel.x + ( cam_resolution_x_ / 2 );
106  pixel_in_cam_unchecked.y = pixel_in_cam_rel.y + ( cam_resolution_y_ / 2 );
107 
108  return pixel_in_cam_unchecked;
109 }
110 
111 
112 /**
113  * @param position the 3dimential position (x, y, z) in the given frame
114  * @param frame the frame where the data are in
115  * @param time the timestamp of position
116  * @return the pixel in the camera
117  *
118  * @throws OutOfBoundsException
119  * ConnectivityException
120  * ExtrapolationException
121  * LookupException
122  */
125 {
126  fawkes::point_t pixel_unchecked = get_pixel_position_unchecked(position, frame, time);
127 
128  if (pixel_unchecked.x < 0 || pixel_unchecked.x > (long int)cam_resolution_x_) {
129  throw fawkes::OutOfBoundsException("horizontal position outside cam viewport.",pixel_unchecked.x,
130  0, cam_resolution_x_);
131  } else if (pixel_unchecked.y < 0 || pixel_unchecked.y > (long int)cam_resolution_y_) {
132  throw fawkes::OutOfBoundsException("vertical position outside cam viewport.", pixel_unchecked.y,
133  0, cam_resolution_y_);
134  }
135 
136  fawkes::upoint_t rv;
137  rv.x = (unsigned int) pixel_unchecked.x;
138  rv.y = (unsigned int) pixel_unchecked.y;
139  return rv;
140 }
141 
142 }
Polar coordinates.
Definition: types.h:91
fawkes::point_t get_pixel_position_unchecked(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
std::string frame_id
The frame_id associated this data.
Definition: types.h:136
float phi
x-y : plane
Definition: types.h:93
float x
x coordinate
Definition: types.h:79
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
float y
y coordinate
Definition: types.h:80
unsigned int y
y coordinate
Definition: types.h:36
unsigned int x
x coordinate
Definition: types.h:35
A class for handling time.
Definition: time.h:91
float theta
plane-z : space
Definition: types.h:94
float r
distance
Definition: types.h:92
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:135
float z
z coordinate
Definition: types.h:81
int y
y coordinate
Definition: types.h:42
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
Cartesian coordinates (3D).
Definition: types.h:78
Point with cartesian coordinates as signed integers.
Definition: types.h:40
void cart2polar3d(float cart_x, float cart_y, float cart_z, float &polar_phi, float &polar_theta, float &polar_r)
Convert a 3D cartesian coordinate (x, y, z) to a 3D polar coordinate.
Definition: coord.h:55
fawkes::upoint_t get_pixel_position(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
Index out of bounds.
Definition: software.h:88
Coordinate transforms between any two frames in a system.
Definition: transformer.h:68
PositionToPixel(fawkes::tf::Transformer *tf, std::string cam_frame, float cam_aperture_x, float cam_aperture_y, unsigned int cam_width_x, unsigned int cam_height_y, float cam_angle_y=0)
Construct a PositionToPixel model with the required camera geometry.
int x
x coordinate
Definition: types.h:41