float FeatureDetector::getNCC(ImageBitstream image, int x, int y, FeatureDescriptor feature)
{
int row, col;
int patchSize = FeatureDescriptor::patchSize_;
int width = image.getWidth();
unsigned char *I = image.getBitstream();
unsigned char *P = feature.get();
// calculate average of image in feature patch
int pavg = 0, iavg = 0;
for(row = 0; row < patchSize; row++)
{
for(col = 0; col < patchSize; col++)
{
pavg += P[row * patchSize + col];
iavg += I[(y + row - (patchSize - 1)/2) * width + (x + col - (patchSize - 1)/2)];
}
}
pavg = pavg / (patchSize * patchSize);
iavg = iavg / (patchSize * patchSize);
// calculate NCC
int pnorm, inorm;
int sumII = 0, sumPP = 0, sumIP = 0;
float ncc;
for(row = 0; row < patchSize; row++)
{
for(col = 0; col < patchSize; col++)
{
pnorm = P[row * patchSize + col] - pavg;
inorm = I[(y + row - (patchSize - 1)/2) * width + (x + col - (patchSize - 1)/2)] - iavg;
sumIP += pnorm * inorm;
sumPP += pnorm * pnorm;
sumII += inorm * inorm;
}
}
ncc = sumIP / (sqrt(sumPP) * sqrt(sumII));
return ncc;
}
Transformation * BowAICK::getTransformation(RGBDFrame * src, RGBDFrame * dst)
{
struct timeval start, end;
gettimeofday(&start, NULL);
Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
Transformation * transformation = new Transformation();
transformation->transformationMatrix = transformationMat;
transformation->src = src;
transformation->dst = dst;
transformation->weight = 100;
if(debugg_BowAICK){printf("BowAICK::getTransformation(%i,%i)\n",src->id,dst->id);}
IplImage* img_combine;
int width;
int height;
if(debugg_BowAICK){
IplImage* rgb_img_src = cvLoadImage(src->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_src = (char *)rgb_img_src->imageData;
IplImage* rgb_img_dst = cvLoadImage(dst->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_dst = (char *)rgb_img_dst->imageData;
width = rgb_img_src->width;
height = rgb_img_src->height;
img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_8U, 3);
char * data = (char *)img_combine->imageData;
for (int j = 0; j < height; j++){
for (int i = 0; i < width; i++){
int ind = 3*(640*j+i);
data[3 * (j * (2*width) + (width+i)) + 0] = data_dst[ind +0];
data[3 * (j * (2*width) + (width+i)) + 1] = data_dst[ind +1];
data[3 * (j * (2*width) + (width+i)) + 2] = data_dst[ind +2];
data[3 * (j * (2*width) + (i)) + 0] = data_src[ind +2];
data[3 * (j * (2*width) + (i)) + 1] = data_src[ind +2];
data[3 * (j * (2*width) + (i)) + 2] = data_src[ind +2];
}
}
cvReleaseImage( &rgb_img_src );
cvReleaseImage( &rgb_img_dst );
}
int nr_loop_src = src->keypoints->valid_key_points.size();
if(nr_loop_src > max_points){nr_loop_src = max_points;}
int nr_loop_dst = dst->keypoints->valid_key_points.size();
if(nr_loop_dst > max_points){nr_loop_dst = max_points;}
vector<KeyPoint * > src_keypoints;
for(int i = 0; i < nr_loop_src; i++){src_keypoints.push_back(src->keypoints->valid_key_points.at(i));}
vector<KeyPoint * > dst_keypoints;
for(int i = 0; i < nr_loop_dst; i++){dst_keypoints.push_back(dst->keypoints->valid_key_points.at(i));}
int src_nr_points = src_keypoints.size();
int dst_nr_points = dst_keypoints.size();
int nr_bow = src->input->calibration->words.size();
vector<int> * bow = new vector<int>[nr_bow];
for(int i = 0; i < nr_bow; i++){bow[i] = vector<int>();}
for(int i = 0; i < dst_keypoints.size(); i++){
KeyPoint * kp = dst_keypoints.at(i);
if(kp->cluster_distance_pairs.size()>0){
int id = kp->cluster_distance_pairs.at(0).first;
bow[id].push_back(i);
}
}
vector< vector< int > > possible_matches;
float ** feature_distances = new float*[src_nr_points];
for(int i = 0; i < src_nr_points; i++){
possible_matches.push_back(vector< int >());
KeyPoint * src_kp = src_keypoints.at(i);
for(int j = 0; j < src_kp->cluster_distance_pairs.size(); j++){
float d = src_kp->cluster_distance_pairs.at(j).second;
int id = src_kp->cluster_distance_pairs.at(j).first;
if(d < bow_threshold){
vector<int> vec = bow[id];
for(int k = 0; k < vec.size(); k++){possible_matches.at(i).push_back(vec.at(k));}
}else{break;}
}
FeatureDescriptor * descriptorA = src_kp->descriptor;
int dst_nr_matches = possible_matches.at(i).size();
feature_distances[i] = new float[dst_nr_matches];
for(int j = 0; j < dst_nr_matches;j++)
{
FeatureDescriptor * descriptorB = dst_keypoints.at(possible_matches.at(i).at(j))->descriptor;
feature_distances[i][j] = feature_scale*(descriptorA->distance(descriptorB));
}
}
/*
for(int i = 0; i < src_nr_points;i++){
FeatureDescriptor * descriptorA = src_keypoints.at(i)->descriptor;
int dst_nr_matches = possible_matches.at(i).size();
feature_distances[i] = new float[dst_nr_matches];
for(int j = 0; j < dst_nr_matches;j++)
{
FeatureDescriptor * descriptorB = dst_keypoints.at(possible_matches.at(i).at(j))->descriptor;
feature_distances[i][j] = feature_scale*(descriptorA->distance(descriptorB));
}
}
*/
float * pos_src_x = new float[src_nr_points];
float * pos_src_y = new float[src_nr_points];
float * pos_src_z = new float[src_nr_points];
for(int i = 0; i < src_nr_points; i++)
{
pos_src_x[i] = src_keypoints.at(i)->point->x;
pos_src_y[i] = src_keypoints.at(i)->point->y;
pos_src_z[i] = src_keypoints.at(i)->point->z;
}
float * pos_dst_x = new float[dst_nr_points];
float * pos_dst_y = new float[dst_nr_points];
float * pos_dst_z = new float[dst_nr_points];
for(int i = 0; i < dst_nr_points; i++)
{
pos_dst_x[i] = dst_keypoints.at(i)->point->x;
pos_dst_y[i] = dst_keypoints.at(i)->point->y;
pos_dst_z[i] = dst_keypoints.at(i)->point->z;
}
for(int iter = 0; iter < nr_iter; iter++)
{
float alpha = getAlpha(iter);
//printf("alpha= %f\n",alpha);
pcl::TransformationFromCorrespondences tfc;
float threshold = distance_threshold*alpha + (1 - alpha)*feature_threshold;
transformation->weight = 0;
int nr_matches = 0;
float mat00 = transformationMat(0,0);
float mat01 = transformationMat(0,1);
float mat02 = transformationMat(0,2);
float mat03 = transformationMat(0,3);
float mat10 = transformationMat(1,0);
float mat11 = transformationMat(1,1);
float mat12 = transformationMat(1,2);
float mat13 = transformationMat(1,3);
float mat20 = transformationMat(2,0);
float mat21 = transformationMat(2,1);
float mat22 = transformationMat(2,2);
float mat23 = transformationMat(2,3);
IplImage * img_combine_clone;
if(debugg_BowAICK){
img_combine_clone = cvCreateImage(cvSize(img_combine->width, img_combine->height), IPL_DEPTH_8U, 3);
cvCopy( img_combine, img_combine_clone, NULL );
}
for(int i = 0; i < src_nr_points;i++)
{
float x_tmp = pos_src_x[i];
float y_tmp = pos_src_y[i];
float z_tmp = pos_src_z[i];
float x = x_tmp*mat00+y_tmp*mat01+z_tmp*mat02+mat03;
float y = x_tmp*mat10+y_tmp*mat11+z_tmp*mat12+mat13;
float z = x_tmp*mat20+y_tmp*mat21+z_tmp*mat22+mat23;
float dx,dy,dz;
int dst_nr_matches = possible_matches.at(i).size();
float best_d = 100000000;
int best_j = -1;
for(int jj = 0; jj < dst_nr_matches;jj++)
{
int j = possible_matches.at(i).at(jj);
dx = x-pos_dst_x[j];
dy = y-pos_dst_y[j];
dz = z-pos_dst_z[j];
float d = (1-alpha)*feature_distances[i][jj] + alpha*sqrt(dx*dx + dy*dy + dz*dz);
if(d < best_d){
best_d = d;
best_j = j;
}
}
if(best_d < threshold && best_j != -1){
KeyPoint * src_kp = src_keypoints.at(i);
KeyPoint * dst_kp = dst_keypoints.at(best_j);
if(debugg_BowAICK){
cvCircle(img_combine_clone,cvPoint(dst_kp->point->w + width , dst_kp->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
cvCircle(img_combine_clone,cvPoint(src_kp->point->w , src_kp->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
cvLine(img_combine_clone,cvPoint(dst_kp->point->w + width ,dst_kp->point->h),cvPoint(src_kp->point->w,src_kp->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
}
tfc.add(src_kp->point->pos, dst_kp->point->pos);
if(iter == nr_iter-1){
transformation->weight++;
transformation->matches.push_back(make_pair (src_kp, dst_kp));
}
}
}
transformationMat = tfc.getTransformation().matrix();
transformation->transformationMatrix = transformationMat;
if(debugg_BowAICK ){
cvShowImage("combined image", img_combine_clone);
cvWaitKey(0);
cvReleaseImage( &img_combine_clone);
}
}
//cout<< "transformation->transformationMatrix\n" <<transformation->transformationMatrix<<endl;
if(debugg_BowAICK){printf("done\n");cvReleaseImage( &img_combine );}
for(int i = 0; i < src_nr_points;i++){delete[] feature_distances[i];}
delete[] feature_distances;
delete[] pos_src_x;
delete[] pos_src_y;
delete[] pos_src_z;
delete[] pos_dst_x;
delete[] pos_dst_y;
delete[] pos_dst_z;
delete[] bow;
gettimeofday(&end, NULL);
float time = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;
//printf("BowAICK cost: %f, weight: %f\n",time,transformation->weight);
//transformation->print();
//if(transformation->weight > 5){printf("BowAICK cost: %f, weight: %f\n",time,transformation->weight);}
return transformation;
}
Transformation * RansacPCLCopyMatcher::getTransformation(RGBDFrame * src, RGBDFrame * dst)
{
if(debugg_RansacPCLCopyMatcher){printf("RansacPCLCopyMatcher::getTransformation(%i,%i)\n",src->id,dst->id);}
Transformation * transformation = new Transformation();
transformation->src = src;
transformation->dst = dst;
transformation->weight = 0;
transformation->transformationMatrix = Eigen::Matrix4f::Identity();
vector<KeyPoint * > src_keypoints = src->keypoints->valid_key_points;
vector<KeyPoint * > dst_keypoints = dst->keypoints->valid_key_points;
int src_nr_points = src_keypoints.size();
int dst_nr_points = dst_keypoints.size();
vector<int> ** possibleMatches = new vector<int>*[src_nr_points];
for(int i = 0; i < src_nr_points;i++)
{
possibleMatches[i] = new vector<int>();
for(int j = 0; j < dst_nr_points;j++)
{
FeatureDescriptor * descriptorA = src_keypoints.at(i)->descriptor;
FeatureDescriptor * descriptorB = dst_keypoints.at(j)->descriptor;
if(descriptorA->distance(descriptorB) < feature_threshold){possibleMatches[i]->push_back(j);}
}
//printf("%i:%i\n",i,possibleMatches[i]->size());
}
float * pos_src_x = new float[src_nr_points];
float * pos_src_y = new float[src_nr_points];
float * pos_src_z = new float[src_nr_points];
float * pos_dst_x = new float[dst_nr_points];
float * pos_dst_y = new float[dst_nr_points];
float * pos_dst_z = new float[dst_nr_points];
Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
for(int i = 0; i < src_nr_points;i++)
{
pos_src_x[i] = src_keypoints[i]->point->x;
pos_src_y[i] = src_keypoints[i]->point->y;
pos_src_z[i] = src_keypoints[i]->point->z;
}
for(int i = 0; i < dst_nr_points;i++)
{
pos_dst_x[i] = dst_keypoints[i]->point->x;
pos_dst_y[i] = dst_keypoints[i]->point->y;
pos_dst_z[i] = dst_keypoints[i]->point->z;
}
int nr_best = 0;
pcl::TransformationFromCorrespondences tfc;
for(int i = 0; i < nr_iter; i++)
{
int src_a,src_b,src_c,dst_a,dst_b,dst_c;
int counter = 0;
src_a = rand() % src_nr_points;
src_b = rand() % src_nr_points;
src_c = rand() % src_nr_points;
while(possibleMatches[src_a]->size() == 0 && counter < 500) {counter++;src_a = rand() % src_nr_points;}
while(possibleMatches[src_b]->size() == 0 || src_a == src_b && counter < 500) {counter++;src_b = rand() % src_nr_points;}
while(possibleMatches[src_c]->size() == 0 || src_c == src_a || src_c == src_b && counter < 500) {counter++;src_c = rand() % src_nr_points;}
if(counter >= 500){continue;}
dst_a = possibleMatches[src_a]->at(rand()%possibleMatches[src_a]->size());
dst_b = possibleMatches[src_b]->at(rand()%possibleMatches[src_b]->size());
dst_c = possibleMatches[src_c]->at(rand()%possibleMatches[src_c]->size());
tfc.reset();
tfc.add(src_keypoints.at(src_a)->point->pos,dst_keypoints.at(dst_a)->point->pos);
tfc.add(src_keypoints.at(src_b)->point->pos,dst_keypoints.at(dst_b)->point->pos);
tfc.add(src_keypoints.at(src_c)->point->pos,dst_keypoints.at(dst_c)->point->pos);
transformationMat = tfc.getTransformation().matrix();
float mat00 = transformationMat(0,0);
float mat01 = transformationMat(0,1);
float mat02 = transformationMat(0,2);
float mat03 = transformationMat(0,3);
float mat10 = transformationMat(1,0);
float mat11 = transformationMat(1,1);
float mat12 = transformationMat(1,2);
float mat13 = transformationMat(1,3);
float mat20 = transformationMat(2,0);
float mat21 = transformationMat(2,1);
float mat22 = transformationMat(2,2);
float mat23 = transformationMat(2,3);
std::vector<int> src_good;
std::vector<int> dst_good;
for(int src_j = 0; src_j < src_nr_points; src_j++)
{
float x = pos_src_x[src_j];
float y = pos_src_y[src_j];
float z = pos_src_z[src_j];
float tmp_x = x*mat00+y*mat01+z*mat02+mat03;
float tmp_y = x*mat10+y*mat11+z*mat12+mat13;
float tmp_z = x*mat20+y*mat21+z*mat22+mat23;
float min = 100;
int closest = -1;
for(int k = 0; k < possibleMatches[src_j]->size(); k++){
int dst_j = possibleMatches[src_j]->at(k);
float dx = pos_dst_x[dst_j] - tmp_x;
float dy = pos_dst_y[dst_j] - tmp_y;
float dz = pos_dst_z[dst_j] - tmp_z;
float dist = sqrt(dx*dx + dy*dy + dz*dz);
if(dist < min){
min = dist;
closest = dst_j;
}
}
if(min < distance_threshold){
//printf("%i : %i -> %f\n",src_j,closest,min);
src_good.push_back(src_j);
dst_good.push_back(closest);
}
}
if(src_good.size() > nr_best){
nr_best = src_good.size();
tfc.reset();
for(int j = 0; j < src_good.size(); j++){
tfc.add(src_keypoints.at(src_good.at(j))->point->pos,dst_keypoints.at(dst_good.at(j))->point->pos);
}
transformation->transformationMatrix = tfc.getTransformation().matrix();
}
}
for(int i = 0; i < src_nr_points;i++)
{
delete possibleMatches[i];
}
delete[] possibleMatches;
delete[] pos_src_x;
delete[] pos_src_y;
delete[] pos_src_z;
delete[] pos_dst_x;
delete[] pos_dst_y;
delete[] pos_dst_z;
return transformation;
}
