mirror of
https://github.com/YutaItoh/3D-Eye-Tracker.git
synced 2025-09-26 23:39:12 +08:00
added a camera parameter reader
This commit is contained in:
parent
87c6fe653d
commit
09cd333d2f
1
docs/cameraintrinsics_eye.txt
Normal file
1
docs/cameraintrinsics_eye.txt
Normal file
@ -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
|
@ -12,7 +12,7 @@
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/flann/flann.hpp>
|
||||
|
||||
#include "iris_util.h"
|
||||
#include "eye_util.h"
|
||||
|
||||
//#include <pupiltracker/pupiltracker.h>
|
||||
#include <singleeyefitter/singleeyefitter.h>
|
||||
|
@ -1,5 +1,5 @@
|
||||
|
||||
#include "iris_util.h"
|
||||
#include "eye_util.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <opencv2/core/cuda.hpp>// CUDA
|
@ -1,5 +1,5 @@
|
||||
#ifndef IRIS_UTIL_H
|
||||
#define IRIS_UTIL_H
|
||||
#ifndef EYE_UTIL_H
|
||||
#define EYE_UTIL_H
|
||||
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <Eigen/Dense>
|
||||
@ -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
|
||||
#endif // EYE_UTIL_H
|
155
main/main.cpp
155
main/main.cpp
@ -12,6 +12,7 @@
|
||||
#include <sstream>
|
||||
|
||||
|
||||
#include "ubitrack_util.h" // claibration file handlers
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <boost/filesystem/path.hpp>
|
||||
@ -25,18 +26,18 @@
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/photo/photo.hpp>
|
||||
|
||||
|
||||
|
||||
#define NOMINMAX
|
||||
|
||||
#include "pupilFitter.h" // 2D pupil detector
|
||||
|
||||
#include "timer.h"
|
||||
#include "pupilFitter.h"
|
||||
#include "eye_model_updater.h"
|
||||
|
||||
#include "eye_cameras.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<double, 8> &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<double, 8> 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<eye_tracker::Caib> 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<double, 8> 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<double>(0,0)+K.at<double>(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;
|
||||
|
||||
// Setup of classes that handle monocular/stereo camera setups
|
||||
// We can encapslate them into a wrapper class in future update
|
||||
std::vector<std::unique_ptr<eyecamera::EyeCameraParent>> eyecams(kCameraNums); // Image sources
|
||||
std::vector<std::unique_ptr<eye_tracker::CameraUndistorter>> camera_undistorters(kCameraNums); // Camera undistorters
|
||||
std::vector<std::string> window_names(kCameraNums); // Window names
|
||||
std::vector<cv::Mat> images(kCameraNums); // buffer images
|
||||
std::vector<std::string> file_stems(kCameraNums); // Output file stem names
|
||||
std::vector<int> camera_indices(kCameraNums); // Camera indices for Opencv capture
|
||||
std::vector<std::unique_ptr<eye_tracker::EyeModelUpdater>> eye_model_updaters(kCameraNums); // 3D eye models
|
||||
|
||||
std::vector<std::unique_ptr<eyecamera::EyeCameraParent>> eyecams(kCameraNums); // Image sources
|
||||
std::vector<std::string> window_names(kCameraNums); // Window names
|
||||
std::vector<cv::Mat> images(kCameraNums); // buffer images
|
||||
std::vector<std::string> file_stems(kCameraNums); // Output file stem names
|
||||
std::vector<int> camera_indices(kCameraNums); // Camera indices for opencv
|
||||
std::vector<std::unique_ptr<eye_tracker::EyeModelUpdater>> eye_model_updaters(kCameraNums); // 3D eye model
|
||||
|
||||
// Instantiate and initialize the class vectors
|
||||
try{
|
||||
switch (input_mode)
|
||||
{
|
||||
case InputMode::IMAGE:
|
||||
eyecams[0]=std::make_unique<eyecamera::EyeCamera>(media_file, false);
|
||||
case eye_tracker::InputMode::IMAGE:
|
||||
eyecams[0] = std::make_unique<eyecamera::EyeCamera>(media_file, false);
|
||||
eye_model_updaters[0] = std::make_unique<eye_tracker::EyeModelUpdater>(focal_length, 5, 0.5);
|
||||
camera_undistorters[0] = std::make_unique<eye_tracker::CameraUndistorter>(K, distCoeffs);
|
||||
window_names = { "Video/Image" };
|
||||
file_stems = { media_file_stem };
|
||||
break;
|
||||
case InputMode::VIDEO:
|
||||
eyecams[0]=std::make_unique<eyecamera::EyeCamera>(media_file, false);
|
||||
eyecams[0]=std::make_unique<eyecamera::EyeCamera>(media_file, false);
|
||||
case eye_tracker::InputMode::VIDEO:
|
||||
eyecams[0] = std::make_unique<eyecamera::EyeCamera>(media_file, false);
|
||||
eye_model_updaters[0] = std::make_unique<eye_tracker::EyeModelUpdater>(focal_length, 5, 0.5);
|
||||
camera_undistorters[0] = std::make_unique<eye_tracker::CameraUndistorter>(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<eyecamera::EyeCamera>(camera_indices[0], false);
|
||||
// eyecams[1] = std::make_unique<eyecamera::EyeCamera>(camera_indices[1], false);
|
||||
#if 0
|
||||
// OpenCV HighGUI frame grabber
|
||||
eyecams[0] = std::make_unique<eyecamera::EyeCamera>(camera_indices[0], false);
|
||||
eyecams[1] = std::make_unique<eyecamera::EyeCamera>(camera_indices[1], false);
|
||||
#else
|
||||
// DirectShow frame grabber
|
||||
eyecams[0] = std::make_unique<eyecamera::EyeCameraDS>("Pupil Cam1 ID0");
|
||||
eyecams[1]=std::make_unique<eyecamera::EyeCameraDS>("Pupil Cam1 ID2");
|
||||
eyecams[1] = std::make_unique<eyecamera::EyeCameraDS>("Pupil Cam1 ID2");
|
||||
#endif
|
||||
eye_model_updaters[0] = std::make_unique<eye_tracker::EyeModelUpdater>(focal_length, 5, 0.5);
|
||||
eye_model_updaters[1] = std::make_unique<eye_tracker::EyeModelUpdater>(focal_length, 5, 0.5);
|
||||
camera_undistorters[0] = std::make_unique<eye_tracker::CameraUndistorter>(K, distCoeffs);
|
||||
camera_undistorters[1] = std::make_unique<eye_tracker::CameraUndistorter>(K, distCoeffs);
|
||||
window_names = { "Cam0", "Cam1" };
|
||||
file_stems = { "cam0", "cam1" };
|
||||
break;
|
||||
case InputMode::CAMERA_MONO:
|
||||
eyecams[0]=std::make_unique<eyecamera::EyeCameraDS>("Pupil Cam1 ID0");
|
||||
case eye_tracker::InputMode::CAMERA_MONO:
|
||||
eyecams[0] = std::make_unique<eyecamera::EyeCameraDS>("Pupil Cam1 ID0"); //
|
||||
eye_model_updaters[0] = std::make_unique<eye_tracker::EyeModelUpdater>(focal_length, 5, 0.5);
|
||||
camera_undistorters[0] = std::make_unique<eye_tracker::CameraUndistorter>(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
|
||||
|
408
main/ubitrack_util.h
Normal file
408
main/ubitrack_util.h
Normal file
@ -0,0 +1,408 @@
|
||||
#ifndef UBITRACK_UTIL_H
|
||||
#define UBITRACK_UTIL_H
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/serialization/serialization.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/serialization/string.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/math/quaternion.hpp>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
|
||||
|
||||
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 <class Archive>
|
||||
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 <class Archive>
|
||||
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<double>
|
||||
{
|
||||
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 <class Archive>
|
||||
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)
|
||||
{
|
||||
// Ž©<C5BD>g‚Ì‘ã“üƒ`ƒFƒbƒN
|
||||
if (this != &other) {
|
||||
q = other.q;
|
||||
pos = other.pos;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template <class Archive>
|
||||
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<double, 8> &distCoeffs){
|
||||
Kmat = (cv::Mat_<double>(3, 3) <<
|
||||
K[0], K[1], -K[2],
|
||||
K[3], K[4], h-1+K[5],
|
||||
K[6], K[7], -K[8]);
|
||||
//cv::Vec<double, 8> 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<double, 8> &distCoeffs){
|
||||
Kmat = (cv::Mat_<double>(3, 3) <<
|
||||
K[0], -K[1], K[2],
|
||||
K[3], -K[4], -(h-1+K[5]),
|
||||
K[6], -K[7], K[8]);
|
||||
//cv::Vec<double, 8> 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 <class Archive>
|
||||
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 DataType>
|
||||
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 DataType>
|
||||
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<char, 1024> 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<Position3D> ubitrack_network_receiver(port);
|
||||
//}
|
||||
|
||||
}
|
||||
#endif // UBITRACK_UTIL_H
|
Loading…
Reference in New Issue
Block a user