void reduce_measurement_g2o::compute_features(const cv::Mat & rgb,
const cv::Mat & depth, const Eigen::Vector3f & intrinsics,
cv::Ptr<cv::FeatureDetector> & fd,
cv::Ptr<cv::DescriptorExtractor> & de,
std::vector<cv::KeyPoint> & filtered_keypoints,
pcl::PointCloud<pcl::PointXYZ> & keypoints3d, cv::Mat & descriptors) {
cv::Mat gray;
if (rgb.channels() != 1) {
cv::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY);
} else {
gray = rgb;
}
cv::GaussianBlur(gray, gray, cv::Size(3, 3), 3);
int threshold = 400;
fd->setInt("hessianThreshold", threshold);
//int threshold = 100;
//fd->setInt("thres", threshold);
std::vector<cv::KeyPoint> keypoints;
cv::Mat mask(depth.size(), CV_8UC1);
depth.convertTo(mask, CV_8U);
fd->detect(gray, keypoints, mask);
for (int i = 0; i < 5; i++) {
if (keypoints.size() < 300) {
threshold = threshold / 2;
fd->setInt("hessianThreshold", threshold);
//fd->setInt("thres", threshold);
keypoints.clear();
fd->detect(gray, keypoints, mask);
} else {
break;
}
}
if (keypoints.size() > 400)
keypoints.resize(400);
filtered_keypoints.clear();
keypoints3d.clear();
for (size_t i = 0; i < keypoints.size(); i++) {
if (depth.at<unsigned short>(keypoints[i].pt) != 0) {
filtered_keypoints.push_back(keypoints[i]);
pcl::PointXYZ p;
p.z = depth.at<unsigned short>(keypoints[i].pt) / 1000.0f;
p.x = (keypoints[i].pt.x - intrinsics[1]) * p.z / intrinsics[0];
p.y = (keypoints[i].pt.y - intrinsics[2]) * p.z / intrinsics[0];
//ROS_INFO("Point %f %f %f from %f %f ", p.x, p.y, p.z, keypoints[i].pt.x, keypoints[i].pt.y);
keypoints3d.push_back(p);
}
}
de->compute(gray, filtered_keypoints, descriptors);
}
