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>