23 #include "position_to_pixel.h" 25 #include <utils/math/coord.h> 26 #include <core/exceptions/software.h> 45 float cam_aperture_x,
float cam_aperture_y,
46 unsigned int cam_width_x,
unsigned int cam_height_y,
float cam_angle_y )
49 cam_frame_ = cam_frame;
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;
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_;
59 cam_angle_max_horizontal_ = cam_aperture_horizontal_ / 2.0;
60 cam_angle_min_horizontal_ = -1.0 * cam_angle_max_horizontal_;
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;
86 coord_in.
stamp = time;
87 coord_in.setX( position.
x );
88 coord_in.setY( position.
y );
89 coord_in.setZ( position.
z );
93 tf_listener->
transform_point(cam_frame_, coord_in, coord_in_transformed);
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) );
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_;
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 );
108 return pixel_in_cam_unchecked;
128 if (pixel_unchecked.
x < 0 || pixel_unchecked.
x > (
long int)cam_resolution_x_) {
130 0, cam_resolution_x_);
131 }
else if (pixel_unchecked.
y < 0 || pixel_unchecked.
y > (
long int)cam_resolution_y_) {
133 0, cam_resolution_y_);
137 rv.
x = (
unsigned int) pixel_unchecked.
x;
138 rv.
y = (
unsigned int) pixel_unchecked.
y;
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.
unsigned int y
y coordinate
unsigned int x
x coordinate
A class for handling time.
float theta
plane-z : space
fawkes::Time stamp
The timestamp associated with this data.
Point with cartesian coordinates as unsigned integers.
Cartesian coordinates (3D).
Point with cartesian coordinates as signed integers.
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.
fawkes::upoint_t get_pixel_position(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.