HectorSlamProcessor.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 _hectorslamprocessor_h__
00030 #define _hectorslamprocessor_h__
00031 
00032 #include "../map/GridMap.h"
00033 #include "../map/OccGridMapUtilConfig.h"
00034 #include "../matcher/ScanMatcher.h"
00035 #include "../scan/DataPointContainer.h"
00036 
00037 #include "../util/UtilFunctions.h"
00038 #include "../util/DrawInterface.h"
00039 #include "../util/HectorDebugInfoInterface.h"
00040 #include "../util/MapLockerInterface.h"
00041 
00042 #include "MapRepresentationInterface.h"
00043 #include "MapRepMultiMap.h"
00044 
00045 
00046 #include <float.h>
00047 
00048 namespace hectorslam{
00049 
00050 class HectorSlamProcessor
00051 {
00052 public:
00053 
00054   HectorSlamProcessor(float mapResolution, int mapSizeX, int mapSizeY , const Eigen::Vector2f& startCoords, int multi_res_size, DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0)
00055     : drawInterface(drawInterfaceIn)
00056     , debugInterface(debugInterfaceIn)
00057   {
00058     mapRep = new MapRepMultiMap(mapResolution, mapSizeX, mapSizeY, multi_res_size, startCoords, drawInterfaceIn, debugInterfaceIn);
00059 
00060     this->reset();
00061 
00062     this->setMapUpdateMinDistDiff(0.4f *1.0f);
00063     this->setMapUpdateMinAngleDiff(0.13f * 1.0f);
00064   }
00065 
00066   ~HectorSlamProcessor()
00067   {
00068     delete mapRep;
00069   }
00070 
00071   void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false)
00072   {
00073     //std::cout << "\nph:\n" << poseHintWorld << "\n";
00074 
00075     Eigen::Vector3f newPoseEstimateWorld;
00076 
00077     if (!map_without_matching){
00078         newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
00079     }else{
00080         newPoseEstimateWorld = poseHintWorld;
00081     }
00082 
00083     lastScanMatchPose = newPoseEstimateWorld;
00084 
00085     //std::cout << "\nt1:\n" << newPoseEstimateWorld << "\n";
00086 
00087     //std::cout << "\n1";
00088     //std::cout << "\n" << lastScanMatchPose << "\n";
00089     if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching){
00090 
00091       mapRep->updateByScan(dataContainer, newPoseEstimateWorld);
00092 
00093       mapRep->onMapUpdated();
00094       lastMapUpdatePose = newPoseEstimateWorld;
00095     }
00096 
00097     if(drawInterface){
00098       const GridMap& gridMapRef (mapRep->getGridMap());
00099       drawInterface->setColor(1.0, 0.0, 0.0);
00100       drawInterface->setScale(0.15);
00101 
00102       drawInterface->drawPoint(gridMapRef.getWorldCoords(Eigen::Vector2f::Zero()));
00103       drawInterface->drawPoint(gridMapRef.getWorldCoords((gridMapRef.getMapDimensions().array()-1).cast<float>()));
00104       drawInterface->drawPoint(Eigen::Vector2f(1.0f, 1.0f));
00105 
00106       drawInterface->sendAndResetData();
00107     }
00108 
00109     if (debugInterface)
00110     {
00111       debugInterface->sendAndResetData();
00112     }
00113   }
00114 
00115   void reset()
00116   {
00117     lastMapUpdatePose = Eigen::Vector3f(FLT_MAX, FLT_MAX, FLT_MAX);
00118     lastScanMatchPose = Eigen::Vector3f::Zero();
00119     //lastScanMatchPose.x() = -10.0f;
00120     //lastScanMatchPose.y() = -15.0f;
00121     //lastScanMatchPose.z() = M_PI*0.15f;
00122 
00123     mapRep->reset();
00124   }
00125 
00126   const Eigen::Vector3f& getLastScanMatchPose() const { return lastScanMatchPose; };
00127   const Eigen::Matrix3f& getLastScanMatchCovariance() const { return lastScanMatchCov; };
00128   float getScaleToMap() const { return mapRep->getScaleToMap(); };
00129 
00130   int getMapLevels() const { return mapRep->getMapLevels(); };
00131   const GridMap& getGridMap(int mapLevel = 0) const { return mapRep->getGridMap(mapLevel); };
00132 
00133   void addMapMutex(int i, MapLockerInterface* mapMutex) { mapRep->addMapMutex(i, mapMutex); };
00134   MapLockerInterface* getMapMutex(int i) { return mapRep->getMapMutex(i); };
00135 
00136   void setUpdateFactorFree(float free_factor) { mapRep->setUpdateFactorFree(free_factor); };
00137   void setUpdateFactorOccupied(float occupied_factor) { mapRep->setUpdateFactorOccupied(occupied_factor); };
00138   void setMapUpdateMinDistDiff(float minDist) { paramMinDistanceDiffForMapUpdate = minDist; };
00139   void setMapUpdateMinAngleDiff(float angleChange) { paramMinAngleDiffForMapUpdate = angleChange; };
00140 
00141 protected:
00142 
00143   MapRepresentationInterface* mapRep;
00144 
00145   Eigen::Vector3f lastMapUpdatePose;
00146   Eigen::Vector3f lastScanMatchPose;
00147   Eigen::Matrix3f lastScanMatchCov;
00148 
00149   float paramMinDistanceDiffForMapUpdate;
00150   float paramMinAngleDiffForMapUpdate;
00151 
00152   DrawInterface* drawInterface;
00153   HectorDebugInfoInterface* debugInterface;
00154 };
00155 
00156 }
00157 
00158 #endif


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