Mat growImage(const Mat &SampleImage, Mat &Image, int windowSize, Mat &map){
double MaxErrThreshold = 0.3;
while(!allFilled(map)){
bool progress = false;
vector<Point2i> PixelList = GetUnfilledNeighbors(map);
//cout<<"PixelList.size = "<<PixelList.size()<<endl;
for(int i = 0; i < PixelList.size(); i++){
vector<matches> BestMatches;
Mat Template = getNeigborhoodWindow(Image, PixelList[i], windowSize);
BestMatches = FindMatches(Template, SampleImage, Image, PixelList[i], windowSize, map);
// randomly pick one
int which = rand() % BestMatches.size();
if(BestMatches[which].err < MaxErrThreshold){
Image.ATD(PixelList[i].y, PixelList[i].x) = SampleImage.ATD(BestMatches[which].pt.y, BestMatches[which].pt.x);
map.ATD(PixelList[i].y, PixelList[i].x) = 1.0;
progress = true;
}
}
if(!progress){
MaxErrThreshold *= 1.1;
}
}
cout<<"MaxErrThreshold = "<<MaxErrThreshold<<endl;
return Image;
}
Mat getNeigborhoodWindow(const Mat &img, Point2i pt, int windowSize){
Mat result = Mat(windowSize * 2 + 1, windowSize * 2 + 1, CV_64FC1);
for(int i = 0; i < result.rows; i++){
for(int j = 0; j < result.cols; j++){
if(isinImage(pt.y - windowSize + i, pt.x - windowSize + j, img)){
result.ATD(i, j) = img.ATD(pt.y - windowSize + i, pt.x - windowSize + j);
}else result.ATD(i, j) = -1;
}
}
return result;
}
vector<Point2i> GetUnfilledNeighbors(const Mat& map){
vector <ufp> data;
for (int i = 0; i < map.rows; i++){
for (int j = 0; j < map.cols; j++){
if (map.ATD(i, j) != 0) continue;
int temp = 0;
if (isinImage(i - 1, j - 1, map)) temp += map.ATD(i - 1, j - 1);
if (isinImage(i - 1, j, map)) temp += map.ATD(i - 1, j);
if (isinImage(i - 1, j + 1, map)) temp += map.ATD(i - 1, j + 1);
if (isinImage(i, j - 1, map)) temp += map.ATD(i, j - 1);
if (isinImage(i, j + 1, map)) temp += map.ATD(i, j + 1);
if (isinImage(i + 1, j - 1, map)) temp += map.ATD(i + 1, j - 1);
if (isinImage(i + 1, j, map)) temp += map.ATD(i + 1, j);
if (isinImage(i + 1, j + 1, map)) temp += map.ATD(i + 1, j + 1);
ufp tufp;
tufp.pt = Point2i(j, i);
tufp.fNeighbors = temp;
data.push_back(tufp);
}
}
random_shuffle(data.begin(), data.end());
sort(data.begin(), data.end(), SortUFP);
vector <Point2i> result;
for (int i = 0; i < data.size(); i++){
if (data[i].fNeighbors > 0){
result.push_back(data[i].pt);
}
}
data.clear();
return result;
}
void
read_batch(string filename, vector<Mat> &vec, Mat &label){
ifstream file (filename, ios::binary);
if (file.is_open())
{
int number_of_images = 10000;
int n_rows = 32;
int n_cols = 32;
for(int i = 0; i < number_of_images; ++i)
{
unsigned char tplabel = 0;
file.read((char*) &tplabel, sizeof(tplabel));
vector<Mat> channels;
Mat fin_img = Mat::zeros(n_rows, n_cols, CV_8UC3);
for(int ch = 0; ch < 3; ++ch){
Mat tp = Mat::zeros(n_rows, n_cols, CV_8UC1);
for(int r = 0; r < n_rows; ++r){
for(int c = 0; c < n_cols; ++c){
unsigned char temp = 0;
file.read((char*) &temp, sizeof(temp));
tp.at<uchar>(r, c) = (int) temp;
}
}
channels.push_back(tp);
}
merge(channels, fin_img);
vec.push_back(fin_img);
label.ATD(0, i) = (double)tplabel;
}
}
}
double get_Sum9(Mat &m, int y, int x){
if(x < 0 || x >= m.cols) return 0;
if(y < 0 || y >= m.rows) return 0;
double val = 0.0;
int tmp = 0;
int w = m.cols;
int h = m.rows;
if(isInsideImage(y - 1, x - 1, m, w, h)){
++ tmp;
val += m.ATD(y - 1, x - 1);
}
if(isInsideImage(y - 1, x, m, w, h)){
++ tmp;
val += m.ATD(y - 1, x);
}
if(isInsideImage(y - 1, x + 1, m, w, h)){
++ tmp;
val += m.ATD(y - 1, x + 1);
}
if(isInsideImage(y, x - 1, m, w, h)){
++ tmp;
val += m.ATD(y, x - 1);
}
if(isInsideImage(y, x, m, w, h)){
++ tmp;
val += m.ATD(y, x);
}
if(isInsideImage(y, x + 1, m, w, h)){
++ tmp;
val += m.ATD(y, x + 1);
}
if(isInsideImage(y + 1, x - 1, m, w, h)){
++ tmp;
val += m.ATD(y + 1, x - 1);
}
if(isInsideImage(y + 1, x, m, w, h)){
++ tmp;
val += m.ATD(y + 1, x);
}
if(isInsideImage(y + 1, x + 1, m, w, h)){
++ tmp;
val += m.ATD(y + 1, x + 1);
}
if(tmp == 9) return val;
else return m.ATD(y, x) * 9;
}
Mat matMul(Mat a, Mat b){
Mat res = Mat::zeros(a.rows, a.cols, CV_64FC1);
int nthr = 4;
omp_set_num_threads(nthr);
//#pragma omp parallel for
for (int r=0; r<a.rows; r++){
//#pragma omp parallel for
for (int c =0; c<a.cols; c++){
res.ATD(r,c) = a.ATD(r,c) * b.ATD(r,c);
}
}
return res;
}
void saveMat(Mat &M, string s){
s += ".txt";
FILE *pOut = fopen(s.c_str(), "w+");
for(int i=0; i<M.rows; i++){
for(int j=0; j<M.cols; j++){
fprintf(pOut, "%lf", M.ATD(i, j));
if(j == M.cols - 1) fprintf(pOut, "\n");
else fprintf(pOut, " ");
}
}
fclose(pOut);
}
Mat get_fy(Mat &src1, Mat &src2){
Mat fy;
Mat kernel = Mat::ones(2, 2, CV_64FC1);
kernel.ATD(0, 0) = -1.0;
kernel.ATD(0, 1) = -1.0;
Mat dst1, dst2;
filter2D(src1, dst1, -1, kernel);
filter2D(src2, dst2, -1, kernel);
fy = dst1 + dst2;
return fy;
}
vector<matches> FindMatches(Mat Template, Mat SampleImage, Mat img, Point2i templateCenter, int windowSize, Mat Map){
Mat SampleF, TemplateF;
int topleftx = templateCenter.x - windowSize;
int toplefty = templateCenter.y - windowSize;
// ValidMask
Mat ValidMask = Mat::zeros(Template.size(), CV_64FC1);
for(int i = 0; i < ValidMask.rows; i++){
for(int j = 0; j < ValidMask.cols; j++){
if(isinImage(toplefty + i, topleftx + j, img)){
ValidMask.ATD(i, j) = Map.ATD(i + toplefty, j + topleftx);
}else ValidMask.ATD(i, j) = 0.0;
}
}
// GaussMask
double kernalsize = (double)windowSize / 6.0;
kernalsize = sqrt(kernalsize);
Mat tmpGaussian = getGaussianKernel(windowSize * 2 + 1, kernalsize);
Mat GaussianMask = tmpGaussian * tmpGaussian.t();
// TotWeight
double TotWeight = sum(ValidMask.mul(GaussianMask))[0];
// Sum of Squared Differences.
Mat SSD = Mat::zeros(Template.size(), CV_64FC1);
int xPara = SampleImage.cols / 2 - windowSize;
int yPara = SampleImage.rows / 2 - windowSize;
double minSSD = (double)INT_MAX;
for(int i = 0; i < Template.rows; i++){
for(int j = 0; j < Template.cols; j++){
if(!isinImage(toplefty + i, topleftx + j, img)){
SSD.ATD(i, j) = -1.0;
continue;
}
Mat dist = Mat::zeros(2 * windowSize + 1, 2 * windowSize + 1, CV_64FC1);
Mat tp1 = Template;
Mat tp2 = SampleImage(Rect(j + xPara - windowSize, i + yPara - windowSize, 2 * windowSize + 1, 2 * windowSize + 1));
dist = tp1 - tp2;
pow(dist, 2.0, dist);
SSD.ATD(i, j) = sum(dist.mul(ValidMask.mul(GaussianMask)))[0] / TotWeight;
}
}
for(int i = 0; i < Template.rows; i++){
for(int j = 0; j < Template.cols; j++){
if(SSD.ATD(i, j) == -1) continue;
if(SSD.ATD(i, j) < minSSD) minSSD = SSD.ATD(i, j);
}
}
double ErrThreshold = 0.1;
vector<matches> PixelList;
for(int i = 0; i < Template.rows; i++){
for(int j = 0; j < Template.cols; j++){
if(SSD.ATD(i, j) == -1) continue;
if(SSD.ATD(i, j) <= minSSD * (1 + ErrThreshold)){
matches tpmatch;
tpmatch.pt = Point2i(j + xPara, i + yPara);
tpmatch.err = fabs(minSSD - SSD.ATD(i, j));
PixelList.push_back(tpmatch);
}
}
}
return PixelList;
}
int main(){
vector<Point2f> points1;
vector<Point2f> points2;
// Capture from video
VideoCapture capture(0);
// Create window
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 400);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 400);
//cout << 800;
capture.set(CV_CAP_PROP_FRAME_WIDTH, 1080);
capture.set(CV_CAP_PROP_FRAME_WIDTH, 1080);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 800);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 800);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 800);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 800);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 200);
//capture.set(CV_CAP_PROP_FRAME_WIDTH, 200);
namedWindow("hand",1);
// Intinite loop until manually broken by end of video or keypress
Mat frame, current_frame, img1, img2;
Mat prevFrame, prevDiff;
bool firstPassFrame = true;
bool firstPassDiff = true;
for(;;){
double totalStart = timestamp();
// first image
capture >> frame;
resize(frame, current_frame, Size(1080, 1080), 0, 0, INTER_CUBIC);
//resize(frame, current_frame, Size(800, 800), 0, 0, INTER_CUBIC);
//resize(frame, current_frame, Size(400, 400), 0, 0, INTER_CUBIC);
//resize(frame, current_frame, Size(200, 200), 0, 0, INTER_CUBIC);
GaussianBlur(current_frame, current_frame, Size(9,9), 1.5, 1.5);
cvtColor(current_frame, current_frame, CV_BGR2GRAY);
if (firstPassFrame) {
firstPassFrame = false;
prevFrame = current_frame;
continue;
}
Mat diff = current_frame - LEARNING_RATE * prevFrame;
prevFrame = current_frame;
threshold(diff, diff, DIFF_THRESH, 255, THRESH_TOZERO);
Mat sobelX1, sobelY1;
// first
Sobel(diff, sobelX1, CV_64FC1, 1, 0);
Sobel(diff, sobelY1, CV_64FC1, 0, 1);
diff = sobelX1 + sobelY1;
dilate(diff, diff, Mat(), Point(-1,-1), 2);
erode(diff, diff, Mat(), Point(-1,-1), 2);
if (firstPassDiff) {
firstPassDiff = false;
prevDiff = diff;
continue;
}
Mat u = Mat::zeros(img1.rows, img1.cols, CV_64FC1);
Mat v = Mat::zeros(img1.rows, img1.cols, CV_64FC1);
/* Start timer */
/* Start algorithm */
// getLucasKanadeOpticalFlow(prevDiff, diff, u, v);
double newStart = timestamp();
int maxLayer = getMaxLayer(prevDiff);
coarseToFineEstimation(prevDiff, diff, u, v, maxLayer);
double newEnd = timestamp();
/* End algorithm */
double newElapsed = newEnd - newStart;
printf("total elapsed time (pyramids) = %f seconds.\n", newElapsed);
//duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
// cout<<"Overall Duration: "<< duration*1000 <<" milliseconds\n";
/* End timer */
prevDiff = diff;
Mat mag = u;
double avgX = 0;
double avgY = 0;
int counts = 0;
for (int i = 0; i < u.rows; i++) {
for (int j = 0; j < u.cols; j++) {
double xFlow = u.ATD(i,j);
double yFlow = v.ATD(i,j);
double val = sqrt(xFlow * xFlow + yFlow * yFlow);
if (val < 20) {
val = 0;
} else {
avgX += j;
avgY += i;
counts++;
// circle(frame, Point2f(avgX, avgY), 1, Scalar(255, 0, 0), 2, 8, 0);
}
mag.ATD(i,j) = val;
}
}
normalize(mag, mag, 255);
int radius = 35;
avgX /= counts;
avgY /= counts;
// rescale
float scale = (frame.cols/current_frame.cols);
avgX *= scale;
avgY *= scale;
if (counts > 500) {
circle(frame, Point2f(avgX, avgY), radius, Scalar(0, 0, 255), 2, 8, 0);
}
imshow("hand", frame);
if(waitKey(30) >= 0) break;
double totalEnd = timestamp();
double totalElapsed = totalEnd - totalStart;
printf("total elapsed time of for loop = %f seconds.\n", totalElapsed);
}
return 0;
}
