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
28namespace 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
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
151 rv.x = (unsigned int)pixel_unchecked.x;
152 rv.y = (unsigned int)pixel_unchecked.y;
153 return rv;
154}
155
156} // namespace firevision
Index out of bounds.
Definition: software.h:86
A class for handling time.
Definition: time.h:93
std::string frame_id
The frame_id associated this data.
Definition: types.h:133
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:132
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
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.
fawkes::point_t get_pixel_position_unchecked(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
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.
fawkes::upoint_t get_pixel_position(fawkes::cart_coord_3d_t &position, std::string &frame, const fawkes::Time &time)
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:53
Cartesian coordinates (3D).
Definition: types.h:88
float y
y coordinate
Definition: types.h:90
float z
z coordinate
Definition: types.h:91
float x
x coordinate
Definition: types.h:89
Point with cartesian coordinates as signed integers.
Definition: types.h:42
int x
x coordinate
Definition: types.h:43
int y
y coordinate
Definition: types.h:44
Polar coordinates.
Definition: types.h:103
float phi
x-y : plane
Definition: types.h:105
float r
distance
Definition: types.h:104
float theta
plane-z : space
Definition: types.h:106
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37