ndt_map_builder.h
Go to the documentation of this file.
00001 #include <ndt_registration/ndt_matcher_p2d.h>
00002 #include <ndt_registration/ndt_matcher_d2d.h>
00003 #include <ndt_map/ndt_histogram.h>
00004 
00005 namespace lslgeneric
00006 {
00007 
00008 template<typename PointT>
00009 class MapVertex
00010 {
00011 public:
00012     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> pose;
00013     pcl::PointCloud<PointT> scan;
00014     int id;
00015     NDTHistogram<PointT> hist;
00016     double timeRegistration;
00017 public:
00018     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00019 };
00020 
00021 class MapEdge
00022 {
00023 
00024 public:
00025     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> relative_pose;
00026     Eigen::Matrix<double,6,6> covariance;
00027     int idFirst, idSecond;
00028 public:
00029     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00030 
00031 };
00032 
00033 
00034 template<typename PointT>
00035 class NDTMapBuilder
00036 {
00037 
00038 public:
00039     NDTMapBuilder(bool _doHistogram = false)
00040     {
00041         isP2F = false;
00042         isF2F=false;
00043         doHistogram = _doHistogram;
00044     }
00045     bool setICP()
00046     {
00047         isP2F = false;
00048         isF2F=false;
00049         return true;
00050     }
00051     bool setMatcherP2F(NDTMatcherP2D<PointT,PointT> *_matcherP2F   )
00052     {
00053         matcherP2F  = _matcherP2F;
00054         isP2F = true;
00055         isF2F=false;
00056         return true;
00057     }
00058 
00059     bool setMatcherF2F(NDTMatcherD2D<PointT,PointT> *_matcherF2F)
00060     {
00061         matcherF2F  = _matcherF2F;
00062         isP2F = false;
00063         isF2F=true;
00064         return true;
00065     }
00066 
00067     bool addScan(pcl::PointCloud<PointT> scan, int id=-1);
00068     void saveG2OlogFile(const char* fname);
00069     void saveDatlogFile(const char* fname);
00070     void printNodePositions();
00071     void theMotherOfAllPointClouds(const char* fname);
00072 
00073     lslgeneric::OctTree<PointT> tr;
00074 
00075 private:
00076     NDTMatcherP2D<PointT,PointT> *matcherP2F;
00077     NDTMatcherD2D<PointT,PointT> *matcherF2F;
00078     bool isP2F, isF2F, doHistogram;
00079     std::vector<MapVertex<PointT>, Eigen::aligned_allocator<MapVertex<PointT> > > vertices;
00080     std::vector<MapEdge, Eigen::aligned_allocator<MapEdge> > edges;
00081 
00082     bool matchICP(pcl::PointCloud<PointT> &target,  pcl::PointCloud<PointT> &source,
00083                   Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout, double &finalscore);
00084 };
00085 };
00086 
00087 #include <ndt_map_builder/impl/ndt_map_builder.hpp>


ndt_map_builder
Author(s): Todor Stoyanov
autogenerated on Mon Oct 6 2014 03:20:19