Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef NDT_HISTOGRAM_HH
00036 #define NDT_HISTOGRAM_HH
00037
00038 #include <ndt_map.h>
00039 #include <vector>
00040
00041 namespace lslgeneric
00042 {
00043
00044 template <typename PointT>
00045 class NDTHistogram
00046 {
00047
00048 private:
00049 std::vector<int> histogramBinsFlat;
00050 std::vector<int> histogramBinsLine;
00051 std::vector<int> histogramBinsSphere;
00052
00053 int N_LINE_BINS;
00054 int N_FLAT_BINS;
00055 int N_SPHERE_BINS;
00056 double D1, D2;
00057 bool inited;
00058
00059 std::vector< Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>,
00060 Eigen::aligned_allocator<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> > > topThree;
00061 double topThreeS[3];
00062
00063 std::vector<int> dist_histogramBinsFlat[3];
00064 std::vector<int> dist_histogramBinsLine[3];
00065 std::vector<int> dist_histogramBinsSphere[3];
00066
00067 std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > averageDirections;
00068 void constructHistogram(NDTMap<PointT> &map);
00069 void incrementLineBin(double d);
00070 void incrementFlatBin(Eigen::Vector3d &normal, double d);
00071 void incrementSphereBin(double d);
00072
00073 void computeDirections();
00074 void closedFormSolution(pcl::PointCloud<PointT> &src, pcl::PointCloud<PointT> &trgt,
00075 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T);
00076 public:
00077 NDTHistogram();
00078 NDTHistogram (NDTMap<PointT> &map);
00079 NDTHistogram (const NDTHistogram<PointT>& other);
00080
00081
00082 void bestFitToHistogram(NDTHistogram<PointT> &target, Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T, bool bound_transform = true);
00083 void printHistogram(bool bMatlab=false);
00084
00085
00086 double getTransform(size_t FIT_NUMBER, Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T)
00087 {
00088 double ret = -1;
00089 T.setIdentity();
00090 if(FIT_NUMBER >=0 && FIT_NUMBER<3)
00091 {
00092 T = topThree[FIT_NUMBER];
00093 ret = topThreeS[FIT_NUMBER];
00094 }
00095 return ret;
00096 }
00097
00098 pcl::PointCloud<PointT> getDominantDirections(int nDirections);
00099 double getSimilarity(NDTHistogram<PointT> &other);
00100 double getSimilarity(NDTHistogram<PointT> &other, Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T);
00101
00102 std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > directions;
00103 public:
00104 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00105
00106 };
00107
00108 };
00109
00110 #include <impl/ndt_histogram.hpp>
00111 #endif