GBL::CmRetCode_t AverageContourDetector::detect(const GBL::Image_t& inputImage, GBL::KeyPointCollection_t& detectedKeypoints) const {
LOG_ENTER("Image = %p, keypoints = %p", &inputImage, &detectedKeypoints);
GBL::CmRetCode_t result = GBL::RESULT_FAILURE;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours(inputImage, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
LOG_INFO("Found %d contours", (uint32_t) contours.size());
for(size_t i = 0; i < contours.size(); i++) {
// Check for the size of the area
if(contours[i].size() > _boundaryLength) {
uint32_t x = 0;
uint32_t y = 0;
for(size_t j = 0; j < contours[i].size(); j++) {
x += (contours[i])[j].x;
y += (contours[i])[j].y;
}
float_t x_center = (float_t) x/ (float_t) contours[i].size();
float_t y_center = (float_t) y/ (float_t) contours[i].size();
// TODO: determine scale and angle for more robust keypoints descriptions
GBL::KeyPoint_t keypoint(x_center, y_center, 1);
detectedKeypoints.push_back(keypoint);
}
}
result = GBL::RESULT_SUCCESS;
LOG_EXIT("result = %d", result);
return result;
}
void MetaData_YML_Backed::keypoint(string name, Point3d value, bool vis)
{
keypoint(name,Point3d(value.x,value.y,qnan),vis);
}
int main(int argc, char** argv)
{
boost::program_options::variables_map vm;
if (!parseCommandLine(argc, argv, vm))
return 0;
const float radius_nms = vm["radiusNMS"].as<float>();
const float radius_features = vm["radiusFeatures"].as<float>();
const float threshold = vm["threshold"].as<float>();
const std::string path_rf = vm["pathRF"].as<std::string>();
const std::string path_cloud = vm["pathCloud"].as<std::string>();
//create detector
pcl::keypoints::KeypointLearningDetector<PointInT, KeypointT>::Ptr detector(new pcl::keypoints::KeypointLearningDetector<PointInT, KeypointT>());
detector->setNAnnulus(ANNULI);
detector->setNBins(BINS);
detector->setNonMaxima(true);
detector->setNonMaxRadius(radius_nms);
detector->setNonMaximaDrawsRemove(false);
detector->setPredictionThreshold(threshold);
detector->setRadiusSearch(radius_features); //40mm change according to the unit of measure used in your point cloud
if (detector->loadForest(path_rf))
{
std::cout << "Detector created." << std::endl;
}
else
{
return -1;
}
//load and subsample point cloud
pcl::PointCloud<PointInT>::Ptr cloud(new pcl::PointCloud<PointInT>());
pcl::io::loadPCDFile(path_cloud, *cloud);
pcl::UniformSampling<PointInT>::Ptr source_uniform_sampling(new pcl::UniformSampling<PointInT>());
bool sub_sampling = vm.count("subSampling") > 0;
float leaf = 0.0;
if(sub_sampling)
{
leaf = vm["leaf"].as<float>();
source_uniform_sampling->setRadiusSearch(leaf);
source_uniform_sampling->setInputCloud(cloud);
source_uniform_sampling->filter(*cloud);
}
std::cout << "Point cloud loaded" << std::endl;
//Compute normals
pcl::NormalEstimation<PointInT, PointNormalT> ne;
pcl::PointCloud<PointNormalT>::Ptr normals(new pcl::PointCloud<PointNormalT>);
ne.setInputCloud(cloud);
pcl::search::KdTree<PointInT>::Ptr kdtree(new pcl::search::KdTree<PointInT>());
ne.setKSearch(10);
ne.setSearchMethod(kdtree);
ne.compute(*normals);
std::cout << "Normals Computed" << std::endl;
//Set true with laser scanner dataset
bool flip_normals = vm.count("flipNormals") > 0;
if (flip_normals)
{
std::cout << "Flipping "<< std::endl;
//Sensor origin is inside the model, flip normals to make normal sign coherent with scenes
std::transform(normals->points.begin(), normals->points.end(), normals->points.begin(), [](pcl::Normal p) -> pcl::Normal {auto q = p; q.normal[0] *= -1; q.normal[1] *= -1; q.normal[2] *= -1; return q; });
}
//setup the detector
detector->setInputCloud(cloud);
detector->setNormals(normals);
//detect keypoints
pcl::PointCloud<KeypointT>::Ptr keypoint(new pcl::PointCloud<KeypointT>());
detector->compute(*keypoint);
std::cout << "Keypoint computed" << std::endl;
//show results
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer.reset(new pcl::visualization::PCLVisualizer);
viewer->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<PointInT> blu(cloud, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloud, blu,"model cloud");
pcl::visualization::PointCloudColorHandlerCustom<KeypointT> red(keypoint, 255, 0, 0);
viewer->addPointCloud<KeypointT>(keypoint, red, "Keypoint");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 6, "Keypoint");
std::cout << "viewer ON" << std::endl;
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
viewer->removeAllPointClouds();
std::cout << "DONE" << std::endl;
if (vm.count("pathKP"))
{
const std::string path = vm["pathKP"].as<std::string>();
pcl::io::savePCDFileASCII<KeypointT>(path, *keypoint);
}
}
int main() {
//! [Define]
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
//! [Define]
vpImage<unsigned char> I;
vpVideoReader reader;
reader.setFileName("video-postcard.mpeg");
reader.acquire(I);
//! [Construction]
const std::string detectorName = "SIFT";
const std::string extractorName = "SIFT";
//Use L2 distance with a matching done using FLANN (Fast Library for Approximate Nearest Neighbors)
const std::string matcherName = "FlannBased";
vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType);
//! [Construction]
//! [Build Reference]
std::cout << "Reference keypoints=" << keypoint.buildReference(I) << std::endl;
//! [Build Reference]
//! [Create image]
vpImage<unsigned char> Idisp;
Idisp.resize(I.getHeight(), 2*I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
//! [Create image]
//! [Init display]
vpDisplayOpenCV d(Idisp, 0, 0, "Matching keypoints with SIFT keypoints") ;
vpDisplay::display(Idisp);
vpDisplay::flush(Idisp);
//! [Init display]
while ( ! reader.end() )
{
//! [Acquisition]
reader.acquire(I);
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
//! [Acquisition]
//! [Display]
vpDisplay::display(Idisp);
vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2);
//! [Display]
//! [Matching]
int nbMatch = keypoint.matchPoint(I);
//! [Matching]
std::cout << "Matches=" << nbMatch << std::endl;
//! [Get matches]
vpImagePoint iPref, iPcur;
for (int i = 0; i < nbMatch; i++)
{
keypoint.getMatchedPoints(i, iPref, iPcur);
//! [Get matches]
//! [Display matches]
vpDisplay::displayLine(Idisp, iPref, iPcur + vpImagePoint(0, I.getWidth()), vpColor::green);
//! [Display matches]
}
//! [Display flush]
vpDisplay::flush(Idisp);
//! [Display flush]
if (vpDisplay::getClick(Idisp, false))
break;
}
vpDisplay::getClick(Idisp);
#endif
return 0;
}
