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 <core/exceptions/software.h>
26 #include <utils/math/coord.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  std::string cam_frame,
46  float cam_aperture_x,
47  float cam_aperture_y,
48  unsigned int cam_width_x,
49  unsigned int cam_height_y,
50  float cam_angle_y)
51 {
52  tf_listener = tf;
53  cam_frame_ = cam_frame;
54 
55  cam_aperture_horizontal_ = cam_aperture_x;
56  cam_aperture_vertical_ = cam_aperture_y;
57  cam_resolution_x_ = cam_width_x;
58  cam_resolution_y_ = cam_height_y;
59 
60  cam_pixel_per_angle_horizontal_ = double(cam_resolution_x_) / cam_aperture_horizontal_;
61  cam_pixel_per_angle_vertical_ = double(cam_resolution_y_) / cam_aperture_vertical_;
62 
63  cam_angle_max_horizontal_ = cam_aperture_horizontal_ / 2.0;
64  cam_angle_min_horizontal_ = -1.0 * cam_angle_max_horizontal_;
65 
66  cam_angle_y_ = cam_angle_y;
67  cam_angle_max_vertical_ = cam_angle_y + cam_aperture_vertical_ / 2.0;
68  cam_angle_min_vertical_ = cam_angle_y - cam_aperture_vertical_ / 2.0;
69 }
70 
71 /**
72  * @param position the 3dimential position (x, y, z) in the given frame
73  * @param frame the frame where the data are in
74  * @param time the timestamp of position
75  * @return the pixel in the camera (may be negative or > resolution!)
76  *
77  * @throws OutOfBoundsException
78  * ConnectivityException
79  * ExtrapolationException
80  * LookupException
81  */
84  std::string & frame,
85  const fawkes::Time & time)
86 {
87  fawkes::point_t pixel_in_cam_unchecked;
88 
90  coord_in.frame_id = frame;
91  coord_in.stamp = time;
92  coord_in.setX(position.x);
93  coord_in.setY(position.y);
94  coord_in.setZ(position.z);
95  fawkes::tf::Stamped<fawkes::tf::Point> coord_in_transformed;
96 
97  //transform frame to cam_frame_ at time
98  tf_listener->transform_point(cam_frame_, coord_in, coord_in_transformed);
99 
100  //calculate into polar
101  fawkes::polar_coord_3d_t polar_in;
102  fawkes::cart2polar3d(coord_in_transformed.getX(),
103  coord_in_transformed.getY(),
104  coord_in_transformed.getZ(),
105  (polar_in.phi),
106  (polar_in.theta),
107  (polar_in.r));
108 
109  //calculate into pixel
110  fawkes::point_t pixel_in_cam_rel;
111  pixel_in_cam_rel.x = -1.0 * polar_in.phi * cam_pixel_per_angle_horizontal_;
112  pixel_in_cam_rel.y = (polar_in.theta - cam_angle_y_) * cam_pixel_per_angle_vertical_;
113 
114  pixel_in_cam_unchecked.x = pixel_in_cam_rel.x + (cam_resolution_x_ / 2);
115  pixel_in_cam_unchecked.y = pixel_in_cam_rel.y + (cam_resolution_y_ / 2);
116 
117  return pixel_in_cam_unchecked;
118 }
119 
120 /**
121  * @param position the 3dimential position (x, y, z) in the given frame
122  * @param frame the frame where the data are in
123  * @param time the timestamp of position
124  * @return the pixel in the camera
125  *
126  * @throws OutOfBoundsException
127  * ConnectivityException
128  * ExtrapolationException
129  * LookupException
130  */
133  std::string & frame,
134  const fawkes::Time & time)
135 {
136  fawkes::point_t pixel_unchecked = get_pixel_position_unchecked(position, frame, time);
137 
138  if (pixel_unchecked.x < 0 || pixel_unchecked.x > (long int)cam_resolution_x_) {
139  throw fawkes::OutOfBoundsException("horizontal position outside cam viewport.",
140  pixel_unchecked.x,
141  0,
142  cam_resolution_x_);
143  } else if (pixel_unchecked.y < 0 || pixel_unchecked.y > (long int)cam_resolution_y_) {
144  throw fawkes::OutOfBoundsException("vertical position outside cam viewport.",
145  pixel_unchecked.y,
146  0,
147  cam_resolution_y_);
148  }
149 
150  fawkes::upoint_t rv;
151  rv.x = (unsigned int)pixel_unchecked.x;
152  rv.y = (unsigned int)pixel_unchecked.y;
153  return rv;
154 }
155 
156 } // namespace firevision
fawkes::cart_coord_3d_t::y
float y
y coordinate
Definition: types.h:89
fawkes::point_struct
Point with cartesian coordinates as signed integers.
Definition: types.h:40
fawkes::polar_coord_3d_t::phi
float phi
x-y : plane
Definition: types.h:104
fawkes::cart2polar3d
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:57
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:33
fawkes::tf::Transformer
Definition: transformer.h:71
fawkes::cart_coord_3d_t
Cartesian coordinates (3D).
Definition: types.h:86
fawkes::polar_coord_3d_t::theta
float theta
plane-z : space
Definition: types.h:105
fawkes::tf::Stamped::stamp
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:139
fawkes::tf::Stamped< fawkes::tf::Point >
fawkes::OutOfBoundsException
Definition: software.h:89
firevision::PositionToPixel::get_pixel_position
fawkes::upoint_t get_pixel_position(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
Definition: position_to_pixel.cpp:137
firevision::PositionToPixel::PositionToPixel
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.
Definition: position_to_pixel.cpp:49
fawkes::cart_coord_3d_t::z
float z
z coordinate
Definition: types.h:90
fawkes::upoint_t::y
unsigned int y
y coordinate
Definition: types.h:36
fawkes::polar_coord_3d_t::r
float r
distance
Definition: types.h:103
fawkes::Time
Definition: time.h:96
fawkes::point_struct::y
int y
y coordinate
Definition: types.h:43
fawkes::polar_coord_3d_t
Polar coordinates.
Definition: types.h:101
fawkes::tf::Transformer::transform_point
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.
Definition: transformer.cpp:417
fawkes::upoint_t::x
unsigned int x
x coordinate
Definition: types.h:35
fawkes::point_struct::x
int x
x coordinate
Definition: types.h:42
fawkes::tf::Stamped::frame_id
std::string frame_id
The frame_id associated this data.
Definition: types.h:140
fawkes::cart_coord_3d_t::x
float x
x coordinate
Definition: types.h:88
firevision::PositionToPixel::get_pixel_position_unchecked
fawkes::point_t get_pixel_position_unchecked(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
Definition: position_to_pixel.cpp:88