/* * call-seq: * * * From: https://code.ros.org/trac/opencv/browser/trunk/opencv/samples/c/find_obj.cpp?rev=2065 */ VALUE rb_flann(VALUE klass, VALUE objectDesc, VALUE imageDesc) { const cv::Mat m_object(CVMAT(objectDesc)); const cv::Mat m_image(CVMAT(imageDesc)); cv::Mat m_indices(m_object.rows, 2, CV_32S); cv::Mat m_dists(m_object.rows, 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 VALUE ptpairs = rb_ary_new(); 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]) { rb_ary_push(ptpairs, rb_int_new(i)); rb_ary_push(ptpairs, rb_int_new(indices_ptr[2 * i])); } } return ptpairs; }
float knn(Mat& m_destinations, Mat& m_object, vector<int>& ptpairs, vector<float>& dists) { // find nearest neighbors using FLANN cv::Mat m_indices(m_object.rows, 1, CV_32S); cv::Mat m_dists(m_object.rows, 1, CV_32F); Mat dest_32f; m_destinations.convertTo(dest_32f,CV_32FC2); Mat obj_32f; m_object.convertTo(obj_32f,CV_32FC2); assert(dest_32f.type() == CV_32F); //cout << "h" << endl; cv::flann::Index flann_index(dest_32f, cv::flann::KDTreeIndexParams(2)); // using 2 randomized kdtrees flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, cv::flann::SearchParams(64) ); //cout << "p" << endl; 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) { ptpairs.push_back(indices_ptr[i]); } dists.resize(m_dists.rows); m_dists.copyTo(Mat(dists)); return cv::sum(m_dists)[0]; }
void FlannMatcher::getMatches(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pc1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pc2, 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,pc1,eigenPoint1); projectTo3D(keypoints2,pc2,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); */ }
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]); } } }