ndt_map_matcher_d2d_2d.h
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 /*
00009  * Software License Agreement (BSD License)
00010  *
00011  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00012  *  All rights reserved.
00013  *
00014  *  Redistribution and use in source and binary forms, with or without
00015  *  modification, are permitted provided that the following conditions
00016  *  are met:
00017  *
00018  *   * Redistributions of source code must retain the above copyright
00019  *     notice, this list of conditions and the following disclaimer.
00020  *   * Redistributions in binary form must reproduce the above
00021  *     copyright notice, this list of conditions and the following
00022  *     disclaimer in the documentation and/or other materials provided
00023  *     with the distribution.
00024  *   * Neither the name of AASS Research Center nor the names of its
00025  *     contributors may be used to endorse or promote products derived
00026  *     from this software without specific prior written permission.
00027  *
00028  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00029  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00030  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00031  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00032  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00033  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00034  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00036  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00037  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00038  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00039  *  POSSIBILITY OF SUCH DAMAGE.
00040  *
00041  */
00055 #ifndef NDT_MAP_MATCHER_D2D_2D_HH
00056 #define NDT_MAP_MATCHER_D2D_2D_HH
00057 
00058 #include <ndt_map/ndt_map.h>
00059 #include <ndt_matcher_d2d_2d.h>
00060 
00061 #include "pcl/point_cloud.h"
00062 #include "Eigen/Core"
00063 
00064 #include <stdlib.h>
00065 #include <stdio.h>
00066 #include <math.h>
00067 
00068 
00069 namespace lslgeneric{
00070 
00071 template <typename PointT>
00072 class NDTMapMatcherD2D_2D{
00073         public:
00074                 lslgeneric::NDTMap<PointT> map;  
00075                 
00083                 NDTMapMatcherD2D_2D(double map_resolution,  lslgeneric::NDTMap<PointT> &nd_map ,double miniz = 0.0, double maxiz = 0.0) : 
00084                         map(new lslgeneric::LazyGrid<PointT>(map_resolution))
00085                 {
00086                         isInit = false;
00087                         resolution = map_resolution;
00088                         min_z = miniz;
00089                         max_z = maxiz;
00090                         sensor_pose.setIdentity();
00093                         double cx,cy,cz;
00094                         if(!nd_map.getCentroid(cx, cy, cz)){
00095                                 fprintf(stderr,"Centroid NOT Given-abort!\n");
00096                                 exit(1);
00097                         }else{
00098                                 fprintf(stderr,"Centroid(%lf,%lf,%lf)\n",cx,cy,cz);
00099                         }
00100                         
00101                         double wx,wy,wz;
00102                         
00103                         if(!nd_map.getGridSizeInMeters(wx, wy, wz)){
00104                                 fprintf(stderr,"Grid size NOT Given-abort!\n");
00105                                 exit(1);
00106                         }else{
00107                                 fprintf(stderr,"GridSize(%lf,%lf,%lf)\n",wx,wy,wz);
00108                         }
00109                         
00110                         map.initialize(cx,cy,cz,wx,wy,wz);
00111                         
00112                         std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00113                         ndts = nd_map.getAllCells();
00114                         fprintf(stderr,"NDTMapMatcherD2D_2D::Inserting %d gaussians to map\n",ndts.size());
00115                         for(unsigned int i=0;i<ndts.size();i++){
00116                                 Eigen::Vector3d m = ndts[i]->getMean(); 
00117                                 if(m[2]>min_z && m[2] < max_z){
00118                                         Eigen::Matrix3d cov = ndts[i]->getCov();
00119                                         unsigned int nump = ndts[i]->getN();
00120                                         map.addDistributionToCell(cov, m,nump);
00121                                 }
00122                         }       
00123                 }
00128                 void setSensorPose(Eigen::Affine3d spose){
00129                         sensor_pose = spose;
00130                 }
00131                 
00138                 bool update(Eigen::Affine3d Tinit, pcl::PointCloud<PointT> &cloud){
00140                         lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
00141                         pcl::PointCloud<PointT> cloud_tmp;
00143                         for(unsigned int i=0;i<cloud.size();i++){
00144                                 bool add = true;
00145                                 if(cloud.points[i].z > max_z || cloud.points[i].z<min_z){
00146                                         add=false;
00147                                 }
00148                                 if(add) cloud_tmp.push_back(cloud.points[i]);
00149                         }
00150                         cloud = cloud_tmp;                      
00151 
00153                         lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00154                         ndlocal.addPointCloudSimple(cloud);
00155                         ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00156                         
00157                         lslgeneric::NDTMatcherD2D_2D<PointT,PointT> matcher; //(false,false);
00158                         return(matcher.match( map, ndlocal,Tinit,true));
00159                 }
00160                 
00164                 bool updateNoFilt(Eigen::Affine3d &Tinit, pcl::PointCloud<PointT> &cloud){
00165                         
00167                         lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00168                         ndlocal.addPointCloudSimple(cloud);
00169                         ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00170                         
00171                         lslgeneric::NDTMatcherD2D_2D<PointT,PointT> matcher; //(false,false);
00172                         return matcher.match( map, ndlocal,Tinit,true);
00173                 }
00174                 
00175         private:
00176                 bool isInit;
00177                 
00178                 double resolution; 
00179                 double min_z;           
00180                 double max_z;     
00181                 
00182                 Eigen::Affine3d sensor_pose;
00183                 
00184                 
00185 };
00186 }
00187 #endif


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30