/** Orthogonal matching pursuit
* x: input signal, N * 1
* D: dictionary, N * M
* L: number of non_zero elements in output
* coeff: coefficent of each atoms in dictionary, M * 1
*/
void OMP(const cv::Mat_<double>& x, const cv::Mat_<double>& D, int L, cv::Mat_<double>& coeff){
int dim = x.rows;
int atom_num = D.cols;
coeff = Mat::zeros(atom_num, 1, CV_64FC1);
Mat_<double> residual = x.clone();
Mat_<double> selected_index(L, 1);
Mat_<double> a;
for (int i = 0; i < L; i++){
cout << "here ok 1" << endl;
Mat_<double> dot_p = D.t() * residual;
Point max_index;
minMaxLoc(abs(dot_p), NULL, NULL, NULL, &max_index);
int max_row = max_index.y;
selected_index(i) = max_row;
Mat_<double> temp(dim, i + 1);
for (int j = 0; j < i + 1; j++){
D.col(selected_index(j)).copyTo(temp.col(j));
}
Mat_<double> invert_temp;
invert(temp, invert_temp, CV_SVD);
a = invert_temp * x;
residual = x - temp * a;
}
for (int i = 0; i < L; i++){
coeff(selected_index(i)) = a(i);
}
}
cv::Mat test_with_args(const cv::Mat_<float>& in, const int& var1 = 1,
const double& var2 = 10.0, const std::string& name=std::string("test_name")) {
std::cerr << "in: " << in << std::endl;
std::cerr << "sz: " << in.size() << std::endl;
std::cerr << "Returning transpose" << std::endl;
return in.t();
}
CrossValidator::CrossValidator( const cv::Mat_<float> &xs, const cv::Mat_<int> &labs, int numEVs)
: _numEVs( numEVs), _txs(xs), _tlabels(labs)
{
assert( _tlabels.total() == _txs.rows);
// Ensure labs are given as a single row vector
if ( labs.rows > labs.cols)
_tlabels = labs.t();
// Count the number of entries of each class
const int* labArray = _tlabels.ptr<int>(0);
for ( int i = 0; i < _tlabels.cols; ++i)
{
const int lab = labArray[i];
while ( lab >= (int)_cCounts.size())
_cCounts.push_back(0);
_cCounts[lab]++;
} // end for
// At the moment, CrossValidator can only deal with two class problems
if ( _cCounts.size() != 2)
std::cerr << "ERROR: Currently CrossValidator can only deal with 2 class problems!" << std::endl;
assert( _cCounts.size() == 2);
if ( _numEVs <= 0)
_numEVs = 0;
if ( _numEVs > _txs.cols)
_numEVs = _txs.cols;
} // end ctor
// static
cv::Mat_<float> CrossValidator::createCrossValidationMatrix( const vector< const vector<cv::Mat_<float> >* >& rowVectors,
cv::Mat_<int>& labels)
{
assert( !rowVectors.empty());
cv::Mat_<float> vecs;
labels.create( 0,1);
for ( int label = 0; label < rowVectors.size(); ++label)
{
assert( rowVectors[label] != 0);
const vector<cv::Mat_<float> >& rvecs = *rowVectors[label];
assert( !rvecs.empty());
const int colDim = rvecs[0].cols; // Should be the length of each row vector in this class
if ( vecs.empty())
vecs.create(0, colDim);
assert( colDim == vecs.cols); // Ensure this class's row vector length matches what's already stored
for ( int i = 0; i < rvecs.size(); ++i)
{
const cv::Mat_<float>& rv = rvecs[i];
if ( rv.rows != 1 || rv.cols != colDim)
{
std::cerr << "ERROR feature vector size: " << rv.size() << std::endl;
assert( rv.rows == 1 && rv.cols == colDim);
} // end if
vecs.push_back( rv); // Append the row vector to the bottom of the matrix
labels.push_back(label); // Set this vector's class label
} // end for
} // end for
labels = labels.t(); // Make row vector
return vecs;
} // end createCrossValidationMatrix
cv::Point3f GetPupilPosition(cv::Mat_<double> eyeLdmks3d){
eyeLdmks3d = eyeLdmks3d.t();
cv::Mat_<double> irisLdmks3d = eyeLdmks3d.rowRange(0,8);
cv::Point3f p (mean(irisLdmks3d.col(0))[0], mean(irisLdmks3d.col(1))[0], mean(irisLdmks3d.col(2))[0]);
return p;
}
void Cv_mat_to_arma_mat(const cv::Mat_<T>& cv_mat_in, arma::Mat<T>& arma_mat_out)
{
cv::Mat_<T> temp(cv_mat_in.t()); //todo any way to not create a temporary?
//This compiles on both but is not as nice
arma_mat_out = arma::Mat<T>(reinterpret_cast<T*>(temp.data),
static_cast<arma::uword>(temp.cols),
static_cast<arma::uword>(temp.rows),
true,
true);
};
//=============================================================================
// Basically Kabsch's algorithm but also allows the collection of points to be different in scale from each other
cv::Matx22f AlignShapesWithScale(cv::Mat_<float>& src, cv::Mat_<float> dst)
{
int n = src.rows;
// First we mean normalise both src and dst
float mean_src_x = cv::mean(src.col(0))[0];
float mean_src_y = cv::mean(src.col(1))[0];
float mean_dst_x = cv::mean(dst.col(0))[0];
float mean_dst_y = cv::mean(dst.col(1))[0];
cv::Mat_<float> src_mean_normed = src.clone();
src_mean_normed.col(0) = src_mean_normed.col(0) - mean_src_x;
src_mean_normed.col(1) = src_mean_normed.col(1) - mean_src_y;
cv::Mat_<float> dst_mean_normed = dst.clone();
dst_mean_normed.col(0) = dst_mean_normed.col(0) - mean_dst_x;
dst_mean_normed.col(1) = dst_mean_normed.col(1) - mean_dst_y;
// Find the scaling factor of each
cv::Mat src_sq;
cv::pow(src_mean_normed, 2, src_sq);
cv::Mat dst_sq;
cv::pow(dst_mean_normed, 2, dst_sq);
float s_src = sqrt(cv::sum(src_sq)[0] / n);
float s_dst = sqrt(cv::sum(dst_sq)[0] / n);
src_mean_normed = src_mean_normed / s_src;
dst_mean_normed = dst_mean_normed / s_dst;
float s = s_dst / s_src;
// Get the rotation
cv::Matx22f R = AlignShapesKabsch2D(src_mean_normed, dst_mean_normed);
cv::Matx22f A;
cv::Mat(s * R).copyTo(A);
cv::Mat_<float> aligned = (cv::Mat(cv::Mat(A) * src.t())).t();
cv::Mat_<float> offset = dst - aligned;
float t_x = cv::mean(offset.col(0))[0];
float t_y = cv::mean(offset.col(1))[0];
return A;
}
//===========================================================================
// Point set and landmark manipulation functions
//===========================================================================
// Using Kabsch's algorithm for aligning shapes
//This assumes that align_from and align_to are already mean normalised
cv::Matx22f AlignShapesKabsch2D(const cv::Mat_<float>& align_from, const cv::Mat_<float>& align_to)
{
cv::SVD svd(align_from.t() * align_to);
// make sure no reflection is there
// corr ensures that we do only rotaitons and not reflections
float d = cv::determinant(svd.vt.t() * svd.u.t());
cv::Matx22f corr = cv::Matx22f::eye();
if (d > 0)
{
corr(1, 1) = 1;
}
else
{
corr(1, 1) = -1;
}
cv::Matx22f R;
cv::Mat(svd.vt.t()*cv::Mat(corr)*svd.u.t()).copyTo(R);
return R;
}