3D-Eye-Tracker/singleeyefitter/cvx.cpp
2016-10-07 13:31:30 +09:00

165 lines
4.9 KiB
C++

#include <singleeyefitter/cvx.h>
#include <singleeyefitter/math.h>
void singleeyefitter::cvx::getROI(const cv::Mat& src, cv::Mat& dst, const cv::Rect& roi, int borderType)
{
cv::Rect bbSrc = boundingBox(src);
cv::Rect validROI = roi & bbSrc;
if (validROI == roi)
{
dst = cv::Mat(src, validROI);
}
else
{
// Figure out how much to add on for top, left, right and bottom
cv::Point tl = roi.tl() - bbSrc.tl();
cv::Point br = roi.br() - bbSrc.br();
int top = std::max(-tl.y, 0); // Top and left are negated because adding a border
int left = std::max(-tl.x, 0); // goes "the wrong way"
int right = std::max(br.x, 0);
int bottom = std::max(br.y, 0);
cv::Mat tmp(src, validROI);
cv::copyMakeBorder(tmp, dst, top, bottom, left, right, borderType);
}
}
float singleeyefitter::cvx::histKmeans(const cv::Mat_<float>& hist, int bin_min, int bin_max, int K, float init_centres[], cv::Mat_<uchar>& labels, cv::TermCriteria termCriteria)
{
using namespace math;
CV_Assert( hist.rows == 1 || hist.cols == 1 && K > 0 );
labels = cv::Mat_<uchar>::zeros(hist.size());
size_t nbins = hist.total();
float binWidth = (bin_max - bin_min)/(float)nbins;
float binStart = bin_min + binWidth/2.0f;
cv::Mat_<float> centres(K, 1, init_centres, 4);
int iters = 0;
bool finalRun = false;
while (true)
{
++iters;
cv::Mat_<float> old_centres = centres.clone();
size_t i_bin;
cv::Mat_<float>::const_iterator i_hist;
cv::Mat_<uchar>::iterator i_labels;
cv::Mat_<float>::iterator i_centres;
uchar label;
float sumDist = 0;
int movedCount = 0;
// Step 1. Assign each element a label
for (i_bin = 0, i_labels = labels.begin(), i_hist = hist.begin();
i_bin < nbins;
++i_bin, ++i_labels, ++i_hist)
{
float bin_val = binStart + i_bin*binWidth;
float minDist = sq(bin_val - centres(*i_labels));
int curLabel = *i_labels;
for (label = 0; label < K; ++label)
{
float dist = sq(bin_val - centres(label));
if (dist < minDist)
{
minDist = dist;
*i_labels = label;
}
}
if (*i_labels != curLabel)
movedCount++;
sumDist += (*i_hist) * std::sqrt(minDist);
}
if (finalRun)
return sumDist;
// Step 2. Recalculate centres
cv::Mat_<float> counts(K, 1, 0.0f);
for (i_bin = 0, i_labels = labels.begin(), i_hist = hist.begin();
i_bin < nbins;
++i_bin, ++i_labels, ++i_hist)
{
float bin_val = binStart + i_bin*binWidth;
centres(*i_labels) += (*i_hist) * bin_val;
counts(*i_labels) += *i_hist;
}
for (label = 0; label < K; ++label)
{
if (counts(label) == 0)
return std::numeric_limits<float>::infinity();
centres(label) /= counts(label);
}
// Step 3. Detect termination criteria
if (movedCount == 0)
finalRun = true;
else if (termCriteria.type | cv::TermCriteria::COUNT && iters >= termCriteria.maxCount)
finalRun = true;
else if (termCriteria.type | cv::TermCriteria::EPS)
{
float max_movement = 0;
for (label = 0; label < K; ++label)
{
max_movement = std::max(max_movement, sq(centres(label) - old_centres(label)));
}
if (sqrt(max_movement) < termCriteria.epsilon)
finalRun = true;
}
}
return std::numeric_limits<float>::infinity();
}
cv::RotatedRect singleeyefitter::cvx::fitEllipse(const cv::Moments& m)
{
using namespace math;
cv::RotatedRect ret;
ret.center.x = (float)(m.m10/m.m00);
ret.center.y = (float)(m.m01/m.m00);
double mu20 = m.m20/m.m00 - ret.center.x*ret.center.x;
double mu02 = m.m02/m.m00 - ret.center.y*ret.center.y;
double mu11 = m.m11/m.m00 - ret.center.x*ret.center.y;
double common = std::sqrt(sq(mu20 - mu02) + 4*sq(mu11));
ret.size.width = (float)std::sqrt(2*(mu20 + mu02 + common));
ret.size.height = (float)std::sqrt(2*(mu20 + mu02 - common));
double num, den;
if (mu02 > mu20) {
num = mu02 - mu20 + common;
den = 2*mu11;
}
else {
num = 2*mu11;
den = mu20 - mu02 + common;
}
if (num == 0 && den == 0)
ret.angle = 0;
else
ret.angle = (float)(180/PI * std::atan2(num,den));
return ret;
}
cv::Vec2f singleeyefitter::cvx::majorAxis(const cv::RotatedRect& ellipse)
{
return cv::Vec2f(
ellipse.size.width*(float)std::cos(PI / 180.0 * (double)ellipse.angle),
ellipse.size.width*(float)std::sin(PI / 180.0 * (double)ellipse.angle));
}