MapRepMultiMap.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef _hectormaprepmultimap_h__
00030 #define _hectormaprepmultimap_h__
00031 
00032 #include "MapRepresentationInterface.h"
00033 #include "MapProcContainer.h"
00034 
00035 #include "../map/GridMap.h"
00036 #include "../map/OccGridMapUtilConfig.h"
00037 #include "../matcher/ScanMatcher.h"
00038 
00039 #include "../util/DrawInterface.h"
00040 #include "../util/HectorDebugInfoInterface.h"
00041 
00042 namespace hectorslam{
00043 
00044 class MapRepMultiMap : public MapRepresentationInterface
00045 {
00046 
00047 public:
00048   MapRepMultiMap(float mapResolution, int mapSizeX, int mapSizeY, unsigned int numDepth, const Eigen::Vector2f& startCoords, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn)
00049   {
00050     //unsigned int numDepth = 3;
00051     Eigen::Vector2i resolution(mapSizeX, mapSizeY);
00052 
00053     float totalMapSizeX = mapResolution * static_cast<float>(mapSizeX);
00054     float mid_offset_x = totalMapSizeX * startCoords.x();
00055 
00056     float totalMapSizeY = mapResolution * static_cast<float>(mapSizeY);
00057     float mid_offset_y = totalMapSizeY * startCoords.y();
00058 
00059     for (unsigned int i = 0; i < numDepth; ++i){
00060       std::cout << "HectorSM map lvl " << i << ": cellLength: " << mapResolution << " res x:" << resolution.x() << " res y: " << resolution.y() << "\n";
00061       GridMap* gridMap = new hectorslam::GridMap(mapResolution,resolution, Eigen::Vector2f(mid_offset_x, mid_offset_y));
00062       OccGridMapUtilConfig<GridMap>* gridMapUtil = new OccGridMapUtilConfig<GridMap>(gridMap);
00063       ScanMatcher<OccGridMapUtilConfig<GridMap> >* scanMatcher = new hectorslam::ScanMatcher<OccGridMapUtilConfig<GridMap> >(drawInterfaceIn, debugInterfaceIn);
00064 
00065       mapContainer.push_back(MapProcContainer(gridMap, gridMapUtil, scanMatcher));
00066 
00067       resolution /= 2;
00068       mapResolution*=2.0f;
00069     }
00070 
00071     dataContainers.resize(numDepth-1);
00072   }
00073 
00074   virtual ~MapRepMultiMap()
00075   {
00076     unsigned int size = mapContainer.size();
00077 
00078     for (unsigned int i = 0; i < size; ++i){
00079       mapContainer[i].cleanup();
00080     }
00081   }
00082 
00083   virtual void reset()
00084   {
00085     unsigned int size = mapContainer.size();
00086 
00087     for (unsigned int i = 0; i < size; ++i){
00088       mapContainer[i].reset();
00089     }
00090   }
00091 
00092   virtual float getScaleToMap() const { return mapContainer[0].getScaleToMap(); };
00093 
00094   virtual int getMapLevels() const { return mapContainer.size(); };
00095   virtual const GridMap& getGridMap(int mapLevel) const { return mapContainer[mapLevel].getGridMap(); };
00096 
00097   virtual void addMapMutex(int i, MapLockerInterface* mapMutex)
00098   {
00099     mapContainer[i].addMapMutex(mapMutex);
00100   }
00101 
00102   MapLockerInterface* getMapMutex(int i)
00103   {
00104     return mapContainer[i].getMapMutex();
00105   }
00106 
00107   virtual void onMapUpdated()
00108   {
00109     unsigned int size = mapContainer.size();
00110 
00111     for (unsigned int i = 0; i < size; ++i){
00112       mapContainer[i].resetCachedData();
00113     }
00114   }
00115 
00116   virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix)
00117   {
00118     size_t size = mapContainer.size();
00119 
00120     Eigen::Vector3f tmp(beginEstimateWorld);
00121 
00122     for (int index = size - 1; index >= 0; --index){
00123       //std::cout << " m " << i;
00124       if (index == 0){
00125         tmp  = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5));
00126       }else{
00127         dataContainers[index-1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
00128         tmp  = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3));
00129       }
00130     }
00131     return tmp;
00132   }
00133 
00134   virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
00135   {
00136     unsigned int size = mapContainer.size();
00137 
00138     for (unsigned int i = 0; i < size; ++i){
00139       //std::cout << " u " <<  i;
00140       if (i==0){
00141         mapContainer[i].updateByScan(dataContainer, robotPoseWorld);
00142       }else{
00143         mapContainer[i].updateByScan(dataContainers[i-1], robotPoseWorld);
00144       }
00145     }
00146     //std::cout << "\n";
00147   }
00148 
00149   virtual void setUpdateFactorFree(float free_factor)
00150   {
00151     size_t size = mapContainer.size();
00152 
00153     for (unsigned int i = 0; i < size; ++i){
00154       GridMap& map = mapContainer[i].getGridMap();
00155       map.setUpdateFreeFactor(free_factor);
00156     }
00157   }
00158 
00159   virtual void setUpdateFactorOccupied(float occupied_factor)
00160   {
00161     size_t size = mapContainer.size();
00162 
00163     for (unsigned int i = 0; i < size; ++i){
00164       GridMap& map = mapContainer[i].getGridMap();
00165       map.setUpdateOccupiedFactor(occupied_factor);
00166     }
00167   }
00168 
00169 protected:
00170   std::vector<MapProcContainer> mapContainer;
00171   std::vector<DataContainer> dataContainers;
00172 };
00173 
00174 }
00175 
00176 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03