added a camera parameter reader

This commit is contained in:
yitoh 2016-10-07 21:37:22 +09:00
parent 87c6fe653d
commit 09cd333d2f
6 changed files with 528 additions and 50 deletions

View 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

View File

@ -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>

View File

@ -1,5 +1,5 @@
#include "iris_util.h"
#include "eye_util.h"
#include <iostream>
#include <opencv2/core/cuda.hpp>// CUDA

View File

@ -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

View File

@ -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
View 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 &timestamp){
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