23 #include "position_to_pixel.h"
25 #include <core/exceptions/software.h>
26 #include <utils/math/coord.h>
28 namespace firevision {
45 std::string cam_frame,
48 unsigned int cam_width_x,
49 unsigned int cam_height_y,
53 cam_frame_ = cam_frame;
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;
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_;
63 cam_angle_max_horizontal_ = cam_aperture_horizontal_ / 2.0;
64 cam_angle_min_horizontal_ = -1.0 * cam_angle_max_horizontal_;
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;
91 coord_in.
stamp = time;
92 coord_in.setX(position.
x);
93 coord_in.setY(position.
y);
94 coord_in.setZ(position.
z);
98 tf_listener->
transform_point(cam_frame_, coord_in, coord_in_transformed);
103 coord_in_transformed.getY(),
104 coord_in_transformed.getZ(),
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_;
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);
117 return pixel_in_cam_unchecked;
138 if (pixel_unchecked.
x < 0 || pixel_unchecked.
x > (
long int)cam_resolution_x_) {
143 }
else if (pixel_unchecked.
y < 0 || pixel_unchecked.
y > (
long int)cam_resolution_y_) {
151 rv.
x = (
unsigned int)pixel_unchecked.
x;
152 rv.
y = (
unsigned int)pixel_unchecked.
y;