Mapper.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #ifndef OPEN_KARTO_MAPPER_H
00019 #define OPEN_KARTO_MAPPER_H
00020 
00021 #include <map>
00022 #include <vector>
00023 
00024 #include <open_karto/Karto.h>
00025 
00026 namespace karto
00027 {
00029   // Listener classes
00030 
00034   class MapperListener
00035   {
00036   public:
00040     virtual void Info(const std::string& /*rInfo*/) {};
00041   };
00042 
00046   class MapperDebugListener
00047   {
00048   public:
00052     virtual void Debug(const std::string& /*rInfo*/) {};
00053   };
00054 
00058   class MapperLoopClosureListener : public MapperListener
00059   {
00060   public:
00064     virtual void LoopClosureCheck(const std::string& /*rInfo*/) {};
00065 
00069     virtual void BeginLoopClosure(const std::string& /*rInfo*/) {};
00070 
00074     virtual void EndLoopClosure(const std::string& /*rInfo*/) {};
00075   };  // MapperListener
00076 
00080 
00084   class EdgeLabel
00085   {
00086   public:
00090     EdgeLabel()
00091     {
00092     }
00093 
00097     virtual ~EdgeLabel()
00098     {
00099     }
00100   };  // EdgeLabel
00101 
00105 
00106   // A LinkInfo object contains the requisite information for the "spring"
00107   // that links two scans together--the pose difference and the uncertainty
00108   // (represented by a covariance matrix).
00109   class LinkInfo : public EdgeLabel
00110   {
00111   public:
00118     LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00119     {
00120       Update(rPose1, rPose2, rCovariance);
00121     }
00122 
00126     virtual ~LinkInfo()
00127     {
00128     }
00129 
00130   public:
00137     void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00138     {
00139       m_Pose1 = rPose1;
00140       m_Pose2 = rPose2;
00141 
00142       // transform second pose into the coordinate system of the first pose
00143       Transform transform(rPose1, Pose2());
00144       m_PoseDifference = transform.TransformPose(rPose2);
00145 
00146       // transform covariance into reference of first pose
00147       Matrix3 rotationMatrix;
00148       rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
00149 
00150       m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
00151     }
00152 
00157     inline const Pose2& GetPose1()
00158     {
00159       return m_Pose1;
00160     }
00161 
00166     inline const Pose2& GetPose2()
00167     {
00168       return m_Pose2;
00169     }
00170 
00175     inline const Pose2& GetPoseDifference()
00176     {
00177       return m_PoseDifference;
00178     }
00179 
00184     inline const Matrix3& GetCovariance()
00185     {
00186       return m_Covariance;
00187     }
00188 
00189   private:
00190     Pose2 m_Pose1;
00191     Pose2 m_Pose2;
00192     Pose2 m_PoseDifference;
00193     Matrix3 m_Covariance;
00194   };  // LinkInfo
00195 
00199 
00200   template<typename T>
00201   class Edge;
00202 
00206   template<typename T>
00207   class Vertex
00208   {
00209     friend class Edge<T>;
00210 
00211   public:
00216     Vertex(T* pObject)
00217       : m_pObject(pObject)
00218     {
00219     }
00220 
00224     virtual ~Vertex()
00225     {
00226     }
00227 
00232     inline const std::vector<Edge<T>*>& GetEdges() const
00233     {
00234       return m_Edges;
00235     }
00236 
00241     inline T* GetObject() const
00242     {
00243       return m_pObject;
00244     }
00245 
00250     std::vector<Vertex<T>*> GetAdjacentVertices() const
00251     {
00252       std::vector<Vertex<T>*> vertices;
00253 
00254       const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
00255       {
00256         Edge<T>* pEdge = *iter;
00257 
00258         // check both source and target because we have a undirected graph
00259         if (pEdge->GetSource() != this)
00260         {
00261           vertices.push_back(pEdge->GetSource());
00262         }
00263 
00264         if (pEdge->GetTarget() != this)
00265         {
00266           vertices.push_back(pEdge->GetTarget());
00267         }
00268       }
00269 
00270       return vertices;
00271     }
00272 
00273   private:
00278     inline void AddEdge(Edge<T>* pEdge)
00279     {
00280       m_Edges.push_back(pEdge);
00281     }
00282 
00283     T* m_pObject;
00284     std::vector<Edge<T>*> m_Edges;
00285   };  // Vertex<T>
00286 
00290 
00294   template<typename T>
00295   class Edge
00296   {
00297   public:
00303     Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
00304       : m_pSource(pSource)
00305       , m_pTarget(pTarget)
00306       , m_pLabel(NULL)
00307     {
00308       m_pSource->AddEdge(this);
00309       m_pTarget->AddEdge(this);
00310     }
00311 
00315     virtual ~Edge()
00316     {
00317       m_pSource = NULL;
00318       m_pTarget = NULL;
00319 
00320       if (m_pLabel != NULL)
00321       {
00322         delete m_pLabel;
00323         m_pLabel = NULL;
00324       }
00325     }
00326 
00327   public:
00332     inline Vertex<T>* GetSource() const
00333     {
00334       return m_pSource;
00335     }
00336 
00341     inline Vertex<T>* GetTarget() const
00342     {
00343       return m_pTarget;
00344     }
00345 
00350     inline EdgeLabel* GetLabel()
00351     {
00352       return m_pLabel;
00353     }
00354 
00359     inline void SetLabel(EdgeLabel* pLabel)
00360     {
00361       m_pLabel = pLabel;
00362     }
00363 
00364   private:
00365     Vertex<T>* m_pSource;
00366     Vertex<T>* m_pTarget;
00367     EdgeLabel* m_pLabel;
00368   };  // class Edge<T>
00369 
00373 
00377   template<typename T>
00378   class Visitor
00379   {
00380   public:
00386     virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
00387   };  // Visitor<T>
00388 
00392 
00393   template<typename T>
00394   class Graph;
00395 
00399   template<typename T>
00400   class GraphTraversal
00401   {
00402   public:
00407     GraphTraversal(Graph<T>* pGraph)
00408       : m_pGraph(pGraph)
00409     {
00410     }
00411 
00415     virtual ~GraphTraversal()
00416     {
00417     }
00418 
00419   public:
00425     virtual std::vector<T*> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
00426 
00427   protected:
00431     Graph<T>* m_pGraph;
00432   };  // GraphTraversal<T>
00433 
00437 
00441   template<typename T>
00442   class Graph
00443   {
00444   public:
00448     typedef std::map<Name, std::vector<Vertex<T>*> > VertexMap;
00449 
00450   public:
00454     Graph()
00455     {
00456     }
00457 
00461     virtual ~Graph()
00462     {
00463       Clear();
00464     }
00465 
00466   public:
00472     inline void AddVertex(const Name& rName, Vertex<T>* pVertex)
00473     {
00474       m_Vertices[rName].push_back(pVertex);
00475     }
00476 
00481     inline void AddEdge(Edge<T>* pEdge)
00482     {
00483       m_Edges.push_back(pEdge);
00484     }
00485 
00489     void Clear()
00490     {
00491       forEachAs(typename VertexMap, &m_Vertices, indexIter)
00492       {
00493         // delete each vertex
00494         forEach(typename std::vector<Vertex<T>*>, &(indexIter->second))
00495         {
00496           delete *iter;
00497         }
00498       }
00499       m_Vertices.clear();
00500 
00501       const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
00502       {
00503         delete *iter;
00504       }
00505       m_Edges.clear();
00506     }
00507 
00512     inline const std::vector<Edge<T>*>& GetEdges() const
00513     {
00514       return m_Edges;
00515     }
00516 
00521     inline const VertexMap& GetVertices() const
00522     {
00523       return m_Vertices;
00524     }
00525 
00526   protected:
00530     VertexMap m_Vertices;
00531 
00535     std::vector<Edge<T>*> m_Edges;
00536   };  // Graph<T>
00537 
00541 
00542   class Mapper;
00543   class ScanMatcher;
00544 
00548   class KARTO_EXPORT MapperGraph : public Graph<LocalizedRangeScan>
00549   {
00550   public:
00556     MapperGraph(Mapper* pMapper, kt_double rangeThreshold);
00557 
00561     virtual ~MapperGraph();
00562 
00563   public:
00568     void AddVertex(LocalizedRangeScan* pScan);
00569 
00578     Edge<LocalizedRangeScan>* AddEdge(LocalizedRangeScan* pSourceScan,
00579                                       LocalizedRangeScan* pTargetScan,
00580                                       kt_bool& rIsNewEdge);
00581 
00587     void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance);
00588 
00594     kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName);
00595 
00601     LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance);
00602 
00607     inline ScanMatcher* GetLoopScanMatcher() const
00608     {
00609       return m_pLoopScanMatcher;
00610     }
00611 
00612   private:
00618     inline Vertex<LocalizedRangeScan>* GetVertex(LocalizedRangeScan* pScan)
00619     {
00620       return m_Vertices[pScan->GetSensorName()][pScan->GetStateId()];
00621     }
00622 
00628     LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const;
00629 
00637     void LinkScans(LocalizedRangeScan* pFromScan,
00638                    LocalizedRangeScan* pToScan,
00639                    const Pose2& rMean,
00640                    const Matrix3& rCovariance);
00641 
00648     void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances);
00649 
00657     void LinkChainToScan(const LocalizedRangeScanVector& rChain,
00658                          LocalizedRangeScan* pScan,
00659                          const Pose2& rMean,
00660                          const Matrix3& rCovariance);
00661 
00667     std::vector<LocalizedRangeScanVector> FindNearChains(LocalizedRangeScan* pScan);
00668 
00675     Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const;
00676 
00685     LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan,
00686                                                      const Name& rSensorName,
00687                                                      kt_int32u& rStartNum);
00688 
00692     void CorrectPoses();
00693 
00694   private:
00698     Mapper* m_pMapper;
00699 
00703     ScanMatcher* m_pLoopScanMatcher;
00704 
00708     GraphTraversal<LocalizedRangeScan>* m_pTraversal;
00709   };  // MapperGraph
00710 
00714 
00718   class ScanSolver
00719   {
00720   public:
00724     typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
00725 
00729     ScanSolver()
00730     {
00731     }
00732 
00736     virtual ~ScanSolver()
00737     {
00738     }
00739 
00740   public:
00744     virtual void Compute() = 0;
00745 
00750     virtual const IdPoseVector& GetCorrections() const = 0;
00751 
00755     virtual void AddNode(Vertex<LocalizedRangeScan>* /*pVertex*/)
00756     {
00757     }
00758 
00762     virtual void RemoveNode(kt_int32s /*id*/)
00763     {
00764     }
00765 
00769     virtual void AddConstraint(Edge<LocalizedRangeScan>* /*pEdge*/)
00770     {
00771     }
00772 
00776     virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
00777     {
00778     }
00779 
00783     virtual void Clear() {};
00784   };  // ScanSolver
00785 
00789 
00793   class CorrelationGrid : public Grid<kt_int8u>
00794   {
00795   public:
00799     virtual ~CorrelationGrid()
00800     {
00801       delete [] m_pKernel;
00802     }
00803 
00804   public:
00813     static CorrelationGrid* CreateGrid(kt_int32s width,
00814                                        kt_int32s height,
00815                                        kt_double resolution,
00816                                        kt_double smearDeviation)
00817     {
00818       assert(resolution != 0.0);
00819 
00820       // +1 in case of roundoff
00821       kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
00822 
00823       CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
00824 
00825       return pGrid;
00826     }
00827 
00834     virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
00835     {
00836       kt_int32s x = rGrid.GetX() + m_Roi.GetX();
00837       kt_int32s y = rGrid.GetY() + m_Roi.GetY();
00838 
00839       return Grid<kt_int8u>::GridIndex(Vector2<kt_int32s>(x, y), boundaryCheck);
00840     }
00841 
00846     inline const Rectangle2<kt_int32s>& GetROI() const
00847     {
00848       return m_Roi;
00849     }
00850 
00855     inline void SetROI(const Rectangle2<kt_int32s>& roi)
00856     {
00857       m_Roi = roi;
00858     }
00859 
00864     inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
00865     {
00866       assert(m_pKernel != NULL);
00867 
00868       int gridIndex = GridIndex(rGridPoint);
00869       if (GetDataPointer()[gridIndex] != GridStates_Occupied)
00870       {
00871         return;
00872       }
00873 
00874       kt_int32s halfKernel = m_KernelSize / 2;
00875 
00876       // apply kernel
00877       for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00878       {
00879         kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
00880 
00881         kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
00882 
00883         // if a point is on the edge of the grid, there is no problem
00884         // with running over the edge of allowable memory, because
00885         // the grid has margins to compensate for the kernel size
00886         for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
00887         {
00888           kt_int32s kernelArrayIndex = i + kernelConstant;
00889 
00890           kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
00891           if (kernelValue > pGridAdr[i])
00892           {
00893             // kernel value is greater, so set it to kernel value
00894             pGridAdr[i] = kernelValue;
00895           }
00896         }
00897       }
00898     }
00899 
00900   protected:
00909     CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
00910                     kt_double resolution, kt_double smearDeviation)
00911       : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
00912       , m_SmearDeviation(smearDeviation)
00913       , m_pKernel(NULL)
00914     {
00915       GetCoordinateConverter()->SetScale(1.0 / resolution);
00916 
00917       // setup region of interest
00918       m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
00919 
00920       // calculate kernel
00921       CalculateKernel();
00922     }
00923 
00927     virtual void CalculateKernel()
00928     {
00929       kt_double resolution = GetResolution();
00930 
00931       assert(resolution != 0.0);
00932       assert(m_SmearDeviation != 0.0);
00933 
00934       // min and max distance deviation for smearing;
00935       // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
00936       const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
00937       const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
00938 
00939       // check if given value too small or too big
00940       if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
00941       {
00942         std::stringstream error;
00943         error << "Mapper Error:  Smear deviation too small:  Must be between "
00944               << MIN_SMEAR_DISTANCE_DEVIATION
00945               << " and "
00946               << MAX_SMEAR_DISTANCE_DEVIATION;
00947         throw std::runtime_error(error.str());
00948       }
00949 
00950       // NOTE:  Currently assumes a two-dimensional kernel
00951 
00952       // +1 for center
00953       m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
00954 
00955       // allocate kernel
00956       m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
00957       if (m_pKernel == NULL)
00958       {
00959         throw std::runtime_error("Unable to allocate memory for kernel!");
00960       }
00961 
00962       // calculate kernel
00963       kt_int32s halfKernel = m_KernelSize / 2;
00964       for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
00965       {
00966         for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00967         {
00968 #ifdef WIN32
00969           kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
00970 #else
00971           kt_double distanceFromMean = hypot(i * resolution, j * resolution);
00972 #endif
00973           kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
00974 
00975           kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
00976           assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
00977 
00978           int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
00979           m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
00980         }
00981       }
00982     }
00983 
00991     static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
00992     {
00993       assert(resolution != 0.0);
00994 
00995       return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
00996     }
00997 
00998   private:
01003     kt_double m_SmearDeviation;
01004 
01005     // Size of one side of the kernel
01006     kt_int32s m_KernelSize;
01007 
01008     // Cached kernel for smearing
01009     kt_int8u* m_pKernel;
01010 
01011     // region of interest
01012     Rectangle2<kt_int32s> m_Roi;
01013   };  // CorrelationGrid
01014 
01018 
01022   class KARTO_EXPORT ScanMatcher
01023   {
01024   public:
01028     virtual ~ScanMatcher();
01029 
01030   public:
01034     static ScanMatcher* Create(Mapper* pMapper,
01035                                kt_double searchSize,
01036                                kt_double resolution,
01037                                kt_double smearDeviation,
01038                                kt_double rangeThreshold);
01039 
01050     kt_double MatchScan(LocalizedRangeScan* pScan,
01051                         const LocalizedRangeScanVector& rBaseScans,
01052                         Pose2& rMean, Matrix3& rCovariance,
01053                         kt_bool doPenalize = true,
01054                         kt_bool doRefineMatch = true);
01055 
01072     kt_double CorrelateScan(LocalizedRangeScan* pScan,
01073                             const Pose2& rSearchCenter,
01074                             const Vector2<kt_double>& rSearchSpaceOffset,
01075                             const Vector2<kt_double>& rSearchSpaceResolution,
01076                             kt_double searchAngleOffset,
01077                             kt_double searchAngleResolution,
01078                             kt_bool doPenalize,
01079                             Pose2& rMean,
01080                             Matrix3& rCovariance,
01081                             kt_bool doingFineMatch);
01082 
01093     void ComputePositionalCovariance(const Pose2& rBestPose,
01094                                      kt_double bestResponse,
01095                                      const Pose2& rSearchCenter,
01096                                      const Vector2<kt_double>& rSearchSpaceOffset,
01097                                      const Vector2<kt_double>& rSearchSpaceResolution,
01098                                      kt_double searchAngleResolution,
01099                                      Matrix3& rCovariance);
01100 
01110     void ComputeAngularCovariance(const Pose2& rBestPose,
01111                                   kt_double bestResponse,
01112                                   const Pose2& rSearchCenter,
01113                                   kt_double searchAngleOffset,
01114                                   kt_double searchAngleResolution,
01115                                   Matrix3& rCovariance);
01116 
01121     inline CorrelationGrid* GetCorrelationGrid() const
01122     {
01123       return m_pCorrelationGrid;
01124     }
01125 
01126   private:
01132     void AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint);
01133 
01140     void AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear = true);
01141 
01148     PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const;
01149 
01156     kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const;
01157 
01158   protected:
01162     ScanMatcher(Mapper* pMapper)
01163       : m_pMapper(pMapper)
01164       , m_pCorrelationGrid(NULL)
01165       , m_pSearchSpaceProbs(NULL)
01166       , m_pGridLookup(NULL)
01167     {
01168     }
01169 
01170   private:
01171     Mapper* m_pMapper;
01172 
01173     CorrelationGrid* m_pCorrelationGrid;
01174     Grid<kt_double>* m_pSearchSpaceProbs;
01175 
01176     GridIndexLookup<kt_int8u>* m_pGridLookup;
01177   };  // ScanMatcher
01178 
01182 
01183   class ScanManager;
01184 
01188   class KARTO_EXPORT MapperSensorManager  // : public SensorManager
01189   {
01190     typedef std::map<Name, ScanManager*> ScanManagerMap;
01191 
01192   public:
01196     MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
01197       : m_RunningBufferMaximumSize(runningBufferMaximumSize)
01198       , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
01199       , m_NextScanId(0)
01200     {
01201     }
01202 
01206     virtual ~MapperSensorManager()
01207     {
01208       Clear();
01209     }
01210 
01211   public:
01217     void RegisterSensor(const Name& rSensorName);
01218 
01225     LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex);
01226 
01231     inline std::vector<Name> GetSensorNames()
01232     {
01233       std::vector<Name> deviceNames;
01234       const_forEach(ScanManagerMap, &m_ScanManagers)
01235       {
01236         deviceNames.push_back(iter->first);
01237       }
01238 
01239       return deviceNames;
01240     }
01241 
01247     LocalizedRangeScan* GetLastScan(const Name& rSensorName);
01248 
01253     inline void SetLastScan(LocalizedRangeScan* pScan);
01254 
01260     inline LocalizedRangeScan* GetScan(kt_int32s id)
01261     {
01262       assert(math::IsUpTo(id, (kt_int32s)m_Scans.size()));
01263 
01264       return m_Scans[id];
01265     }
01266 
01271     void AddScan(LocalizedRangeScan* pScan);
01272 
01277     void AddRunningScan(LocalizedRangeScan* pScan);
01278 
01284     LocalizedRangeScanVector& GetScans(const Name& rSensorName);
01285 
01291     LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName);
01292 
01297     LocalizedRangeScanVector GetAllScans();
01298 
01302     void Clear();
01303 
01304   private:
01309     inline ScanManager* GetScanManager(LocalizedRangeScan* pScan)
01310     {
01311       return GetScanManager(pScan->GetSensorName());
01312     }
01313 
01319     inline ScanManager* GetScanManager(const Name& rSensorName)
01320     {
01321       if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end())
01322       {
01323         return m_ScanManagers[rSensorName];
01324       }
01325 
01326       return NULL;
01327     }
01328 
01329   private:
01330     // map from device ID to scan data
01331     ScanManagerMap m_ScanManagers;
01332 
01333     kt_int32u m_RunningBufferMaximumSize;
01334     kt_double m_RunningBufferMaximumDistance;
01335 
01336     kt_int32s m_NextScanId;
01337 
01338     std::vector<LocalizedRangeScan*> m_Scans;
01339   };  // MapperSensorManager
01340 
01344 
01502   class KARTO_EXPORT Mapper : public Module
01503   {
01504     friend class MapperGraph;
01505     friend class ScanMatcher;
01506 
01507   public:
01511     Mapper();
01512 
01517     Mapper(const std::string& rName);
01518 
01522     virtual ~Mapper();
01523 
01524   public:
01529     void Initialize(kt_double rangeThreshold);
01530 
01535     void Reset();
01536 
01548     virtual kt_bool Process(LocalizedRangeScan* pScan);
01549 
01553     virtual kt_bool Process(Object* pObject);
01554 
01561     virtual const LocalizedRangeScanVector GetAllProcessedScans() const;
01562 
01567     void AddListener(MapperListener* pListener);
01568 
01573     void RemoveListener(MapperListener* pListener);
01574 
01579     void SetScanSolver(ScanSolver* pSolver);
01580 
01585     virtual MapperGraph* GetGraph() const;
01586 
01591     virtual ScanMatcher* GetSequentialScanMatcher() const;
01592 
01597     virtual ScanMatcher* GetLoopScanMatcher() const;
01598 
01603     inline MapperSensorManager* GetMapperSensorManager() const
01604     {
01605       return m_pMapperSensorManager;
01606     }
01607 
01613     inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
01614     {
01615       return m_pGraph->TryCloseLoop(pScan, rSensorName);
01616     }
01617 
01618   private:
01619     void InitializeParameters();
01620 
01628     kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
01629 
01630   public:
01632     // fire information for listeners!!
01633 
01638     void FireInfo(const std::string& rInfo) const;
01639 
01644     void FireDebug(const std::string& rInfo) const;
01645 
01650     void FireLoopClosureCheck(const std::string& rInfo) const;
01651 
01656     void FireBeginLoopClosure(const std::string& rInfo) const;
01657 
01662     void FireEndLoopClosure(const std::string& rInfo) const;
01663 
01664     // FireRunningScansUpdated
01665 
01666     // FireCovarianceCalculated
01667 
01668     // FireLoopClosureChain
01669 
01670   private:
01674     Mapper(const Mapper&);
01675 
01679     const Mapper& operator=(const Mapper&);
01680 
01681   public:
01682     void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); }
01683 
01684   private:
01685     kt_bool m_Initialized;
01686 
01687     ScanMatcher* m_pSequentialScanMatcher;
01688 
01689     MapperSensorManager* m_pMapperSensorManager;
01690 
01691     MapperGraph* m_pGraph;
01692     ScanSolver* m_pScanOptimizer;
01693 
01694     std::vector<MapperListener*> m_Listeners;
01695 
01696 
01705     Parameter<kt_bool>* m_pUseScanMatching;
01706 
01710     Parameter<kt_bool>* m_pUseScanBarycenter;
01711 
01723     Parameter<kt_double>* m_pMinimumTimeInterval;
01724 
01733     Parameter<kt_double>* m_pMinimumTravelDistance;
01734 
01743     Parameter<kt_double>* m_pMinimumTravelHeading;
01744 
01753     Parameter<kt_int32u>* m_pScanBufferSize;
01754 
01760     Parameter<kt_double>* m_pScanBufferMaximumScanDistance;
01761 
01766     Parameter<kt_double>* m_pLinkMatchMinimumResponseFine;
01767 
01773     Parameter<kt_double>* m_pLinkScanMaximumDistance;
01774 
01779     Parameter<kt_bool>* m_pDoLoopClosing;
01780 
01786     Parameter<kt_double>* m_pLoopSearchMaximumDistance;
01787 
01794     Parameter<kt_int32u>* m_pLoopMatchMinimumChainSize;
01795 
01801     Parameter<kt_double>* m_pLoopMatchMaximumVarianceCoarse;
01802 
01807     Parameter<kt_double>* m_pLoopMatchMinimumResponseCoarse;
01808 
01813     Parameter<kt_double>* m_pLoopMatchMinimumResponseFine;
01814 
01816     //    CorrelationParameters correlationParameters;
01817 
01822     Parameter<kt_double>* m_pCorrelationSearchSpaceDimension;
01823 
01828     Parameter<kt_double>* m_pCorrelationSearchSpaceResolution;
01829 
01834     Parameter<kt_double>* m_pCorrelationSearchSpaceSmearDeviation;
01835 
01836 
01838     //    CorrelationParameters loopCorrelationParameters;
01839 
01844     Parameter<kt_double>* m_pLoopSearchSpaceDimension;
01845 
01850     Parameter<kt_double>* m_pLoopSearchSpaceResolution;
01851 
01856     Parameter<kt_double>* m_pLoopSearchSpaceSmearDeviation;
01857 
01859     // ScanMatcherParameters;
01860 
01861     // Variance of penalty for deviating from odometry when scan-matching.
01862     // The penalty is a multiplier (less than 1.0) is a function of the
01863     // delta of the scan position being tested and the odometric pose
01864     Parameter<kt_double>* m_pDistanceVariancePenalty;
01865     Parameter<kt_double>* m_pAngleVariancePenalty;
01866 
01867     // The range of angles to search during a coarse search and a finer search
01868     Parameter<kt_double>* m_pFineSearchAngleOffset;
01869     Parameter<kt_double>* m_pCoarseSearchAngleOffset;
01870 
01871     // Resolution of angles to search during a coarse search
01872     Parameter<kt_double>* m_pCoarseAngleResolution;
01873 
01874     // Minimum value of the penalty multiplier so scores do not
01875     // become too small
01876     Parameter<kt_double>* m_pMinimumAnglePenalty;
01877     Parameter<kt_double>* m_pMinimumDistancePenalty;
01878 
01879     // whether to increase the search space if no good matches are initially found
01880     Parameter<kt_bool>* m_pUseResponseExpansion;
01881 
01882   public:
01883     /* Abstract methods for parameter setters and getters */
01884 
01885     /* Getters */
01886     // General Parameters
01887     bool getParamUseScanMatching();
01888     bool getParamUseScanBarycenter();
01889     double getParamMinimumTimeInterval();
01890     double getParamMinimumTravelDistance();
01891     double getParamMinimumTravelHeading();
01892     int getParamScanBufferSize();
01893     double getParamScanBufferMaximumScanDistance();
01894     double getParamLinkMatchMinimumResponseFine();
01895     double getParamLinkScanMaximumDistance();
01896     double getParamLoopSearchMaximumDistance();
01897     bool getParamDoLoopClosing();
01898     int getParamLoopMatchMinimumChainSize();
01899     double getParamLoopMatchMaximumVarianceCoarse();
01900     double getParamLoopMatchMinimumResponseCoarse();
01901     double getParamLoopMatchMinimumResponseFine();
01902 
01903     // Correlation Parameters - Correlation Parameters
01904     double getParamCorrelationSearchSpaceDimension();
01905     double getParamCorrelationSearchSpaceResolution();
01906     double getParamCorrelationSearchSpaceSmearDeviation();
01907 
01908     // Correlation Parameters - Loop Closure Parameters
01909     double getParamLoopSearchSpaceDimension();
01910     double getParamLoopSearchSpaceResolution();
01911     double getParamLoopSearchSpaceSmearDeviation();
01912 
01913     // Scan Matcher Parameters
01914     double getParamDistanceVariancePenalty();
01915     double getParamAngleVariancePenalty();
01916     double getParamFineSearchAngleOffset();
01917     double getParamCoarseSearchAngleOffset();
01918     double getParamCoarseAngleResolution();
01919     double getParamMinimumAnglePenalty();
01920     double getParamMinimumDistancePenalty();
01921     bool getParamUseResponseExpansion();
01922 
01923     /* Setters */
01924     // General Parameters
01925     void setParamUseScanMatching(bool b);
01926     void setParamUseScanBarycenter(bool b);
01927     void setParamMinimumTimeInterval(double d);
01928     void setParamMinimumTravelDistance(double d);
01929     void setParamMinimumTravelHeading(double d);
01930     void setParamScanBufferSize(int i);
01931     void setParamScanBufferMaximumScanDistance(double d);
01932     void setParamLinkMatchMinimumResponseFine(double d);
01933     void setParamLinkScanMaximumDistance(double d);
01934     void setParamLoopSearchMaximumDistance(double d);
01935     void setParamDoLoopClosing(bool b);
01936     void setParamLoopMatchMinimumChainSize(int i);
01937     void setParamLoopMatchMaximumVarianceCoarse(double d);
01938     void setParamLoopMatchMinimumResponseCoarse(double d);
01939     void setParamLoopMatchMinimumResponseFine(double d);
01940 
01941     // Correlation Parameters - Correlation Parameters
01942     void setParamCorrelationSearchSpaceDimension(double d);
01943     void setParamCorrelationSearchSpaceResolution(double d);
01944     void setParamCorrelationSearchSpaceSmearDeviation(double d);
01945 
01946     // Correlation Parameters - Loop Closure Parameters
01947     void setParamLoopSearchSpaceDimension(double d);
01948     void setParamLoopSearchSpaceResolution(double d);
01949     void setParamLoopSearchSpaceSmearDeviation(double d);
01950 
01951     // Scan Matcher Parameters
01952     void setParamDistanceVariancePenalty(double d);
01953     void setParamAngleVariancePenalty(double d);
01954     void setParamFineSearchAngleOffset(double d);
01955     void setParamCoarseSearchAngleOffset(double d);
01956     void setParamCoarseAngleResolution(double d);
01957     void setParamMinimumAnglePenalty(double d);
01958     void setParamMinimumDistancePenalty(double d);
01959     void setParamUseResponseExpansion(bool b);
01960   };
01961 }  // namespace karto
01962 
01963 #endif  // OPEN_KARTO_MAPPER_H


open_karto
Author(s):
autogenerated on Thu Jun 6 2019 21:02:56