void pozitiveScore(
char *cc1
,char *cc2
,double *simm /*similarity matrix*/
,int mdim /*dimension of similarity matrix*/
,double mdelta /*gap penalty*/
,double gapext /*gap extendsion costs*/
,char *align_type /*type of alignment*/
,double *score
,char *errormsg
,char *score_t
)
{
DYNAM test;
int malloccount=0;
malloccount++;
test.c1 = cc1;
test.c2 = cc2;
/*set dimensions of the dynamic programming matrices*/
test.lc1 = strlen(test.c1)+1; /*length of the numerical representation of the string.*/
test.lc2 = strlen(test.c2)+1;
/*set dimension of the similarity matrix*/
test.dim = mdim;
/*set gap opening cost and gap extend*/
test.delta = mdelta;
test.gapext = gapext;
/*alocate memmory*/
allocDYNAM(&test);
/*pozitive*/
test.pozitiv=1;
test.ungl=0;
/*allocate space for inidices of ungapped strings.*/
if(test.lc1<test.lc2)
{
test.ung1 = calloc(test.lc1 , sizeof(int));
malloccount++;
test.ung2 = calloc(test.lc1 , sizeof(int));
malloccount++;
}
else
{
test.ung1 = calloc(test.lc2 , sizeof(int));
malloccount++;
test.ung2 = calloc(test.lc2 , sizeof(int));
malloccount++;
}
/* translate char to similartity matrix indices */
if(-1==seq2index(test.nc1, test.c1 , test.lc1))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
}
if(-1==seq2index(test.nc2, test.c2 , test.lc2))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
};
/*copy stuff into similarity matrix*/
initsim(test.sim,test.dim,simm);
/*printstrut(&test);*/
if(strcmp(align_type,"global")==0)
{
globalAlign(&test);
tracebackPOZITIVGlobal(&test);
}
else if(strcmp(align_type,"local")==0)
{
localAlign(&test);
tracebackPOZITIVLocal(&test);
}
else if(strcmp(align_type,"overlap")==0)
{
overlapAlign(&test);
tracebackPOZITIVOverlap(&test);
}
else
{
printf("No such type of alignment : [ %s ] \n",align_type);
return;
}
*score=pozitive(&test);
free(test.ung1);
malloccount--;
free(test.ung2);
malloccount--;
if(malloccount==0)
error("The Devil are in your Code!\n");
freeDYNAM(&test);
}
/*Returns Smith Watermann Score*/
void globalScore
(
char *cc1
,char *cc2
,double *simm /*similarity matrix*/
,int mdim /*dimension of similarity matrix*/
,double mdelta /*gap penalty*/
,double gapext /*gap extendsion costs*/
,char *alig_type /*type of alignment*/
,double *score /*score of alignment*/
,char *errormsg
,char *score_t
)
{
DYNAM test;
double selfscore1,selfscore2;
/*set chars*/
test.c1=cc1;
test.c2=cc2;
/*set dimensions of the dynamic programming matrices*/
test.lc1 = strlen(test.c1)+1; /*length of the numerical representation of the string.*/
test.lc2 = strlen(test.c2)+1;
/*set dimension of the similarity matrix*/
test.dim = mdim;
/*set gap opening cost and gap extend*/
test.delta = mdelta;
test.gapext = gapext;
/*alocate memmory*/
allocDYNAM(&test);
/* translate char to similartity matrix indices */
if(-1==seq2index(test.nc1, test.c1 , test.lc1)){
strcpy(errormsg,"Nonstandard amino acid");
return;
}
if(-1==seq2index(test.nc2, test.c2 , test.lc2)){
strcpy(errormsg,"Nonstandard amino acid");
return;
};
/*copy stuff into similarity matrix*/
initsim(test.sim,test.dim,simm);
if(strcmp(alig_type,"global")==0)
{
globalAlign(&test);
}
else if(strcmp(alig_type,"local")==0)
{
localAlign(&test);
}
else if(strcmp(alig_type,"overlap")==0)
{
overlapAlign(&test);
}
else
{
printf("No such type of alignment : %s !\n",alig_type);
}
if(strcmp(score_t,"scoreN")==0)
{
selfscore1 = getselfalign(test.sim , test.c1);
selfscore2 = getselfalign(test.sim , test.c2);
if(test.score<=0)
{
*score=0;
}
else
{
if(selfscore1<selfscore2)
{
*score = test.score/selfscore1;
}
else
{
*score = test.score/selfscore2;
}
}
}
else
{
*score = test.score;
}
freeDYNAM(&test);
}
void identSimilarScore(
char *cc1
,char *cc2
,double *simm /*similarity matrix*/
,int mdim /*dimension of similarity matrix*/
,double mdelta /*gap penalty*/
,double gapext /*gap extendsion costs*/
,char *align_type /*type of alignment*/
,double *score
,char *errormsg
,char *score_t
)
{
/* printf("all %d",*all);
printf("score %f\n",*score);
printf("mdim %d\n",*mdim);
*/
DYNAM test;
/*set chars*/
int malloccount=0;
test.c1 = cc1;
test.c2 = cc2;
test.ac1 = malloc((strlen(cc1) + strlen(cc2))*sizeof(char));
malloccount++;
test.ac2 = malloc((strlen(cc1) + strlen(cc2))*sizeof(char));
malloccount++;
/*set dimensions of the dynamic programming matrices*/
test.lc1 = strlen(test.c1)+1; /*length of the numerical representation of the string.*/
test.lc2 = strlen(test.c2)+1;
/*set dimension of the similarity matrix*/
test.dim = mdim;
/*set gap opening cost and gap extend*/
test.delta = mdelta;
test.gapext = gapext;
/*alocate memmory*/
allocDYNAM(&test);
/* translate char to similartity matrix indices */
if(-1==seq2index(test.nc1, test.c1 , test.lc1))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
}
if(-1==seq2index(test.nc2, test.c2 , test.lc2))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
};
/*copy stuff into similarity matrix*/
initsim(test.sim,test.dim,simm);
/* printstrut(&test);*/
if(strcmp(align_type,"global")==0)
{
globalAlign(&test);
tracebackGlobal(&test);
}
else if(strcmp(align_type,"local")==0)
{
localAlign(&test);
tracebackLocal(&test);
}
else if(strcmp(align_type,"overlap")==0)
{
overlapAlign(&test);
tracebackOverlap(&test);
}
else
{
printf("No such type of alignment : [ %s ] \n",align_type);
return;
}
if(strcmp(score_t,"identity")==0)
{
if(test.lc1<=test.lc2)
{
*score = ((double)getalignidentity(test.ac1,test.ac2))/((double)(strlen(test.c1)));
}
else
*score = ((double)getalignidentity(test.ac1,test.ac2))/((double)(strlen(test.c2)));
}
else
{
if(test.lc1<=test.lc2)
{
*score = ((double)getalignsimilarity(test.sim, test.ac1 , test.ac2 ))/((double)(strlen(test.c1)));
}
else
*score = ((double)getalignsimilarity(test.sim, test.ac1 , test.ac2 ))/((double)(strlen(test.c2)));
}
free(test.ac1);
malloccount--;
free(test.ac2);
malloccount--;
freeDYNAM(&test);
}
void globalB(
char *cc1,
char *cc2,
double *simm, /*similarity matrix*/
int mdim, /*dimension of similarity matrix*/
double mdelta, /*gap penalty*/
double gapext, /*gap extendsion costs*/
char *type, /*type of alignment*/
char *al1, /*aligment for string 2*/
char *al2, /*alignment fo r string 2*/
int *all, /*alignment length*/
double *score, /*score of alignment*/
double *selfscore1, /*score of selfalignment*/
double *selfscore2, /*score of selfalignment*/
int *identity, /*nr of identities*/
int *alignsimilarity, /*the similarity of the alignment*/
char *errormsg
)
{
/* printf("all %d",*all);
printf("score %f\n",*score);
printf("mdim %d\n",*mdim);
*/
DYNAM test;
/*set chars*/
test.c1 = cc1;
test.c2 = cc2;
test.ac1 = al1;
test.ac2 = al2;
/*set dimensions of the dynamic programming matrices*/
test.lc1 = strlen(test.c1)+1; /*length of the numerical representation of the string.*/
test.lc2 = strlen(test.c2)+1;
/*set dimension of the similarity matrix*/
test.dim = mdim;
/*set gap opening cost and gap extend*/
test.delta = mdelta;
test.gapext = gapext;
/*alocate memmory*/
allocDYNAM(&test);
/* translate char to similartity matrix indices */
if(-1==seq2index(test.nc1, test.c1 , test.lc1))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
}
if(-1==seq2index(test.nc2, test.c2 , test.lc2))
{
strcpy(errormsg,"Nonstandard amino acid");
return;
};
/*copy stuff into similarity matrix*/
initsim(test.sim,test.dim,simm);
/* printstrut(&test);*/
if(strcmp(type,"global")==0)
{
globalAlign(&test);
/* printmat(test.h,test.lc1,test.lc2);*/
/* printmat(test.Ix,test.lc1,test.lc2);*/
/* printmat(test.Iy,test.lc1,test.lc2);*/
tracebackGlobal(&test);
}
else if(strcmp(type,"local")==0)
{
localAlign(&test);
tracebackLocal(&test);
}
else if(strcmp(type,"overlap")==0)
{
overlapAlign(&test);
tracebackOverlap(&test);
}
else
{
printf("No such type of alignment : [ %s ] \n",type);
return;
}
*score = test.score;
al1=(test.ac1);
al2=(test.ac2);
*selfscore1 = getselfalign(test.sim , test.c1);
*selfscore2 = getselfalign(test.sim , test.c2);
*all = strlen(test.ac1);
*alignsimilarity = getalignsimilarity(test.sim, test.ac1 , test.ac2 );
*identity = getalignidentity(test.ac1,test.ac2);
freeDYNAM(&test);
}
int main (int argc, char** argv)
{
double f;
double vd;
double r;
int iter;
double h;
// argiment parsing
if (pcl::console::parse_argument (argc, argv, "-r", r) >= 0)
{
MIN_DISTANCE = r;
cout << "adjust the registration threshold to " << r << endl;
}
if (pcl::console::parse_argument (argc, argv, "-giter", iter) >= 0)
{
GITER = iter;
cout << "adjust global alignment iterations to " << iter <<" iterations"<< endl;
}
if (pcl::console::parse_argument (argc, argv, "-hessian", h) >= 0)
{
HESSIAN=h;
cout << "adjust SIFT matching threshold to " << h << endl;
}
if (pcl::console::find_argument (argc, argv, "-h") >= 0 || argc == 1)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-g") >= 0)
{
GLOBAL_FLAG = 1;
cout << "Global Reistration is ON" << endl;
}
if (pcl::console::find_argument (argc, argv, "-lcoff") >= 0)
{
LOOP_CLOSURE = 0;
cout << "Loop CLosure Detection is OFF" << endl;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
MLS = 1;
cout << "Moving least square smoothing is ON" << endl;
}
if (pcl::console::find_argument (argc, argv, "-dn") >= 0)
{
DENOISING= 1;
cout << "De-noising (statistic) is ON" << endl;
}
if (pcl::console::parse_argument (argc, argv, "-vd", vd) >= 0)
{
DEPTH_AVG = 1;
cout << "Down-sampling is ON" << endl;
if (vd>0.00001)
{
DOWN_SAMPLE = vd;
cout<<"Down sample threshold is changed to "<<DOWN_SAMPLE<<endl;
}
}
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > data;
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > key3data;
vector<vector<KeyPoint> > key2data;
vector<pcl::CorrespondencesPtr> correspondencesdata;
Mat previousrgb;
vector<KeyPoint> previouskey2d;
// read the rgb-d data one by one
ifstream index;
string root_dir = argv[1];
index.open (string(root_dir + "/input.txt").c_str());
int i = 0;
while (!index.eof ())
{
string file;
index >> file;
if (file.empty ())
break;
file = root_dir + "/" + file;
Mat rgbimg;
Mat depimg;
Mat mask;
// read data
readImg (file, "_rgb.png", rgbimg);
readImg (file, "_depth.png", depimg);
readImg (file, "_mask.png", mask);
if (rgbimg.empty () || depimg.empty () || mask.empty ())
{
printf (
"Error: The RGB-D file does not exit. Please note that the right format should be: \n");
printf (
"X_rgb.png, X_depth.png, X_mask.png with the same size. ('X' is your input) \n");
return (0);
}
if (rgbimg.size () != depimg.size () || rgbimg.size () != mask.size ())
{
printf (
"Error: The RGB-D files are not consistent. The rgb, depth and mask images should have the same size. \n");
return (0);
}
cout << "loading: " << file << endl;
// check the image size and the offest coordinate
int x1, y1;
if (rgbimg.rows == 480 && rgbimg.cols == 640)
{
// if the image is full size (480*640), no offset coordinate
x1 = 0;
y1 = 0;
}
else
{
// if not, read the offset file
ifstream loc;
string location1 = file + "_loc.txt";
loc.open (location1.c_str ());
if (!loc.is_open ())
{
if (TOP_LEFT1!=0 && TOP_LEFT2!=0)
{
x1=TOP_LEFT2;
y1=TOP_LEFT1;
}
else
{
cout << "Error! There is no associated off-set location file." << endl;
return 0;
}
}
else
{
char comma;
loc >> x1 >> comma >> y1;
}
loc.close();
}
// perform erosion to remove the noises around the boundary
erosion (mask, 4);
vector<KeyPoint> key2d;
PointCloud pointcloud;
PointCloud keypoints;
// read data and perform surf feature matching
read (rgbimg, depimg, mask, pointcloud, keypoints, key2d, x1, y1, HESSIAN);
filter (pointcloud, 0.001, pointcloud);
data.push_back (pointcloud);
key3data.push_back (keypoints);
key2data.push_back (key2d);
// find the pairwise correspondecnes based on surf feature
if (i >= 1)
{
pcl::CorrespondencesPtr correspondences (new pcl::Correspondences ()); //may have bugs??
vector<DMatch> good_matches;
match (rgbimg, previousrgb, key2d, previouskey2d, good_matches,
correspondences);
correspondencesdata.push_back (correspondences);
correspondences.reset (new pcl::Correspondences);
}
previousrgb = rgbimg;
previouskey2d = key2d;
i++;
}
index.close();
printf ("Loaded %d datasets.\n", (int) data.size ());
PointCloudPtr final (new PointCloud);
vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > matrix_buffer;
// registration: initial+fine+global
//initialize the PCL viewer
p = new pcl::visualization::PCLVisualizer (argc, argv, "3D Moeling example");
p->setSize (480, 640);
p->setPosition (480, 200);
// registration
int first = 0;
int last = 0;
if (!LOOP_CLOSURE)
{
last=(int) data.size ()-1;
}
// need to change back
double threshold = MIN_DISTANCE;
for (int z = 0; z < GITER; z++)
{
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > out;
PointCloudPtr output (new PointCloud);
PointCloudPtr final_temp (new PointCloud);
if (GLOBAL_FLAG)
{
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > out_global;
if (z == 0)
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 1, threshold);
else
{
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 0, threshold);
}
// global optimization
cout << endl << "Global optimization begins ... at iteration " <<z+1<< endl;
globalAlign (out, final_temp, matrix_buffer, out_global, first, last);
data = out_global;
}
else
{
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 1, threshold);
final = output;
break;
}
threshold = threshold * 0.9;
final = final_temp;
