//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); }