////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood is set up "manually"
//
void GeneralGraph_DArraySArray(int width,int height,int num_pixels,int num_labels)
{
int *result = new int[num_pixels]; // stores result of optimization
// first set up the array for data costs
int *data = new int[num_pixels*num_labels];
for ( int i = 0; i < num_pixels; i++ )
for (int l = 0; l < num_labels; l++ )
if (i < 25 ){
if( l == 0 ) data[i*num_labels+l] = 0;
else data[i*num_labels+l] = 10;
}
else {
if( l == 5 ) data[i*num_labels+l] = 0;
else data[i*num_labels+l] = 10;
}
// next set up the array for smooth costs
int *smooth = new int[num_labels*num_labels];
for ( int l1 = 0; l1 < num_labels; l1++ )
for (int l2 = 0; l2 < num_labels; l2++ )
smooth[l1+l2*num_labels] = (l1-l2)*(l1-l2) <= 4 ? (l1-l2)*(l1-l2):4;
try{
GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels,num_labels);
gc->setDataCost(data);
gc->setSmoothCost(smooth);
// now set up a grid neighborhood system
// first set up horizontal neighbors
for (int y = 0; y < height; y++ )
for (int x = 1; x < width; x++ )
gc->setNeighbors(x+y*width,x-1+y*width);
// next set up vertical neighbors
for (int y = 1; y < height; y++ )
for (int x = 0; x < width; x++ )
gc->setNeighbors(x+y*width,x+(y-1)*width);
printf("\nBefore optimization energy is %d",gc->compute_energy());
gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
printf("\nAfter optimization energy is %d",gc->compute_energy());
for ( int i = 0; i < num_pixels; i++ )
result[i] = gc->whatLabel(i);
delete gc;
}
catch (GCException e){
e.Report();
}
delete [] result;
delete [] smooth;
delete [] data;
}
void mexFunction(
int nout, /* number of expected outputs */
mxArray *out[], /* mxArray output pointer array */
int nin, /* number of inputs */
const mxArray *in[] /* mxArray input pointer array */
)
{
enum {IN_CLASS=0,IN_UNARY,IN_PAIRWISE,IN_LABELCOST,IN_EXPANSION} ;
enum {OUT_LABELS=0, OUT_ENERGY, OUT_ENERGYAFTER} ;
bool expansion = false;
/****************************************************************************
* ERROR CHECKING
***************************************************************************/
expansion = *mxGetPr(in[IN_EXPANSION]) > 0;
if (nout > 3)
mexErrMsgTxt("At most three outputs are allowed.");
if(mxGetClassID(in[IN_CLASS]) != mxDOUBLE_CLASS)
mexErrMsgTxt("Class must be a vector of class DOUBLE");
if(mxGetM(in[IN_CLASS]) != 1 && mxGetN(in[IN_CLASS]) != 1)
mexErrMsgTxt("Class must be a vector");
if(mxGetClassID(in[IN_LABELCOST]) != mxSINGLE_CLASS)
mexErrMsgTxt("Labelcost term must be a matrix of class SINGLE");
int num_labels = mxGetM(in[IN_UNARY]);
int num_pixels = mxGetN(in[IN_UNARY]);
if(mxGetM(in[IN_CLASS]) != num_pixels && mxGetN(in[IN_CLASS]) != num_pixels)
mexErrMsgTxt("Class size does not match cols in Unary term.");
if(mxGetM(in[IN_LABELCOST]) != mxGetN(in[IN_LABELCOST]) ||
mxGetM(in[IN_LABELCOST]) != num_labels)
mexErrMsgTxt("Labelcost is not symmetric or does not match rows in Unary term.");
if(mxGetM(in[IN_PAIRWISE]) != num_pixels ||
mxGetN(in[IN_PAIRWISE]) != num_pixels)
mexErrMsgTxt("Pairwise is not symmetric or does not match cols in Unary term.");
/* Create output arrays */
mwSize dims[2] = {1,0};
out[OUT_ENERGY] = mxCreateNumericArray(1, dims, mxDOUBLE_CLASS, mxREAL);
out[OUT_ENERGYAFTER] = mxCreateNumericArray(1, dims, mxDOUBLE_CLASS, mxREAL);
double * energy = mxGetPr(out[OUT_ENERGY]);
double * energy_after = mxGetPr(out[OUT_ENERGYAFTER]);
mwSize pdims[2] = {num_pixels,1};
out[OUT_LABELS] = mxCreateNumericArray(1,pdims,mxDOUBLE_CLASS, mxREAL);
double * labels = mxGetPr(out[OUT_LABELS]);
/* Data costs are nlabels rows x npixels cols */
double * data = (double *)mxGetData(in[IN_UNARY]);
double * classes = mxGetPr(in[IN_CLASS]);
if (num_pixels == 1) { /* one pixel is a special case */
*energy = data[(int)classes[0]];
int minlabel = (int)classes[0];
double mincost = *energy;
for(int i = 0; i < num_labels; i++)
if(data[i] < mincost) {
mincost = data[i];
minlabel = i;
}
labels[0] = minlabel;
*energy_after = mincost;
return;
}
/****************************************************************************
* Setup Graph and Perform Optimization
***************************************************************************/
try {
GCoptimizationGeneralGraph * gc = new GCoptimizationGeneralGraph(num_pixels, num_labels);
for (int i = 0; i < num_pixels; i++) {
gc->setLabel(i, (int)classes[i]);
}
gc->setDataCost(data);
Images imgs;
int n_images = *((int*)mxGetData(in[5]));
imgs.images = new unsigned char* [n_images];
for (int i_img = 0; i_img < n_images; i_img++)
imgs.images[i_img] = (unsigned char *)mxGetData(mxGetCell(in[6], i_img));
gc->setSmoothCost(smooth_cost, (void*)(&imgs));
/* Set spatialy varying part of the smoothness cost with the neighborhood
*/
mwSize total = 0;
double * pair = mxGetPr(in[IN_PAIRWISE]);
mwIndex * ir = mxGetIr(in[IN_PAIRWISE]);
mwIndex * jc = mxGetJc(in[IN_PAIRWISE]);
for (int col=0; col < num_pixels; col++) {
mwIndex starting_row_index = jc[col];
mwIndex stopping_row_index = jc[col+1];
if (starting_row_index == stopping_row_index)
continue;
for (int idx = starting_row_index; idx < stopping_row_index; idx++) {
/* only set bottom triangle of pairwise, per GC_README */
if ( ir[idx] > col )
gc->setNeighbors(ir[idx], col, pair[total]);
total++;
}
}
*energy = gc->compute_energy();
/* From GC_README
* The expansion algorithm for energy minimization can be used whenever for
* any 3 labels a,b,c V(a,a) + V(b,c) <= V(a,c)+V(b,a). In other words,
* expansion algorithm can be used if the binary energy for the expansion
* algorithm step is regular, using V. Kolmogorov's terminology.
*
* The swap algorithm for energy minimization can be used whenever for any 2
* labels a,b V(a,a) + V(b,b) <= V(a,b)+V(b,a). In other words, swap
* algorithm can be used if the binary energy for the swap algorithm step is
* regular, using V. Kolmogorov's terminology.
*/
if(expansion)
gc->expansion();
else
gc->swap();
*energy_after = gc->compute_energy();
for (int i = 0; i < num_pixels; i++ )
labels[i] = gc->whatLabel(i);
delete gc;
delete[] imgs.images;
}
catch (GCException e) {
mexErrMsgTxt(e.message);
}
}
int main(int argc, char **argv)
{
/** Argumetns. */
std::string path = argv[1];
int ID_IMG_i = std::stoi(argv[2]);
int ID_IMG_e = std::stoi(argv[3]);
/**
* label = 0 // Reliabel ok
* label = 1 // No-Reliabel missing occluder
* label = 2 // No-Reliabel extra occluder
* label = 3 // No geometry
*/
int N_LABELS = 4;
if (argc == 5) N_LABELS = std::stoi(argv[4]);
/** Big containers. */
float *sdepth, *descEr;
int *neigh, *color;
/** Set width and height */
int w, h;
sdepth = new float[2000*2000]; // TODO: find a better way to get w and h
read_sdepth(path + Utils::sprint("/labeling_cues/%08d.depth", ID_IMG_i), sdepth, w, h);
delete[] sdepth;
int N_IMGS = ID_IMG_e - ID_IMG_i + 1;
int N_NODES = N_IMGS*w*h; // N pixels
int *result = new int[N_NODES]; // Stores result of optimization
sdepth = new float[N_NODES];
descEr = new float[N_NODES * 2];
neigh = new int[N_NODES*N_NEIGHS];
color = new int[N_NODES];
/**--------------------- Loading labeling cues ----------------------*/
memset(sdepth, 0, sizeof(float)*N_NODES);
memset(descEr, 0, sizeof(float)*N_NODES * 2);
memset(neigh, 0, sizeof(int)*N_NODES * N_NEIGHS);
memset(color, 0, sizeof(int)*N_NODES);
for (int i = 0; i <N_IMGS; i++)
{
int ID = ID_IMG_i + i;
// Read cues
std::string sdepth_name = path + Utils::sprint("/labeling_cues/%08d.depth", ID);
std::string conf_name = path + Utils::sprint("/labeling_cues/%08d.conf", ID);
std::string conf_neig = path + Utils::sprint("/labeling_cues/%08d.neig", ID);
std::string color_name = path + Utils::sprint("/labeling_cues/%08d.color", ID);
read_sdepth(sdepth_name, &sdepth[ i*h*w ], w, h);
read_descErr(conf_name, &descEr[ i*h*w*2 ], w, h);
read_neighbor(conf_neig, &neigh[ i*h*w*N_NEIGHS ], w, h);
read_color(color_name, &color[ i*h*w ], w, h);
}
float mean_good, std_good, mean_bad, std_bad;
std::string di_filename = path + Utils::sprint("/labeling_cues/di_distribution.txt");
read_di_distr(di_filename, mean_good, std_good, mean_bad, std_bad );
/**-------------------------------------------------------------------*/
try{
GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph( N_NODES, N_LABELS);
// set up the needed data to pass to cost functions
Img_cues cues;
cues.numLab = N_LABELS;
cues.w = w;
cues.h = h;
cues.sdepth = sdepth;
cues.desc_er = descEr;
//cues.neigh = neigh;
cues.color = color;
cues.mean_good = mean_good;
cues.std_good = std_good;
cues.mean_bad = mean_bad;
cues.std_bad = std_bad;
if (N_LABELS == 4)
{
gc->setDataCost(&dataFn_4, &cues);
gc->setSmoothCost(&smoothFn_4, &cues); // smoothness comes from function pointer
}
else
{
gc->setDataCost(&dataFn_3, &cues);
gc->setSmoothCost(&smoothFn_3, &cues); // smoothness comes from function pointer
}
/** ********************* Set up a grid neighborhood system ********************* */
// first set up horizontal neighbors
for (int im_id = 0; im_id < N_IMGS; im_id++)
for (int y = 0; y < h; y++ )
for (int x = 1; x < w; x++ )
gc->setNeighbors(x + y*w + im_id*w*h, x - 1 + y*w + im_id*w*h);
// next set up vertical neighbors
for (int im_id = 0; im_id < N_IMGS; im_id++)
for (int y = 1; y < h; y++ )
for (int x = 0; x < w; x++ )
gc->setNeighbors(x + y*w + im_id*w*h, x + (y - 1)*w + im_id*w*h);
// next set MV neighs
for (int im_id = 0; im_id < N_IMGS; im_id++)
{
for (int y = STARTING_PADDING; y < h; y = y + MAX_PADDING)
{
for (int x = STARTING_PADDING; x < w; x = x + MAX_PADDING)
{
for (int layer_id = 0; layer_id < N_VIEWS_DAISY_ERR; layer_id ++)
{
int id_curr_px = x + y*w + im_id*w*h;
int id_mv_px = neigh[x + y*w + im_id*w*h*N_NEIGHS + layer_id*w*h];
id_mv_px = id_mv_px - ID_IMG_i*w*h; // Remap the node id according
if (id_mv_px < 0 || id_mv_px >= N_NODES)
break; // No more neighbors
gc->setNeighbors(id_curr_px, id_mv_px);
}}}}
delete[] neigh;
//--------------------------------------------------------------------------------------
printf("\nBefore optimization energy is %d", gc->compute_energy());
gc->expansion(3);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
//gc->expansion(-1);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
//gc->swap(2);
//gc->swap(-1);
printf("\nAfter optimization energy is %d", gc->compute_energy());
for (int i = 0; i < N_NODES; i++)
result[i] = gc->whatLabel(i);
std::string labels_name = path + Utils::sprint("/labeling_cues/labels%u_from_%u_to_%u.bin", N_LABELS, ID_IMG_i, ID_IMG_e);
write_labels(labels_name, result, N_LABELS, w, h, N_IMGS, N_NODES);
delete gc;
}
catch (GCException e){
e.Report();
}
delete[] result;
delete[] sdepth;
delete[] descEr;
delete[] color;
printf("\n Finished %d (%d) clock per sec %d", clock() / CLOCKS_PER_SEC, clock(), CLOCKS_PER_SEC);
return 0;
}
// int smoothFn(int p1, int p2, int l1, int l2)
// {
// int length=(float)len[p1][p2];
// cout<<" Length "<<length<<endl;
// int diff=1+differ[p1][p2];
// cout<<" Diff "<<diff<<endl;
// float xx=((float)length/(float)diff)*100;
// cout<<xx<<" Smoothfun "<<endl;
// return xx;
// }
vector<int> GeneralGraph_DArraySArray(int width,int height,int num_pixels,int num_labels)
{
// int *result = new int[num_pixels]; // stores result of optimization
cout<<width<<" "<<height<<" "<<num_pixels<<" "<<num_labels <<endl;
vector<int>result(num_pixels);
// first set up the array for data costs
int *data = new int[num_pixels*num_labels];
for ( int i = 0; i < num_pixels; i++ )
{
for (int l = 0; l < num_labels; l++ )
{
int temp=prob[i]*1000;
if(l%2==0)
{
data[i*num_labels+l] =temp;
}
else
{
data[i*num_labels+l] =-temp;
}
}
}
// next set up the array for smooth costs
int *smooth = new int[num_labels*num_labels];
smooth[0]=0;
smooth[1]=0;
smooth[2]=0;
smooth[3]=0;
for(int p1=0; p1<num_pixels; p1++)
{
for(int p2=0; p2<num_pixels; p2++)
{
for ( int l1 = 0; l1 < num_labels; l1++ )
{
for (int l2 = 0; l2 < num_labels; l2++ )
{
int length=len[p1][p2];
int differ=1+abs(prob[p1]-prob[p2]);
float val=((float)length/(float)differ);
smooth[l1+l2*num_labels] =smooth[l1+l2*num_labels]+val;
}
}
}
}
try
{
GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels,num_labels);
gc->setDataCost(data);
gc->setSmoothCost(smooth);
// now set up a grid neighborhood system
// first set up horizontal neighbors
for (int x = 0; x < num_pixels; x++ )
{
for (int y = 0; y < num_pixels; y++ )
{
if(GR[x][y]==1 && x!=y)
{
gc->setNeighbors(x,y);
}
}
}
printf("\nBefore optimization energy is %d",gc->compute_energy());
gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
printf("\nAfter optimization energy is %d",gc->compute_energy());
for ( int i = 0; i < num_pixels; i++ )
result[i] = gc->whatLabel(i);
delete gc;
return result;
}
catch (GCException e)
{
e.Report();
}
delete [] smooth;
delete [] data;
vector<int>xx;
return xx;
}
long long CGraphFitting::fastFitting()
{
int num_pixels=pImg3D->foreNum+1;
int num_labels=getValidNum();
////////////////////////////////////////////////////////////
int *data = new int[num_pixels*num_labels];
int curIndex=0;
int slice=pImg3D->getS();
int width=pImg3D->getW();
int height=pImg3D->getH();
for(int z = 0;z<slice;++z)
for(int y=0;y<height;++y)
for(int x=0;x<width;++x)
{
bool vessel=pImg3D->isVessel(x,y,z);
if(vessel)
{
for (int l=0; l < num_labels; l++ )
{
if(l==num_labels-1)
data[curIndex*num_labels+l]=5000;//INFINIT;
else
data[curIndex*num_labels+l]=models[l].compEnergy(x,y,z,*pImg3D);
}
curIndex++;
}
}
for (int l=0; l < num_labels; l++ )
{
if(l==num_labels-1)
data[curIndex*num_labels+l]=0;
else
data[curIndex*num_labels+l]=INFINIT;
}
//////////////////////////////////////////////////////////////////
int *label = new int[num_labels];
for(int k=0;k<num_labels;++k) label[k]=LABELCOST;
/////////////////////////////////////////////////////////////////
int *smooth = new int[num_labels*num_labels];
memset(smooth,0,sizeof(int)*num_labels*num_labels);
for( int i=0; i<num_labels; i++ )
for ( int j=0; j<num_labels; j++ )
smooth[ i*num_labels+j ] = smooth[ i+j*num_labels ] =SMOOTHCOST* int(i!=j);
long long energy=0;
try{
GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels,num_labels);
gc->setDataCost(data);
gc->setLabelCost(label);
#ifdef USE_SMOOTHCOST
gc->setSmoothCost(smooth);
for(int z = 0;z<slice;++z)
for(int y=0;y<height;++y)
for(int x=0;x<width;++x)
{
if(!pImg3D->isVessel(x,y,z)) continue;
int numHoles=0;
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x+1,y,z)))//right neighbour
{
if(pImg3D->isVessel(x+1,y,z))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x+1,y,z));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x,y+1,z)))//top neighbour
{
if(pImg3D->isVessel(x,y+1,z))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x,y+1,z));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x,y,z+1)))//front neighbour
{
if(pImg3D->isVessel(x,y,z+1))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x,y,z+1));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x-1,y,z))&& !pImg3D->isVessel(x-1,y,z) ) ++numHoles;//left hole
if(inRange(Voxel(x,y-1,z))&& !pImg3D->isVessel(x,y-1,z) ) ++numHoles;//down hole
if(inRange(Voxel(x,y,z-1))&& !pImg3D->isVessel(x,y,z-1) ) ++numHoles;//back hole
if(numHoles>0)
gc->setNeighbors(pImg3D->getRealPos(x,y,z),num_pixels-1,numHoles);
}
#endif
//printf("\nBefore optimization energy is %d",gc->compute_energy());
//std::cout<<"\nBefore optimization energy is "<<gc->compute_energy();
energy=gc->compute_energy();
gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
//printf("\nAfter optimization energy is %d",gc->compute_energy());
//std::cout<<"\nAfter optimization energy is "<<gc->compute_energy();
gc->compute_energy();
for ( int i = 0; i < num_pixels; i++ )
{
int tag = gc->whatLabel(i);
models[tag].addSupport();
pLabels[i]=tag;
//if(result[i]!=num_labels-1) printf("%d ",result[i]);
}
////////////////////////////////////////////////////////////////////////////////////
for(int i=0;i<num_labels;++i)
{
int sp=models[i].getSupport();
models[i].setValid(sp>0);
if(i==num_labels-1)//last model ,must be valid
models[i].setValid(true);
}
///////////////////////////////////////////////
for(int i=0;i<num_labels-1;++i)
{
if(models[i].isValid())
{
fitLine(i,gc);
}
}
delete gc;
}
catch (GCException e){
e.Report();
}
delete [] smooth;
delete []label;
delete [] data;
return energy;
}