OpenMapper.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
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 #pragma once
00019 
00020 #ifndef __OpenKarto_Mapper_h__
00021 #define __OpenKarto_Mapper_h__
00022 
00023 #ifdef USE_TBB
00024 #include <tbb/mutex.h>
00025 #include <tbb/parallel_for.h>
00026 #include <tbb/blocked_range.h>
00027 #include <tbb/blocked_range3d.h>
00028 #endif
00029 
00030 #include <OpenKarto/Event.h>
00031 #include <OpenKarto/Pair.h>
00032 #include <OpenKarto/Geometry.h>
00033 #include <OpenKarto/StringHelper.h>
00034 #include <OpenKarto/SensorData.h>
00035 #include <OpenKarto/Grid.h>
00036 #include <OpenKarto/GridIndexLookup.h>
00037 #include <OpenKarto/Module.h>
00038 #include <OpenKarto/OccupancyGrid.h>
00039 #include <OpenKarto/TypeCasts.h>
00040 
00041 namespace karto
00042 {
00043 
00045 
00046 
00050   class MapperEventArguments : public EventArguments
00051   {
00052   public:
00057     MapperEventArguments(const String& rMessage)
00058       : m_Message(rMessage)
00059     {
00060     }
00061 
00065     virtual ~MapperEventArguments()
00066     {
00067     }
00068 
00069   public:
00074     const String& GetEventMessage() const
00075     {
00076       return m_Message;
00077     }
00078 
00079   private:
00080     String m_Message;
00081   };
00082 
00083 #ifdef WIN32
00084   EXPORT_KARTO_EVENT(KARTO_EXPORT, MapperEventArguments)
00085 #endif
00086 
00090 
00094   class EdgeLabel
00095   {
00096   public:
00100     EdgeLabel()
00101     {
00102     }
00103 
00107     virtual ~EdgeLabel()
00108     {
00109     }
00110   }; // EdgeLabel
00111 
00115 
00116   // Contains the requisite information for the "spring"
00117   // that links two scans together--the pose difference and the uncertainty
00118   // (represented by a covariance matrix).
00119   class LinkInfo : public EdgeLabel
00120   {
00121   public:
00128     LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00129     {
00130       Update(rPose1, rPose2, rCovariance);
00131     }
00132 
00136     virtual ~LinkInfo()
00137     {
00138     }
00139 
00140   public:
00147     void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00148     {
00149       m_Pose1 = rPose1;
00150       m_Pose2 = rPose2;
00151 
00152       // transform second pose into the coordinate system of the first pose
00153       Transform transform(rPose1, Pose2());
00154       m_PoseDifference = transform.TransformPose(rPose2);
00155 
00156       // transform covariance into reference of first pose
00157       Matrix3 rotationMatrix;
00158       rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
00159 
00160       m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
00161     }
00162 
00167     inline const Pose2& GetPose1()
00168     {
00169       return m_Pose1;
00170     }
00171 
00176     inline const Pose2& GetPose2()
00177     {
00178       return m_Pose2;
00179     }
00180 
00185     inline const Pose2& GetPoseDifference()
00186     {
00187       return m_PoseDifference;
00188     }
00189 
00194     inline const Matrix3& GetCovariance()
00195     {
00196       return m_Covariance;
00197     }
00198 
00199   private:
00200     Pose2 m_Pose1;
00201     Pose2 m_Pose2;
00202     Pose2 m_PoseDifference;
00203     Matrix3 m_Covariance;
00204   }; // LinkInfo
00205 
00209 
00210   template<typename T>
00211   class Edge;
00212 
00216   template<typename T>
00217   class Vertex
00218   {
00219     friend class Edge<T>;
00220 
00221   public:
00226     Vertex(T pObject)
00227       : m_pObject(pObject)
00228     {
00229     }
00230 
00234     virtual ~Vertex()
00235     {
00236     }
00237 
00242     inline const List<Edge<T>*>& GetEdges() const
00243     {
00244       return m_Edges;
00245     }
00246 
00251     inline T GetVertexObject() const
00252     {
00253       return m_pObject;
00254     }
00255 
00260     List<Vertex<T>*> GetAdjacentVertices() const
00261     {
00262       List<Vertex<T>*> vertices;
00263 
00264       karto_const_forEach(typename List<Edge<T>*>, &m_Edges)
00265       {
00266         Edge<T>* pEdge = *iter;
00267 
00268         // check both source and target because we have a undirected graph
00269         if (pEdge->GetSource() != this)
00270         {
00271           vertices.Add(pEdge->GetSource());
00272         }
00273 
00274         if (pEdge->GetTarget() != this)
00275         {
00276           vertices.Add(pEdge->GetTarget());
00277         }
00278       }
00279 
00280       return vertices;
00281     }
00282 
00283   private:
00288     inline void AddEdge(Edge<T>* pEdge)
00289     {
00290       m_Edges.Add(pEdge);
00291     }
00292 
00293     T m_pObject;
00294     List<Edge<T>*> m_Edges;
00295   }; // Vertex<T>
00296 
00300 
00304   template<typename T>
00305   class Edge
00306   {
00307   public:
00313     Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
00314       : m_pSource(pSource)
00315       , m_pTarget(pTarget)
00316       , m_pLabel(NULL)
00317     {
00318       m_pSource->AddEdge(this);
00319       m_pTarget->AddEdge(this);
00320     }
00321 
00325     virtual ~Edge()
00326     {
00327       m_pSource = NULL;
00328       m_pTarget = NULL;
00329 
00330       if (m_pLabel != NULL)
00331       {
00332         delete m_pLabel;
00333         m_pLabel = NULL;
00334       }
00335     }
00336 
00337   public:
00342     inline Vertex<T>* GetSource() const
00343     {
00344       return m_pSource;
00345     }
00346 
00351     inline Vertex<T>* GetTarget() const
00352     {
00353       return m_pTarget;
00354     }
00355 
00360     inline EdgeLabel* GetLabel()
00361     {
00362       return m_pLabel;
00363     }
00364 
00369     inline void SetLabel(EdgeLabel* pLabel)
00370     {
00371       m_pLabel = pLabel;
00372     }
00373 
00374   private:
00375     Vertex<T>* m_pSource;
00376     Vertex<T>* m_pTarget;
00377     EdgeLabel* m_pLabel;
00378   }; // class Edge<T>
00379 
00383 
00387   template<typename T>
00388   class Visitor
00389   {
00390   public:
00396     virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
00397   }; // Visitor<T>
00398 
00402 
00403   template<typename T>
00404   class Graph;
00405 
00409   template<typename T>
00410   class GraphTraversal
00411   {
00412   public:
00417     GraphTraversal(Graph<T>* pGraph)
00418       : m_pGraph(pGraph)
00419     {
00420     }
00421 
00425     virtual ~GraphTraversal()
00426     {
00427     }
00428 
00429   public:
00436     virtual List<T> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
00437 
00438   protected:
00442     Graph<T>* m_pGraph;
00443   }; // GraphTraversal<T>
00444 
00448 
00452   template<typename T>
00453   class Graph
00454   {
00455   public:
00459     typedef List<Vertex<T>*> VertexList;
00460     
00464     typedef List<Edge<T>*> EdgeList;
00465 
00466   public:
00470     Graph()
00471     {
00472     }
00473 
00477     virtual ~Graph()
00478     {
00479       Clear();
00480     }
00481 
00482   public:
00487     inline void AddVertex(Vertex<T>* pVertex)
00488     {
00489       m_Vertices.Add(pVertex);
00490     }
00491 
00496     inline void AddEdge(Edge<T>* pEdge)
00497     {
00498       m_Edges.Add(pEdge);
00499     }
00500 
00504     void Clear()
00505     {
00506       karto_const_forEach(typename VertexList, &m_Vertices)
00507       {
00508         // delete each vertex
00509         delete *iter;
00510       }
00511 
00512       m_Vertices.Clear();
00513 
00514       karto_const_forEach(typename EdgeList, &m_Edges)
00515       {
00516         // delete each edge
00517         delete *iter;
00518       }
00519 
00520       m_Edges.Clear();
00521     }
00522 
00527     inline const EdgeList& GetEdges() const
00528     {
00529       return m_Edges;
00530     }
00531 
00536     inline const VertexList& GetVertices() const
00537     {
00538       return m_Vertices;
00539     }
00540 
00541   protected:
00545     VertexList m_Vertices;
00546     
00550     EdgeList m_Edges;    
00551   }; // Graph<T>
00552 
00556 
00557   class OpenMapper;
00558   class ScanMatcher;
00559 
00563   class KARTO_EXPORT MapperGraph : public Graph<LocalizedObjectPtr>
00564   {
00565   public:
00571     MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold);
00572     
00576     virtual ~MapperGraph();
00577     
00578   public:
00583     void AddVertex(LocalizedObject* pObject);
00584     
00593     Edge<LocalizedObjectPtr>* AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge);
00594 
00599     void AddEdges(LocalizedObject* pObject);
00600     
00606     void AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance);
00607     
00614     kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName);
00615     
00622     LocalizedLaserScanList FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance);
00623 
00629      LocalizedLaserScanList FindOverlappingScans(LocalizedLaserScan* pScan);
00630     
00635     inline ScanMatcher* GetLoopScanMatcher() const
00636     {
00637       return m_pLoopScanMatcher;
00638     }
00639 
00647     void LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance);
00648     
00649   private:
00655     inline Vertex<LocalizedObjectPtr>* GetVertex(LocalizedObject* pObject)
00656     {
00657       return m_Vertices[pObject->GetUniqueId()];
00658     }
00659         
00665     LocalizedLaserScan* GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const;
00666             
00674     void LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance);
00675     
00682     void LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances);
00683     
00689     List<LocalizedLaserScanList> FindNearChains(LocalizedLaserScan* pScan);
00690         
00697     Pose2 ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const;
00698     
00707     LocalizedLaserScanList FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex);
00708     
00712     void CorrectPoses();
00713     
00714   private:
00718     OpenMapper* m_pOpenMapper;
00719     
00723     ScanMatcher* m_pLoopScanMatcher;    
00724     
00728     GraphTraversal<LocalizedObjectPtr>* m_pTraversal;    
00729   }; // MapperGraph
00730 
00734   
00738   class ScanSolver : public Referenced
00739   {
00740   public:
00744     typedef List<Pair<kt_int32s, Pose2> > IdPoseVector;
00745 
00749     ScanSolver()
00750     {
00751     }
00752 
00753   protected:
00754     //@cond EXCLUDE
00758     virtual ~ScanSolver()
00759     {
00760     }
00761     //@endcond
00762 
00763   public:
00767     virtual void Compute() = 0;
00768 
00773     virtual const IdPoseVector& GetCorrections() const = 0;
00774 
00778     virtual void AddNode(Vertex<LocalizedObjectPtr>* /*pVertex*/)
00779     {
00780     }
00781 
00785     virtual void RemoveNode(kt_int32s /*id*/)
00786     {
00787     }
00788 
00792     virtual void AddConstraint(Edge<LocalizedObjectPtr>* /*pEdge*/)
00793     {
00794     }
00795     
00799     virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
00800     {
00801     }
00802     
00806     virtual void Clear() {};
00807   }; // ScanSolver
00808 
00812   
00816   class CorrelationGrid : public Grid<kt_int8u>
00817   {    
00818   protected:
00819     //@cond EXCLUDE
00823     virtual ~CorrelationGrid()
00824     {
00825       delete [] m_pKernel;
00826     }
00827     //@endcond
00828     
00829   public:
00838     static CorrelationGrid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
00839     {
00840       assert(resolution != 0.0);
00841       
00842       // +1 in case of roundoff
00843       kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
00844       
00845       CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
00846       
00847       return pGrid;
00848     }
00849         
00856     virtual kt_int32s GridIndex(const Vector2i& rGrid, kt_bool boundaryCheck = true) const
00857     {
00858       kt_int32s x = rGrid.GetX() + m_Roi.GetX();
00859       kt_int32s y = rGrid.GetY() + m_Roi.GetY();
00860       
00861       return Grid<kt_int8u>::GridIndex(Vector2i(x, y), boundaryCheck);
00862     }
00863     
00868     inline const Rectangle2<kt_int32s>& GetROI() const
00869     {
00870       return m_Roi;
00871     }
00872     
00877     inline void SetROI(const Rectangle2<kt_int32s>& roi)
00878     {
00879       m_Roi = roi;
00880     }
00881     
00886     inline void SmearPoint(const Vector2i& rGridPoint)
00887     {
00888       assert(m_pKernel != NULL);
00889       
00890       int gridIndex = GridIndex(rGridPoint);
00891       if (GetDataPointer()[gridIndex] != GridStates_Occupied)
00892       {
00893         return;
00894       }
00895       
00896       kt_int32s halfKernel = m_KernelSize / 2;
00897       
00898       // apply kernel
00899       for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00900       {
00901         kt_int8u* pGridAdr = GetDataPointer(Vector2i(rGridPoint.GetX(), rGridPoint.GetY() + j));
00902         
00903         kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
00904         
00905         // if a point is on the edge of the grid, there is no problem
00906         // with running over the edge of allowable memory, because
00907         // the grid has margins to compensate for the kernel size
00908         SmearInternal(halfKernel, kernelConstant, pGridAdr);
00909       }      
00910     }
00911     
00912   protected:
00921     CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation)
00922       : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
00923       , m_SmearDeviation(smearDeviation)
00924       , m_pKernel(NULL)
00925     {            
00926       GetCoordinateConverter()->SetScale(1.0 / resolution);
00927 
00928       // setup region of interest
00929       m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
00930       
00931       // calculate kernel
00932       CalculateKernel();
00933     }
00934     
00938     virtual void CalculateKernel()
00939     {
00940       kt_double resolution = GetResolution();
00941 
00942       assert(resolution != 0.0);
00943       assert(m_SmearDeviation != 0.0);      
00944       
00945       // min and max distance deviation for smearing;
00946       // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
00947       const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
00948       const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
00949       
00950       // check if given value too small or too big
00951       if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
00952       {
00953         StringBuilder error;
00954         error << "Mapper Error:  Smear deviation too small:  Must be between " << MIN_SMEAR_DISTANCE_DEVIATION << " and " << MAX_SMEAR_DISTANCE_DEVIATION;
00955         throw Exception(error.ToString());
00956       }
00957       
00958       // NOTE:  Currently assumes a two-dimensional kernel
00959       
00960       // +1 for center
00961       m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
00962       
00963       // allocate kernel
00964       m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
00965       if (m_pKernel == NULL)
00966       {
00967         throw Exception("Unable to allocate memory for kernel!");
00968       }
00969       
00970       // calculate kernel
00971       kt_int32s halfKernel = m_KernelSize / 2;
00972       for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
00973       {
00974         for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00975         {
00976 #ifdef WIN32
00977           kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
00978 #else
00979           kt_double distanceFromMean = hypot(i * resolution, j * resolution);
00980 #endif
00981           kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
00982           
00983           kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
00984           assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
00985           
00986           int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
00987           m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
00988         }
00989       }
00990     }
00991     
00999     static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
01000     {
01001       assert(resolution != 0.0);
01002       
01003       return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
01004     }
01005     
01006   private:
01007     // \todo 1/5/2011: was this separated from SmearPoint in preparation for optimization?
01008     inline void SmearInternal(kt_int32s halfKernel, kt_int32s kernelConstant, kt_int8u* pGridAdr)
01009     {
01010       kt_int8u kernelValue;
01011       kt_int32s kernelArrayIndex;
01012       kt_int32s i;
01013       
01014       for (i = -halfKernel; i <= halfKernel; i++)
01015       {
01016         kernelArrayIndex = i + kernelConstant;
01017         
01018         kernelValue = m_pKernel[kernelArrayIndex];
01019         if (kernelValue > pGridAdr[i])
01020         {
01021           // kernel value is greater, so set it to kernel value
01022           pGridAdr[i] = kernelValue;
01023         }
01024       }
01025     }
01026     
01031     kt_double m_SmearDeviation;
01032     
01033     // Size of one side of the kernel
01034     kt_int32s m_KernelSize;
01035     
01036     // Cached kernel for smearing
01037     kt_int8u* m_pKernel;
01038     
01039     // Region of interest
01040     Rectangle2<kt_int32s> m_Roi;
01041   }; // CorrelationGrid
01042   
01046 
01047   class ScanMatcherGridSet : public Referenced
01048   {
01049   public:
01050     ScanMatcherGridSet(CorrelationGrid* pCorrelationGrid,
01051       Grid<kt_double>* pSearchSpaceProbs,
01052       GridIndexLookup<kt_int8u>* pGridLookup)
01053       : m_pCorrelationGrid(pCorrelationGrid)
01054       , m_pSearchSpaceProbs(pSearchSpaceProbs)
01055       , m_pGridLookup(pGridLookup)
01056     {
01057     }
01058 
01059     virtual ~ScanMatcherGridSet()
01060     {
01061       delete m_pGridLookup;
01062     }
01063 
01064     SmartPointer<CorrelationGrid> m_pCorrelationGrid;
01065     SmartPointer<Grid<kt_double> > m_pSearchSpaceProbs;
01066     GridIndexLookup<kt_int8u>* m_pGridLookup;
01067   };
01068 
01072   
01073   class ScanMatcherGridSetBank;
01074   
01078   class KARTO_EXPORT ScanMatcher
01079   {
01080   public:
01084     virtual ~ScanMatcher();
01085     
01086   public:
01096     static ScanMatcher* Create(OpenMapper* pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold);
01097     
01108     kt_double MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance,
01109                         kt_bool doPenalize = true, kt_bool doRefineMatch = true);
01110     
01128     kt_double CorrelateScan(ScanMatcherGridSet* pScanMatcherGridSet, LocalizedLaserScan* pScan, const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
01129                             kt_double searchAngleOffset, kt_double searchAngleResolution,       kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch);
01130     
01142     static void ComputePositionalCovariance(Grid<kt_double>* pSearchSpaceProbs, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01143                                             const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
01144                                             kt_double searchAngleResolution, Matrix3& rCovariance);
01145     
01156     static void ComputeAngularCovariance(ScanMatcherGridSet* pScanMatcherGridSet, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01157                                          kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance);
01158 
01165     CorrelationGrid* GetCorrelationGrid() const;
01166     
01173     Grid<kt_double>* GetSearchGrid() const;
01174     
01182     static kt_double GetResponse(ScanMatcherGridSet* pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex);    
01183     
01184   private:
01191     static void AddScans(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint);
01192     static void AddScansNew(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint);
01193     
01201     static void AddScan(CorrelationGrid* pCorrelationGrid, LocalizedLaserScan* pScan, const Vector2d& rViewPoint, kt_bool doSmear = true);
01202     static void AddScanNew(CorrelationGrid* pCorrelationGrid, const Vector2dList& rValidPoints, kt_bool doSmear = true);
01203     
01210     static Vector2dList FindValidPoints(LocalizedLaserScan* pScan, const Vector2d& rViewPoint);
01211     
01212   protected:
01216     ScanMatcher(OpenMapper* pOpenMapper)
01217       : m_pOpenMapper(pOpenMapper)
01218       , m_pScanMatcherGridSet(NULL)
01219       , m_pScanMatcherGridSetBank(NULL)
01220     {
01221     }
01222     
01223   private:
01224     OpenMapper* m_pOpenMapper;
01225     
01226     SmartPointer<ScanMatcherGridSet> m_pScanMatcherGridSet;
01227     ScanMatcherGridSetBank* m_pScanMatcherGridSetBank;
01228   }; // ScanMatcher
01229   
01233 
01234   class SensorDataManager;
01235   struct MapperSensorManagerPrivate;
01236   
01240   class KARTO_EXPORT MapperSensorManager
01241   {    
01242   public:
01248     MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance);
01249     
01253     virtual ~MapperSensorManager();
01254     
01255   public:
01261     void RegisterSensor(const Identifier& rSensorName);
01262     
01269     LocalizedObject* GetLocalizedObject(const Identifier& rSensorName, kt_int32s stateId);
01270     
01275     List<Identifier> GetSensorNames();
01276     
01282     LocalizedLaserScan* GetLastScan(const Identifier& rSensorName);
01283     
01288     void SetLastScan(LocalizedLaserScan* pScan);
01289     
01294     void ClearLastScan(const Identifier& rSensorName);
01295     
01301     LocalizedObject* GetLocalizedObject(kt_int32s uniqueId);
01302     
01307     void AddLocalizedObject(LocalizedObject* pObject);
01308     
01313     void AddRunningScan(LocalizedLaserScan* pScan);
01314     
01320     LocalizedLaserScanList& GetScans(const Identifier& rSensorName);
01321     
01328     kt_int32s GetScanIndex(LocalizedLaserScan* pScan);
01329     
01335     LocalizedLaserScanList& GetRunningScans(const Identifier& rSensorName);
01336     
01341     LocalizedLaserScanList GetAllScans();
01342     
01347     LocalizedObjectList GetAllObjects();
01348 
01352     void Clear();
01353     
01354   private:
01359     inline SensorDataManager* GetSensorDataManager(LocalizedObject* pObject)
01360     {
01361       return GetSensorDataManager(pObject->GetSensorIdentifier());
01362     }
01363     
01369     SensorDataManager* GetSensorDataManager(const Identifier& rSensorName);
01370     
01371   private:
01372     MapperSensorManagerPrivate* m_pMapperSensorManagerPrivate;    
01373   }; // MapperSensorManager
01374   
01378   
01531   class KARTO_EXPORT OpenMapper : public Module
01532   {
01533     friend class MapperGraph;
01534     friend class ScanMatcher;
01535 
01536   public:
01541     OpenMapper(kt_bool multiThreaded = true);
01542 
01548     OpenMapper(const char* pName, kt_bool multiThreaded = true);
01549 
01550   public:
01556     BasicEvent<MapperEventArguments> Message;
01557 
01563     BasicEvent<MapperEventArguments> PreLoopClosed;
01564 
01568     BasicEvent<MapperEventArguments> PostLoopClosed;
01569     
01574     BasicEvent<EventArguments> ScansUpdated;
01575 
01576   protected:
01577     //@cond EXCLUDE
01581     virtual ~OpenMapper();
01582     //@endcond
01583     
01584   public:
01589     inline kt_bool IsMultiThreaded()
01590     {
01591       return m_MultiThreaded;
01592     }
01593 
01599     void Initialize(kt_double rangeThreshold);
01600     
01605     void Reset();
01606 
01614     virtual kt_bool Process(Object* pObject);
01615     
01622     const LocalizedLaserScanList GetAllProcessedScans() const;
01623 
01630     const LocalizedObjectList GetAllProcessedObjects() const;
01631 
01636     ScanSolver* GetScanSolver() const;
01637 
01642     void SetScanSolver(ScanSolver* pSolver);
01643 
01648     virtual MapperGraph* GetGraph() const;
01649 
01654     ScanMatcher* GetSequentialScanMatcher() const;
01655 
01660     ScanMatcher* GetLoopScanMatcher() const;
01661     
01666     inline MapperSensorManager* GetMapperSensorManager() const
01667     {
01668       return m_pMapperSensorManager;
01669     }
01670     
01677     inline kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName)
01678     {
01679       return m_pGraph->TryCloseLoop(pScan, rSensorName);
01680     }
01681     
01682   protected:
01687     virtual void ScanMatched(LocalizedLaserScan* pScan) {};
01688     
01693     virtual void ScanMatchingEnd(LocalizedLaserScan* pScan) {};
01694 
01695   private:
01696     void InitializeParameters();
01697 
01705     kt_bool HasMovedEnough(LocalizedLaserScan* pScan, LocalizedLaserScan* pLastScan) const;
01706 
01707   public:
01709     // fire information for listeners!!
01710 
01714     kt_bool IsInitialized()
01715     {
01716       return m_Initialized;
01717     }
01718 
01719   private:
01720     // restrict the following functions
01721     OpenMapper(const OpenMapper&);
01722     const OpenMapper& operator=(const OpenMapper&);
01723 
01724   protected:
01728     SmartPointer<ScanSolver> m_pScanSolver;
01729 
01730   private:
01731     kt_bool m_Initialized;
01732     kt_bool m_MultiThreaded;
01733 
01734     ScanMatcher* m_pSequentialScanMatcher;
01735 
01736     MapperSensorManager* m_pMapperSensorManager;
01737 
01738     MapperGraph* m_pGraph;
01739 
01741     // Parameters
01742     // NOTE: Maintain the descriptions to these parameters in the comment above the class!
01744 
01745     Parameter<kt_bool>* m_pUseScanMatching;
01746     Parameter<kt_bool>* m_pUseScanBarycenter;    
01747     Parameter<kt_double>* m_pMinimumTravelDistance;
01748     Parameter<kt_double>* m_pMinimumTravelHeading;
01749 
01751     // scan matcher parameters
01752     
01753     Parameter<kt_int32u>* m_pScanBufferSize;
01754     Parameter<kt_double>* m_pScanBufferMaximumScanDistance;
01755     Parameter<kt_bool>* m_pUseResponseExpansion;
01756 
01757     Parameter<kt_double>* m_pDistanceVariancePenalty;
01758     Parameter<kt_double>* m_pMinimumDistancePenalty;
01759     Parameter<kt_double>* m_pAngleVariancePenalty;
01760     Parameter<kt_double>* m_pMinimumAnglePenalty;
01761     
01762     Parameter<kt_double>* m_pLinkMatchMinimumResponseFine;
01763     Parameter<kt_double>* m_pLinkScanMaximumDistance;
01764 
01766     // correlation parameters
01767 
01768     Parameter<kt_double>* m_pCorrelationSearchSpaceDimension;
01769     Parameter<kt_double>* m_pCorrelationSearchSpaceResolution;
01770     Parameter<kt_double>* m_pCorrelationSearchSpaceSmearDeviation;
01771     Parameter<kt_double>* m_pCoarseSearchAngleOffset;
01772     Parameter<kt_double>* m_pFineSearchAngleOffset;
01773     Parameter<kt_double>* m_pCoarseAngleResolution;
01774 
01776     // loop correlation parameters
01777 
01778     Parameter<kt_double>* m_pLoopSearchSpaceDimension;
01779     Parameter<kt_double>* m_pLoopSearchSpaceResolution;
01780     Parameter<kt_double>* m_pLoopSearchSpaceSmearDeviation;
01781 
01783     // loop detection parameters
01784     
01785     Parameter<kt_double>* m_pLoopSearchMaximumDistance;    
01786     Parameter<kt_int32u>* m_pLoopMatchMinimumChainSize;    
01787     Parameter<kt_double>* m_pLoopMatchMaximumVarianceCoarse;    
01788     Parameter<kt_double>* m_pLoopMatchMinimumResponseCoarse;    
01789     Parameter<kt_double>* m_pLoopMatchMinimumResponseFine;
01790   };
01791 
01793 }
01794 
01795 #endif // __OpenKarto_Mapper_h__


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 04:05:24