23#include "position_to_pixel.h"
25#include <core/exceptions/software.h>
26#include <utils/math/coord.h>
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;
A class for handling time.
std::string frame_id
The frame_id associated this data.
fawkes::Time stamp
The timestamp associated with this data.
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.
Cartesian coordinates (3D).
Point with cartesian coordinates as signed integers.
float theta
plane-z : space
Point with cartesian coordinates as unsigned integers.
unsigned int x
x coordinate
unsigned int y
y coordinate