diff options
| -rw-r--r-- | facetracknoir/rotation.h | 60 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.h | 22 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/camera.h | 53 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.h | 26 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/trans_calib.h | 22 |
5 files changed, 92 insertions, 91 deletions
diff --git a/facetracknoir/rotation.h b/facetracknoir/rotation.h index d40fb6cf..5ff5ce61 100644 --- a/facetracknoir/rotation.h +++ b/facetracknoir/rotation.h @@ -11,40 +11,40 @@ class RotationType { public: - RotationType() : a(1.0),b(0.0),c(0.0),d(0.0) {} - RotationType(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); } - RotationType(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {} - - RotationType inv(){ - return RotationType(a,-b,-c, -d); - } - - - // conversions - // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - void fromEuler(double yaw, double pitch, double roll) - { - - double sin_phi = sin(roll/2.0); - double cos_phi = cos(roll/2.0); - double sin_the = sin(pitch/2.0); - double cos_the = cos(pitch/2.0); - double sin_psi = sin(yaw/2.0); - double cos_psi = cos(yaw/2.0); - - a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi; - b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi; - c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi; - d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi; - } - + RotationType() : a(1.0),b(0.0),c(0.0),d(0.0) {} + RotationType(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); } + RotationType(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {} + + RotationType inv(){ + return RotationType(a,-b,-c, -d); + } + + + // conversions + // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + void fromEuler(double yaw, double pitch, double roll) + { + + double sin_phi = sin(roll/2.0); + double cos_phi = cos(roll/2.0); + double sin_the = sin(pitch/2.0); + double cos_the = cos(pitch/2.0); + double sin_psi = sin(yaw/2.0); + double cos_psi = cos(yaw/2.0); + + a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi; + b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi; + c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi; + d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi; + } + void toEuler(double& yaw, double& pitch, double& roll) const { roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c)); pitch = asin(2.0*(a*c - b*d)); yaw = atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d)); } - + const RotationType operator*(const RotationType& B) const { const RotationType& A = *this; @@ -54,6 +54,6 @@ public: A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a); } -protected: - double a,b,c,d; // quaternion coefficients +private: + double a,b,c,d; // quaternion coefficients }; diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h index 609c9af1..5c321b2c 100644 --- a/ftnoir_tracker_aruco/trans_calib.h +++ b/ftnoir_tracker_aruco/trans_calib.h @@ -20,20 +20,20 @@ class TranslationCalibrator { public: - TranslationCalibrator(); + TranslationCalibrator(); - // reset the calibration process - void reset(); + // reset the calibration process + void reset(); - // update the current estimate - void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + // update the current estimate + void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); - // get the current estimate for t_MH - cv::Vec3f get_estimate(); + // get the current estimate for t_MH + cv::Vec3f get_estimate(); -protected: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) +private: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) }; -#endif //TRANSCALIB_H
\ No newline at end of file +#endif //TRANSCALIB_H diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h index 86cafd42..2bca88a1 100644 --- a/ftnoir_tracker_pt/camera.h +++ b/ftnoir_tracker_pt/camera.h @@ -25,11 +25,11 @@ void get_camera_device_names(std::vector<std::string>& device_names); // ---------------------------------------------------------------------------- struct CamInfo { - CamInfo() : res_x(0), res_y(0), fps(0) {} + CamInfo() : res_x(0), res_y(0), fps(0) {} - int res_x; - int res_y; - int fps; + int res_x; + int res_y; + int fps; }; // ---------------------------------------------------------------------------- @@ -65,7 +65,7 @@ protected: virtual void _set_device_index() = 0; virtual void _set_fps() = 0; virtual void _set_res() = 0; - +private: float dt_valid; float dt_mean; int desired_index; @@ -82,18 +82,19 @@ inline Camera::~Camera() {} class CVCamera : public Camera { public: - CVCamera() : cap(NULL) {} - ~CVCamera() { stop(); } + CVCamera() : cap(NULL) {} + ~CVCamera() { stop(); } - void start() override; - void stop() override; + void start() override; + void stop() override; protected: - bool _get_frame(cv::Mat* frame) override; - void _set_fps() override; - void _set_res() override; + bool _get_frame(cv::Mat* frame) override; + void _set_fps() override; + void _set_res() override; void _set_device_index() override; +private: cv::VideoCapture* cap; }; #else @@ -102,21 +103,21 @@ protected: class VICamera : public Camera { public: - VICamera(); - ~VICamera() { stop(); } + VICamera(); + ~VICamera() { stop(); } - virtual void start(); - virtual void stop(); + virtual void start(); + virtual void stop(); protected: - virtual bool _get_frame(cv::Mat* frame); - virtual void _set_device_index(); - virtual void _set_fps(); - virtual void _set_res(); - - videoInput VI; - cv::Mat new_frame; - unsigned char* frame_buffer; + virtual bool _get_frame(cv::Mat* frame); + virtual void _set_device_index(); + virtual void _set_fps(); + virtual void _set_res(); + + videoInput VI; + cv::Mat new_frame; + unsigned char* frame_buffer; }; #endif @@ -128,12 +129,12 @@ enum RotationType }; // ---------------------------------------------------------------------------- -class FrameRotation +class FrameRotation { public: RotationType rotation; - cv::Mat rotate_frame(cv::Mat frame); + cv::Mat rotate_frame(cv::Mat frame); }; #endif //CAMERA_H diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h index 21d548af..3ef82900 100644 --- a/ftnoir_tracker_pt/point_extractor.h +++ b/ftnoir_tracker_pt/point_extractor.h @@ -15,21 +15,21 @@ // Extracts points from an opencv image class PointExtractor { -public: - // extracts points from frame and draws some processing info into frame, if draw_output is set - // dt: time since last call in seconds - // WARNING: returned reference is valid as long as object - const std::vector<cv::Vec2f>& extract_points(cv::Mat &frame); - const std::vector<cv::Vec2f>& get_points() { return points; } - PointExtractor(); +public: + // extracts points from frame and draws some processing info into frame, if draw_output is set + // dt: time since last call in seconds + // WARNING: returned reference is valid as long as object + const std::vector<cv::Vec2f>& extract_points(cv::Mat &frame); + const std::vector<cv::Vec2f>& get_points() { return points; } + PointExtractor(); - int threshold_val; - int threshold_secondary_val; - int min_size, max_size; + int threshold_val; + int threshold_secondary_val; + int min_size, max_size; -protected: - std::vector<cv::Vec2f> points; - cv::Mat frame_last; +private: + std::vector<cv::Vec2f> points; + cv::Mat frame_last; }; #endif //POINTEXTRACTOR_H diff --git a/ftnoir_tracker_pt/trans_calib.h b/ftnoir_tracker_pt/trans_calib.h index 609c9af1..5c321b2c 100644 --- a/ftnoir_tracker_pt/trans_calib.h +++ b/ftnoir_tracker_pt/trans_calib.h @@ -20,20 +20,20 @@ class TranslationCalibrator { public: - TranslationCalibrator(); + TranslationCalibrator(); - // reset the calibration process - void reset(); + // reset the calibration process + void reset(); - // update the current estimate - void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + // update the current estimate + void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); - // get the current estimate for t_MH - cv::Vec3f get_estimate(); + // get the current estimate for t_MH + cv::Vec3f get_estimate(); -protected: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) +private: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) }; -#endif //TRANSCALIB_H
\ No newline at end of file +#endif //TRANSCALIB_H |