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