void Project(Mat_<float>& dest, const Mat_<float>& mesh, Size size, double fx, double fy, double cx, double cy)
{
dest = Mat_<float>(mesh.rows,2, 0.0);
int NbPoints = mesh.rows;
register float X, Y, Z;
Mat_<float>::const_iterator mData = mesh.begin();
Mat_<float>::iterator projected = dest.begin();
for(int i = 0;i < NbPoints; i++)
{
// Get the points
X = *(mData++);
Y = *(mData++);
Z = *(mData++);
float x;
float y;
// if depth is 0 the projection is different
if(Z != 0)
{
x = (float)((X * fx / Z) + cx);
y = (float)((Y * fy / Z) + cy);
}
else
{
x = X;
y = Y;
}
// Clamping to image size
if( x < 0 )
{
x = 0.0;
}
else if (x > size.width - 1)
{
x = size.width - 1.0f;
}
if( y < 0 )
{
y = 0.0;
}
else if( y > size.height - 1)
{
y = size.height - 1.0f;
}
// Project and store in dest matrix
(*projected++) = x;
(*projected++) = y;
}
}
void output_HOG_frame(std::ofstream* hog_file, bool good_frame, const Mat_<double>& hog_descriptor, int num_rows, int num_cols)
{
// Using FHOGs, hence 31 channels
int num_channels = 31;
hog_file->write((char*)(&num_cols), 4);
hog_file->write((char*)(&num_rows), 4);
hog_file->write((char*)(&num_channels), 4);
// Not the best way to store a bool, but will be much easier to read it
float good_frame_float;
if(good_frame)
good_frame_float = 1;
else
good_frame_float = -1;
hog_file->write((char*)(&good_frame_float), 4);
cv::MatConstIterator_<double> descriptor_it = hog_descriptor.begin();
for(int y = 0; y < num_cols; ++y)
{
for(int x = 0; x < num_rows; ++x)
{
for(unsigned int o = 0; o < 31; ++o)
{
float hog_data = (float)(*descriptor_it++);
hog_file->write ((char*)&hog_data, 4);
}
}
}
}
bool intersects(const vector<Point> &contour, const Mat_<uchar> &mask)
{
Mat_<uchar> c = Mat_<uchar>::zeros(mask.size());
fillConvexPoly(c, contour.data(), contour.size(), 255);
/*
Below is:
bitwise_and(mask, c, c);
return countNonZero(c);
optimized.
*/
auto it_m = mask.begin();
auto it_c = c.begin();
while (it_m != mask.end()) {
if (*it_m && *it_c) return true;
++it_m, ++it_c;
}
return false;
}
//===========================================================================
void SVR_patch_expert::Response(const Mat_<float>& area_of_interest, Mat_<double>& response)
{
int response_height = area_of_interest.rows - weights.rows + 1;
int response_width = area_of_interest.cols - weights.cols + 1;
// the patch area on which we will calculate reponses
cv::Mat_<float> normalised_area_of_interest;
if(response.rows != response_height || response.cols != response_width)
{
response.create(response_height, response_width);
}
// If type is raw just normalise mean and standard deviation
if(type == 0)
{
// Perform normalisation across whole patch
cv::Scalar mean;
cv::Scalar std;
cv::meanStdDev(area_of_interest, mean, std);
// Avoid division by zero
if(std[0] == 0)
{
std[0] = 1;
}
normalised_area_of_interest = (area_of_interest - mean[0]) / std[0];
}
// If type is gradient, perform the image gradient computation
else if(type == 1)
{
Grad(area_of_interest, normalised_area_of_interest);
}
else
{
printf("ERROR(%s,%d): Unsupported patch type %d!\n", __FILE__,__LINE__, type);
abort();
}
Mat_<float> svr_response;
// The empty matrix as we don't pass precomputed dft's of image
Mat_<double> empty_matrix_0(0,0,0.0);
Mat_<float> empty_matrix_1(0,0,0.0);
Mat_<float> empty_matrix_2(0,0,0.0);
// Efficient calc of patch expert SVR response across the area of interest
matchTemplate_m(normalised_area_of_interest, empty_matrix_0, empty_matrix_1, empty_matrix_2, weights, weights_dfts, svr_response, CV_TM_CCOEFF_NORMED);
response.create(svr_response.size());
MatIterator_<double> p = response.begin();
cv::MatIterator_<float> q1 = svr_response.begin(); // respone for each pixel
cv::MatIterator_<float> q2 = svr_response.end();
while(q1 != q2)
{
// the SVR response passed into logistic regressor
*p++ = 1.0/(1.0 + exp( -(*q1++ * scaling + bias )));
}
}
//======================================================================
// Compute the mapping coefficients
void PAW::WarpRegion(Mat_<float>& mapx, Mat_<float>& mapy)
{
cv::MatIterator_<float> xp = mapx.begin();
cv::MatIterator_<float> yp = mapy.begin();
cv::MatIterator_<uchar> mp = pixel_mask.begin();
cv::MatIterator_<int> tp = triangle_id.begin();
// The coefficients corresponding to the current triangle
double * a;
// Current triangle being processed
int k=-1;
for(int y = 0; y < pixel_mask.rows; y++)
{
double yi = double(y) + min_y;
for(int x = 0; x < pixel_mask.cols; x++)
{
double xi = double(x) + min_x;
if(*mp == 0)
{
*xp = -1;
*yp = -1;
}
else
{
// triangle corresponding to the current pixel
int j = *tp;
// If it is different from the previous triangle point to new coefficients
// This will always be the case in the first iteration, hence a will not point to nothing
if(j != k)
{
// Update the coefficient pointer if a new triangle is being processed
a = coefficients.ptr<double>(j);
k = j;
}
//ap is now the pointer to the coefficients
double *ap = a;
//look at the first coefficient (and increment). first coefficient is an x offset
double xo = *ap++;
//second coefficient is an x scale as a function of x
xo += *ap++ * xi;
//third coefficient ap(2) is an x scale as a function of y
*xp = float(xo + *ap++ * yi);
//then fourth coefficient ap(3) is a y offset
double yo = *ap++;
//fifth coeff adds coeff[4]*x to y
yo += *ap++ * xi;
//final coeff adds coeff[5]*y to y
*yp = float(yo + *ap++ * yi);
}
mp++; tp++; xp++; yp++;
}
}
}
static int inner_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
int count=0;
for(;;){
dprintf(("iteration #%d\n",count));
count++;
static MatIterator_<double> pos_ptr;
int e=-1,pos_ctr=0,min_var=INT_MAX;
bool all_nonzero=true;
for(pos_ptr=c.begin();pos_ptr!=c.end();pos_ptr++,pos_ctr++){
if(*pos_ptr==0){
all_nonzero=false;
}
if(*pos_ptr>0){
if(N[pos_ctr]<min_var){
e=pos_ctr;
min_var=N[pos_ctr];
}
}
}
if(e==-1){
dprintf(("hello from e==-1\n"));
print_matrix(c);
if(all_nonzero==true){
return SOLVELP_SINGLE;
}else{
return SOLVELP_MULTI;
}
}
int l=-1;
min_var=INT_MAX;
double min=DBL_MAX;
int row_it=0;
MatIterator_<double> min_row_ptr=b.begin();
for(MatIterator_<double> it=b.begin();it!=b.end();it+=b.cols,row_it++){
double myite=0;
//check constraints, select the tightest one, reinforcing Bland's rule
if((myite=it[e])>0){
double val=it[b.cols-1]/myite;
if(val<min || (val==min && B[row_it]<min_var)){
min_var=B[row_it];
min_row_ptr=it;
min=val;
l=row_it;
}
}
}
if(l==-1){
return SOLVELP_UNBOUNDED;
}
dprintf(("the tightest constraint is in row %d with %g\n",l,min));
pivot(c,b,v,N,B,l,e,indexToRow);
dprintf(("objective, v=%g\n",v));
print_matrix(c);
dprintf(("constraints\n"));
print_matrix(b);
dprintf(("non-basic: "));
print_matrix(Mat(N));
dprintf(("basic: "));
print_matrix(Mat(B));
}
}
