void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//A,B,offsets,distances,wsearch,patch_w
const mwSize *N = mxGetDimensions(prhs[1]);
//mexPrintf("dimensions: %d ", dimensions);
float *A = (float *) mxGetData(prhs[0]);
float *B = (float *) mxGetData(prhs[1]);
//int *offsets = (int *) mxGetData(prhs[2]);
//float *distances = (float *) mxGetData(prhs[3]);
int wsearch= mxGetScalar(prhs[4]);
int patch_w= mxGetScalar(prhs[5]);
mexPrintf("wsearc: %d ", wsearch);
mexPrintf("\nwpatch: %d ", patch_w);
int rows=N[0];
int cols=N[1];
int slices=N[2];
int objects=N[3];
int aew = cols- patch_w+1, aeh = rows - patch_w + 1, aez= slices - patch_w +1; /* Effective width and height (possible upper left corners of patches). */
//const mwSize dim_offsets[4]= {N[0],N[1],N[2], 4};
// plhs[0]= mxCreateNumericMatrix(0, 0,mxINT32_CLASS, mxREAL); /* Create an empty array */
//mxSetDimensions(plhs[0],(const mwSize *)dim_offsets, 4); /* Set the dimensions to N[0] x ... x N[K?1] */
//mxSetData(plhs[0], mxMalloc(sizeof(int)*(N[0]*N[1]*N[2]*4))); /* Allocate memory */
//int *offsets_o=(int *) mxGetData(plhs[0]);
//plhs[1]= mxCreateNumericMatrix(0, 0,mxSINGLE_CLASS, mxREAL); /* Create an empty array */
//mxSetDimensions(plhs[1], (const mwSize *)N, 3); /* Set the dimensions to N[0] x ... x N[K?1] */
//mxSetData(plhs[1], mxMalloc(sizeof(float)*(N[0]*N[1]*N[2]))); /* Allocate memory */
//float *distances_o=(float *) mxGetData(plhs[1]);
//MIRAR COMO COPIAR DATA
plhs[0]=mxDuplicateArray(prhs[2]);
int *offsets_o=(int *) mxGetData(plhs[0]);
plhs[1]=mxDuplicateArray(prhs[3]);
float *distances_o=(float *) mxGetData(plhs[1]);
int pm_iters=4;
mexPrintf("\naew=%d ",aew);
mexPrintf("\naeh=%d",aeh);
mexPrintf("\naez=%d",aez);
for (int iter = 0; iter < pm_iters; iter++){
mexPrintf("\nPar");
int r_start = 0, r_end = aeh, r_change = 1;
int c_start = 0, c_end = aew, c_change = 1;
int z_start = 0, z_end = aez, z_change = 1;
if (iter % 2 == 1) {//impares
r_start = r_end-1, r_end = -1, r_change = -1;
c_start = c_end-1, c_end = -1, c_change = -1;
z_start = z_end-1, z_end = -1, z_change = -1;
mexPrintf("\nImpar");
}
for (int az = z_start; az != z_end; az+=z_change){
for (int ar = r_start; ar != r_end; ar+=r_change){
for(int ac = c_start; ac != c_end; ac+=c_change){
int r_best=offsets_o[0*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar];
int c_best=offsets_o[1*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar];
int z_best=offsets_o[2*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar];
int t_best=offsets_o[3*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar];
float d_best = distances_o[az*(rows*cols)+ac*rows+ar];
//mexPrintf("\nfinished reading distances and offsets");
/* Propagation: Improve current guess by trying instead correspondences from left and above (below and right on odd iterations). */
//check left()
if ((unsigned) (ac - c_change) < (unsigned) aew){
int rp = offsets_o[0*(slices*rows*cols)+az*(rows*cols)+(ac-c_change)*rows+ar];//row of the element in the left() of the current position
int cp = offsets_o[1*(slices*rows*cols)+az*(rows*cols)+(ac-c_change)*rows+ar];//row of the element in the left() of the current position
cp=cp+c_change;//col right()
int zp = offsets_o[2*(slices*rows*cols)+az*(rows*cols)+(ac-c_change)*rows+ar];//row of the element in the left() of the current position
int tp = offsets_o[3*(slices*rows*cols)+az*(rows*cols)+(ac-c_change)*rows+ar];//idx in the left() of the current position
if ((unsigned) cp < (unsigned) aew){
improve_guess(A, B,ar,ac,az,r_best,c_best,z_best,t_best,d_best,rp,cp,zp,tp,patch_w,rows,cols,slices);
}
}
//mexPrintf("\nfinished checking left");
//check above ()
if ((unsigned) (ar - r_change) < (unsigned) aeh) {
int rp = offsets_o[0*(slices*rows*cols)+az*(rows*cols)+ac*rows+(ar-r_change)];//row of the element above() of the current position
rp=rp+r_change;// row down()
int cp=offsets_o[1*(slices*rows*cols)+az*(rows*cols)+ac*rows+(ar-r_change)];//col of the element above() of the current position
int zp=offsets_o[2*(slices*rows*cols)+az*(rows*cols)+ac*rows+(ar-r_change)];//slice of the element above() of the current position
int tp=offsets_o[3*(slices*rows*cols)+az*(rows*cols)+ac*rows+(ar-r_change)];//idx of elt above
if ((unsigned) rp < (unsigned) aeh) {
improve_guess(A, B,ar,ac,az,r_best,c_best,z_best,t_best,d_best,rp,cp,zp,tp,patch_w,rows,cols,slices);
}
}
//("\nfinished checking above");
//check slice above ()
if ((unsigned) (az - z_change) < (unsigned) aez) {
int rp = offsets_o[0*(slices*rows*cols)+(az-z_change)*(rows*cols)+ac*rows+ar];//row of the element in slice above() of the current position
int cp=offsets_o[1*(slices*rows*cols)+(az-z_change)*(rows*cols)+ac*rows+ar];//col of the element in slice above() of the current position
int zp=offsets_o[2*(slices*rows*cols)+(az-z_change)*(rows*cols)+ac*rows+ar];//slice of the element in slice above() of the current position
zp=zp+z_change;//slice down()
int tp=offsets_o[3*(slices*rows*cols)+(az-z_change)*(rows*cols)+ac*rows+ar];//idx of the element in slice above() of the current position
if ((unsigned) zp < (unsigned) aez) {
improve_guess(A, B,ar,ac,az,r_best,c_best,z_best,t_best,d_best,rp,cp,zp,tp,patch_w,rows,cols,slices);
}
}
//mexPrintf("\nfinished checking slice above");
//Random Search
//rand() % (HIGH - LOW + 1) + LOW;
// mexPrintf("antes del Random");
for (int mag = wsearch; mag >= 1; mag /= 2) {
// Sampling window //
int cmin = MAX(c_best-mag, 0), cmax = MIN(c_best+mag+1,aew);
int rmin = MAX(r_best-mag, 0), rmax = MIN(r_best+mag+1,aeh);
int zmin = MAX(z_best-mag, 0), zmax = MIN(z_best+mag+1,aez);
//mexPrintf("\nfinished computing boundaries");
//mexPrintf("\nrmax:%d \nrmin:%d \ncmax:%d \ncmin:%d \nzmax:%d \nzmin:%d",rmax,rmin,cmax,cmin,zmax,zmin);
int rp = rmin+rand()%(rmax-rmin);
int cp = cmin+rand()%(cmax-cmin);
int zp = zmin+rand()%(zmax-zmin);
int tp = t_best;
//mexPrintf("\nfinished computing random values");
//mexPrintf("\nrp:%d \ncp:%d \nzp:%d \ntp:%d",rp,cp,zp,tp);
improve_guess(A, B,ar,ac,az,r_best,c_best,z_best,t_best,d_best,rp,cp,zp,tp,patch_w,rows,cols,slices);
}
//mexPrintf("\nfinish random search");
//End Random Search
offsets_o[0*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar]=r_best;
offsets_o[1*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar]=c_best;
offsets_o[2*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar]=z_best;
offsets_o[3*(slices*rows*cols)+az*(rows*cols)+ac*rows+ar]=t_best;
distances_o[az*(rows*cols)+ac*rows+ar]=d_best;
//mexPrintf("\nUpdate offsets and distances random search");
}
}
}
}
}
static void patchmatch(IplImage *a, IplImage *b, IplImage *ann, IplImage *annd, IplImage *mask)
{
int iter, rs_start, mag;
int w = ann->width, h = ann->height;
int32_t *data = (int32_t*)ann->imageData;
int32_t *ddata = (int32_t*)annd->imageData;
int stride = ann->widthStep/sizeof(int32_t);
int aew = a->width - PATCH_W + 1;
int aeh = a->height - PATCH_W + 1;
int bew = b->width - PATCH_W + 1;
int beh = b->height - PATCH_W + 1;
int ax, ay;
// initialize with random values
for (ay = 0; ay < h; ay++) {
int xy = ay * stride;
for (ax = 0; ax < w; ax++) {
uint16_t bx = ax;
uint16_t by = ay;
while (masked(mask, bx, by)) {
bx = rand() % bew;
by = rand() % beh;
}
*(data + xy) = XY_TO_INT(bx, by);
*(ddata + xy) = dist(a, b, ax, ay, bx, by, 0, mask);
xy += 1;
}
}
for (iter = 0; iter < PM_ITERS; iter++) {
int ystart = 0, yend = aeh, ychange = 1;
int xstart = 0, xend = aew, xchange = 1;
if (iter % 2) {
ystart = yend - 1; yend = -1; ychange = -1;
xstart = xend - 1; xend = -1; xchange = -1;
}
for (ay = ystart; ay != yend; ay += ychange) {
for (ax = xstart; ax != xend; ax += xchange) {
if (!masked(mask, ax, ay)) continue;
// current best guess
int xy = ay * stride + ax;
int v = *(data + xy);
int xbest = XY_TO_X(v), ybest = XY_TO_Y(v);
int dbest = *(ddata + xy);
// Propagation: Improve current guess by trying
// correspondences from left and above,
// or below and right on odd iterations
if ((unsigned)(ax - xchange) < (unsigned)aew) {
int vp = *(data + ay * stride + (ax - xchange));
int xp = XY_TO_X(vp) + xchange, yp = XY_TO_Y(vp);
if ((unsigned)xp < (unsigned)bew) {
improve_guess(a, b, ax, ay, &xbest, &ybest, &dbest, xp, yp, mask);
}
}
if ((unsigned)(ay - ychange) < (unsigned)aeh) {
int vp = *(data + ay * stride + (ax - xchange));
int xp = XY_TO_X(vp), yp = XY_TO_Y(vp) + ychange;
if ((unsigned)yp < (unsigned)beh) {
improve_guess(a, b, ax, ay, &xbest, &ybest, &dbest, xp, yp, mask);
}
}
rs_start = RS_MAX;
if (rs_start > max(b->width, b->height)) rs_start = max(b->width, b->height);
for (mag = rs_start; mag >= 1; mag /= 2) {
// sampling window
int xmin = max(xbest - mag, 0);
int xmax = min(xbest + mag + 1, bew);
int ymin = max(ybest - mag, 0);
int ymax = min(ybest + mag + 1, beh);
int xp = xmin + rand() % (xmax - xmin);
int yp = ymin + rand() % (ymax - ymin);
improve_guess(a, b, ax, ay, &xbest, &ybest, &dbest, xp, yp, mask);
}
*(data + xy) = XY_TO_INT(xbest, ybest);
*(ddata + xy) = dbest;
}
}
}
}
/* Match image a to image b, returning the nearest neighbor field mapping a => b coords, stored in an RGB 24-bit image as (by<<12)|bx. */
void patchmatch(BITMAP *a, BITMAP *b, BITMAP *&ann, BITMAP *&annd) {
/* Initialize with random nearest neighbor field (NNF). */
ann = new BITMAP(a->w, a->h);
annd = new BITMAP(a->w, a->h);
int aew = a->w - patch_w+1, aeh = a->h - patch_w + 1; /* Effective width and height (possible upper left corners of patches). */
int bew = b->w - patch_w+1, beh = b->h - patch_w + 1;
memset(ann->data, 0, sizeof(int)*a->w*a->h);
memset(annd->data, 0, sizeof(int)*a->w*a->h);
for (int ay = 0; ay < aeh; ay++) {
for (int ax = 0; ax < aew; ax++) {
int bx = rand()%bew;
int by = rand()%beh;
(*ann)[ay][ax] = XY_TO_INT(bx, by);
(*annd)[ay][ax] = dist(a, b, ax, ay, bx, by);
}
}
for (int iter = 0; iter < pm_iters; iter++) {
/* In each iteration, improve the NNF, by looping in scanline or reverse-scanline order. */
int ystart = 0, yend = aeh, ychange = 1;
int xstart = 0, xend = aew, xchange = 1;
if (iter % 2 == 1) {
xstart = xend-1; xend = -1; xchange = -1;
ystart = yend-1; yend = -1; ychange = -1;
}
for (int ay = ystart; ay != yend; ay += ychange) {
for (int ax = xstart; ax != xend; ax += xchange) {
/* Current (best) guess. */
int v = (*ann)[ay][ax];
int xbest = INT_TO_X(v), ybest = INT_TO_Y(v);
int dbest = (*annd)[ay][ax];
/* Propagation: Improve current guess by trying instead correspondences from left and above (below and right on odd iterations). */
if ((unsigned) (ax - xchange) < (unsigned) aew) {
int vp = (*ann)[ay][ax-xchange];
int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
if ((unsigned) xp < (unsigned) bew) {
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
}
if ((unsigned) (ay - ychange) < (unsigned) aeh) {
int vp = (*ann)[ay-ychange][ax];
int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
if ((unsigned) yp < (unsigned) beh) {
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
}
/* Random search: Improve current guess by searching in boxes of exponentially decreasing size around the current best guess. */
int rs_start = rs_max;
if (rs_start > MAX(b->w, b->h)) { rs_start = MAX(b->w, b->h); }
for (int mag = rs_start; mag >= 1; mag /= 2) {
/* Sampling window */
int xmin = MAX(xbest-mag, 0), xmax = MIN(xbest+mag+1,bew);
int ymin = MAX(ybest-mag, 0), ymax = MIN(ybest+mag+1,beh);
int xp = xmin+rand()%(xmax-xmin);
int yp = ymin+rand()%(ymax-ymin);
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
(*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
(*annd)[ay][ax] = dbest;
}
}
}
}
/* Match image a to image b, returning the nearest neighbor field mapping a => b coords, stored in an RGB 24-bit image as (by<<12)|bx. */
void PatchMatch::patchmatch(cv::Mat &image, cv::Mat &b, BITMAP *ann, BITMAP &annd) {
/* Initialize with random nearest neighbor field (NNF). */
cv::Mat a(image.size(), CV_64F);
image.copyTo(a);
int aew = a.cols - patch_w+1, aeh = a.rows - patch_w + 1; /* Effective width and height (possible upper left corners of patches). */
int bew = b.cols - patch_w+1, beh = b.rows - patch_w + 1;
for (int ay = 0; ay < aeh; ay++) {
for (int ax = 0; ax < aew; ax++) {
int bx, by;
//if((*ann)[ay][ax] == 0){
// bx = rand() % bew;
// by = rand() % beh;
// (*ann)[ay][ax] = XY_TO_INT(bx, by);
//}
//else{
// bx = INT_TO_X((*ann)[ay][ax]);
// by = INT_TO_Y((*ann)[ay][ax]);
//}
bx = rand() % bew;
by = rand() % beh;
(*ann)[ay][ax] = XY_TO_INT(bx, by);
annd[ay][ax] = dist(a, b, ax, ay, bx, by);
}
}
int as;
std::ofstream o_file;
o_file.open("1.txt");
for (int iter = 0; iter < pm_iters; iter++) {
/* In each iteration, improve the NNF, by looping in scanline or reverse-scanline order. */
int ystart = 0, yend = aeh, ychange = 1;
int xstart = 0, xend = aew, xchange = 1;
if (iter % 2 == 1) {
xstart = xend-1; xend = -1; xchange = -1;
ystart = yend-1; yend = -1; ychange = -1;
}
// if(iter == 2)
as = 0;
for (int ay = ystart; ay != yend; ay += ychange) {
for (int ax = xstart; ax != xend; ax += xchange) {
/* Current (best) guess. */
//if(ax == 366 && ay == 367)
// as++;
// int sdsd = targetMask[ay * a.cols + ax];
//if(ax == 352 && ay == 73){
// as++;
//}
int v = (*ann)[ay][ax];
int xbest = INT_TO_X(v), ybest = INT_TO_Y(v);
int dbest = annd[ay][ax];
/* Propagation: Improve current guess by trying instead correspondences from left and above (below and right on odd iterations). */
int targetCount = 0;
for(int i = 0; i < patch_w; i++){
for(int j = 0; j < patch_w; j++){
if(targetMask[(ay + i) * a.cols + ax + j] > 0)
targetCount++;
}
}
if(targetCount > patch_w){
int vp = (*ann)[ay][ax - xchange];
int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
if ((unsigned) xp < (unsigned) bew) {
xbest = xp;
ybest = yp;
(*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
annd[ay][ax] = annd[ay][ax - xchange];
continue;
}
//else{
// vp = (*ann)[ay - ychange][ax];
// xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
// if ((unsigned) yp < (unsigned) beh) {
// xbest = xp;
// ybest = yp;
// (*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
// annd[ay][ax] = annd[ay - ychange][ax];
// continue;
// }
//}
if((unsigned) (ay - ychange) < (unsigned) aeh){
int vp = (*ann)[ay - ychange][ax];
int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
if ((unsigned) yp < (unsigned) beh) {
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
}
}
else{
if ((unsigned) (ax - xchange) < (unsigned) aew) {
int vp = (*ann)[ay][ax - xchange];
int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
if ((unsigned) xp < (unsigned) bew) {
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
}
if ((unsigned) (ay - ychange) < (unsigned) aeh) {
int vp = (*ann)[ay - ychange][ax];
int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
if ((unsigned) yp < (unsigned) beh) {
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
}
}
/* Random search: Improve current guess by searching in boxes of exponentially decreasing size around the current best guess. */
int rs_start = rs_max;
if (rs_start > MAX(b.cols, b.rows)) { rs_start = MAX(b.cols, b.rows); }
for (int mag = rs_start; mag >= 1; mag /= 2) {
/* Sampling window */
int xmin = MAX(xbest-mag, 0), xmax = MIN(xbest+mag+1,bew);
int ymin = MAX(ybest-mag, 0), ymax = MIN(ybest+mag+1,beh);
int xp = xmin+rand()%(xmax-xmin);
int yp = ymin+rand()%(ymax-ymin);
int targetCount = 0;
for(int i = 0; i < patch_w; i++){
for(int j = 0; j < patch_w; j++){
if(targetMask[(yp + i) * a.cols + xp + j] > 0){
targetCount++;
break;
}
}
if(targetCount > 0)
break;
}
if(targetCount > 0){
mag *= 2;
continue;
}
improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
}
(*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
annd[ay][ax] = dbest;
//a.at<cv::Vec3b>(ay, ax) = b.at<cv::Vec3b>(ybest, xbest);
//for (int dy = 0; dy < patch_w; dy++) {
// for (int dx = 0; dx < patch_w; dx++) {
// a.at<cv::Vec3b>(ay + dy, ax + dx) = b.at<cv::Vec3b>(ybest + dy, xbest + dx);
// }
// }
if(ay != ybest || ax != xbest)
as++;
if(ax == 352 && ay == 73){
as++;
}
// for(int i = 0; i < cvImage.rows - 7 + 1; i++){
// for(int j = 0; j < cvImage.cols - 7 + 1; j++){
// int v = (*ann)[i][j];
// int x = INT_TO_X(v);
// int y = INT_TO_Y(v);
// anni.at<cv::Vec3b>(i, j).val[0] = (0+ x) % 256;
// anni.at<cv::Vec3b>(i, j).val[1] = (0 + y) % 256;
// anni.at<cv::Vec3b>(i, j).val[2] = 0;
// if(annd[i][j] > 0){
// anndi.at<cv::Vec3b>(i, j).val[0] = annd[i][j] % 256;
// }
// else
// anndi.at<cv::Vec3b>(i, j).val[0] = 0;
// anndi.at<cv::Vec3b>(i, j).val[1] = 0;
// anndi.at<cv::Vec3b>(i, j).val[2] = 0;
// }
//}
//cv::imshow("r", anni);
//cv::imshow("rd", anndi);
}
}
//std::string s[] = {"0","1","2","3","4","5","6","7","8","9","10","11","12","13","14","15","16","17","18","19"};
//cv::imshow(s[iter], a);
o_file<<as<<std::endl;
std::cout<<as<<std::endl;
}
o_file.close();
a.copyTo(image);
}
