00001 #pragma once 00002 #ifndef PLACE_MATCHER_HIST_DISSIMILARITY_GETTER_H 00003 #define PLACE_MATCHER_HIST_DISSIMILARITY_GETTER_H 00004 00005 #include <cmath> // std::sqrt, atan2 00006 #include <vector> 00007 #include <limits> 00008 00009 #include <lama_common/polygon_utils.h> // for normalizePolygon 00010 #include <place_matcher_msgs/PolygonDissimilarity.h> 00011 #include <ros/ros.h> 00012 #include <tf/tf.h> // for createQuaternionMsgFromYaw 00013 00014 namespace place_matcher_hist 00015 { 00016 00017 class DissimilarityGetter 00018 { 00019 public : 00020 00021 DissimilarityGetter(const ros::NodeHandle& nh_private); 00022 00023 bool getDissimilarity(place_matcher_msgs::PolygonDissimilarityRequest& req, place_matcher_msgs::PolygonDissimilarityResponse& res); 00024 00025 private : 00026 00027 typedef std::vector<unsigned int> Hist; 00028 00029 // ROS parameters. 00030 double trans_resolution_; 00031 int max_trans_bin_count_; 00032 00033 00034 00035 double rot_resolution_; 00036 double max_dist_; 00037 00038 00039 00040 // Hard-coded parameters. 00041 static const size_t shift_ = 4; 00042 00043 00044 // Internals. 00045 void initParams(); 00046 Hist getRotHist(const geometry_msgs::Polygon poly); 00047 size_t crossCorrelationHist(Hist h1, Hist h2); 00048 00049 inline int getTransBinCount(double min, double max) 00050 { 00051 return (int) std::ceil((max - min) / ((double) trans_resolution_)); 00052 } 00053 00054 00055 ros::NodeHandle nh_private_; 00056 }; 00057 00058 } /* namespace place_matcher_hist */ 00059 00060 #endif /* PLACE_MATCHER_HIST_DISSIMILARITY_GETTER_H */