diff --git a/docs/cameraintrinsics_eye.txt b/docs/cameraintrinsics_eye.txt new file mode 100644 index 0000000..d467783 --- /dev/null +++ b/docs/cameraintrinsics_eye.txt @@ -0,0 +1 @@ +22 serialization::archive 12 0 0 1472782944866617537 0 0 6.23496154785156250e+002 0.00000000000000000e+000 -2.94488525390625000e+002 0.00000000000000000e+000 6.26096130371093750e+002 -2.40432937622070310e+002 0.00000000000000000e+000 0.00000000000000000e+000 -1.00000000000000000e+000 640 480 6 2.78319090604782100e-001 -1.18223369121551510e+000 -6.84279918670654300e-001 1.83514893054962160e-001 3.48034203052520750e-001 -3.23891353607177730e+000 2 8.93868226557970050e-003 2.85103498026728630e-003 diff --git a/main/eye_model_updater.h b/main/eye_model_updater.h index d86827b..c7479b9 100644 --- a/main/eye_model_updater.h +++ b/main/eye_model_updater.h @@ -12,7 +12,7 @@ #include #include -#include "iris_util.h" +#include "eye_util.h" //#include #include diff --git a/main/iris_util.cpp b/main/eye_util.cpp similarity index 98% rename from main/iris_util.cpp rename to main/eye_util.cpp index 14033ef..05b36c5 100644 --- a/main/iris_util.cpp +++ b/main/eye_util.cpp @@ -1,5 +1,5 @@ -#include "iris_util.h" +#include "eye_util.h" #include #include // CUDA diff --git a/main/iris_util.h b/main/eye_util.h similarity index 92% rename from main/iris_util.h rename to main/eye_util.h index 224014e..b6432a9 100644 --- a/main/iris_util.h +++ b/main/eye_util.h @@ -1,5 +1,5 @@ -#ifndef IRIS_UTIL_H -#define IRIS_UTIL_H +#ifndef EYE_UTIL_H +#define EYE_UTIL_H #include #include @@ -16,5 +16,7 @@ namespace eye_tracker{ cv::Point toImgCoordInv(const cv::Point& point, const cv::Mat& m, double scale = 1, int shift = 0); cv::RotatedRect toImgCoordInv(const cv::RotatedRect& rect, const cv::Mat& m, float scale = 1); + + } -#endif // IRIS_UTIL_H \ No newline at end of file +#endif // EYE_UTIL_H \ No newline at end of file diff --git a/main/main.cpp b/main/main.cpp index 6d6ab7d..f8c619c 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -12,6 +12,7 @@ #include +#include "ubitrack_util.h" // claibration file handlers #include #include #include @@ -25,18 +26,18 @@ #include #include - - #define NOMINMAX -#include "timer.h" -#include "pupilFitter.h" -#include "eye_model_updater.h" -#include "eye_cameras.h" +#include "pupilFitter.h" // 2D pupil detector + +#include "timer.h" + +#include "eye_model_updater.h" // 3D model builder +#include "eye_cameras.h" // Camera interfaces -namespace { +namespace eye_tracker { class FrameRateCounter { @@ -71,6 +72,39 @@ private: FrameRateCounter& operator=(const FrameRateCounter& rhs); }; + +class CameraUndistorter +{ +public: + CameraUndistorter(const cv::Mat &K, const cv::Vec &distCoeffs) + : K0_(K.clone()), distCoeffs_(distCoeffs) + { + std::cout << "Intrinsic (matrix): " << K0_ << std::endl; + std::cout << "Intrinsic (distortion): " << distCoeffs_ << std::endl; + + } + ~CameraUndistorter() { + } + void init_maps(const cv::Size &s) { + cv::initUndistortRectifyMap(K0_, distCoeffs_, cv::Mat(), K0_, s, CV_32FC1, mapx_, mapy_); + } + void undistort(const cv::Mat &in, cv::Mat &out) { + if (is_dist_map_initialized_ == false) { + init_maps(in.size()); + is_dist_map_initialized_ = true; + } + cv::remap(in, out, mapx_, mapy_, cv::INTER_LINEAR); + } +protected: + // Local variables initialized at the constructor + cv::Mat K0_; + cv::Vec distCoeffs_; // (k1 k2 p1 p2 [k3 [k4 k5 k6]]) + bool is_dist_map_initialized_ = false; + cv::Mat mapx_, mapy_; +private: + // Prevent copying +}; + enum InputMode { CAMERA, CAMERA_MONO, VIDEO, IMAGE }; } @@ -79,48 +113,49 @@ enum InputMode { CAMERA, CAMERA_MONO, VIDEO, IMAGE }; int main(int argc, char *argv[]){ // Variables for FPS - FrameRateCounter frame_rate_counter; + eye_tracker::FrameRateCounter frame_rate_counter; bool kVisualization = false; kVisualization = true; - InputMode input_mode = - // InputMode::VIDEO; // Set a video as a video source + eye_tracker::InputMode input_mode = + eye_tracker::InputMode::VIDEO; // Set a video as a video source // InputMode::CAMERA; // Set cameras as video sources - InputMode::CAMERA_MONO; // Set cameras as video sources + // InputMode::CAMERA_MONO; // Set cameras as video sources // InputMode::IMAGE;// Set an image as a video source + + ////// Command line opitions ///////////// std::string kDir = "C:/Users/Yuta/Dropbox/work/Projects/20150427_Alex_EyeTracker/"; std::string media_file; std::string media_file_stem; - std::string kOutputDataDirectory(kDir + "out/"); // Data output directroy - + //std::string kOutputDataDirectory(kDir + "out/"); // Data output directroy if (argc > 2) { boost::filesystem::path file_name = std::string(argv[2]); kDir = std::string(argv[1]); media_file_stem = file_name.stem().string(); media_file = kDir + file_name.string(); - kOutputDataDirectory = kDir + "./"; + //kOutputDataDirectory = kDir + "./"; std::cout << "Load " << media_file << std::endl; std::string media_file_ext = file_name.extension().string(); if (media_file_ext == ".avi" || media_file_ext == ".mp4" || media_file_ext == ".wmv") { - input_mode = InputMode::VIDEO; + input_mode = eye_tracker::InputMode::VIDEO; }else{ - input_mode = InputMode::IMAGE; + input_mode = eye_tracker::InputMode::IMAGE; } } else { - if (input_mode == InputMode::IMAGE || input_mode == InputMode::VIDEO) { + if (input_mode == eye_tracker::InputMode::IMAGE || input_mode == eye_tracker::InputMode::VIDEO) { switch (input_mode) { - case InputMode::IMAGE: + case eye_tracker::InputMode::IMAGE: media_file = kDir + "data3/test.png"; media_file_stem = "test"; break; - case InputMode::VIDEO: + case eye_tracker::InputMode::VIDEO: media_file = kDir + "out/test.avi"; media_file_stem = "test"; break; @@ -129,66 +164,92 @@ int main(int argc, char *argv[]){ } } } + /////////////// + + + //// Camera intrinsic parameters + std::string calib_path="../../docs/cameraintrinsics_eye.txt"; + eye_tracker::UbitrackTextReader ubitrack_calib_text_reader; + if (ubitrack_calib_text_reader.read(calib_path) == false){ + std::cout << "Calibration file onpen error: " << calib_path << std::endl; + return -1; + } + cv::Mat K; // Camera intrinsic matrix in OpenCV format + cv::Vec distCoeffs; // (k1 k2 p1 p2 [k3 [k4 k5 k6]]) // k: radial, p: tangential + ubitrack_calib_text_reader.data_.get_parameters_opencv_default(K, distCoeffs); + + // Focal distance used in the 3D eye model fitter + double focal_length = (K.at(0,0)+K.at(1,1))*0.5; // Required for the 3D model fitting // Set mode parameters size_t kCameraNums; switch (input_mode) { - case InputMode::IMAGE: - case InputMode::VIDEO: - case InputMode::CAMERA_MONO: + case eye_tracker::InputMode::IMAGE: + case eye_tracker::InputMode::VIDEO: + case eye_tracker::InputMode::CAMERA_MONO: kCameraNums = 1; break; - case InputMode::CAMERA: + case eye_tracker::InputMode::CAMERA: kCameraNums = 2; break; default: break; } - - double focal_length = 600; - const int kDETECTOR_MAX = 2; - std::vector> eyecams(kCameraNums); // Image sources - std::vector window_names(kCameraNums); // Window names - std::vector images(kCameraNums); // buffer images - std::vector file_stems(kCameraNums); // Output file stem names - std::vector camera_indices(kCameraNums); // Camera indices for opencv - std::vector> eye_model_updaters(kCameraNums); // 3D eye model + // Setup of classes that handle monocular/stereo camera setups + // We can encapslate them into a wrapper class in future update + std::vector> eyecams(kCameraNums); // Image sources + std::vector> camera_undistorters(kCameraNums); // Camera undistorters + std::vector window_names(kCameraNums); // Window names + std::vector images(kCameraNums); // buffer images + std::vector file_stems(kCameraNums); // Output file stem names + std::vector camera_indices(kCameraNums); // Camera indices for Opencv capture + std::vector> eye_model_updaters(kCameraNums); // 3D eye models + // Instantiate and initialize the class vectors try{ switch (input_mode) { - case InputMode::IMAGE: - eyecams[0]=std::make_unique(media_file, false); + case eye_tracker::InputMode::IMAGE: + eyecams[0] = std::make_unique(media_file, false); eye_model_updaters[0] = std::make_unique(focal_length, 5, 0.5); + camera_undistorters[0] = std::make_unique(K, distCoeffs); window_names = { "Video/Image" }; file_stems = { media_file_stem }; break; - case InputMode::VIDEO: - eyecams[0]=std::make_unique(media_file, false); - eyecams[0]=std::make_unique(media_file, false); + case eye_tracker::InputMode::VIDEO: + eyecams[0] = std::make_unique(media_file, false); eye_model_updaters[0] = std::make_unique(focal_length, 5, 0.5); + camera_undistorters[0] = std::make_unique(K, distCoeffs); window_names = { "Video/Image" }; file_stems = { media_file_stem }; break; - case InputMode::CAMERA: + case eye_tracker::InputMode::CAMERA: camera_indices[0] = 0; camera_indices[1] = 2; - // eyecams[0]=std::make_unique(camera_indices[0], false); - // eyecams[1] = std::make_unique(camera_indices[1], false); +#if 0 + // OpenCV HighGUI frame grabber + eyecams[0] = std::make_unique(camera_indices[0], false); + eyecams[1] = std::make_unique(camera_indices[1], false); +#else + // DirectShow frame grabber eyecams[0] = std::make_unique("Pupil Cam1 ID0"); - eyecams[1]=std::make_unique("Pupil Cam1 ID2"); + eyecams[1] = std::make_unique("Pupil Cam1 ID2"); +#endif eye_model_updaters[0] = std::make_unique(focal_length, 5, 0.5); eye_model_updaters[1] = std::make_unique(focal_length, 5, 0.5); + camera_undistorters[0] = std::make_unique(K, distCoeffs); + camera_undistorters[1] = std::make_unique(K, distCoeffs); window_names = { "Cam0", "Cam1" }; file_stems = { "cam0", "cam1" }; break; - case InputMode::CAMERA_MONO: - eyecams[0]=std::make_unique("Pupil Cam1 ID0"); + case eye_tracker::InputMode::CAMERA_MONO: + eyecams[0] = std::make_unique("Pupil Cam1 ID0"); // eye_model_updaters[0] = std::make_unique(focal_length, 5, 0.5); + camera_undistorters[0] = std::make_unique(K, distCoeffs); window_names = { "Cam0" }; file_stems = { "cam0" }; break; @@ -213,6 +274,7 @@ int main(int argc, char *argv[]){ const char kTerminate = 27;//Escape 0x1b bool is_run = true; while (is_run) { + // Fetch key input char kKEY = 0; if (kVisualization) { @@ -235,6 +297,10 @@ int main(int argc, char *argv[]){ //is_run = false; break; } + + // Undistort a captured image + camera_undistorters[cam]->undistort(img, img); + cv::Mat img_rgb_debug = img.clone(); cv::Mat img_grey; @@ -260,7 +326,7 @@ int main(int argc, char *argv[]){ // 3D eye pose estimation bool is_reliable = false; bool is_added = false; - bool force_add = false; + const bool force_add = false; const double kReliabilityThreshold = 0.8;// 0.96; double ellipse_realiability = 0.0; /// Reliability of a detected 2D ellipse based on 3D eye model if (is_pupil_found) { @@ -276,7 +342,7 @@ int main(int argc, char *argv[]){ } // Visualize results - if (cam==0&&kVisualization) { + if (cam == 0 && kVisualization) { // 2D pupil if (is_pupil_found) { @@ -296,6 +362,7 @@ int main(int argc, char *argv[]){ } cv::imshow(window_names[cam], img_rgb_debug); + } // Visualization } // For each cameras diff --git a/main/ubitrack_util.h b/main/ubitrack_util.h new file mode 100644 index 0000000..22e8faa --- /dev/null +++ b/main/ubitrack_util.h @@ -0,0 +1,408 @@ +#ifndef UBITRACK_UTIL_H +#define UBITRACK_UTIL_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + + +using boost::asio::ip::udp; +#define MAX_MASSAGE_BUFFER_SIZE 2048 + +namespace eye_tracker{ + +/// Timestamp: nanoseconds since epoch (UNIX birth) +typedef unsigned long long int Timestamp; + +/// A dummy timestamp class to parse serialized Ubitrack message +class UbitrackTimestamp { +public: + UbitrackTimestamp(){} + Timestamp t; +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, unsigned int /*version*/) + { + ar&t; + } +}; + +/// A data class to parse serialized Ubitrack message +class Position3D { +public: + double x, y, z; + + Position3D() : + x(0), y(0), z(0){ + } + + Position3D(double x0, double y0, double z0) : + x(x0), y(y0), z(z0){ + } + + Position3D(const Position3D &other) + :x(other.x), y(other.y), z(other.z) + { + } + + Position3D &operator=(const Position3D &other) + { + if (this != &other) { + x = other.x; + y = other.y; + z = other.z; + } + return *this; + } + + Position3D &operator+(const Position3D &other) + { + x += other.x; + y += other.y; + z += other.z; + return *this; + } + + void print() + { + std::cout << x << " " << y << " " << z << std::endl; + } + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, unsigned int /*version*/) + { + ar & x; + ar & y; + ar & z; + } +}; + +/// A data class to parse serialized Ubitrack message +class Quaternion + : public boost::math::quaternion +{ +public: + + double w() const { return a; } + + double x() const { return b; } + double y() const { return c; } + double z() const { return d; } + + Quaternion() + : boost::math::quaternion< double >(1, 0, 0, 0) + { + } + + void print() + { + std::cout << this << std::endl; + } + + // rotate a vector by a quaternion + Position3D operator*(const Position3D& vec) const + { + Position3D r; + + // precomputation of some values + double xy = x() * y(); + double xz = x() * z(); + double yz = y() * z(); + double ww = w() * w(); + double wx = w() * x(); + double wy = w() * y(); + double wz = w() * z(); + + r.x = vec.x * (2 * (x()*x() + ww) - 1) + vec.y * 2 * (xy - wz) + vec.z * 2 * (wy + xz); + r.y = vec.x * 2 * (xy + wz) + vec.y * (2 * (y()*y() + ww) - 1) + vec.z * 2 * (yz - wx); + r.z = vec.x * 2 * (xz - wy) + vec.y * 2 * (wx + yz) + vec.z * (2 * (z()*z() + ww) - 1); + + return r; + + } +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, unsigned int /*version*/) + { + ar & b; + ar & c; + ar & d; + ar & a; + } + int dummy; +}; + + + +/// A data class to parse serialized Ubitrack message +class Pose { +public: + Quaternion q; + Position3D pos; + + Pose() : + q(), pos(){ + }; + + + Position3D Pose::operator*(const Position3D& x) const + { + return Position3D((q * x) + pos); + } + + void print() + { + q.print(); + pos.print(); + } + + Pose &operator=(const Pose &other) + { + // 自身の代入チェック + if (this != &other) { + q = other.q; + pos = other.pos; + } + return *this; + } + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, unsigned int /*version*/) + { + ar & q; + ar & pos; + } +}; + +class Caib{ + +public: + double K[9]; + int w; + int h; + int rad_dis_num; + double rad_dis[6]; + int tan_dis_num; + double tan_dis[2]; + Caib(){ + for (int k = 0; k < 9; k++){ + K[k] = k + 0.1; + } + w = 640; + h = 480; + rad_dis_num = 6; + for (int k = 0; k < rad_dis_num; k++){ + rad_dis[k] = k + 0.2; + } + tan_dis_num = 2; + for (int k = 0; k < tan_dis_num; k++){ + tan_dis[k] = k + 0.3; + } + }; + + // Convert parameters style From Ubitrack to OpenCV (image top-left) + void get_parameters_opencv_default(cv::Mat &Kmat, cv::Vec &distCoeffs){ + Kmat = (cv::Mat_(3, 3) << + K[0], K[1], -K[2], + K[3], K[4], h-1+K[5], + K[6], K[7], -K[8]); + //cv::Vec distCoeffs(k1, k2, p1, p2, k3, k4, k5, k6); // (k1 k2 p1 p2 [k3 [k4 k5 k6]]) + distCoeffs[0] = rad_dis[0]; + distCoeffs[1] = rad_dis[1]; + distCoeffs[2] = tan_dis[0]; + distCoeffs[3] = -tan_dis[1]; /// for Opencv + distCoeffs[4] = rad_dis[2]; + distCoeffs[5] = rad_dis[3]; + distCoeffs[6] = rad_dis[4]; + distCoeffs[7] = rad_dis[5]; + } + // Convert parameters style From Ubitrack to OpenGL (image top-left) + void set_parameters_opengl(cv::Mat &Kmat, cv::Vec &distCoeffs){ + Kmat = (cv::Mat_(3, 3) << + K[0], -K[1], K[2], + K[3], -K[4], -(h-1+K[5]), + K[6], -K[7], K[8]); + //cv::Vec distCoeffs(k1, k2, p1, p2, k3, k4, k5, k6); // (k1 k2 p1 p2 [k3 [k4 k5 k6]]) + distCoeffs[0] = rad_dis[0]; + distCoeffs[1] = rad_dis[1]; + distCoeffs[2] = tan_dis[0]; + distCoeffs[3] = tan_dis[1]; + distCoeffs[4] = rad_dis[2]; + distCoeffs[5] = rad_dis[3]; + distCoeffs[6] = rad_dis[4]; + distCoeffs[7] = rad_dis[5]; + } + void project(const Position3D &p, double &u, double &v){ + double x = K[0] * p.x + K[1] * p.y + K[2] * p.z; + double y = K[3] * p.x + K[4] * p.y + K[5] * p.z; + double z = K[6] * p.x + K[7] * p.y + K[8] * p.z; + u = x / z; + v = y / z; + } + void print(){ + std::cout << K[0] << " " << K[1] << " " << K[2] << std::endl; + std::cout << K[3] << " " << K[4] << " " << K[5] << std::endl; + std::cout << K[6] << " " << K[7] << " " << K[8] << std::endl; + } + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, unsigned int /*version*/) + { + for (int k = 0; k < 9; k++){ + ar &K[k]; + } + ar&w; + ar&h; + ar&rad_dis_num; + for (int k = 0; k < rad_dis_num; k++){ + ar &rad_dis[k]; + } + ar&tan_dis_num; + for (int k = 0; k < tan_dis_num; k++){ + ar &tan_dis[k]; + } + } +}; + +template +class UbitrackTextReader +{ +public: + bool read(std::string path){ + std::string line; + std::ifstream myfile(path); + + if (myfile.is_open() == false) return false; + + getline(myfile, line); + + if (line.empty() ) return false; + + std::istringstream archive_stream(line); + boost::archive::text_iarchive archive(archive_stream); + boost::archive::text_oarchive ot(std::cout); + ot << t_; + ot << data_; + try { + archive >> t_; + archive >> data_; + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + return false; + } + return true; + } + UbitrackTimestamp t_; + DataType data_; +private: + +}; + +template +class UbitrackUDPClient +{ +public: + + UbitrackUDPClient(unsigned short port) + : io_service(), + socket(io_service, { udp::v4(), port }) + { + } + void start(){ + do_receive(); + io_service.run(); + + } + void stop(){ + io_service.stop(); + } + void get(DataType &data, Timestamp ×tamp){ + boost::mutex::scoped_lock lock(mtx_); +// mutex_.lock(); + data = data_; + timestamp = time_stamp_.t; +// std::cout << data_.t << std::endl; +// mutex_.unlock(); + } + +private: + void do_receive() + { + socket.async_receive_from(boost::asio::buffer(recv_buffer), receiver_endpoint, + boost::bind(&UbitrackUDPClient::handle_receive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + } + + void handle_receive(const boost::system::error_code& error, size_t bytes_transferred) + { + + std::string input_data_(recv_buffer.data(), bytes_transferred); + std::istringstream archive_stream(input_data_); + boost::archive::text_iarchive archive(archive_stream); + try { + boost::mutex::scoped_lock lock(mtx_); + archive >> parttrn_name_; + archive >> time_stamp_; + archive >> data_; + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + } + + if (!error || error == boost::asio::error::message_size) + do_receive(); + } + + // Network stuffx + boost::asio::io_service io_service; + udp::socket socket; + udp::endpoint receiver_endpoint; + boost::array recv_buffer; + + // Ubitrack data stuff + boost::mutex mtx_; + DataType data_; + UbitrackTimestamp time_stamp_; + std::string parttrn_name_; +}; + + +//void ubitrack_network_receiver_test(){ +// +// std::string s = "pattern_2"; +// UbitrackTimestamp t; +// Position3D data; +// long timestamp = 1471776266586383032L; +// +// boost::archive::text_oarchive ot(std::cout); +// ot << s; +// ot << t; +// ot << data; +// const unsigned short port = 21844; +// UbitrackUDPClient ubitrack_network_receiver(port); +//} + +} +#endif // UBITRACK_UTIL_H \ No newline at end of file