//get matches from depth image
void FlannMatcher::getMatches(cv::Mat& depth1,cv::Mat& depth2,
cv::Mat& rgb1,cv::Mat& rgb2,
std::vector<cv::DMatch>& matches,
vector<cv::KeyPoint>& keypoints1,
vector<cv::KeyPoint>& keypoints2)
{
//vector<cv::KeyPoint> keypoints1,keypoints2;
cv::Mat desp1,desp2;
vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > eigenPoint1,eigenPoint2;
detector->detect(rgb1,keypoints1);
detector->detect(rgb2,keypoints2);
projectTo3D(keypoints1,depth1,eigenPoint1);
projectTo3D(keypoints2,depth2,eigenPoint2);
//extract descriptors
extractor->compute(rgb1,keypoints1,desp1);
extractor->compute(rgb2,keypoints2,desp2);
cout<<"descriptors size is: "<<desp1.rows<<" "<<desp2.rows<<endl;
//flann match
cv::Mat m_indices(desp1.rows,2,CV_32S);
cv::Mat m_dists(desp1.rows,2,CV_32S);
cv::flann::Index flann_index(desp2,cv::flann::KDTreeIndexParams(4));
flann_index.knnSearch(desp1,m_indices,m_dists,2,cv::flann::SearchParams(64));
int* indices_ptr=m_indices.ptr<int>(0);
float* dists_ptr=m_dists.ptr<float>(0);
cv::DMatch match;
//vector<cv::DMatch> matches;
for (int i=0;i<m_indices.rows;++i) {
if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
match.queryIdx=i;
match.trainIdx=indices_ptr[ 2*i ];
match.distance=dists_ptr[ 2*i ];
matches.push_back(match);
}
}
cout<<"matches size is: "<<matches.size()<<endl;
cout<<"keypoints1 size is: "<<keypoints1.size()<<endl;
cout<<"keypoints2 size is: "<<keypoints2.size()<<endl;
/*
//draw matches
cv::Mat img_matches;
cv::drawMatches(rgb1,keypoints1,rgb2,keypoints2,
matches,img_matches);
cv::imshow("test matches",img_matches);
*/
}
int Index::radiusSearch(const Mat& query, Mat& indices, Mat& dists, float radius, const SearchParams& searchParams)
{
CV_Assert(query.type() == CV_32F);
CV_Assert(query.isContinuous());
::cvflann::Matrix<float> m_query(query.rows, query.cols, (float*)query.ptr<float>(0));
CV_Assert(indices.type() == CV_32S);
CV_Assert(indices.isContinuous());
::cvflann::Matrix<int> m_indices(indices.rows, indices.cols, (int*)indices.ptr<int>(0));
CV_Assert(dists.type() == CV_32F);
CV_Assert(dists.isContinuous());
::cvflann::Matrix<float> m_dists(dists.rows, dists.cols, (float*)dists.ptr<float>(0));
return nnIndex->radiusSearch(m_query,m_indices,m_dists,radius,::cvflann::SearchParams(searchParams.checks));
}
void
flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,
const CvSeq*, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
int length = (int)(objectDescriptors->elem_size/sizeof(float));
cv::Mat m_object(objectDescriptors->total, length, CV_32F);
cv::Mat m_image(imageDescriptors->total, length, CV_32F);
// copy descriptors
CvSeqReader obj_reader;
float* obj_ptr = m_object.ptr<float>(0);
cvStartReadSeq( objectDescriptors, &obj_reader );
for(int i = 0; i < objectDescriptors->total; i++ )
{
const float* descriptor = (const float*)obj_reader.ptr;
CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
memcpy(obj_ptr, descriptor, length*sizeof(float));
obj_ptr += length;
}
CvSeqReader img_reader;
float* img_ptr = m_image.ptr<float>(0);
cvStartReadSeq( imageDescriptors, &img_reader );
for(int i = 0; i < imageDescriptors->total; i++ )
{
const float* descriptor = (const float*)img_reader.ptr;
CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
memcpy(img_ptr, descriptor, length*sizeof(float));
img_ptr += length;
}
// find nearest neighbors using FLANN
cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4)); // using 4 randomized kdtrees
flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
int* indices_ptr = m_indices.ptr<int>(0);
float* dists_ptr = m_dists.ptr<float>(0);
for (int i=0;i<m_indices.rows;++i) {
if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
ptpairs.push_back(i);
ptpairs.push_back(indices_ptr[2*i]);
}
}
}
void Index::knnSearch(const Mat& queries, Mat& indices, Mat& dists, int knn, const SearchParams& searchParams)
{
CV_Assert(queries.type() == CV_32F);
CV_Assert(queries.isContinuous());
::cvflann::Matrix<float> m_queries(queries.rows, queries.cols, (float*)queries.ptr<float>(0));
CV_Assert(indices.type() == CV_32S);
CV_Assert(indices.isContinuous());
::cvflann::Matrix<int> m_indices(indices.rows, indices.cols, (int*)indices.ptr<int>(0));
CV_Assert(dists.type() == CV_32F);
CV_Assert(dists.isContinuous());
::cvflann::Matrix<float> m_dists(dists.rows, dists.cols, (float*)dists.ptr<float>(0));
nnIndex->knnSearch(m_queries,m_indices,m_dists,knn,::cvflann::SearchParams(searchParams.checks));
}
void FindOneWayDescriptor(cv::flann::Index* m_pca_descriptors_tree, CvSize patch_size, int m_pca_dim_low, int m_pose_count, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
CvMat* avg, CvMat* eigenvectors)
{
desc_idx = -1;
pose_idx = -1;
distance = 1e10;
//--------
//PCA_coeffs precalculating
CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
int patch_width = patch_size.width;
int patch_height = patch_size.height;
//if (avg)
//{
CvRect _roi = cvGetImageROI((IplImage*)patch);
IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
if(_roi.width != patch_width|| _roi.height != patch_height)
{
cvResize(patch, test_img);
_roi = cvGetImageROI(test_img);
}
else
{
cvCopy(patch,test_img);
}
IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
float sum = cvSum(test_img).val[0];
cvConvertScale(test_img, patch_32f, 1.0f/sum);
//ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
//Projecting PCA
CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
cvProjectPCA(patch_mat, avg, eigenvectors, temp);
CvMat temp1;
cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
cvCopy(&temp1, pca_coeffs);
cvReleaseMat(&temp);
cvReleaseMat(&patch_mat);
//End of projecting
cvReleaseImage(&patch_32f);
cvReleaseImage(&test_img);
// }
//--------
//float* target = new float[m_pca_dim_low];
//::flann::KNNResultSet res(1,pca_coeffs->data.fl,m_pca_dim_low);
//::flann::SearchParams params;
//params.checks = -1;
//int maxDepth = 1000000;
//int neighbors_count = 1;
//int* neighborsIdx = new int[neighbors_count];
//float* distances = new float[neighbors_count];
//if (m_pca_descriptors_tree->findNearest(pca_coeffs->data.fl,neighbors_count,maxDepth,neighborsIdx,0,distances) > 0)
//{
// desc_idx = neighborsIdx[0] / m_pose_count;
// pose_idx = neighborsIdx[0] % m_pose_count;
// distance = distances[0];
//}
//delete[] neighborsIdx;
//delete[] distances;
cv::Mat m_object(1, m_pca_dim_low, CV_32F);
cv::Mat m_indices(1, 1, CV_32S);
cv::Mat m_dists(1, 1, CV_32F);
float* object_ptr = m_object.ptr<float>(0);
for (int i=0;i<m_pca_dim_low;i++)
{
object_ptr[i] = pca_coeffs->data.fl[i];
}
m_pca_descriptors_tree->knnSearch(m_object, m_indices, m_dists, 1, cv::flann::SearchParams(-1) );
desc_idx = ((int*)(m_indices.ptr<int>(0)))[0] / m_pose_count;
pose_idx = ((int*)(m_indices.ptr<int>(0)))[0] % m_pose_count;
distance = ((float*)(m_dists.ptr<float>(0)))[0];
// delete[] target;
// for(int i = 0; i < desc_count; i++)
// {
// int _pose_idx = -1;
// float _distance = 0;
//
//#if 0
// descriptors[i].EstimatePose(patch, _pose_idx, _distance);
//#else
// if (!avg)
// {
// descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
// }
// else
// {
// descriptors[i].EstimatePosePCA(pca_coeffs, _pose_idx, _distance, avg, eigenvectors);
// }
//#endif
//
// if(_distance < distance)
// {
// desc_idx = i;
// pose_idx = _pose_idx;
// distance = _distance;
// }
// }
cvReleaseMat(&pca_coeffs);
}
