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);
*/
}
Node::Node(ros::NodeHandle* nh,
const cv::Mat& visual, const cv::Mat& depth,
image_geometry::PinholeCameraModel cam_model,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr& point_cloud,
unsigned int msg_id,
unsigned int id,
const cv::Mat& detection_mask):
nh_(nh),
msg_id_(msg_id),
id_(id),
cloudMessage_(*point_cloud),
cam_model_(cam_model),
matcher_(matcher)
{
std::clock_t starttime=std::clock();
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
ROS_INFO("Found %d Keypoints", (int)feature_locations_2d_.size());
cloud_pub = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
cloud_pub2 = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/my_clouds",20);
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: This takes 0.1 seconds and is not strictly necessary
//pcl::fromROSMsg(*point_cloud,pc);
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
projectTo3D(depth, feature_locations_2d_, feature_locations_3d_,pc_col); //takes less than 0.01 sec
std::clock_t starttime4=std::clock();
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
std::clock_t starttime2=std::clock();
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
flannIndex = NULL;
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
Node::Node(ros::NodeHandle& nh, const cv::Mat& visual,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr point_cloud,
const cv::Mat& detection_mask)
: id_(0),
flannIndex(NULL),
matcher_(matcher)
{
#ifdef USE_ICP_CODE
gicp_initialized = false;
#endif
std::clock_t starttime=std::clock();
#ifdef USE_SIFT_GPU
SiftGPUFeatureDetector* siftgpu = SiftGPUFeatureDetector::GetInstance();
float* descriptors = siftgpu->detect(visual, feature_locations_2d_);
if (descriptors == NULL) {
ROS_FATAL("Can't run SiftGPU");
}
#else
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
#endif
ROS_INFO("Feature detection and descriptor extraction runtime: %f", ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
/*
if (id_ == 0)
cloud_pub_ = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_base",10);
else{
*/
cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
// cloud_pub_ransac = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_current_ransac",10);
//} */
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: If batch sending/saving of clouds would be removed, the pointcloud wouldn't have to be saved
// which would slim down the Memory requirements
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "pc2->pcl conversion runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
#ifdef USE_SIFT_GPU
// removes also unused descriptors from the descriptors matrix
// build descriptor matrix
projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_); //takes less than 0.01 sec
if (descriptors != NULL) delete descriptors;
#else
projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col); //takes less than 0.01 sec
#endif
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
#ifdef USE_ICP_CODE
std::clock_t starttime4=std::clock();
createGICPStructures();
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "gicp runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
#endif
std::clock_t starttime2=std::clock();
#ifndef USE_SIFT_GPU
// ROS_INFO("Use extractor");
//cv::Mat topleft, topright;
//topleft = visual.colRange(0,visual.cols/2+50);
//topright= visual.colRange(visual.cols/2+50, visual.cols-1);
//std::vector<cv::KeyPoint> kp1, kp2;
//extractor->compute(topleft, kp1, feature_descriptors_); //fill feature_descriptors_ with information
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
#endif
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}