24 #include <core/exception.h>
26 #include <fvutils/camera/tracker.h>
27 #include <utils/system/console_colors.h>
28 #include <utils/math/angle.h>
30 #include <fvmodels/relative_position/relativepositionmodel.h>
34 namespace firevision {
56 const unsigned int CameraTracker::MODE_MODEL = 0;
58 const unsigned int CameraTracker::MODE_WORLD = 1;
74 rpm = relative_position_model;
76 this->camera_height = camera_height;
82 CameraTracker::~CameraTracker()
94 if (mode == MODE_MODEL) {
95 new_pan = rpm->get_bearing() - camera_orientation;
96 new_tilt = rpm->get_slope();
97 }
else if (mode == MODE_WORLD) {
99 float w_r_x = world_x - robot_x;
100 float w_r_y = world_y - robot_y;
102 float distance = sqrt( w_r_x * w_r_x + w_r_y * w_r_y );
127 new_pan = atan2f( w_r_y, w_r_x );
129 new_tilt = atan2f( camera_height, distance );
138 CameraTracker::get_new_pan()
148 CameraTracker::get_new_tilt()
159 CameraTracker::set_mode(
unsigned int mode)
161 if ( (mode == MODE_WORLD) || (mode == MODE_MODEL)) {
187 CameraTracker::set_robot_position(
float x,
float y,
float ori)
203 CameraTracker::set_world_point(
float x,
float y)