mirror of
https://github.com/YutaItoh/3D-Eye-Tracker.git
synced 2025-09-26 23:09:19 +08:00
1790 lines
64 KiB
C++
1790 lines
64 KiB
C++
// SingleEyeFitter.cpp : Defines the entry point for the console application.
|
|
//
|
|
|
|
#include <boost/math/special_functions/sign.hpp>
|
|
|
|
#include <Eigen/StdVector>
|
|
|
|
#include <ceres/ceres.h>
|
|
#include <ceres/problem.h>
|
|
#include <ceres/autodiff_cost_function.h>
|
|
#include <ceres/solver.h>
|
|
#include <ceres/jet.h>
|
|
|
|
#include <singleeyefitter/singleeyefitter.h>
|
|
|
|
#include <singleeyefitter/utils.h>
|
|
#include <singleeyefitter/cvx.h>
|
|
#include <singleeyefitter/Conic.h>
|
|
#include <singleeyefitter/Ellipse.h>
|
|
#include <singleeyefitter/Circle.h>
|
|
#include <singleeyefitter/Conicoid.h>
|
|
#include <singleeyefitter/Sphere.h>
|
|
#include <singleeyefitter/solve.h>
|
|
#include <singleeyefitter/intersect.h>
|
|
#include <singleeyefitter/projection.h>
|
|
#include <singleeyefitter/fun.h>
|
|
#include <singleeyefitter/math.h>
|
|
|
|
#include "distance.h"
|
|
|
|
#include <spii/spii.h>
|
|
#include <spii/term.h>
|
|
#include <spii/function.h>
|
|
#include <spii/solver.h>
|
|
|
|
#define _USE_MATH_DEFINES
|
|
#include <math.h>
|
|
|
|
|
|
|
|
namespace ceres {
|
|
using singleeyefitter::math::sq;
|
|
|
|
template<typename T, int N>
|
|
inline Jet<T,N> sq(Jet<T,N> val) {
|
|
val.v *= 2*val.a;
|
|
val.a *= val.a;
|
|
return val;
|
|
}
|
|
}
|
|
|
|
namespace singleeyefitter {
|
|
|
|
struct scalar_tag{};
|
|
struct ceres_jet_tag{};
|
|
|
|
template<typename T, typename Enabled=void>
|
|
struct ad_traits;
|
|
|
|
template<typename T>
|
|
struct ad_traits<T, typename std::enable_if< std::is_arithmetic<T>::value >::type >
|
|
{
|
|
typedef scalar_tag ad_tag;
|
|
typedef T scalar;
|
|
static inline scalar value(const T& x) { return x; }
|
|
};
|
|
|
|
template<typename T, int N>
|
|
struct ad_traits<::ceres::Jet<T,N>>
|
|
{
|
|
typedef ceres_jet_tag ad_tag;
|
|
typedef T scalar;
|
|
static inline scalar get(const ::ceres::Jet<T,N>& x) { return x.a; }
|
|
};
|
|
|
|
template<typename T>
|
|
struct ad_traits<T, typename std::enable_if< !std::is_same<T, typename std::decay<T>::type>::value >::type >
|
|
: public ad_traits<typename std::decay<T>::type>
|
|
{
|
|
};
|
|
|
|
template<typename T>
|
|
inline T smootherstep(T edge0, T edge1, T x, scalar_tag)
|
|
{
|
|
if (x >= edge1)
|
|
return T(1);
|
|
else if (x <= edge0)
|
|
return T(0);
|
|
else {
|
|
x = (x - edge0)/(edge1 - edge0);
|
|
return x*x*x*(x*(x*T(6) - T(15)) + T(10));
|
|
}
|
|
}
|
|
template<typename T, int N>
|
|
inline ::ceres::Jet<T,N> smootherstep(T edge0, T edge1, const ::ceres::Jet<T,N>& f, ceres_jet_tag)
|
|
{
|
|
if (f.a >= edge1)
|
|
return ::ceres::Jet<T,N>(1);
|
|
else if (f.a <= edge0)
|
|
return ::ceres::Jet<T,N>(0);
|
|
else {
|
|
T x = (f.a - edge0)/(edge1 - edge0);
|
|
|
|
// f is referenced by this function, so create new value for return.
|
|
::ceres::Jet<T,N> g;
|
|
g.a = x*x*x*(x*(x*T(6) - T(15)) + T(10));
|
|
g.v = f.v * (x*x*(x*(x*T(30) - T(60)) + T(30))/(edge1 - edge0));
|
|
return g;
|
|
}
|
|
}
|
|
template<typename T, int N>
|
|
inline ::ceres::Jet<T,N> smootherstep(T edge0, T edge1, ::ceres::Jet<T,N>&& f, ceres_jet_tag)
|
|
{
|
|
if (f.a >= edge1)
|
|
return ::ceres::Jet<T,N>(1);
|
|
else if (f.a <= edge0)
|
|
return ::ceres::Jet<T,N>(0);
|
|
else {
|
|
T x = (f.a - edge0)/(edge1 - edge0);
|
|
|
|
// f is moved into this function, so reuse it.
|
|
f.a = x*x*x*(x*(x*T(6) - T(15)) + T(10));
|
|
f.v *= (x*x*(x*(x*T(30) - T(60)) + T(30))/(edge1 - edge0));
|
|
return f;
|
|
}
|
|
}
|
|
template<typename T>
|
|
inline auto smootherstep(typename ad_traits<T>::scalar edge0, typename ad_traits<T>::scalar edge1, T&& val)
|
|
-> decltype(smootherstep(edge0, edge1, std::forward<T>(val), typename ad_traits<T>::ad_tag()))
|
|
{
|
|
return smootherstep(edge0, edge1, std::forward<T>(val), typename ad_traits<T>::ad_tag());
|
|
}
|
|
|
|
template<typename T>
|
|
inline T norm(T x, T y, scalar_tag) {
|
|
using std::sqrt;
|
|
using math::sq;
|
|
|
|
return sqrt(sq(x) + sq(y));
|
|
}
|
|
template<typename T, int N>
|
|
inline ::ceres::Jet<T,N> norm(const ::ceres::Jet<T,N>& x, const ::ceres::Jet<T,N>& y, ceres_jet_tag) {
|
|
T anorm = norm<T>(x.a, y.a, scalar_tag());
|
|
|
|
::ceres::Jet<T,N> g;
|
|
g.a = anorm;
|
|
g.v = (x.a/anorm)*x.v + (y.a/anorm)*y.v;
|
|
|
|
return g;
|
|
}
|
|
template<typename T>
|
|
inline typename std::decay<T>::type norm(T&& x, T&& y) {
|
|
return norm(std::forward<T>(x), std::forward<T>(y), typename ad_traits<T>::ad_tag());
|
|
}
|
|
|
|
template<typename T>
|
|
inline auto Heaviside(T&& val, typename ad_traits<T>::scalar epsilon) -> decltype(smootherstep(-epsilon, epsilon, std::forward<T>(val))) {
|
|
return smootherstep(-epsilon, epsilon, std::forward<T>(val));
|
|
}
|
|
|
|
template<typename Scalar>
|
|
cv::Rect bounding_box(const Ellipse2D<Scalar>& ellipse) {
|
|
using std::sin;
|
|
using std::cos;
|
|
using std::sqrt;
|
|
using std::floor;
|
|
using std::ceil;
|
|
|
|
Scalar ux = ellipse.major_radius * cos(ellipse.angle);
|
|
Scalar uy = ellipse.major_radius * sin(ellipse.angle);
|
|
Scalar vx = ellipse.minor_radius * cos(ellipse.angle + PI/2);
|
|
Scalar vy = ellipse.minor_radius * sin(ellipse.angle + PI/2);
|
|
|
|
Scalar bbox_halfwidth = sqrt(ux*ux + vx*vx);
|
|
Scalar bbox_halfheight = sqrt(uy*uy + vy*vy);
|
|
|
|
return cv::Rect((int)floor(ellipse.centre[0] - bbox_halfwidth), (int)floor(ellipse.centre[1] - bbox_halfheight),
|
|
(int)(2.0 * ceil(bbox_halfwidth) + 1.0),
|
|
(int)(2.0 * ceil(bbox_halfheight) + 1.0));
|
|
}
|
|
|
|
// Calculates:
|
|
// r * (1 - ||A(p - t)||)
|
|
//
|
|
// ||A(p - t)|| maps the ellipse to a unit circle
|
|
// 1 - ||A(p - t)|| measures signed distance from unit circle edge
|
|
// r * (1 - ||A(p - t)||) scales this to major radius of ellipse, for (roughly) pixel distance
|
|
//
|
|
// Actually use (r - ||rAp - rAt||) and precalculate r, rA and rAt.
|
|
template<typename T>
|
|
class EllipseDistCalculator {
|
|
public:
|
|
typedef typename ad_traits<T>::scalar Const;
|
|
|
|
EllipseDistCalculator(const Ellipse2D<T>& ellipse) : r(ellipse.major_radius)
|
|
{
|
|
using std::sin;
|
|
using std::cos;
|
|
rA << r*cos(ellipse.angle)/ellipse.major_radius, r*sin(ellipse.angle)/ellipse.major_radius,
|
|
-r*sin(ellipse.angle)/ellipse.minor_radius, r*cos(ellipse.angle)/ellipse.minor_radius;
|
|
rAt = rA*ellipse.centre;
|
|
}
|
|
template<typename U>
|
|
T operator()(U&& x, U&& y) {
|
|
return calculate(std::forward<U>(x), std::forward<U>(y), typename ad_traits<T>::ad_tag(), typename ad_traits<U>::ad_tag());
|
|
}
|
|
|
|
template<typename U>
|
|
T calculate(U&& x, U&& y, scalar_tag, scalar_tag) {
|
|
T rAxt((rA(0,0) * x + rA(0,1) * y) - rAt[0]);
|
|
T rAyt((rA(1,0) * x + rA(1,1) * y) - rAt[1]);
|
|
|
|
T xy_dist = norm(rAxt, rAyt);
|
|
|
|
return (r - xy_dist);
|
|
}
|
|
|
|
// Expanded versions for Jet calculations so that Eigen can do some of its expression magic
|
|
template<typename U>
|
|
T calculate(U&& x, U&& y, scalar_tag, ceres_jet_tag) {
|
|
T rAxt(rA(0,0) * x.a + rA(0,1) * y.a - rAt[0],
|
|
rA(0,0) * x.v + rA(0,1) * y.v);
|
|
T rAyt(rA(1,0) * x.a + rA(1,1) * y.a - rAt[1],
|
|
rA(1,0) * x.v + rA(1,1) * y.v);
|
|
|
|
T xy_dist = norm(rAxt, rAyt);
|
|
|
|
return (r - xy_dist);
|
|
}
|
|
template<typename U>
|
|
T calculate(U&& x, U&& y, ceres_jet_tag, scalar_tag) {
|
|
T rAxt(rA(0,0).a * x + rA(0,1).a * y - rAt[0].a,
|
|
rA(0,0).v * x + rA(0,1).v * y - rAt[0].v);
|
|
T rAyt(rA(1,0).a * x + rA(1,1).a * y - rAt[1].a,
|
|
rA(1,0).v * x + rA(1,1).v * y - rAt[1].v);
|
|
|
|
T xy_dist = norm(rAxt, rAyt);
|
|
|
|
return (r - xy_dist);
|
|
}
|
|
template<typename U>
|
|
T calculate(U&& x, U&& y, ceres_jet_tag, ceres_jet_tag) {
|
|
T rAxt(rA(0,0).a * x.a + rA(0,1).a * y.a - rAt[0].a,
|
|
rA(0,0).v * x.a + rA(0,0).a * x.v + rA(0,1).v * y.a + rA(0,1).a * y.v - rAt[0].v);
|
|
T rAyt(rA(1,0).a * x.a + rA(1,1).a * y.a - rAt[1].a,
|
|
rA(1,0).v * x.a + rA(1,0).a * x.v + rA(1,1).v * y.a + rA(1,1).a * y.v - rAt[1].v);
|
|
|
|
T xy_dist = norm(rAxt, rAyt);
|
|
|
|
return (r - xy_dist);
|
|
}
|
|
private:
|
|
Eigen::Matrix<T, 2, 2> rA;
|
|
Eigen::Matrix<T, 2, 1> rAt;
|
|
T r;
|
|
};
|
|
|
|
// Calculates the x crossings of a conic at a given y value. Returns the number of crossings (0, 1 or 2)
|
|
template<typename Scalar>
|
|
int getXCrossing(const Conic<Scalar>& conic, Scalar y, Scalar& x1, Scalar& x2) {
|
|
using std::sqrt;
|
|
|
|
Scalar a = conic.A;
|
|
Scalar b = conic.B*y + conic.D;
|
|
Scalar c = conic.C*y*y + conic.E*y + conic.F;
|
|
|
|
Scalar det = b*b - 4*a*c;
|
|
if (det == 0) {
|
|
x1 = -b/(2*a);
|
|
return 1;
|
|
} else if (det < 0) {
|
|
return 0;
|
|
} else {
|
|
Scalar sqrtdet = sqrt(det);
|
|
x1 = (-b - sqrtdet)/(2*a);
|
|
x2 = (-b + sqrtdet)/(2*a);
|
|
return 2;
|
|
}
|
|
}
|
|
|
|
template<template<class, int> class Jet, class T, int N>
|
|
typename std::enable_if<std::is_same<typename ad_traits<Jet<T,N>>::ad_tag, ceres_jet_tag>::value, Ellipse2D<T>>::type
|
|
toConst(const Ellipse2D<Jet<T,N>>& ellipse) {
|
|
return Ellipse2D<T>(
|
|
ellipse.centre[0].a,
|
|
ellipse.centre[1].a,
|
|
ellipse.major_radius.a,
|
|
ellipse.minor_radius.a,
|
|
ellipse.angle.a);
|
|
}
|
|
|
|
template<class T>
|
|
Ellipse2D<T> scaledMajorRadius(const Ellipse2D<T>& ellipse, const T& target_radius) {
|
|
return Ellipse2D<T>(
|
|
ellipse.centre[0],
|
|
ellipse.centre[1],
|
|
target_radius,
|
|
target_radius * ellipse.minor_radius/ellipse.major_radius,
|
|
ellipse.angle);
|
|
};
|
|
|
|
namespace internal {
|
|
template<class T> T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, T band_width, T step_epsilon, scalar_tag);
|
|
template<class T> T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon, ceres_jet_tag);
|
|
}
|
|
|
|
// Calculates the "goodness" of an ellipse.
|
|
//
|
|
// This is defined as the difference in region means:
|
|
//
|
|
// μ⁻ - μ⁺
|
|
//
|
|
// where
|
|
// Σ_p (H(d(p)+w) - H(d(p))) I(p)
|
|
// μ⁻ = ------------------------------
|
|
// Σ_p (H(d(p)+w) - H(d(p)))
|
|
//
|
|
// Σ_p (H(d(p)+w) - H(d(p))) I(p)
|
|
// μ⁺ = ------------------------------
|
|
// Σ_p (H(d(p)+w) - H(d(p)))
|
|
//
|
|
// (see eqs 16, 20, 21 in the PETMEI paper)
|
|
//
|
|
// The ellipse distance d(p) is defined as
|
|
//
|
|
// d(p) = r * (1 - ||A(p - t)||)
|
|
//
|
|
// with r as the major radius and A as the matrix that transforms the ellipse to a unit circle.
|
|
//
|
|
// ||A(p - t)|| maps the ellipse to a unit circle
|
|
// 1 - ||A(p - t)|| measures signed distance from unit circle edge
|
|
// r * (1 - ||A(p - t)||) scales this to major radius of ellipse, for (roughly) pixel distance
|
|
//
|
|
template<class T>
|
|
inline T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon) {
|
|
// band_width The width of each band (inner and outer)
|
|
// step_epsilon The epsilon of the soft step function
|
|
|
|
return internal::ellipseGoodness<T>(ellipse, eye, band_width, step_epsilon, typename ad_traits<T>::ad_tag());
|
|
}
|
|
|
|
//#define DEBUG_ELLIPSE_GOODNESS
|
|
//#define USE_INLINED_ELLIPSE_DIST
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
#define IF_INLINED_ELLIPSE_DIST(...) __VA_ARGS__
|
|
#else
|
|
#define IF_INLINED_ELLIPSE_DIST(...)
|
|
#endif
|
|
|
|
namespace internal {
|
|
// Non autodiff version of ellipse goodness calculation
|
|
template<class T>
|
|
T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, T band_width, T step_epsilon, scalar_tag) {
|
|
using std::max;
|
|
using std::min;
|
|
using std::ceil;
|
|
using std::floor;
|
|
using std::sin;
|
|
using std::cos;
|
|
|
|
// Ellipses (and corresponding conics) delimiting the region in which the band masks will be non-zero
|
|
Ellipse2D<T> outerEllipse = scaledMajorRadius(ellipse, ellipse.major_radius + ((band_width + step_epsilon) + 0.5));
|
|
Ellipse2D<T> innerEllipse = scaledMajorRadius(ellipse, ellipse.major_radius - ((band_width + step_epsilon) + 0.5));
|
|
Conic<T> outerConic(outerEllipse);
|
|
Conic<T> innerConic(innerEllipse);
|
|
|
|
// Variables for calculating the mean
|
|
T sum_inner = T(0), count_inner = T(0), sum_outer = T(0), count_outer = T(0);
|
|
|
|
// Only iterate over pixels within the outer ellipse's bounding box
|
|
cv::Rect bb = bounding_box(outerEllipse);
|
|
bb &= cv::Rect(-eye.cols/2,-eye.rows/2,eye.cols,eye.rows);
|
|
|
|
|
|
#ifndef USE_INLINED_ELLIPSE_DIST
|
|
// Ellipse distance calculator
|
|
EllipseDistCalculator<T> ellipDist(ellipse);
|
|
#else
|
|
// Instead of calculating
|
|
// r * (1 - ||A(p - t)||)
|
|
// we use
|
|
// (r - ||rAp - rAt||)
|
|
// and precalculate r, rA and rAt.
|
|
Eigen::Matrix<T, 2, 2> rA;
|
|
T r = ellipse.major_radius;
|
|
rA << r*cos(ellipse.angle)/ellipse.major_radius, r*sin(ellipse.angle)/ellipse.major_radius,
|
|
-r*sin(ellipse.angle)/ellipse.minor_radius, r*cos(ellipse.angle)/ellipse.minor_radius;
|
|
Eigen::Matrix<T, 2, 1> rAt = rA*ellipse.centre;
|
|
|
|
// Actually,
|
|
/// rAp - rAt = rA(0,y) + rA(x,0) - rAt
|
|
// So, can perform a strength reduction to calculate rAp iteratively.
|
|
|
|
// rA(0,y) - rAt, with y_0 = bb.y
|
|
Eigen::Matrix<T, 2, 1> rA0yrAt(rA(0,1) * bb.y - rAt[0], rA(1,1) * bb.y - rAt[1]);
|
|
// rA(1,0), for incrementing x
|
|
Eigen::Matrix<T, 2, 1> rA10 = rA.col(0);
|
|
// rA(0,1), for incrementing y
|
|
Eigen::Matrix<T, 2, 1> rA01 = rA.col(1);
|
|
#endif
|
|
|
|
for (int i = bb.y; i < bb.y + bb.height; ++i IF_INLINED_ELLIPSE_DIST(, rA0yrAt += rA01)) {
|
|
// Image row pointer -- (0,0) is centre of image, so shift accordingly
|
|
const uint8_t* eye_i = eye[i + eye.rows/2];
|
|
|
|
// Only iterate over pixels between the inner and outer ellipse
|
|
T ox1, ox2;
|
|
int outerCrossings = getXCrossing<T>(outerConic, i, ox1, ox2);
|
|
if (outerCrossings < 2) {
|
|
// If we don't cross the outer ellipse at all, exit early
|
|
continue;
|
|
}
|
|
T ix1, ix2;
|
|
int innerCrossings = innerEllipse.minor_radius > 0 ? getXCrossing<T>(innerConic, i, ix1, ix2) : 0;
|
|
|
|
// Define pairs of x values to iterate between
|
|
std::vector<std::pair<int,int>> xpairs;
|
|
if (innerCrossings < 2) {
|
|
// If we don't cross the inner ellipse, iterate between the two crossings of the outer ellipse
|
|
xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
|
|
} else {
|
|
// Otherwise, iterate between outer-->inner, then inner-->outer.
|
|
xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ix1), bb.x+bb.width-1));
|
|
xpairs.emplace_back(max<int>((int)floor(ix2),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
|
|
}
|
|
|
|
// Go over x pairs (that is, outer-->outer or outer-->inner,inner-->outer)
|
|
for (const auto& xpair : xpairs) {
|
|
// Pixel pointer, shifted accordingly
|
|
const uint8_t* eye_ij = eye_i + xpair.first + eye.cols/2;
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
// rA(0,y) + rA(x,0) - rAt, with x_0 = xpair.first
|
|
Eigen::Matrix<T, 2, 1> rApt(rA0yrAt(0) + rA(0,0)*xpair.first, rA0yrAt(1) + rA(1,0)*xpair.first);
|
|
#endif
|
|
|
|
for (int j = xpair.first; j <= xpair.second; ++j, ++eye_ij IF_INLINED_ELLIPSE_DIST(, rApt += rA10)) {
|
|
auto eye_ij_val = *eye_ij;
|
|
if (eye_ij_val > 200) {
|
|
// Ignore bright areas (i.e. glints)
|
|
continue;
|
|
}
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
T dist = (r - norm(rApt(0), rApt(1)));
|
|
#else
|
|
T dist = ellipDist(T(j), T(i));
|
|
#endif
|
|
|
|
// Calculate mask values for each band
|
|
T Hellip = Heaviside(dist, step_epsilon);
|
|
T Houter = Heaviside(dist+band_width, step_epsilon);
|
|
T Hinner = Heaviside(dist-band_width, step_epsilon);
|
|
|
|
T outer_weight = (Houter - Hellip);
|
|
T inner_weight = (Hellip - Hinner);
|
|
|
|
sum_outer += outer_weight * eye_ij_val;
|
|
count_outer += outer_weight;
|
|
|
|
sum_inner += inner_weight * eye_ij_val;
|
|
count_inner += inner_weight;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get mean values, defaulting to 255 and 0 if count_inner/count_outer are 0 (respectively)
|
|
// Using 255 and 0 because these are the "worst" values, so some pixels will be preferred over none.
|
|
T mu_inner = (count_inner==0 ? 255 : sum_inner/count_inner);
|
|
T mu_outer = (count_outer==0 ? 0 : sum_outer/count_outer);
|
|
|
|
// If count < 100 pixels, interpolate between mean value and "worst" value. This will push the
|
|
// gradient away from small pixel counts in a vaguely smooth way.
|
|
if (count_outer < 100) {
|
|
mu_outer = math::lerp<T>(0, mu_outer, count_outer/100.0);
|
|
}
|
|
if (count_inner < 100) {
|
|
mu_inner = math::lerp<T>(255, mu_inner, count_inner/100.0);
|
|
}
|
|
|
|
// Return difference of mean values
|
|
return mu_outer - mu_inner;
|
|
}
|
|
|
|
// Autodiff version of ellipse goodness calculation
|
|
template<class Jet>
|
|
Jet ellipseGoodness(const Ellipse2D<Jet>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<Jet>::scalar band_width, typename ad_traits<Jet>::scalar step_epsilon, ceres_jet_tag) {
|
|
using std::max;
|
|
using std::min;
|
|
using std::ceil;
|
|
using std::floor;
|
|
|
|
#ifdef DEBUG_ELLIPSE_GOODNESS
|
|
cv::Mat_<cv::Vec3b> eye_proc = cv::Mat_<cv::Vec3b>::zeros(eye.rows, eye.cols);
|
|
cv::Mat_<cv::Vec3b> eye_H = cv::Mat_<cv::Vec3b>::zeros(eye.rows, eye.cols);
|
|
#endif
|
|
|
|
typedef typename ad_traits<Jet>::scalar T;
|
|
typedef Jet Jet_t;
|
|
|
|
// A constant version of the ellipse
|
|
Ellipse2D<T> constEllipse = toConst(ellipse);
|
|
|
|
// Ellipses (and corresponding conics) delimiting the region in which the band masks will be non-zero
|
|
Ellipse2D<T> constOuterEllipse = scaledMajorRadius(constEllipse, constEllipse.major_radius + ((band_width + step_epsilon) + 0.5));
|
|
Ellipse2D<T> constInnerEllipse = scaledMajorRadius(constEllipse, constEllipse.major_radius - ((band_width + step_epsilon) + 0.5));
|
|
Conic<T> constOuterConic(constOuterEllipse);
|
|
Conic<T> constInnerConic(constInnerEllipse);
|
|
|
|
// Variables for calculating the mean
|
|
Jet_t sum_inner = Jet_t(0), count_inner = Jet_t(0), sum_outer = Jet_t(0), count_outer = Jet_t(0);
|
|
|
|
// Only iterate over pixels within the outer ellipse's bounding box
|
|
cv::Rect bb = bounding_box(constOuterEllipse);
|
|
bb &= cv::Rect(-eye.cols/2,-eye.rows/2,eye.cols,eye.rows);
|
|
|
|
|
|
#ifndef USE_INLINED_ELLIPSE_DIST
|
|
// Ellipse distance calculator
|
|
EllipseDistCalculator<Jet_t> ellipDist(ellipse);
|
|
EllipseDistCalculator<T> constEllipDist(constEllipse);
|
|
#else
|
|
// Instead of calculating
|
|
// r * (1 - ||A(p - t)||)
|
|
// we use
|
|
// (r - ||rAp - rAt||)
|
|
// and precalculate r, rA and rAt.
|
|
Eigen::Matrix<T, 2, 2> rA;
|
|
T r = constEllipse.major_radius;
|
|
rA << r*cos(constEllipse.angle)/constEllipse.major_radius, r*sin(constEllipse.angle)/constEllipse.major_radius,
|
|
-r*sin(constEllipse.angle)/constEllipse.minor_radius, r*cos(constEllipse.angle)/constEllipse.minor_radius;
|
|
Eigen::Matrix<T, 2, 1> rAt = rA*constEllipse.centre;
|
|
|
|
// And non-constant versions of the above
|
|
Eigen::Matrix<Jet_t, 2, 2> rA_jet;
|
|
Jet_t r_jet = ellipse.major_radius;
|
|
rA_jet << r_jet*cos(ellipse.angle)/ellipse.major_radius, r_jet*sin(ellipse.angle)/ellipse.major_radius,
|
|
-r_jet*sin(ellipse.angle)/ellipse.minor_radius, r_jet*cos(ellipse.angle)/ellipse.minor_radius;
|
|
Eigen::Matrix<Jet_t, 2, 1> rAt_jet = rA_jet*ellipse.centre;
|
|
|
|
// Actually,
|
|
/// rAp - rAt = rA(0,y) + rA(x,0) - rAt
|
|
// So, can perform a strength reduction to calculate rAp iteratively.
|
|
|
|
// rA(0,y) - rAt, with y_0 = bb.y
|
|
Eigen::Matrix<T, 2, 1> rA0yrAt(rA(0,1) * bb.y - rAt[0], rA(1,1) * bb.y - rAt[1]);
|
|
// rA(1,0), for incrementing x
|
|
Eigen::Matrix<T, 2, 1> rA10 = rA.col(0);
|
|
// rA(0,1), for incrementing y
|
|
Eigen::Matrix<T, 2, 1> rA01 = rA.col(1);
|
|
#endif
|
|
|
|
for (int i = bb.y; i < bb.y + bb.height; ++i IF_INLINED_ELLIPSE_DIST(, rA0yrAt += rA01)) {
|
|
// Image row pointer -- (0,0) is centre of image, so shift accordingly
|
|
const uint8_t* eye_i = eye[i + eye.rows/2];
|
|
|
|
// Only iterate over pixels between the inner and outer ellipse
|
|
T ox1, ox2;
|
|
int outerCrossings = getXCrossing<T>(constOuterConic, i, ox1, ox2);
|
|
if (outerCrossings < 2) {
|
|
// If we don't cross the outer ellipse at all, exit early
|
|
continue;
|
|
}
|
|
T ix1, ix2;
|
|
int innerCrossings = constInnerEllipse.major_radius > 0 ? getXCrossing<T>(constInnerConic, i, ix1, ix2) : 0;
|
|
|
|
// Define pairs of x values to iterate between
|
|
std::vector<std::pair<int,int>> xpairs;
|
|
if (innerCrossings < 2) {
|
|
// If we don't cross the inner ellipse, iterate between the two crossings of the outer ellipse
|
|
xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
|
|
} else {
|
|
// Otherwise, iterate between outer-->inner, then inner-->outer.
|
|
xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ix1), bb.x+bb.width-1));
|
|
xpairs.emplace_back(max<int>((int)floor(ix2),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
|
|
}
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
// Precalculate the gradient of
|
|
// rA(y,0) - rAt
|
|
auto rAy0rAt_x_v = (rA_jet(0,1).v * i - rAt_jet(0).v).eval();
|
|
auto rAy0rAt_y_v = (rA_jet(1,1).v * i - rAt_jet(1).v).eval();
|
|
#endif
|
|
|
|
// Go over x pairs (that is, outer-->outer or outer-->inner,inner-->outer)
|
|
for (const auto& xpair : xpairs) {
|
|
|
|
// Pixel pointer, shifted accordingly
|
|
const uint8_t* eye_ij = eye_i + xpair.first + eye.cols/2;
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
// rA(0,y) + rA(x,0) - rAt, with x_0 = xpair.first
|
|
Eigen::Matrix<T, 2, 1> rApt(rA0yrAt(0) + rA(0,0)*xpair.first, rA0yrAt(1) + rA(1,0)*xpair.first);
|
|
#endif
|
|
|
|
for (int j = xpair.first; j <= xpair.second; ++j, ++eye_ij IF_INLINED_ELLIPSE_DIST(, rApt += rA10)) {
|
|
|
|
T eye_ij_val = *eye_ij;
|
|
if (eye_ij_val > 200) {
|
|
// Ignore bright areas (i.e. glints)
|
|
continue;
|
|
}
|
|
|
|
// Calculate signed ellipse distance without gradient first, in case the gradient is 0
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
T dist_const = (r - norm(rApt(0), rApt(1)));
|
|
#else
|
|
T dist_const = constEllipDist(T(j), T(i));
|
|
#endif
|
|
|
|
// Check if we are within step_epsilon of the edges of the bands. If yes, calculate
|
|
// the gradient. Otherwise, the gradient is known to be 0.
|
|
if (abs(dist_const) < step_epsilon
|
|
|| abs(dist_const-band_width) < step_epsilon
|
|
|| abs(dist_const+band_width) < step_epsilon) {
|
|
|
|
#ifdef USE_INLINED_ELLIPSE_DIST
|
|
// Calculate the gradients of rApt, and use those to get the dist
|
|
Jet_t rAxt_jet(rApt(0),
|
|
rA_jet(0,0).v * j + rAy0rAt_x_v);
|
|
Jet_t rAyt_jet(rApt(1),
|
|
rA_jet(1,0).v * j + rAy0rAt_y_v);
|
|
|
|
//Eigen::Matrix<Jet,2,1> rApt_jet2 = rA_jet*Eigen::Matrix<Jet,2,1>(Jet(j),Jet(i)) - rAt_jet;
|
|
|
|
Jet_t dist = (r_jet - norm(rAxt_jet, rAyt_jet));
|
|
//Jet_t dist2 = ellipDist(T(j), T(i));
|
|
#else
|
|
Jet_t dist = ellipDist(T(j), T(i));
|
|
#endif
|
|
|
|
// Calculate mask values and derivatives for each band
|
|
Jet_t Hellip = Heaviside(dist, step_epsilon);
|
|
Jet_t Houter = Heaviside(dist+band_width, step_epsilon);
|
|
Jet_t Hinner = Heaviside(dist-band_width, step_epsilon);
|
|
|
|
Jet_t outer_weight = (Houter - Hellip);
|
|
Jet_t inner_weight = (Hellip - Hinner);
|
|
|
|
// Inline the Jet operator+= to allow eigen expression and noalias magic.
|
|
sum_outer.a += outer_weight.a * eye_ij_val;
|
|
sum_outer.v.noalias() += outer_weight.v * eye_ij_val;
|
|
count_outer.a += outer_weight.a;
|
|
count_outer.v.noalias() += outer_weight.v;
|
|
|
|
sum_inner.a += inner_weight.a * eye_ij_val;
|
|
sum_inner.v.noalias() += inner_weight.v * eye_ij_val;
|
|
count_inner.a += inner_weight.a;
|
|
count_inner.v.noalias() += inner_weight.v;
|
|
|
|
#ifdef DEBUG_ELLIPSE_GOODNESS
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight.a*255;
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight.a*255;
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[0] = 255;
|
|
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight.a * eye_ij_val;
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight.a * eye_ij_val;
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[0] = 255;
|
|
#endif
|
|
|
|
} else {
|
|
// Calculate mask values for each band
|
|
T Hellip = Heaviside(dist_const, step_epsilon);
|
|
T Houter = Heaviside(dist_const+band_width, step_epsilon);
|
|
T Hinner = Heaviside(dist_const-band_width, step_epsilon);
|
|
|
|
T outer_weight = (Houter - Hellip);
|
|
T inner_weight = (Hellip - Hinner);
|
|
|
|
sum_outer.a += outer_weight * eye_ij_val;
|
|
count_outer.a += outer_weight;
|
|
|
|
sum_inner.a += inner_weight * eye_ij_val;
|
|
count_inner.a += inner_weight;
|
|
|
|
#ifdef DEBUG_ELLIPSE_GOODNESS
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight*255;
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight*255;
|
|
eye_H(i + eye.rows/2,j + eye.cols/2)[0] = 0;
|
|
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight * eye_ij_val;
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight * eye_ij_val;
|
|
eye_proc(i + eye.rows/2,j + eye.cols/2)[0] = 255;
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get mean values, defaulting to 255 and 0 if count_inner/count_outer are 0 (respectively)
|
|
// Using 255 and 0 because these are the "worst" values, so some pixels will be preferred over none.
|
|
Jet mu_inner = (count_inner.a==0 ? Jet(255) : sum_inner/count_inner);
|
|
Jet mu_outer = (count_outer.a==0 ? Jet(0) : sum_outer/count_outer);
|
|
|
|
// If count < 100 pixels, interpolate between mean value and "worst" value. This will push the
|
|
// gradient away from small pixel counts in a vaguely smooth way.
|
|
if (count_outer.a < 100) {
|
|
mu_outer = math::lerp<Jet>(Jet(0), mu_outer, count_outer/100.0);
|
|
}
|
|
if (count_inner.a < 100) {
|
|
mu_inner = math::lerp<Jet>(Jet(255), mu_inner, count_inner/100.0);
|
|
}
|
|
|
|
// Return difference of mean values
|
|
return mu_outer - mu_inner;
|
|
}
|
|
}
|
|
|
|
template<typename T>
|
|
Eigen::Matrix<T,3,1> sph2cart(T r, T theta, T psi) {
|
|
using std::sin;
|
|
using std::cos;
|
|
|
|
return r * Eigen::Matrix<T,3,1>(sin(theta)*cos(psi), cos(theta), sin(theta)*sin(psi));
|
|
}
|
|
|
|
template<typename T>
|
|
T angleDiffGoodness(T theta1, T psi1, T theta2, T psi2, typename ad_traits<T>::scalar sigma) {
|
|
using std::sin;
|
|
using std::cos;
|
|
using std::acos;
|
|
using std::asin;
|
|
using std::atan2;
|
|
using std::sqrt;
|
|
|
|
if (theta1 == theta2 && psi1 == psi2) {
|
|
return T(1);
|
|
}
|
|
|
|
// Haversine distance
|
|
auto dist = T(2)*asin(sqrt(sq(sin((theta1-theta2)/T(2))) + cos(theta1)*cos(theta2)*sq(sin((psi1-psi2)/T(2)))));
|
|
return exp(-sq(dist)/sq(sigma));
|
|
}
|
|
|
|
template<typename T>
|
|
Circle3D<T> circleOnSphere(const Sphere<T>& sphere, T theta, T psi, T circle_radius) {
|
|
typedef Eigen::Matrix<T,3,1> Vector3;
|
|
|
|
Vector3 radial = sph2cart<T>(T(1), theta, psi);
|
|
return Circle3D<T>(sphere.centre + sphere.radius * radial,
|
|
radial,
|
|
circle_radius);
|
|
}
|
|
|
|
template<typename T>
|
|
struct EllipseGoodnessFunction {
|
|
T operator()(const Sphere<T>& eye, T theta, T psi, T pupil_radius, T focal_length, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon, const cv::Mat& mEye) {
|
|
typedef Eigen::Matrix<T,3,1> Vector3;
|
|
typedef typename ad_traits<T>::scalar Const;
|
|
|
|
static const Vector3 camera_centre(T(0),T(0),T(0));
|
|
|
|
// Check for bounds. The worst possible value of ellipseGoodness is -255, so use that as a starting point for out-of-bounds pupils
|
|
|
|
// Pupil radius must be positive
|
|
if (pupil_radius <= Const(0))
|
|
{
|
|
// Return -255 for radius == 0, and even lower values for
|
|
// radius < 0
|
|
// This should push the gradient towards positive radius,
|
|
// rather than just returning flat -255
|
|
return Const(-255.0) + pupil_radius;
|
|
}
|
|
|
|
Circle3D<T> pupil_circle = circleOnSphere(eye, theta, psi, pupil_radius);
|
|
|
|
// Ellipse normal must point towards camera
|
|
T normalDotPos = pupil_circle.normal.dot(camera_centre - pupil_circle.centre);
|
|
if (normalDotPos <= Const(0))
|
|
{
|
|
// Return -255 for normalDotPos == 0, and even lower values for
|
|
// normalDotPos < 0
|
|
// This should push the gradient towards positive normalDotPos,
|
|
// rather than just returning flat -255
|
|
return Const(-255.0) + normalDotPos;
|
|
}
|
|
|
|
// Angles should be in the range
|
|
// theta: 0 -> pi
|
|
// psi: -pi -> 0
|
|
// If we're outside of this range AND radialDotEye > 0, then we must
|
|
// have gone all the way around, so just return worst case (i.e as bad
|
|
// as radialDotEye == -1) with additional penalty for how far out we
|
|
// are, again to push the gradient back inwards.
|
|
if (theta < Const(0) || theta > Const(PI) || psi < Const(-PI) || psi > Const(0))
|
|
{
|
|
T ret = Const(-255.0) - (camera_centre - pupil_circle.centre).norm();
|
|
if (theta < Const(0))
|
|
ret -= (Const(0) - theta);
|
|
else if (theta > Const(PI))
|
|
ret -= (theta - Const(PI));
|
|
if (psi < Const(-PI))
|
|
ret -= (Const(-PI) - psi);
|
|
else if (psi > Const(0))
|
|
ret -= (psi - Const(0));
|
|
}
|
|
|
|
// Ok, everything looks good so far, calculate the actual goodness.
|
|
|
|
Ellipse2D<T> pupil_ellipse(project(pupil_circle, focal_length));
|
|
|
|
return ellipseGoodness<T>(pupil_ellipse, mEye, band_width, step_epsilon);
|
|
}
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
template<typename Scalar>
|
|
class EllipseDistanceResidualFunction {
|
|
public:
|
|
EllipseDistanceResidualFunction(const cv::Mat& eye_image, const std::vector<cv::Point2f>& pupil_inliers, const Scalar& eye_radius, const Scalar& focal_length) :
|
|
eye_image(eye_image), pupil_inliers(pupil_inliers), eye_radius(eye_radius), focal_length(focal_length) {}
|
|
|
|
template <typename T>
|
|
bool operator()(const T* const eye_param, const T* const pupil_param, T* e) const {
|
|
typedef typename ad_traits<T>::scalar Const;
|
|
|
|
Eigen::Matrix<T,3,1> eye_pos(eye_param[0], eye_param[1], eye_param[2]);
|
|
Sphere<T> eye(eye_pos, T(eye_radius));
|
|
|
|
Ellipse2D<T> pupil_ellipse(project(circleOnSphere(eye, pupil_param[0], pupil_param[1], pupil_param[2]), T(focal_length)));
|
|
|
|
EllipseDistCalculator<T> ellipDist(pupil_ellipse);
|
|
|
|
for (int i = 0; i < pupil_inliers.size(); ++i) {
|
|
const cv::Point2f& inlier = pupil_inliers[i];
|
|
e[i] = ellipDist(Const(inlier.x), Const(inlier.y));
|
|
}
|
|
|
|
return true;
|
|
}
|
|
private:
|
|
const cv::Mat& eye_image;
|
|
const std::vector<cv::Point2f>& pupil_inliers;
|
|
const Scalar& eye_radius;
|
|
const Scalar& focal_length;
|
|
};
|
|
|
|
|
|
template<typename Scalar>
|
|
struct EllipsePointDistanceFunction {
|
|
EllipsePointDistanceFunction(const Ellipse2D<Scalar>& el, Scalar x, Scalar y) : el(el), x(x), y(y) {}
|
|
|
|
template <typename T>
|
|
bool operator()(const T* const t, T* e) const
|
|
{
|
|
using std::sin;
|
|
using std::cos;
|
|
|
|
auto&& pt = pointAlongEllipse(el, t[0]);
|
|
e[0] = norm(x - pt.x(), y - pt.y());
|
|
|
|
return true;
|
|
}
|
|
|
|
const Ellipse2D<Scalar>& el;
|
|
Scalar x, y;
|
|
};
|
|
|
|
template<bool has_eye_var=true>
|
|
struct PupilContrastTerm : public spii::Term {
|
|
const Sphere<double>& init_eye;
|
|
double focal_length;
|
|
const cv::Mat eye_image;
|
|
double band_width;
|
|
double step_epsilon;
|
|
|
|
int eye_var_idx() const { return has_eye_var ? 0 : -1; }
|
|
int pupil_var_idx() const { return has_eye_var ? 1 : 0; }
|
|
|
|
PupilContrastTerm(const Sphere<double>& eye, double focal_length, cv::Mat eye_image, double band_width, double step_epsilon) :
|
|
init_eye(eye),
|
|
focal_length(focal_length),
|
|
eye_image(eye_image),
|
|
band_width(band_width),
|
|
step_epsilon(step_epsilon)
|
|
{}
|
|
|
|
virtual int number_of_variables() const override {
|
|
int nvars = 1; // This pupil params
|
|
if (has_eye_var)
|
|
nvars++; // Eye params
|
|
|
|
return nvars;
|
|
}
|
|
virtual int variable_dimension(int var) const override {
|
|
if (var == eye_var_idx()) // Eye params (x,y,z)
|
|
return 3;
|
|
if (var == pupil_var_idx()) // This pupil params (theta, psi, r)
|
|
return 3;
|
|
return -1;
|
|
};
|
|
virtual double evaluate(double * const * const vars) const override
|
|
{
|
|
auto& pupil_vars = vars[pupil_var_idx()];
|
|
|
|
auto eye = init_eye;
|
|
if (has_eye_var) {
|
|
auto& eye_vars = vars[eye_var_idx()];
|
|
eye.centre = Sphere<double>::Vector(eye_vars[0], eye_vars[1], eye_vars[2]);
|
|
}
|
|
|
|
EllipseGoodnessFunction<double> goodnessFunction;
|
|
auto theta = pupil_vars[0];
|
|
auto psi = pupil_vars[1];
|
|
auto r = pupil_vars[2];
|
|
auto goodness = goodnessFunction(eye,
|
|
theta, psi, r,
|
|
focal_length,
|
|
band_width, step_epsilon,
|
|
eye_image);
|
|
|
|
return -goodness;
|
|
}
|
|
virtual double evaluate(double * const * const vars, std::vector<Eigen::VectorXd>* gradient) const override
|
|
{
|
|
auto& pupil_vars = vars[pupil_var_idx()];
|
|
|
|
double contrast_goodness_a;
|
|
Eigen::Matrix<double,3,1> eye_contrast_goodness_v;
|
|
Eigen::Matrix<double,3,1> pupil_contrast_goodness_v;
|
|
|
|
// Get region contrast goodness using EllipseGoodnessFunction.
|
|
if (has_eye_var) {
|
|
// If varying the eye parameters, calculate the gradient wrt. to 6 params (3 eye + 3 pupil)
|
|
typedef ceres::Jet<double, 6> EyePupilJet;
|
|
|
|
auto& eye_vars = vars[eye_var_idx()];
|
|
Eigen::Matrix<EyePupilJet,3,1> eye_pos(EyePupilJet(eye_vars[0], 0), EyePupilJet(eye_vars[1], 1), EyePupilJet(eye_vars[2], 2));
|
|
Sphere<EyePupilJet> eye(eye_pos, EyePupilJet(init_eye.radius));
|
|
|
|
EyePupilJet contrast_goodness;
|
|
{
|
|
EllipseGoodnessFunction<EyePupilJet> goodnessFunction;
|
|
auto theta = EyePupilJet(pupil_vars[0], 3);
|
|
auto psi = EyePupilJet(pupil_vars[1], 4);
|
|
auto r = EyePupilJet(pupil_vars[2], 5);
|
|
contrast_goodness = goodnessFunction(eye,
|
|
theta, psi, r,
|
|
EyePupilJet(focal_length),
|
|
band_width, step_epsilon,
|
|
eye_image);
|
|
}
|
|
|
|
contrast_goodness_a = contrast_goodness.a;
|
|
eye_contrast_goodness_v = contrast_goodness.v.segment<3>(0);
|
|
pupil_contrast_goodness_v = contrast_goodness.v.segment<3>(3);
|
|
} else {
|
|
// Otherwise, calculate the gradient wrt. to the 3 pupil params
|
|
typedef ::ceres::Jet<double,3> PupilJet;
|
|
|
|
Eigen::Matrix<PupilJet,3,1> eye_pos(PupilJet(init_eye.centre[0]), PupilJet(init_eye.centre[1]), PupilJet(init_eye.centre[2]));
|
|
::Sphere<PupilJet> eye(eye_pos, PupilJet(init_eye.radius));
|
|
|
|
PupilJet contrast_goodness;
|
|
{
|
|
EllipseGoodnessFunction<PupilJet> goodnessFunction;
|
|
auto theta = PupilJet(pupil_vars[0], 0);
|
|
auto psi = PupilJet(pupil_vars[1], 1);
|
|
auto r = PupilJet(pupil_vars[2], 2);
|
|
contrast_goodness = goodnessFunction(eye,
|
|
theta, psi, r,
|
|
PupilJet(focal_length),
|
|
band_width, step_epsilon,
|
|
eye_image);
|
|
}
|
|
|
|
contrast_goodness_a = contrast_goodness.a;
|
|
pupil_contrast_goodness_v = contrast_goodness.v;
|
|
}
|
|
|
|
double goodness;
|
|
auto& eye_gradient = (*gradient)[eye_var_idx()];
|
|
auto& pupil_gradient = (*gradient)[pupil_var_idx()];
|
|
|
|
// No smoothness term, goodness and gradient are based only on frame goodness
|
|
goodness = contrast_goodness_a;
|
|
if (has_eye_var)
|
|
eye_gradient = eye_contrast_goodness_v;
|
|
pupil_gradient = pupil_contrast_goodness_v;
|
|
|
|
// Flip sign to change goodness into cost (i.e. maximising into minimising)
|
|
auto cost = -goodness;
|
|
for (int i = 0; i < number_of_variables(); ++i) {
|
|
(*gradient)[i] = -(*gradient)[i];
|
|
}
|
|
return cost;
|
|
}
|
|
virtual double evaluate(double * const * const variables,
|
|
std::vector<Eigen::VectorXd>* gradient,
|
|
std::vector< std::vector<Eigen::MatrixXd> >* hessian) const override {
|
|
throw std::runtime_error("Not implemented");
|
|
}
|
|
|
|
};
|
|
|
|
// Anthropomorphic term
|
|
struct PupilAnthroTerm : public spii::Term {
|
|
double mean;
|
|
double sigma;
|
|
double scale;
|
|
|
|
PupilAnthroTerm(double mean, double sigma, double scale) : mean(mean), sigma(sigma), scale(scale)
|
|
{}
|
|
|
|
virtual int number_of_variables() const override {
|
|
int nvars = 1; // This pupil params
|
|
return nvars;
|
|
}
|
|
virtual int variable_dimension(int var) const override {
|
|
if (var == 0) // This pupil params (r)
|
|
return 3;
|
|
return -1;
|
|
}
|
|
virtual double evaluate(double * const * const vars) const override
|
|
{
|
|
using math::sq;
|
|
|
|
auto r = vars[0][2];
|
|
auto radius_anthro_goodness = exp(-sq(r - mean)/sq(sigma));
|
|
|
|
double goodness = radius_anthro_goodness;
|
|
|
|
// Flip sign to change goodness into cost (i.e. maximising into minimising)
|
|
auto cost = -goodness*scale;
|
|
return cost;
|
|
}
|
|
virtual double evaluate(double * const * const vars, std::vector<Eigen::VectorXd>* gradient) const override
|
|
{
|
|
using math::sq;
|
|
|
|
auto r = ceres::Jet<double,1>(vars[0][2], 0);
|
|
auto radius_anthro_goodness = exp(-sq(r - mean)/sq(sigma));
|
|
|
|
double goodness = radius_anthro_goodness.a;
|
|
(*gradient)[0].segment<1>(2) = radius_anthro_goodness.v;
|
|
|
|
// Flip sign to change goodness into cost (i.e. maximising into minimising)
|
|
auto cost = -goodness*scale;
|
|
for (int i = 0; i < number_of_variables(); ++i) {
|
|
(*gradient)[i] = -(*gradient)[i]*scale;
|
|
}
|
|
return cost;
|
|
}
|
|
virtual double evaluate(double * const * const variables,
|
|
std::vector<Eigen::VectorXd>* gradient,
|
|
std::vector< std::vector<Eigen::MatrixXd> >* hessian) const override {
|
|
throw std::runtime_error("Not implemented");
|
|
}
|
|
|
|
};
|
|
|
|
const EyeModelFitter::Vector3 EyeModelFitter::camera_centre = EyeModelFitter::Vector3::Zero();
|
|
|
|
|
|
EyeModelFitter::Pupil::Pupil(Observation observation) : observation(observation), params(0, 0, 0)
|
|
{
|
|
|
|
}
|
|
|
|
EyeModelFitter::Pupil::Pupil()
|
|
{
|
|
|
|
}
|
|
|
|
|
|
EyeModelFitter::PupilParams::PupilParams(double theta, double psi, double radius) : theta(theta), psi(psi), radius(radius)
|
|
{
|
|
|
|
}
|
|
|
|
EyeModelFitter::PupilParams::PupilParams() : theta(0), psi(0), radius(0)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
EyeModelFitter::Observation::Observation(cv::Mat image, Ellipse ellipse, std::vector<cv::Point2f> inliers) : image(std::move(image)), ellipse(std::move(ellipse)), inliers(std::move(inliers))
|
|
{
|
|
|
|
}
|
|
|
|
EyeModelFitter::Observation::Observation()
|
|
{
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
singleeyefitter::EyeModelFitter::EyeModelFitter() : region_band_width(5), region_step_epsilon(0.5), region_scale(1)
|
|
{
|
|
|
|
}
|
|
singleeyefitter::EyeModelFitter::EyeModelFitter(double focal_length, double region_band_width, double region_step_epsilon) : focal_length(focal_length), region_band_width(region_band_width), region_step_epsilon(region_step_epsilon), region_scale(1)
|
|
{
|
|
|
|
}
|
|
|
|
singleeyefitter::EyeModelFitter::Index singleeyefitter::EyeModelFitter::add_observation(cv::Mat image, Ellipse pupil, int n_pseudo_inliers /*= 0*/)
|
|
{
|
|
std::vector<cv::Point2f> pupil_inliers;
|
|
for (int i = 0; i < n_pseudo_inliers; ++i) {
|
|
auto p = pointAlongEllipse(pupil, i * 2 * M_PI / n_pseudo_inliers);
|
|
pupil_inliers.emplace_back(static_cast<float>(p[0]), static_cast<float>(p[1]));
|
|
}
|
|
return add_observation(std::move(image), std::move(pupil), std::move(pupil_inliers));
|
|
}
|
|
|
|
singleeyefitter::EyeModelFitter::Index singleeyefitter::EyeModelFitter::add_observation(cv::Mat image, Ellipse pupil, std::vector<cv::Point2f> pupil_inliers)
|
|
{
|
|
assert(image.channels() == 1 && image.depth() == CV_8U);
|
|
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
pupils.emplace_back(
|
|
Observation(std::move(image), std::move(pupil), std::move(pupil_inliers))
|
|
);
|
|
return pupils.size() - 1;
|
|
}
|
|
|
|
void EyeModelFitter::reset()
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
pupils.clear();
|
|
eye = Sphere::Null;
|
|
model_version++;
|
|
}
|
|
|
|
singleeyefitter::EyeModelFitter::Circle singleeyefitter::EyeModelFitter::circleFromParams(const Sphere& eye, const PupilParams& params)
|
|
{
|
|
if (params.radius == 0)
|
|
return Circle::Null;
|
|
|
|
Vector3 radial = sph2cart<double>(double(1), params.theta, params.psi);
|
|
return Circle(eye.centre + eye.radius * radial,
|
|
radial,
|
|
params.radius);
|
|
}
|
|
|
|
singleeyefitter::EyeModelFitter::Circle singleeyefitter::EyeModelFitter::circleFromParams(const PupilParams& params) const
|
|
{
|
|
return circleFromParams(eye, params);
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::print_single_contrast_metric(const Pupil& pupil) const
|
|
{
|
|
if (!pupil.circle) {
|
|
std::cout << "No pupil" << std::endl;
|
|
return;
|
|
}
|
|
|
|
double params[3];
|
|
params[0] = pupil.params.theta;
|
|
params[1] = pupil.params.psi;
|
|
params[2] = pupil.params.radius;
|
|
double* vars[1];
|
|
vars[0] = params;
|
|
|
|
std::vector<Eigen::VectorXd> gradient;
|
|
gradient.push_back(Eigen::VectorXd::Zero(3));
|
|
|
|
PupilContrastTerm<false> contrast_term(
|
|
eye,
|
|
focal_length * region_scale,
|
|
cvx::resize(pupil.observation.image, region_scale),
|
|
region_band_width,
|
|
region_step_epsilon);
|
|
|
|
double contrast_val = contrast_term.evaluate(vars, &gradient);
|
|
|
|
std::cout << "Contrast term: " << contrast_val << std::endl;
|
|
std::cout << " gradient: [ " << gradient[0].transpose() << " ]" << std::endl;
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::print_single_contrast_metric(Index id) const
|
|
{
|
|
print_single_contrast_metric(pupils[id]);
|
|
}
|
|
|
|
double singleeyefitter::EyeModelFitter::single_contrast_metric(const Pupil& pupil) const
|
|
{
|
|
if (!pupil.circle) {
|
|
std::cout << "No pupil" << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
double params[3];
|
|
params[0] = pupil.params.theta;
|
|
params[1] = pupil.params.psi;
|
|
params[2] = pupil.params.radius;
|
|
double* vars[1];
|
|
vars[0] = params;
|
|
|
|
PupilContrastTerm<false> contrast_term(
|
|
eye,
|
|
focal_length * region_scale,
|
|
cvx::resize(pupil.observation.image, region_scale),
|
|
region_band_width,
|
|
region_step_epsilon);
|
|
|
|
double contrast_val = contrast_term.evaluate(vars);
|
|
|
|
return contrast_val;
|
|
}
|
|
|
|
double singleeyefitter::EyeModelFitter::single_contrast_metric(Index id) const
|
|
{
|
|
return single_contrast_metric(pupils[id]);
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::refine_single_with_contrast(Pupil& pupil)
|
|
{
|
|
if (!pupil.circle)
|
|
return pupil.circle;
|
|
|
|
double params[3];
|
|
params[0] = pupil.params.theta;
|
|
params[1] = pupil.params.psi;
|
|
params[2] = pupil.params.radius;
|
|
|
|
spii::Function f;
|
|
f.add_variable(¶ms[0], 3);
|
|
f.add_term(std::make_shared<PupilContrastTerm<false>>(
|
|
eye,
|
|
focal_length * region_scale,
|
|
cvx::resize(pupil.observation.image, region_scale),
|
|
region_band_width,
|
|
region_step_epsilon), ¶ms[0]);
|
|
|
|
spii::LBFGSSolver solver;
|
|
|
|
// Commented out due to Visual Studio 2015 error
|
|
//solver.log_function = [](const std::string&) {};
|
|
|
|
//solver.function_improvement_tolerance = 1e-5;
|
|
spii::SolverResults results;
|
|
solver.solve(f, &results);
|
|
//std::cout << results << std::endl;
|
|
|
|
pupil.params = PupilParams(params[0], params[1], params[2]);
|
|
pupil.circle = circleFromParams(pupil.params);
|
|
|
|
return pupil.circle;
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::refine_single_with_contrast(Index id)
|
|
{
|
|
return refine_single_with_contrast(pupils[id]);
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::initialise_single_observation(Pupil& pupil)
|
|
{
|
|
// Ignore the pupil circle normal, and intersect the pupil circle
|
|
// centre projection line with the eyeball sphere
|
|
try {
|
|
auto pupil_centre_sphere_intersect = intersect(Line3(camera_centre, pupil.circle.centre.normalized()),
|
|
eye);
|
|
auto new_pupil_centre = pupil_centre_sphere_intersect.first;
|
|
|
|
// Now that we have 3D positions for the pupil (rather than just a
|
|
// projection line), recalculate the pupil radius at that position.
|
|
auto pupil_radius_at_1 = pupil.circle.radius / pupil.circle.centre.z();
|
|
auto new_pupil_radius = pupil_radius_at_1 * new_pupil_centre.z();
|
|
|
|
// Parametrise this new pupil position using spherical coordinates
|
|
Vector3 centre_to_pupil = new_pupil_centre - eye.centre;
|
|
double r = centre_to_pupil.norm();
|
|
pupil.params.theta = acos(centre_to_pupil[1] / r);
|
|
pupil.params.psi = atan2(centre_to_pupil[2], centre_to_pupil[0]);
|
|
pupil.params.radius = new_pupil_radius;
|
|
|
|
// Update pupil circle to match parameters
|
|
pupil.circle = circleFromParams(pupil.params);
|
|
}
|
|
catch (no_intersection_exception&) {
|
|
pupil.circle = Circle::Null;
|
|
pupil.params.theta = 0;
|
|
pupil.params.psi = 0;
|
|
pupil.params.radius = 0;
|
|
}
|
|
|
|
return pupil.circle;
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::initialise_single_observation(Index id)
|
|
{
|
|
initialise_single_observation(pupils[id]);
|
|
|
|
/*if (id > 0 && pupils[id-1].circle) {
|
|
// Try previous circle in case of bad fits
|
|
EllipseGoodnessFunction<double> goodnessFunction;
|
|
auto& pupil = pupils[id];
|
|
auto& prevPupil = pupils[id-1];
|
|
|
|
double currentGoodness, prevGoodness;
|
|
if (pupil.circle) {
|
|
currentGoodness = goodnessFunction(eye, pupil.params.theta, pupil.params.psi, pupil.params.radius, focal_length, pupil.observation.image);
|
|
prevGoodness = goodnessFunction(eye, prevPupil.params.theta, prevPupil.params.psi, prevPupil.params.radius, focal_length, pupil.observation.image);
|
|
}
|
|
|
|
if (!pupil.circle || prevGoodness > currentGoodness) {
|
|
pupil.circle = prevPupil.circle;
|
|
pupil.params = prevPupil.params;
|
|
}
|
|
}*/
|
|
|
|
return pupils[id].circle;
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::unproject_single_observation(Pupil& pupil, double pupil_radius /*= 1*/) const
|
|
{
|
|
if (eye == Sphere::Null) {
|
|
throw std::runtime_error("Need to get eye centre estimate first (by unprojecting multiple observations)");
|
|
}
|
|
|
|
// Single pupil version of "unproject_observations"
|
|
|
|
auto unprojection_pair = unproject(pupil.observation.ellipse, pupil_radius, focal_length);
|
|
|
|
const Vector3& c = unprojection_pair.first.centre;
|
|
const Vector3& v = unprojection_pair.first.normal;
|
|
|
|
Vector2 c_proj = project(c, focal_length);
|
|
Vector2 v_proj = project(v + c, focal_length) - c_proj;
|
|
|
|
v_proj.normalize();
|
|
|
|
Vector2 eye_centre_proj = project(eye.centre, focal_length);
|
|
|
|
if ((c_proj - eye_centre_proj).dot(v_proj) >= 0) {
|
|
pupil.circle = std::move(unprojection_pair.first);
|
|
}
|
|
else {
|
|
pupil.circle = std::move(unprojection_pair.second);
|
|
}
|
|
|
|
return pupil.circle;
|
|
}
|
|
|
|
const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::unproject_single_observation(Index id, double pupil_radius /*= 1*/)
|
|
{
|
|
return unproject_single_observation(pupils[id], pupil_radius);
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::refine_with_inliers(const CallbackFunction& callback /*= CallbackFunction()*/)
|
|
{
|
|
int current_model_version;
|
|
Eigen::Matrix<double, Eigen::Dynamic, 1> x;
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
current_model_version = model_version;
|
|
|
|
x = Eigen::Matrix<double, Eigen::Dynamic, 1>(3 + 3 * pupils.size());
|
|
x.segment<3>(0) = eye.centre;
|
|
for (int i = 0; i < pupils.size(); ++i) {
|
|
const PupilParams& pupil_params = pupils[i].params;
|
|
x.segment<3>(3 + 3 * i)[0] = pupil_params.theta;
|
|
x.segment<3>(3 + 3 * i)[1] = pupil_params.psi;
|
|
x.segment<3>(3 + 3 * i)[2] = pupil_params.radius;
|
|
}
|
|
}
|
|
|
|
ceres::Problem problem;
|
|
for (int i = 0; i < pupils.size(); ++i) {
|
|
const cv::Mat& eye_image = pupils[i].observation.image;
|
|
const auto& pupil_inliers = pupils[i].observation.inliers;
|
|
|
|
problem.AddResidualBlock(
|
|
new ceres::AutoDiffCostFunction<EllipseDistanceResidualFunction<double>, ceres::DYNAMIC, 3, 3>(
|
|
new EllipseDistanceResidualFunction<double>(eye_image, pupil_inliers, eye.radius, focal_length),
|
|
(int)pupil_inliers.size()
|
|
),
|
|
NULL, &x[0], &x[3 + 3 * i]);
|
|
}
|
|
|
|
ceres::Solver::Options options;
|
|
options.linear_solver_type = ceres::DENSE_SCHUR;
|
|
options.max_num_iterations = 1000;
|
|
options.function_tolerance = 1e-10;
|
|
options.minimizer_progress_to_stdout = true;
|
|
options.update_state_every_iteration = true;
|
|
if (callback) {
|
|
struct CallCallbackWrapper : public ceres::IterationCallback
|
|
{
|
|
double eye_radius;
|
|
const CallbackFunction& callback;
|
|
const Eigen::Matrix<double, Eigen::Dynamic, 1>& x;
|
|
|
|
CallCallbackWrapper(const EyeModelFitter& fitter, const CallbackFunction& callback, const Eigen::Matrix<double, Eigen::Dynamic, 1>& x)
|
|
: eye_radius(fitter.eye.radius), callback(callback), x(x) {}
|
|
|
|
virtual ceres::CallbackReturnType operator() (const ceres::IterationSummary& summary) {
|
|
Eigen::Matrix<double, 3, 1> eye_pos(x[0], x[1], x[2]);
|
|
Sphere eye(eye_pos, eye_radius);
|
|
|
|
std::vector<Circle> pupils;
|
|
for (int i = 0; i < (x.size() - 3)/3; ++i) {
|
|
auto&& pupil_param_v = x.segment<3>(3 + 3 * i);
|
|
pupils.push_back(EyeModelFitter::circleFromParams(eye, PupilParams(pupil_param_v[0], pupil_param_v[1], pupil_param_v[2])));
|
|
}
|
|
|
|
callback(eye, pupils);
|
|
|
|
return ceres::SOLVER_CONTINUE;
|
|
}
|
|
};
|
|
options.callbacks.push_back(new CallCallbackWrapper(*this, callback, x));
|
|
}
|
|
ceres::Solver::Summary summary;
|
|
ceres::Solve(options, &problem, &summary);
|
|
std::cout << summary.BriefReport() << "\n";
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
if (current_model_version != model_version) {
|
|
std::cout << "Old model, not applying refined parameters" << std::endl;
|
|
return;
|
|
}
|
|
|
|
eye.centre = x.segment<3>(0);
|
|
|
|
for (int i = 0; i < pupils.size(); ++i) {
|
|
auto&& pupil_param = x.segment<3>(3 + 3 * i);
|
|
pupils[i].params = PupilParams(pupil_param[0], pupil_param[1], pupil_param[2]);
|
|
pupils[i].circle = circleFromParams(eye, pupils[i].params);
|
|
}
|
|
}
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::refine_with_region_contrast(const CallbackFunction& callback /*= CallbackFunction()*/)
|
|
{
|
|
int current_model_version;
|
|
Eigen::Matrix<double, Eigen::Dynamic, 1> x0;
|
|
spii::Function f;
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
current_model_version = model_version;
|
|
|
|
x0 = Eigen::Matrix<double, Eigen::Dynamic, 1>(3 + 3 * pupils.size());
|
|
x0.segment<3>(0) = eye.centre;
|
|
for (int i = 0; i < pupils.size(); ++i) {
|
|
const PupilParams& pupil_params = pupils[i].params;
|
|
x0.segment<3>(3 + 3 * i)[0] = pupil_params.theta;
|
|
x0.segment<3>(3 + 3 * i)[1] = pupil_params.psi;
|
|
x0.segment<3>(3 + 3 * i)[2] = pupil_params.radius;
|
|
}
|
|
|
|
f.add_variable(&x0[0], 3);
|
|
for (int i = 0; i < pupils.size(); ++i) {
|
|
if (pupils[i].circle) {
|
|
f.add_variable(&x0[3 + 3 * i], 3);
|
|
|
|
f.add_term(
|
|
std::make_shared<PupilContrastTerm<true>>(
|
|
eye,
|
|
focal_length * region_scale,
|
|
cvx::resize(pupils[i].observation.image, region_scale),
|
|
region_band_width,
|
|
region_step_epsilon),
|
|
&x0[0], &x0[3 + 3 * i]);
|
|
//f.add_term(std::make_shared<PupilAnthroTerm>(2.5, 1, 0.001), &x0[3+3*i]);
|
|
|
|
/*if (i == 0 || !pupils[i-1].circle) {
|
|
} else {
|
|
vars.push_back(&x0[3+3*(i-1)]);
|
|
f.add_term(std::make_shared<SinglePupilTerm<true,true,true>>(*this, pupils[i].observation.image), vars);
|
|
}*/
|
|
}
|
|
}
|
|
}
|
|
|
|
spii::LBFGSSolver solver;
|
|
solver.maximum_iterations = 1000;
|
|
solver.function_improvement_tolerance = 1e-5;
|
|
spii::SolverResults results;
|
|
|
|
#if 0 // Turned off due to Visual Studio 2015 error
|
|
if (callback) {
|
|
double eye_radius = eye.radius;
|
|
solver.callback_function = [eye_radius, &callback](const spii::CallbackInformation& info) {
|
|
auto& x = *info.x;
|
|
|
|
Eigen::Matrix<double, 3, 1> eye_pos(x(0), x(1), x(2));
|
|
Sphere eye(eye_pos, eye_radius);
|
|
|
|
std::vector<Circle> pupils;
|
|
for (int i = 0; i < (x.size() - 3) / 3; ++i) {
|
|
pupils.push_back(EyeModelFitter::circleFromParams(eye, PupilParams(x(3 + 3 * i + 0), x(3 + 3 * i + 1), x(3 + 3 * i + 2))));
|
|
}
|
|
|
|
callback(eye, pupils);
|
|
|
|
return true;
|
|
};
|
|
}
|
|
#endif
|
|
solver.solve(f, &results);
|
|
std::cout << results << std::endl;
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
if (current_model_version != model_version) {
|
|
std::cout << "Old model, not applying refined parameters" << std::endl;
|
|
return;
|
|
}
|
|
|
|
eye.centre = x0.segment<3>(0);
|
|
|
|
for (int i = 0; i < (x0.size() - 3) / 3; ++i) {
|
|
auto pupil_param = x0.segment<3>(3 + 3 * i);
|
|
pupils[i].params = PupilParams(pupil_param[0], pupil_param[1], pupil_param[2]);
|
|
pupils[i].circle = circleFromParams(pupils[i].params);
|
|
}
|
|
}
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::initialise_model()
|
|
{
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
if (eye == Sphere::Null) {
|
|
return;
|
|
}
|
|
|
|
// Find pupil positions on eyeball to get radius
|
|
//
|
|
// For each image, calculate the 'most likely' position of the pupil
|
|
// circle given the eyeball sphere estimate and gaze vector. Re-estimate
|
|
// the gaze vector to be consistent with this position.
|
|
|
|
// First estimate of pupil centre, used only to get an estimate of eye radius
|
|
|
|
double eye_radius_acc = 0;
|
|
int eye_radius_count = 0;
|
|
|
|
for (const auto& pupil : pupils) {
|
|
if (!pupil.circle) {
|
|
continue;
|
|
}
|
|
if (!pupil.init_valid) {
|
|
continue;
|
|
}
|
|
|
|
// Intersect the gaze from the eye centre with the pupil circle
|
|
// centre projection line (with perfect estimates of gaze, eye
|
|
// centre and pupil circle centre, these should intersect,
|
|
// otherwise find the nearest point to both lines)
|
|
|
|
Vector3 pupil_centre = nearest_intersect(Line3(eye.centre, pupil.circle.normal),
|
|
Line3(camera_centre, pupil.circle.centre.normalized()));
|
|
|
|
auto distance = (pupil_centre - eye.centre).norm();
|
|
|
|
eye_radius_acc += distance;
|
|
++eye_radius_count;
|
|
}
|
|
|
|
// Set the eye radius as the mean distance from pupil centres to eye centre
|
|
eye.radius = eye_radius_acc / eye_radius_count;
|
|
|
|
// Second estimate of pupil radius, used to get position of pupil on eye
|
|
|
|
for (auto& pupil : pupils) {
|
|
initialise_single_observation(pupil);
|
|
}
|
|
|
|
// Scale eye to anthropomorphic average radius of 12mm
|
|
auto scale = 12.0 / eye.radius;
|
|
eye.radius = 12.0;
|
|
eye.centre *= scale;
|
|
for (auto& pupil : pupils) {
|
|
pupil.params.radius *= scale;
|
|
pupil.circle = circleFromParams(pupil.params);
|
|
}
|
|
|
|
model_version++;
|
|
|
|
// Try previous circle in case of bad fits
|
|
/*EllipseGoodnessFunction<double> goodnessFunction;
|
|
for (int i = 1; i < pupils.size(); ++i) {
|
|
auto& pupil = pupils[i];
|
|
auto& prevPupil = pupils[i-1];
|
|
|
|
if (prevPupil.circle) {
|
|
double currentGoodness, prevGoodness;
|
|
if (pupil.circle) {
|
|
currentGoodness = goodnessFunction(eye, pupil.params.theta, pupil.params.psi, pupil.params.radius, focal_length, pupil.observation.image);
|
|
prevGoodness = goodnessFunction(eye, prevPupil.params.theta, prevPupil.params.psi, prevPupil.params.radius, focal_length, pupil.observation.image);
|
|
}
|
|
|
|
if (!pupil.circle || prevGoodness > currentGoodness) {
|
|
pupil.circle = prevPupil.circle;
|
|
pupil.params = prevPupil.params;
|
|
}
|
|
}
|
|
}*/
|
|
}
|
|
|
|
void singleeyefitter::EyeModelFitter::unproject_observations(double pupil_radius /*= 1*/, double eye_z /*= 20*/, bool use_ransac /*= true*/)
|
|
{
|
|
using math::sq;
|
|
|
|
std::lock_guard<std::mutex> lock_model(model_mutex);
|
|
|
|
if (pupils.size() < 2) {
|
|
throw std::runtime_error("Need at least two observations");
|
|
}
|
|
|
|
std::vector<std::pair<Circle, Circle>> pupil_unprojection_pairs;
|
|
std::vector<Line> pupil_gazelines_proj;
|
|
|
|
for (const auto& pupil : pupils) {
|
|
// Get pupil circles (up to depth)
|
|
//
|
|
// Do a per-image unprojection of the pupil ellipse into the two fixed
|
|
// size circles that would project onto it. The size of the circles
|
|
// doesn't matter here, only their centre and normal does.
|
|
auto unprojection_pair = unproject(pupil.observation.ellipse,
|
|
pupil_radius, focal_length);
|
|
|
|
// Get projected circles and gaze vectors
|
|
//
|
|
// Project the circle centres and gaze vectors down back onto the image
|
|
// plane. We're only using them as line parametrisations, so it doesn't
|
|
// matter which of the two centres/gaze vectors we use, as the
|
|
// two gazes are parallel and the centres are co-linear.
|
|
|
|
const auto& c = unprojection_pair.first.centre;
|
|
const auto& v = unprojection_pair.first.normal;
|
|
|
|
Vector2 c_proj = project(c, focal_length);
|
|
Vector2 v_proj = project(v + c, focal_length) - c_proj;
|
|
|
|
v_proj.normalize();
|
|
|
|
pupil_unprojection_pairs.push_back(std::move(unprojection_pair));
|
|
pupil_gazelines_proj.emplace_back(c_proj, v_proj);
|
|
}
|
|
|
|
|
|
// Get eyeball centre
|
|
//
|
|
// Find a least-squares 'intersection' (point nearest to all lines) of
|
|
// the projected 2D gaze vectors. Then, unproject that circle onto a
|
|
// point a fixed distance away.
|
|
//
|
|
// For robustness, use RANSAC to eliminate stray gaze lines
|
|
//
|
|
// (This has to be done here because it's used by the pupil circle
|
|
// disambiguation)
|
|
|
|
Vector2 eye_centre_proj;
|
|
bool valid_eye;
|
|
|
|
if (use_ransac) {
|
|
auto indices = fun::range_<std::vector<size_t>>(pupil_gazelines_proj.size());
|
|
|
|
const int n = 2;
|
|
double w = 0.3;
|
|
double p = 0.9999;
|
|
int k = (int)ceil(log(1 - p) / log(1 - pow(w, n)));
|
|
|
|
double epsilon = 10;
|
|
auto huber_error = [&](const Vector2& point, const Line& line) {
|
|
double dist = euclidean_distance(point, line);
|
|
if (sq(dist) < sq(epsilon))
|
|
return sq(dist) / 2;
|
|
else
|
|
return epsilon*(abs(dist) - epsilon / 2);
|
|
};
|
|
auto m_error = [&](const Vector2& point, const Line& line) {
|
|
double dist = euclidean_distance(point, line);
|
|
if (sq(dist) < sq(epsilon))
|
|
return sq(dist);
|
|
else
|
|
return sq(epsilon);
|
|
};
|
|
auto error = m_error;
|
|
|
|
auto best_inlier_indices = decltype(indices)();
|
|
Vector2 best_eye_centre_proj;// = nearest_intersect(pupil_gazelines_proj);
|
|
double best_line_distance_error = std::numeric_limits<double>::infinity();// = fun::sum(LAMBDA(const Line& line)(error(best_eye_centre_proj,line)), pupil_gazelines_proj);
|
|
|
|
for (int i = 0; i < k; ++i) {
|
|
auto index_sample = singleeyefitter::randomSubset(indices, n);
|
|
auto sample = fun::map([&](size_t i){ return pupil_gazelines_proj[i]; }, index_sample);
|
|
|
|
auto sample_centre_proj = nearest_intersect(sample);
|
|
|
|
auto index_inliers = fun::filter(
|
|
[&](size_t i){ return euclidean_distance(sample_centre_proj, pupil_gazelines_proj[i]) < epsilon; },
|
|
indices);
|
|
auto inliers = fun::map([&](size_t i){ return pupil_gazelines_proj[i]; }, index_inliers);
|
|
|
|
if (inliers.size() <= w*pupil_gazelines_proj.size()) {
|
|
continue;
|
|
}
|
|
|
|
auto inlier_centre_proj = nearest_intersect(inliers);
|
|
|
|
double line_distance_error = fun::sum(
|
|
[&](size_t i){ return error(inlier_centre_proj, pupil_gazelines_proj[i]); },
|
|
indices);
|
|
|
|
if (line_distance_error < best_line_distance_error) {
|
|
best_eye_centre_proj = inlier_centre_proj;
|
|
best_line_distance_error = line_distance_error;
|
|
best_inlier_indices = std::move(index_inliers);
|
|
}
|
|
}
|
|
|
|
std::cout << "Inliers: " << best_inlier_indices.size()
|
|
<< " (" << (100.0*best_inlier_indices.size() / pupil_gazelines_proj.size()) << "%)"
|
|
<< " = " << best_line_distance_error
|
|
<< std::endl;
|
|
|
|
for (auto& pupil : pupils) {
|
|
pupil.init_valid = false;
|
|
}
|
|
for (auto& i : best_inlier_indices) {
|
|
pupils[i].init_valid = true;
|
|
}
|
|
|
|
if (best_inlier_indices.size() > 0) {
|
|
eye_centre_proj = best_eye_centre_proj;
|
|
valid_eye = true;
|
|
}
|
|
else {
|
|
valid_eye = false;
|
|
}
|
|
}
|
|
else {
|
|
for (auto& pupil : pupils) {
|
|
pupil.init_valid = true;
|
|
}
|
|
eye_centre_proj = nearest_intersect(pupil_gazelines_proj);
|
|
valid_eye = true;
|
|
}
|
|
|
|
if (valid_eye) {
|
|
eye.centre << eye_centre_proj * eye_z / focal_length,
|
|
eye_z;
|
|
eye.radius = 1;
|
|
|
|
// Disambiguate pupil circles using projected eyeball centre
|
|
//
|
|
// Assume that the gaze vector points away from the eye centre, and
|
|
// so projected gaze points away from projected eye centre. Pick the
|
|
// solution which satisfies this assumption
|
|
for (size_t i = 0; i < pupils.size(); ++i) {
|
|
const auto& pupil_pair = pupil_unprojection_pairs[i];
|
|
const auto& line = pupil_gazelines_proj[i];
|
|
|
|
const auto& c_proj = line.origin();
|
|
const auto& v_proj = line.direction();
|
|
|
|
// Check if v_proj going away from est eye centre. If it is, then
|
|
// the first circle was correct. Otherwise, take the second one.
|
|
// The two normals will point in opposite directions, so only need
|
|
// to check one.
|
|
if ((c_proj - eye_centre_proj).dot(v_proj) >= 0) {
|
|
pupils[i].circle = std::move(pupil_pair.first);
|
|
}
|
|
else {
|
|
pupils[i].circle = std::move(pupil_pair.second);
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
// No inliers, so no eye
|
|
eye = Sphere::Null;
|
|
|
|
// Arbitrarily pick first circle
|
|
for (size_t i = 0; i < pupils.size(); ++i) {
|
|
const auto& pupil_pair = pupil_unprojection_pairs[i];
|
|
pupils[i].circle = std::move(pupil_pair.first);
|
|
}
|
|
}
|
|
|
|
model_version++;
|
|
}
|