fourier_dist.cpp
Go to the documentation of this file.
00001 #include <pm_fourier/fourier_dist.h>
00002 
00003 FourierDistance::FourierDistance(ros::NodeHandle &node) :
00004   node(node)
00005 {
00006 }
00007 
00008 bool FourierDistance::dissimilarity(place_matcher_msgs::PolygonDissimilarity::Request& req,
00009     place_matcher_msgs::PolygonDissimilarity::Response& res)
00010 {
00011   ros::Time start = ros::Time::now();
00012 
00013   vector<Point2> pts1;
00014   for(size_t i = 0; i < req.polygon1.points.size(); i++)
00015   {
00016     pts1.push_back(Point2(req.polygon1.points[i].x, req.polygon1.points[i].y));
00017   }
00018   vector<Point2> pts2;
00019   for(size_t i = 0; i < req.polygon2.points.size(); i++)
00020   {
00021     pts2.push_back(Point2(req.polygon2.points[i].x, req.polygon2.points[i].y));
00022   }
00023 
00024   const int numOfSamples = 361;
00025   double delta1;
00026   double delta2;
00027   vector<Point2> rpol1(resamplePolygon(pts1, numOfSamples, delta1));
00028   vector<Point2> rpol2(resamplePolygon(pts2, numOfSamples, delta2));
00029 
00030   const int fftSize = 30; // number of harmonics
00031   res.raw_dissimilarity = getDissimilarityFourier(rpol1, rpol2, fftSize);
00032   res.processing_time = ros::Time::now() - start;
00033   ROS_DEBUG("sending back response in %f s", res.processing_time.toSec());
00034 
00035   return true;
00036 }


place_matcher_fourier
Author(s): Gaël Ecorchard , Karel Košnar , Vojtěch Vonásek
autogenerated on Sat Jun 8 2019 19:53:01