OpenMapper.cpp
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 #ifdef USE_TBB
00019 #include <tbb/tbb_thread.h>
00020 #include <tbb/concurrent_queue.h>
00021 #endif
00022 
00023 #include <stdexcept>
00024 #include <queue>
00025 #include <set>
00026 #include <iterator>
00027 #include <map>
00028 #include <iostream>
00029 
00030 #include <OpenKarto/OpenMapper.h>
00031 #include <OpenKarto/Logger.h>
00032 
00033 namespace karto
00034 {
00035 
00036   // enable this for verbose debug information
00037   //#define KARTO_DEBUG
00038   //#define KARTO_DEBUG2
00039 
00040   #define MAX_VARIANCE            500.0
00041   #define DISTANCE_PENALTY_GAIN   0.2
00042   #define ANGLE_PENALTY_GAIN      0.2
00043 
00047 
00051   class SensorDataManager
00052   {
00053   public:
00057     SensorDataManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
00058       : m_pLastScan(NULL)
00059       , m_RunningBufferMaximumSize(runningBufferMaximumSize)
00060       , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
00061     {
00062     }
00063 
00067     virtual ~SensorDataManager()
00068     {
00069       Clear();
00070     }
00071 
00072   public:
00079     inline void AddObject(LocalizedObject* pObject, kt_int32s uniqueId)
00080     {
00081       // assign state id to object
00082       pObject->SetStateId(static_cast<kt_int32u>(m_Objects.Size()));
00083 
00084       // assign unique id to object
00085       pObject->SetUniqueId(uniqueId);
00086 
00087       m_Objects.Add(pObject);
00088       
00089       // if object is scan and it was scan-matched, add it to scan buffer
00090       LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
00091       if (pScan != NULL)
00092       {
00093         m_Scans.Add(pScan);
00094       }      
00095     }
00096 
00101     inline LocalizedLaserScan* GetLastScan()
00102     {
00103       return m_pLastScan;
00104     }
00105 
00110     inline void SetLastScan(LocalizedLaserScan* pScan)
00111     {
00112       m_pLastScan = pScan;
00113     }
00114 
00119     inline LocalizedObjectList& GetObjects()
00120     {
00121       return m_Objects;
00122     }
00123     
00128     inline LocalizedLaserScanList& GetScans()
00129     {
00130       return m_Scans;
00131     }
00132     
00138     inline kt_int32s GetScanIndex(LocalizedLaserScan* pScan)
00139     {
00140       LocalizedLaserScanPtr pSmartScan(pScan);
00141       return m_Scans.BinarySearch(pSmartScan, ScanIndexComparator);
00142     }
00143 
00148     inline LocalizedLaserScanList& GetRunningScans()
00149     {
00150       return m_RunningScans;
00151     }
00152 
00157     void AddRunningScan(LocalizedLaserScan* pScan)
00158     {
00159       m_RunningScans.Add(pScan);
00160 
00161       // list has at least one element (first line of this function), so this is valid
00162       Pose2 frontScanPose = m_RunningScans.Front()->GetSensorPose();
00163       Pose2 backScanPose = m_RunningScans.Back()->GetSensorPose();
00164 
00165       // cap list size and remove all scans from front of list that are too far from end of list
00166       kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00167       while (m_RunningScans.Size() > m_RunningBufferMaximumSize || squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE)
00168       {
00169         // remove front of running scans
00170         m_RunningScans.Remove(m_RunningScans.Front());
00171 
00172         // recompute stats of running scans
00173         frontScanPose = m_RunningScans.Front()->GetSensorPose();
00174         backScanPose = m_RunningScans.Back()->GetSensorPose();
00175         squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00176       }
00177     }
00178 
00182     void Clear()
00183     {
00184       m_Objects.Clear();
00185       m_Scans.Clear();
00186       m_RunningScans.Clear();
00187       m_pLastScan = NULL;
00188     }
00189 
00190   private:
00191     static kt_int32s ScanIndexComparator(const LocalizedLaserScanPtr& pScan1, const LocalizedLaserScanPtr& pScan2)
00192     {
00193       return pScan1->GetStateId() - pScan2->GetStateId();
00194     }
00195     
00196   private:
00197     LocalizedObjectList m_Objects;
00198     
00199     LocalizedLaserScanList m_Scans;
00200     LocalizedLaserScanList m_RunningScans;
00201     LocalizedLaserScanPtr m_pLastScan;
00202 
00203     kt_int32u m_RunningBufferMaximumSize;
00204     kt_double m_RunningBufferMaximumDistance;
00205   }; // SensorDataManager
00206 
00210 
00211   typedef std::map<Identifier, SensorDataManager*> SensorDataManagerMap;
00212 
00213   struct MapperSensorManagerPrivate
00214   {
00215     // map from sensor name to sensor data
00216     SensorDataManagerMap m_SensorDataManagers;
00217     
00218     kt_int32u m_RunningBufferMaximumSize;
00219     kt_double m_RunningBufferMaximumDistance;
00220     
00221     kt_int32s m_NextUniqueId;    
00222     
00223     LocalizedObjectList m_Objects;
00224   };
00225 
00226   MapperSensorManager::MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
00227     : m_pMapperSensorManagerPrivate(new MapperSensorManagerPrivate())
00228   {
00229     m_pMapperSensorManagerPrivate->m_RunningBufferMaximumSize = runningBufferMaximumSize;
00230     m_pMapperSensorManagerPrivate->m_RunningBufferMaximumDistance = runningBufferMaximumDistance;
00231     m_pMapperSensorManagerPrivate->m_NextUniqueId = 0;
00232   }
00233   
00234   MapperSensorManager::~MapperSensorManager()
00235   {
00236     Clear();
00237     delete m_pMapperSensorManagerPrivate;
00238   }
00239   
00240   
00241   void MapperSensorManager::RegisterSensor(const Identifier& rSensorName)
00242   {
00243     if (GetSensorDataManager(rSensorName) == NULL)
00244     {
00245       m_pMapperSensorManagerPrivate->m_SensorDataManagers[rSensorName] = new SensorDataManager(m_pMapperSensorManagerPrivate->m_RunningBufferMaximumSize, m_pMapperSensorManagerPrivate->m_RunningBufferMaximumDistance);
00246     }
00247   }
00248 
00249   LocalizedObject* MapperSensorManager::GetLocalizedObject(const Identifier& rSensorName, kt_int32s stateId)
00250   {
00251     SensorDataManager* pSensorDataManager = GetSensorDataManager(rSensorName);
00252     if (pSensorDataManager != NULL)
00253     {
00254       return pSensorDataManager->GetObjects().Get(stateId);
00255     }
00256 
00257     assert(false);
00258     return NULL;
00259   }
00260   
00261   // for use by scan solver
00262   LocalizedObject* MapperSensorManager::GetLocalizedObject(kt_int32s uniqueId)
00263   {
00264     assert(math::IsUpTo(uniqueId, (kt_int32s)m_pMapperSensorManagerPrivate->m_Objects.Size()));
00265     return m_pMapperSensorManagerPrivate->m_Objects[uniqueId];
00266   }
00267   
00268   List<Identifier> MapperSensorManager::GetSensorNames()
00269   {
00270     List<Identifier> sensorNames;
00271     const_forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00272     {
00273       sensorNames.Add(iter->first);
00274     }
00275     
00276     return sensorNames;
00277   }
00278   
00279   LocalizedLaserScan* MapperSensorManager::GetLastScan(const Identifier& rSensorName)
00280   {
00281     return GetSensorDataManager(rSensorName)->GetLastScan();
00282   }
00283   
00284   void MapperSensorManager::SetLastScan(LocalizedLaserScan* pScan)
00285   {
00286     GetSensorDataManager(pScan)->SetLastScan(pScan);
00287   }
00288   
00289   void MapperSensorManager::ClearLastScan(const Identifier& rSensorName)
00290   {
00291     GetSensorDataManager(rSensorName)->SetLastScan(NULL);
00292   }
00293 
00294   void MapperSensorManager::AddLocalizedObject(LocalizedObject* pObject)
00295   {
00296     GetSensorDataManager(pObject)->AddObject(pObject, m_pMapperSensorManagerPrivate->m_NextUniqueId);
00297     m_pMapperSensorManagerPrivate->m_Objects.Add(pObject);
00298     m_pMapperSensorManagerPrivate->m_NextUniqueId++;
00299   }
00300   
00301   void MapperSensorManager::AddRunningScan(LocalizedLaserScan* pScan)
00302   {
00303     GetSensorDataManager(pScan)->AddRunningScan(pScan);
00304   }
00305   
00306   LocalizedLaserScanList& MapperSensorManager::GetScans(const Identifier& rSensorName)
00307   {
00308     return GetSensorDataManager(rSensorName)->GetScans();
00309   }
00310   
00311   kt_int32s MapperSensorManager::GetScanIndex(LocalizedLaserScan* pScan)
00312   {
00313     return GetSensorDataManager(pScan->GetSensorIdentifier())->GetScanIndex(pScan);
00314   }
00315 
00316   LocalizedLaserScanList& MapperSensorManager::GetRunningScans(const Identifier& rSensorName)
00317   {
00318     return GetSensorDataManager(rSensorName)->GetRunningScans();
00319   }
00320   
00321   LocalizedLaserScanList MapperSensorManager::GetAllScans()
00322   {
00323     LocalizedLaserScanList scans;
00324     
00325     forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00326     {
00327       LocalizedLaserScanList& rScans = iter->second->GetScans();
00328       scans.Add(rScans);
00329     }
00330     
00331     return scans;
00332   }
00333 
00334   karto::LocalizedObjectList MapperSensorManager::GetAllObjects()
00335   {
00336     LocalizedObjectList objects;
00337 
00338     forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00339     {
00340       LocalizedObjectList& rObjects = iter->second->GetObjects();
00341       objects.Add(rObjects);
00342     }
00343 
00344     return objects;
00345   }
00346 
00347   void MapperSensorManager::Clear()
00348   {
00349     forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00350     {
00351       delete iter->second;
00352     }
00353     
00354     m_pMapperSensorManagerPrivate->m_SensorDataManagers.clear();
00355   }
00356   
00357   SensorDataManager* MapperSensorManager::GetSensorDataManager(const Identifier& rSensorName)
00358   {
00359     if (m_pMapperSensorManagerPrivate->m_SensorDataManagers.find(rSensorName) != m_pMapperSensorManagerPrivate->m_SensorDataManagers.end())
00360     {
00361       return m_pMapperSensorManagerPrivate->m_SensorDataManagers[rSensorName];
00362     }
00363     
00364     return NULL;
00365   }  
00366 
00370    
00371 #ifdef USE_TBB
00372   class ScanMatcherGridSetBank
00373   {
00374   public:
00375     ScanMatcherGridSetBank(kt_int32u nGrids, kt_int32s corrGridWidth, kt_int32s corrGridHeight, kt_double resolution, kt_double smearDeviation,
00376                         kt_int32s searchSpaceGridWidth, kt_int32s searchSpaceGridHeight)
00377     {
00378       if (nGrids <= 0)
00379       {
00380         throw Exception("ScanMatcherGridSetBank requires at least 1 grid: " + StringHelper::ToString(nGrids));
00381         assert(false);
00382       }
00383            
00384       for (kt_int32u i = 0; i < nGrids; i++)
00385       {
00386         CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(corrGridWidth, corrGridHeight, resolution, smearDeviation);
00387         Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceGridWidth, searchSpaceGridHeight, resolution);
00388         GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
00389         
00390         m_ScanMatcherGridSets.push(new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup));
00391       }
00392     }
00393     
00394     virtual ~ScanMatcherGridSetBank()
00395     {
00396       // we add a NULL item on the queue in case we are stuck in CheckOut!
00397       m_ScanMatcherGridSets.push(NULL);
00398 
00399       m_ScanMatcherGridSets.clear();
00400     }
00401     
00402   public:
00406     SmartPointer<ScanMatcherGridSet> CheckOut()
00407     {
00408       SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet = NULL;
00409 
00410       m_ScanMatcherGridSets.pop(pScanMatcherGridSet);
00411       
00412       return pScanMatcherGridSet;
00413     }
00414     
00418     void Return(SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet)
00419     {
00420       m_ScanMatcherGridSets.push(pScanMatcherGridSet);
00421     }
00422     
00423   private:
00424     tbb::concurrent_bounded_queue<SmartPointer<ScanMatcherGridSet> > m_ScanMatcherGridSets;    
00425   };
00426 #else
00427   class ScanMatcherGridSetBank
00428   {
00429   };
00430 #endif
00431   
00435   
00436   ScanMatcher::~ScanMatcher()
00437   {
00438     delete m_pScanMatcherGridSetBank;
00439   }
00440   
00441   ScanMatcher* ScanMatcher::Create(OpenMapper* pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
00442   {
00443     // invalid parameters
00444     if (resolution <= 0)
00445     {
00446       return NULL;
00447     }
00448     if (searchSize <= 0)
00449     {
00450       return NULL;
00451     }
00452     if (smearDeviation < 0)
00453     {
00454       return NULL;
00455     }
00456     if (rangeThreshold <= 0)
00457     {
00458       return NULL;
00459     }
00460     
00461     assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
00462     
00463     // calculate search space in grid coordinates
00464     kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
00465     
00466     // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
00467     // if a scan is on the border of the search space)
00468     kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
00469     
00470     kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
00471     
00472     // create correlation grid
00473     assert(gridSize % 2 == 1);
00474     CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
00475 
00476     // create search space probabilities
00477     Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize, searchSpaceSideSize, resolution);
00478     
00479     GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
00480     
00481     ScanMatcher* pScanMatcher = new ScanMatcher(pOpenMapper);
00482     pScanMatcher->m_pScanMatcherGridSet = new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup);
00483     
00484     if (pOpenMapper->IsMultiThreaded())
00485     {
00486 #ifdef USE_TBB
00487       pScanMatcher->m_pScanMatcherGridSetBank = new ScanMatcherGridSetBank(10, gridSize, gridSize, resolution, smearDeviation, searchSpaceSideSize, searchSpaceSideSize);                                                                         
00488 #else
00489       pScanMatcher->m_pScanMatcherGridSetBank = NULL;
00490 #endif
00491     }
00492 
00493     return pScanMatcher;
00494   }
00495   
00496   kt_double ScanMatcher::MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
00497   {
00498     SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet;
00499 
00500     if (m_pOpenMapper->IsMultiThreaded())
00501     {
00502 #ifdef USE_TBB
00503       pScanMatcherGridSet = m_pScanMatcherGridSetBank->CheckOut();
00504 #else
00505       pScanMatcherGridSet = m_pScanMatcherGridSet;
00506 #endif
00507     }
00508     else
00509     {
00510       pScanMatcherGridSet = m_pScanMatcherGridSet;
00511     }
00512 
00513     CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
00514     Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
00515     
00517     // set scan pose to be center of grid
00518     
00519     // 1. get scan position
00520     Pose2 scanPose = pScan->GetSensorPose();
00521     
00522     // scan has no readings; cannot do scan matching
00523     // best guess of pose is based off of adjusted odometer reading
00524     if (pScan->GetPointReadings(true).Size() == 0)
00525     {
00526       rMean = scanPose;
00527       
00528       // maximum covariance
00529       rCovariance(0, 0) = MAX_VARIANCE; // XX
00530       rCovariance(1, 1) = MAX_VARIANCE; // YY
00531       rCovariance(2, 2) = 4 * math::Square(m_pOpenMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH
00532       
00533       if (m_pOpenMapper->IsMultiThreaded())
00534       {
00535 #ifdef USE_TBB
00536         m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
00537 #endif
00538       }
00539 
00540       return 0.0;
00541     }
00542     
00543     // 2. get size of grid
00544     Rectangle2<kt_int32s> roi = pCorrelationGrid->GetROI();
00545     
00546     // 3. compute offset (in meters - lower left corner)
00547     Vector2d offset;
00548     offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * pCorrelationGrid->GetResolution()));
00549     offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * pCorrelationGrid->GetResolution()));
00550     
00551     // 4. set offset
00552     pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
00553     
00555     
00556     // set up correlation grid
00557     AddScansNew(pCorrelationGrid, rBaseScans, scanPose.GetPosition());
00558     
00559     // compute how far to search in each direction
00560     Vector2d searchDimensions(pSearchSpaceProbs->GetWidth(), pSearchSpaceProbs->GetHeight());
00561     Vector2d coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * pCorrelationGrid->GetResolution());
00562     
00563     // a coarse search only checks half the cells in each dimension
00564     Vector2d coarseSearchResolution(2 * pCorrelationGrid->GetResolution(), 2 * pCorrelationGrid->GetResolution());
00565     
00566     // actual scan-matching
00567     kt_double bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose,        coarseSearchOffset, coarseSearchResolution, m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue(), m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
00568     
00569     if (m_pOpenMapper->m_pUseResponseExpansion->GetValue() == true)
00570     {
00571       if (math::DoubleEqual(bestResponse, 0.0))
00572       {
00573 #ifdef KARTO_DEBUG
00574         std::cout << "Mapper Info: Expanding response search space!" << std::endl;
00575 #endif
00576         // try and increase search angle offset with 20 degrees and do another match
00577         kt_double newSearchAngleOffset = m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue();
00578         for (kt_int32u i = 0; i < 3; i++)
00579         {
00580           newSearchAngleOffset += math::DegreesToRadians(20);
00581           
00582           bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose,    coarseSearchOffset, coarseSearchResolution, newSearchAngleOffset, m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
00583           
00584           if (math::DoubleEqual(bestResponse, 0.0) == false)
00585           {
00586             break;
00587           }
00588         }
00589         
00590 #ifdef KARTO_DEBUG
00591         if (math::DoubleEqual(bestResponse, 0.0))
00592         {
00593           std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
00594         }
00595 #endif
00596       }
00597     }
00598     
00599     if (doRefineMatch)
00600     {
00601       Vector2d fineSearchOffset(coarseSearchResolution * 0.5);
00602       Vector2d fineSearchResolution(pCorrelationGrid->GetResolution(), pCorrelationGrid->GetResolution());
00603       bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, rMean, fineSearchOffset, fineSearchResolution, 0.5 * m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), m_pOpenMapper->m_pFineSearchAngleOffset->GetValue(), doPenalize, rMean, rCovariance, true);
00604     }
00605     
00606 #ifdef KARTO_DEBUG
00607     std::cout << "  BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ",  VARIANCE = " << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
00608 #endif
00609     
00610     assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00611 
00612     if (m_pOpenMapper->IsMultiThreaded())
00613     {
00614 #ifdef USE_TBB
00615       m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
00616 #endif
00617     }
00618     
00619     return bestResponse;
00620   }
00621   
00622 #ifdef USE_TBB
00623   class Parallel_CorrelateScan
00624   {
00625   public:
00626     Parallel_CorrelateScan(std::vector<kt_double>* pNewPositionsY, std::vector<kt_double>* pSquaresY,
00627                            std::vector<kt_double>* pNewPositionsX, std::vector<kt_double>* pSquaresX,
00628                            std::vector<kt_double>* pAngles,
00629                            std::vector<std::pair<kt_double, Pose2> >* pPoseResponses,
00630                            ScanMatcher* pScanMatcher, kt_bool doPenalize,
00631                            kt_double distanceVariancePenalty, kt_double minimumDistancePenalty,
00632                            kt_double angleVariancePenalty, kt_double minimumAnglePenalty,
00633                            kt_double searchCenterHeading, ScanMatcherGridSet* pScanMatcherGridSet)
00634       : m_pNewPositionsY(pNewPositionsY)
00635       , m_pSquaresY(pSquaresY)
00636       , m_pNewPositionsX(pNewPositionsX)
00637       , m_pSquaresX(pSquaresX)
00638       , m_pAngles(pAngles)
00639       , m_pPoseResponses(pPoseResponses)
00640       , m_pScanMatcher(pScanMatcher)
00641       , m_DoPenalize(doPenalize)
00642       , m_DistanceVariancePenalty(distanceVariancePenalty)
00643       , m_MinimumDistancePenalty(minimumDistancePenalty)
00644       , m_AngleVariancePenalty(angleVariancePenalty)
00645       , m_MinimumAnglePenalty(minimumAnglePenalty)
00646       , m_SearchCenterHeading(searchCenterHeading)
00647       , m_pScanMatcherGridSet(pScanMatcherGridSet)
00648     {
00649       m_nX = pNewPositionsX->size();
00650       m_nAngles = pAngles->size();
00651     }
00652           
00653     void operator()(const tbb::blocked_range3d<kt_int32s, kt_int32s, kt_int32s>& rRange) const
00654     {      
00655       CorrelationGrid* pCorrelationGrid = m_pScanMatcherGridSet->m_pCorrelationGrid;
00656       
00657       for (tbb::blocked_range<kt_int32s>::const_iterator yIndex = rRange.pages().begin(); yIndex != rRange.pages().end(); yIndex++)
00658       {
00659         kt_double newPositionY = m_pNewPositionsY->at(yIndex);
00660         kt_double squareY = m_pSquaresY->at(yIndex);
00661 
00662         for (tbb::blocked_range<kt_int32s>::const_iterator xIndex = rRange.rows().begin(); xIndex != rRange.rows().end(); xIndex++)
00663         {
00664           kt_double newPositionX = m_pNewPositionsX->at(xIndex);
00665           kt_double squareX = m_pSquaresX->at(xIndex);
00666 
00667           Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
00668           kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
00669           assert(gridIndex >= 0);
00670           
00671           kt_double squaredDistance = squareX + squareY;
00672           
00673           for (tbb::blocked_range<kt_int32s>::const_iterator angleIndex = rRange.cols().begin(); angleIndex != rRange.cols().end(); angleIndex++)
00674           {
00675             kt_int32u poseResponseIndex = (m_nX * m_nAngles) * yIndex + m_nAngles * xIndex + angleIndex;
00676             
00677             kt_double angle = m_pAngles->at(angleIndex);
00678             
00679             kt_double response = m_pScanMatcher->GetResponse(m_pScanMatcherGridSet, angleIndex, gridIndex);
00680             if (m_DoPenalize && (math::DoubleEqual(response, 0.0) == false))
00681             {
00682               // simple model (approximate Gaussian) to take odometry into account
00683               
00684               kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_DistanceVariancePenalty);
00685               distancePenalty = math::Maximum(distancePenalty, m_MinimumDistancePenalty);
00686               
00687               kt_double squaredAngleDistance = math::Square(angle - m_SearchCenterHeading);
00688               kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_AngleVariancePenalty);
00689               anglePenalty = math::Maximum(anglePenalty, m_MinimumAnglePenalty);
00690               
00691               response *= (distancePenalty * anglePenalty);
00692             }
00693             
00694             // store response and pose
00695             m_pPoseResponses->at(poseResponseIndex) = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
00696           }
00697         }
00698       }
00699     }
00700     
00701   private:
00702     std::vector<kt_double>* m_pNewPositionsY;
00703     std::vector<kt_double>* m_pSquaresY;
00704     std::vector<kt_double>* m_pNewPositionsX;
00705     std::vector<kt_double>* m_pSquaresX;
00706     std::vector<kt_double>* m_pAngles;
00707     std::vector<std::pair<kt_double, Pose2> >* m_pPoseResponses;
00708     ScanMatcher* m_pScanMatcher;
00709     kt_bool m_DoPenalize;
00710     kt_double m_DistanceVariancePenalty, m_MinimumDistancePenalty;
00711     kt_double m_AngleVariancePenalty, m_MinimumAnglePenalty;
00712     kt_double m_SearchCenterHeading;
00713     kt_int32u m_nX, m_nAngles;
00714     ScanMatcherGridSet* m_pScanMatcherGridSet;
00715   };
00716 #endif
00717   
00718   kt_double ScanMatcher::CorrelateScan(ScanMatcherGridSet* pScanMatcherGridSet, LocalizedLaserScan* pScan, const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
00719                                        kt_double searchAngleOffset, kt_double searchAngleResolution,    kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
00720   {
00721     assert(searchAngleResolution != 0.0);
00722 
00723     CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
00724     Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
00725     GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
00726 
00727     // setup lookup arrays
00728     pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
00729     
00730     // only initialize probability grid if computing positional covariance (during coarse match)
00731     if (!doingFineMatch)
00732     {
00733       pSearchSpaceProbs->Clear();
00734       
00735       // position search grid - finds lower left corner of search grid
00736       Vector2d offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
00737       pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
00738     }
00739     
00740     // calculate position arrays
00741     
00742     kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1);
00743     std::vector<kt_double> xPoses(nX), newPositionsX(nX), squaresX(nX);
00744     kt_double startX = -rSearchSpaceOffset.GetX();
00745     for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00746     {
00747       kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
00748       xPoses[xIndex] = x;
00749       newPositionsX[xIndex] = rSearchCenter.GetX() + x;
00750       squaresX[xIndex] = math::Square(x);
00751     }
00752     assert(math::DoubleEqual(xPoses.back(), -startX));
00753     
00754     kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1);
00755     std::vector<kt_double> yPoses(nY), newPositionsY(nY), squaresY(nY);
00756     kt_double startY = -rSearchSpaceOffset.GetY();
00757     for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00758     {
00759       kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
00760       yPoses[yIndex] = y;
00761       newPositionsY[yIndex] = rSearchCenter.GetY() + y;
00762       squaresY[yIndex] = math::Square(y);
00763     }
00764     assert(math::DoubleEqual(yPoses.back(), -startY));
00765     
00766     // calculate pose response array size
00767     kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
00768     std::vector<kt_double> angles(nAngles);
00769     kt_double angle = 0.0;
00770     kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
00771     for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00772     {
00773       angle = startAngle + angleIndex * searchAngleResolution;
00774       angles[angleIndex] = angle;
00775     }
00776     assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
00777     
00778     // allocate array
00779     kt_int32u poseResponseSize = nX * nY * nAngles;
00780     std::vector<std::pair<kt_double, Pose2> > poseResponses = std::vector<std::pair<kt_double, Pose2> >(poseResponseSize);
00781     
00782     Vector2i startGridPoint = pCorrelationGrid->WorldToGrid(Vector2d(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY));
00783     
00784     // use tbb if enabled and in multi threaded mode!
00785     kt_bool gotTbb = false;
00786     if (m_pOpenMapper->IsMultiThreaded())
00787     {
00788 #ifdef USE_TBB
00789       gotTbb = true;
00790       Parallel_CorrelateScan myTask(&newPositionsY, &squaresY, &newPositionsX, &squaresX, &angles,
00791         &poseResponses, this, doPenalize,
00792         m_pOpenMapper->m_pDistanceVariancePenalty->GetValue(),
00793         m_pOpenMapper->m_pMinimumDistancePenalty->GetValue(),
00794         m_pOpenMapper->m_pAngleVariancePenalty->GetValue(),
00795         m_pOpenMapper->m_pMinimumAnglePenalty->GetValue(),
00796         rSearchCenter.GetHeading(), pScanMatcherGridSet);
00797       int grainSizeY = 10;
00798       int grainSizeX = 10;
00799       int grainSizeAngle = 10;
00800       tbb::parallel_for(tbb::blocked_range3d<kt_int32s>(0, static_cast<kt_int32s>(nY), grainSizeY, 0, static_cast<kt_int32s>(nX), grainSizeX, 0, nAngles, grainSizeAngle), myTask);
00801 #endif
00802     }
00803 
00804     // fallback to single threaded calculation
00805     if (gotTbb == false)
00806     {
00807       kt_int32u poseResponseCounter = 0;
00808       for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00809       {
00810         kt_double newPositionY = newPositionsY[yIndex];
00811         kt_double squareY = squaresY[yIndex];
00812 
00813         for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00814         {
00815           kt_double newPositionX = newPositionsX[xIndex];
00816           kt_double squareX = squaresX[xIndex];
00817 
00818           Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
00819           kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
00820           assert(gridIndex >= 0);
00821 
00822           kt_double squaredDistance = squareX + squareY;
00823           for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00824           {
00825             kt_double angle = angles[angleIndex];
00826 
00827             kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
00828             if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
00829             {
00830               // simple model (approximate Gaussian) to take odometry into account
00831 
00832               kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_pOpenMapper->m_pDistanceVariancePenalty->GetValue());
00833               distancePenalty = math::Maximum(distancePenalty, m_pOpenMapper->m_pMinimumDistancePenalty->GetValue());
00834 
00835               kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
00836               kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_pOpenMapper->m_pAngleVariancePenalty->GetValue());
00837               anglePenalty = math::Maximum(anglePenalty, m_pOpenMapper->m_pMinimumAnglePenalty->GetValue());
00838 
00839               response *= (distancePenalty * anglePenalty);
00840             }
00841 
00842             // store response and pose
00843             poseResponses[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
00844             poseResponseCounter++;
00845           }        
00846         }
00847       }
00848 
00849       assert(poseResponseSize == poseResponseCounter);
00850     }
00851     
00852     // find value of best response (in [0; 1])
00853     kt_double bestResponse = -1;
00854     for (kt_int32u i = 0; i < poseResponseSize; i++)
00855     {
00856       bestResponse = math::Maximum(bestResponse, poseResponses[i].first);
00857       
00858       // will compute positional covariance, save best relative probability for each cell
00859       if (!doingFineMatch)
00860       {
00861         const Pose2& rPose = poseResponses[i].second;
00862         Vector2i grid = pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
00863         
00864         kt_double* ptr = (kt_double*)pSearchSpaceProbs->GetDataPointer(grid);
00865         if (ptr == NULL)
00866         {
00867           throw Exception("Mapper FATAL ERROR - Index out of range in probability search!");
00868         }
00869         
00870         *ptr = math::Maximum(poseResponses[i].first, *ptr);
00871       }
00872     }
00873     
00874     // average all poses with same highest response
00875     Vector2d averagePosition;
00876     kt_double thetaX = 0.0;
00877     kt_double thetaY = 0.0;
00878     kt_int32s averagePoseCount = 0;
00879     for (kt_int32u i = 0; i < poseResponseSize; i++)
00880     {
00881       if (math::DoubleEqual(poseResponses[i].first, bestResponse))
00882       {
00883         averagePosition += poseResponses[i].second.GetPosition();
00884         
00885         kt_double heading = poseResponses[i].second.GetHeading();
00886         thetaX += cos(heading);
00887         thetaY += sin(heading);
00888         
00889         averagePoseCount++;
00890       }
00891     }
00892     
00893     Pose2 averagePose;
00894     if (averagePoseCount > 0)
00895     {
00896       averagePosition /= averagePoseCount;
00897       
00898       thetaX /= averagePoseCount;
00899       thetaY /= averagePoseCount;
00900       
00901       averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
00902     }
00903     else
00904     {
00905       throw Exception("Mapper FATAL ERROR - Unable to find best position");
00906     }
00907     
00908 #ifdef KARTO_DEBUG
00909     std::cout << "bestPose: " << averagePose << std::endl;
00910     std::cout << "bestResponse: " << bestResponse << std::endl;
00911 #endif
00912     
00913     if (!doingFineMatch)
00914     {
00915       ComputePositionalCovariance(pSearchSpaceProbs, averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, rSearchSpaceResolution, searchAngleResolution, rCovariance);
00916     }
00917     else
00918     {
00919       ComputeAngularCovariance(pScanMatcherGridSet, averagePose, bestResponse, rSearchCenter, searchAngleOffset, searchAngleResolution, rCovariance);
00920     }
00921     
00922     rMean = averagePose;
00923     
00924 #ifdef KARTO_DEBUG
00925     std::cout << "bestPose: " << averagePose << std::endl;
00926 #endif
00927     
00928     if (bestResponse > 1.0)
00929     {
00930       bestResponse = 1.0;
00931     }
00932     
00933     assert(math::InRange(bestResponse, 0.0, 1.0));
00934     assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00935     
00936     return bestResponse;
00937   }
00938   
00939   void ScanMatcher::ComputePositionalCovariance(Grid<kt_double>* pSearchSpaceProbs, const Pose2& rBestPose, kt_double bestResponse,
00940                                                 const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset,
00941                                                 const Vector2d& rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3& rCovariance)
00942   {
00943     // reset covariance to identity matrix
00944     rCovariance.SetToIdentity();
00945     
00946     // if best response is vary small return max variance
00947     if (bestResponse < KT_TOLERANCE)
00948     {
00949       rCovariance(0, 0) = MAX_VARIANCE; // XX
00950       rCovariance(1, 1) = MAX_VARIANCE; // YY
00951       rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH
00952       
00953       return;
00954     }
00955     
00956     kt_double accumulatedVarianceXX = 0;
00957     kt_double accumulatedVarianceXY = 0;
00958     kt_double accumulatedVarianceYY = 0;
00959     kt_double norm = 0;
00960     
00961     kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
00962     kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
00963     
00964     kt_double offsetX = rSearchSpaceOffset.GetX();
00965     kt_double offsetY = rSearchSpaceOffset.GetY();
00966     
00967     kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
00968     kt_double startX = -offsetX;
00969     assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
00970     
00971     kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
00972     kt_double startY = -offsetY;
00973     assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
00974     
00975     for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00976     {
00977       kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
00978       
00979       for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00980       {
00981         kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
00982         
00983         Vector2i gridPoint = pSearchSpaceProbs->WorldToGrid(Vector2d(rSearchCenter.GetX() + x, rSearchCenter.GetY() + y));
00984         kt_double response = *(pSearchSpaceProbs->GetDataPointer(gridPoint));
00985         
00986         // response is not a low response
00987         if (response >= (bestResponse - 0.1))
00988         {
00989           norm += response;
00990           accumulatedVarianceXX += (math::Square(x - dx) * response);
00991           accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
00992           accumulatedVarianceYY += (math::Square(y - dy) * response);
00993         }
00994       }
00995     }
00996     
00997     if (norm > KT_TOLERANCE)
00998     {
00999       kt_double varianceXX = accumulatedVarianceXX / norm;
01000       kt_double varianceXY = accumulatedVarianceXY / norm;
01001       kt_double varianceYY = accumulatedVarianceYY / norm;
01002       kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
01003       
01004       // lower-bound variances so that they are not too small;
01005       // ensures that links are not too tight
01006       kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
01007       kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
01008       varianceXX = math::Maximum(varianceXX, minVarianceXX);
01009       varianceYY = math::Maximum(varianceYY, minVarianceYY);
01010       
01011       // increase variance for poorer responses
01012       kt_double multiplier = 1.0 / bestResponse;
01013       rCovariance(0, 0) = varianceXX * multiplier;
01014       rCovariance(0, 1) = varianceXY * multiplier;
01015       rCovariance(1, 0) = varianceXY * multiplier;
01016       rCovariance(1, 1) = varianceYY * multiplier;
01017       rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance
01018     }
01019     
01020     // if values are 0, set to MAX_VARIANCE
01021     // values might be 0 if points are too sparse and thus don't hit other points
01022     if (math::DoubleEqual(rCovariance(0, 0), 0.0))
01023     {
01024       rCovariance(0, 0) = MAX_VARIANCE;
01025     }
01026     
01027     if (math::DoubleEqual(rCovariance(1, 1), 0.0))
01028     {
01029       rCovariance(1, 1) = MAX_VARIANCE;
01030     }
01031   }
01032   
01033   void ScanMatcher::ComputeAngularCovariance(ScanMatcherGridSet* pScanMatcherGridSet, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01034                                              kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance)
01035   {
01036     // NOTE: do not reset covariance matrix
01037     
01038     CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
01039     
01040     // normalize angle difference
01041     kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
01042     
01043     Vector2i gridPoint = pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
01044     kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01045     
01046     kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
01047     
01048     kt_double angle = 0.0;
01049     kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
01050     
01051     kt_double norm = 0.0;
01052     kt_double accumulatedVarianceThTh = 0.0;
01053     for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
01054     {
01055       angle = startAngle + angleIndex * searchAngleResolution;
01056       kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
01057       
01058       // response is not a low response
01059       if (response >= (bestResponse - 0.1))
01060       {
01061         norm += response;
01062         accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
01063       }
01064     }
01065     assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
01066     
01067     if (norm > KT_TOLERANCE)
01068     {
01069       if (accumulatedVarianceThTh < KT_TOLERANCE)
01070       {
01071         accumulatedVarianceThTh = math::Square(searchAngleResolution);
01072       }
01073       
01074       accumulatedVarianceThTh /= norm;
01075     }
01076     else
01077     {
01078       accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
01079     }
01080     
01081     rCovariance(2, 2) = accumulatedVarianceThTh;
01082   }
01083   
01084   void ScanMatcher::AddScans(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
01085   {
01086     pCorrelationGrid->Clear();
01087     
01088     // add all scans to grid
01089     karto_const_forEach(LocalizedLaserScanList, &rScans)
01090     {
01091       AddScan(pCorrelationGrid, *iter, rViewPoint);
01092     }
01093   }
01094   
01095   void ScanMatcher::AddScansNew(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
01096   {
01097     pCorrelationGrid->Clear();
01098 
01099     kt_int32s index = 0;
01100     kt_size_t nScans = rScans.Size();
01101     Vector2dList* pValidPoints = new Vector2dList[nScans];
01102 
01103     // first find all valid points
01104 //#pragma omp parallel for
01105 //    for (kt_int32s i = 0; i < nScans; i++)
01106 //    {
01107 //      pValidPoints[i] = FindValidPoints(rScans[i], rViewPoint);
01108 //    }
01109 
01110     karto_const_forEach(LocalizedLaserScanList, &rScans)
01111     {
01112       pValidPoints[index++] = FindValidPoints(*iter, rViewPoint);
01113     }
01114 
01115     // then add all valid points to correlation grid
01116     for (kt_size_t i = 0; i < nScans; i++)
01117     {
01118       AddScanNew(pCorrelationGrid, pValidPoints[i]);
01119     }
01120 
01121     delete[] pValidPoints;
01122   }
01123 
01124   void ScanMatcher::AddScan(CorrelationGrid* pCorrelationGrid, LocalizedLaserScan* pScan, const Vector2d& rViewPoint, kt_bool doSmear)
01125   {
01126     Vector2dList validPoints = FindValidPoints(pScan, rViewPoint);
01127     
01128     // put in all valid points
01129     karto_const_forEach(Vector2dList, &validPoints)
01130     {
01131       Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
01132       if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
01133       {
01134         // point not in grid
01135         continue;
01136       }
01137       
01138       int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01139       
01140       // set grid cell as occupied
01141       if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
01142       {
01143         // value already set
01144         continue;
01145       }
01146       
01147       pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
01148       
01149       // smear grid
01150       if (doSmear == true)
01151       {
01152         pCorrelationGrid->SmearPoint(gridPoint);        
01153       }
01154     }
01155   }
01156   
01157   void ScanMatcher::AddScanNew(CorrelationGrid* pCorrelationGrid, const Vector2dList& rValidPoints, kt_bool doSmear)
01158   {
01159     // put in all valid points
01160     karto_const_forEach(Vector2dList, &rValidPoints)
01161     {
01162       Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
01163       if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
01164       {
01165         // point not in grid
01166         continue;
01167       }
01168 
01169       int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01170 
01171       // set grid cell as occupied
01172       if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
01173       {
01174         // value already set
01175         continue;
01176       }
01177 
01178       pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
01179 
01180       // smear grid
01181       if (doSmear == true)
01182       {
01183         pCorrelationGrid->SmearPoint(gridPoint);
01184       }
01185     }
01186   }
01187 
01188   Vector2dList ScanMatcher::FindValidPoints(LocalizedLaserScan* pScan, const Vector2d& rViewPoint)
01189   {
01190     const Vector2dList& rPointReadings = pScan->GetPointReadings(true);
01191     
01192     // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
01193     const kt_double minSquareDistance = math::Square(0.1); // in m^2
01194     
01195     // this iterator lags from the main iterator adding points only when the points are on
01196     // the same side as the viewpoint
01197     Vector2dList::ConstIterator trailingPointIter = rPointReadings.GetConstIterator();
01198     Vector2dList validPoints;
01199     
01200     Vector2d firstPoint;
01201     kt_bool firstTime = true;
01202     karto_const_forEach(Vector2dList, &rPointReadings)
01203     {
01204       Vector2d currentPoint = *iter;
01205       
01206       if (firstTime)
01207       {
01208         firstPoint = currentPoint;
01209         firstTime = false;
01210       }
01211       
01212       Vector2d delta = firstPoint - currentPoint;
01213       if (delta.SquaredLength() > minSquareDistance)
01214       {
01215         // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
01216         // Which computes the direction of rotation, if the rotation is counterclock
01217         // wise then we are looking at data we should keep. If it's negative rotation
01218         // we should not included in in the matching
01219         // have enough distance, check viewpoint
01220         double a = rViewPoint.GetY() - firstPoint.GetY();
01221         double b = firstPoint.GetX() - rViewPoint.GetX();
01222         double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
01223         double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
01224         
01225         // reset beginning point
01226         firstPoint = currentPoint;
01227         
01228         if (ss < 0.0)   // wrong side, skip and keep going
01229         {
01230           trailingPointIter = iter;
01231         }
01232         else
01233         {
01234           for (; trailingPointIter != iter; trailingPointIter++)
01235           {
01236             validPoints.Add(*trailingPointIter);
01237           }
01238         }
01239       }
01240     }
01241     
01242     return validPoints;
01243   }  
01244   
01245   kt_double ScanMatcher::GetResponse(ScanMatcherGridSet* pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex)
01246   {
01247     CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
01248     GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
01249     
01250     kt_double response = 0.0;
01251     
01252     // add up value for each point
01253     kt_int8u* pByte = pCorrelationGrid->GetDataPointer() + gridPositionIndex;
01254     
01255     const LookupArray* pOffsets = pGridLookup->GetLookupArray(angleIndex);
01256     assert(pOffsets != NULL);
01257     
01258     // get number of points in offset list
01259     kt_int32u nPoints = pOffsets->GetSize();
01260     if (nPoints == 0)
01261     {
01262       return response;
01263     }
01264     
01265     // calculate response
01266     kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
01267     for (kt_int32u i = 0; i < nPoints; i++)
01268     {
01269       // ignore points that fall off the grid
01270       kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
01271       if (!math::IsUpTo(pointGridIndex, pCorrelationGrid->GetDataSize()))
01272       {
01273         continue;
01274       }
01275 
01276       // uses index offsets to efficiently find location of point in the grid
01277       response += pByte[pAngleIndexPointer[i]];
01278     }
01279     
01280     // normalize response
01281     response /= (nPoints * GridStates_Occupied);
01282     assert(fabs(response) <= 1.0);
01283     
01284     return response;
01285   }
01286   
01287   CorrelationGrid* ScanMatcher::GetCorrelationGrid() const
01288   {
01289     if (m_pOpenMapper->IsMultiThreaded())
01290     {
01291       throw Exception("Correlation grid only available in single-threaded mode");
01292     }
01293     else
01294     {
01295       return m_pScanMatcherGridSet->m_pCorrelationGrid;
01296     }
01297   }
01298   
01299   Grid<kt_double>* ScanMatcher::GetSearchGrid() const
01300   {
01301     if (m_pOpenMapper->IsMultiThreaded())
01302     {
01303       throw Exception("Search grid only available in single-threaded mode");
01304     }
01305     else
01306     {
01307       return m_pScanMatcherGridSet->m_pSearchSpaceProbs;
01308     }
01309   }
01310   
01314 
01315   template<typename T>
01316   class BreadthFirstTraversal : public GraphTraversal<T>
01317   {
01318   public:
01322     BreadthFirstTraversal(Graph<T>* pGraph)
01323       : GraphTraversal<T>(pGraph)
01324     {
01325     }
01326 
01330     virtual ~BreadthFirstTraversal()
01331     {
01332     }
01333 
01334   public:
01341     virtual List<T> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
01342     {
01343       std::queue<Vertex<T>*> toVisit;
01344       std::set<Vertex<T>*> seenVertices;
01345       std::vector<Vertex<T>*> validVertices;
01346 
01347       toVisit.push(pStartVertex);
01348       seenVertices.insert(pStartVertex);
01349 
01350       do
01351       {
01352         Vertex<T>* pNext = toVisit.front();
01353         toVisit.pop();
01354 
01355         if (pVisitor->Visit(pNext))
01356         {
01357           // vertex is valid, explore neighbors
01358           validVertices.push_back(pNext);
01359 
01360           List<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
01361           karto_const_forEach(typename List<Vertex<T>*>, &adjacentVertices)
01362           {
01363             Vertex<T>* pAdjacent = *iter;
01364 
01365             // adjacent vertex has not yet been seen, add to queue for processing
01366             if (seenVertices.find(pAdjacent) == seenVertices.end())
01367             {
01368               toVisit.push(pAdjacent);
01369               seenVertices.insert(pAdjacent);
01370             }
01371           }
01372         }
01373       } while (toVisit.empty() == false);
01374 
01375       List<T> objects;
01376       forEach(typename std::vector<Vertex<T>*>, &validVertices)
01377       {
01378         objects.Add((*iter)->GetVertexObject());
01379       }
01380 
01381       return objects;
01382     }
01383 
01384   }; // class BreadthFirstTraversal
01385 
01389 
01390   class NearScanVisitor : public Visitor<LocalizedObjectPtr>
01391   {
01392   public:
01393     NearScanVisitor(LocalizedLaserScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
01394       : m_MaxDistanceSquared(math::Square(maxDistance))
01395       , m_UseScanBarycenter(useScanBarycenter)
01396     {
01397       m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
01398     }
01399 
01400     virtual kt_bool Visit(Vertex<LocalizedObjectPtr>* pVertex)
01401     {
01402       LocalizedObject* pObject = pVertex->GetVertexObject();
01403 
01404       LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01405       
01406       // object is not a scan or wasn't scan matched, ignore
01407       if (pScan == NULL)
01408       {
01409         return false;
01410       }
01411       
01412       Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
01413 
01414       kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
01415       return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
01416     }
01417 
01418   protected:
01419     Pose2 m_CenterPose;
01420     kt_double m_MaxDistanceSquared;
01421     kt_bool m_UseScanBarycenter;
01422 
01423   }; // NearScanVisitor
01424 
01428 
01429   MapperGraph::MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold)
01430     : m_pOpenMapper(pOpenMapper)
01431   {
01432     m_pLoopScanMatcher = ScanMatcher::Create(pOpenMapper, m_pOpenMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pOpenMapper->m_pLoopSearchSpaceResolution->GetValue(), m_pOpenMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold);
01433     assert(m_pLoopScanMatcher);
01434     
01435     m_pTraversal = new BreadthFirstTraversal<LocalizedObjectPtr>(this);
01436   }
01437   
01438   MapperGraph::~MapperGraph()
01439   {
01440     delete m_pLoopScanMatcher;
01441     m_pLoopScanMatcher = NULL;
01442     
01443     delete m_pTraversal;
01444     m_pTraversal = NULL;
01445   }
01446   
01447   void MapperGraph::AddVertex(LocalizedObject* pObject)
01448   {
01449     assert(pObject);
01450 
01451     if (pObject == NULL)
01452     {
01453       return;
01454     }
01455     
01456     Vertex<LocalizedObjectPtr>* pVertex = new Vertex<LocalizedObjectPtr>(pObject);
01457     Graph<LocalizedObjectPtr>::AddVertex(pVertex);
01458     if (m_pOpenMapper->m_pScanSolver != NULL)
01459     {
01460       m_pOpenMapper->m_pScanSolver->AddNode(pVertex);
01461     }
01462   }
01463 
01464   void MapperGraph::AddEdges(LocalizedObject* pObject)
01465   {
01466     // loose "spring"
01467     Matrix3 covariance;
01468     covariance(0, 0) = MAX_VARIANCE;
01469     covariance(1, 1) = MAX_VARIANCE;
01470     covariance(2, 2) = MAX_VARIANCE;
01471     
01472     LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01473     if (pScan != NULL)
01474     {      
01475       AddEdges(pScan, covariance);
01476     }
01477     else
01478     {
01479       MapperSensorManager* pSensorManager = m_pOpenMapper->m_pMapperSensorManager;      
01480       const Identifier& rSensorName = pObject->GetSensorIdentifier();
01481       
01482       LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
01483       if (pLastScan != NULL)
01484       {
01485         LinkObjects(pLastScan, pObject, pObject->GetCorrectedPose(), covariance);
01486       }
01487     }
01488   }
01489 
01490   void MapperGraph::AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance)
01491   {
01492     MapperSensorManager* pSensorManager = m_pOpenMapper->m_pMapperSensorManager;
01493     
01494     const Identifier& rSensorName = pScan->GetSensorIdentifier();
01495     
01496     Pose2List means;
01497     List<Matrix3> covariances;
01498     
01499     LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
01500     if (pLastScan == NULL)
01501     {
01502       // first scan (link to first scan of other robots)
01503 
01504       assert(pSensorManager->GetScans(rSensorName).Size() == 1);
01505       
01506       List<Identifier> sensorNames = pSensorManager->GetSensorNames();
01507       karto_const_forEach(List<Identifier>, &sensorNames)
01508       {
01509         const Identifier& rCandidateSensorName = *iter;
01510         
01511         // skip if candidate sensor is the same or other sensor has no scans
01512         if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).IsEmpty()))
01513         {
01514           continue;
01515         }
01516         
01517         Pose2 bestPose;
01518         Matrix3 covariance;
01519         kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance);
01520         LinkObjects(pSensorManager->GetScans(rCandidateSensorName)[0], pScan, bestPose, covariance);
01521         
01522         // only add to means and covariances if response was high "enough"
01523         if (response > m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue())
01524         {
01525           means.Add(bestPose);
01526           covariances.Add(covariance);
01527         }
01528       }
01529     }
01530     else
01531     {
01532       // link to previous scan
01533       LinkObjects(pLastScan, pScan, pScan->GetSensorPose(), rCovariance);
01534 
01535       // link to running scans
01536       Pose2 scanPose = pScan->GetSensorPose();
01537       means.Add(scanPose);
01538       covariances.Add(rCovariance);
01539       LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
01540     }
01541     
01542     // link to other near chains (chains that include new scan are invalid)
01543     LinkNearChains(pScan, means, covariances);
01544     
01545     if (!means.IsEmpty())
01546     {
01547       pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
01548     }
01549   }
01550   
01551   kt_bool MapperGraph::TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName)
01552   {
01553     kt_bool loopClosed = false;
01554     
01555     kt_int32u scanIndex = 0;
01556     
01557     LocalizedLaserScanList candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01558     
01559     while (!candidateChain.IsEmpty())
01560     {
01561 #ifdef KARTO_DEBUG2
01562       std::cout << "Candidate chain for " << pScan->GetStateId() << ": [ ";
01563       karto_const_forEach(LocalizedLaserScanList, &candidateChain)
01564       {
01565         std::cout << (*iter)->GetStateId() << " ";
01566       }
01567       std::cout << "]" << std::endl;
01568 #endif
01569         
01570       Pose2 bestPose;
01571       Matrix3 covariance;
01572       kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false);
01573       
01574       StringBuilder message;
01575       message << "COARSE RESPONSE: " << coarseResponse << " (> " << m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")\n";
01576       message << "            var: " << covariance(0, 0) << ",  " << covariance(1, 1) << " (< " << m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
01577        
01578       MapperEventArguments eventArguments(message.ToString());
01579       m_pOpenMapper->Message.Notify(this, eventArguments);
01580       
01581       if (((coarseResponse > m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
01582            (covariance(0, 0) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
01583            (covariance(1, 1) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
01584           ||
01585           // be more lenient if the variance is really small
01586           ((coarseResponse > 0.9 * m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
01587            (covariance(0, 0) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
01588            (covariance(1, 1) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())))
01589       {
01590         // save for reversion
01591         Pose2 oldPose = pScan->GetSensorPose();
01592         
01593         pScan->SetSensorPose(bestPose);
01594         kt_double fineResponse = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false);
01595         
01596         message.Clear();
01597         message << "FINE RESPONSE: " << fineResponse << " (>" << m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")";
01598         MapperEventArguments eventArguments(message.ToString());
01599         m_pOpenMapper->Message.Notify(this, eventArguments);
01600         
01601         if (fineResponse < m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue())
01602         {
01603           // failed verification test, revert
01604           pScan->SetSensorPose(oldPose);
01605           
01606           MapperEventArguments eventArguments("REJECTED!");
01607           m_pOpenMapper->Message.Notify(this, eventArguments);
01608         }
01609         else
01610         {
01611           MapperEventArguments eventArguments1("Closing loop...");
01612           m_pOpenMapper->PreLoopClosed.Notify(this, eventArguments1);
01613           
01614           pScan->SetSensorPose(bestPose);
01615           LinkChainToScan(candidateChain, pScan, bestPose, covariance);
01616           CorrectPoses();
01617 
01618           MapperEventArguments eventArguments2("Loop closed!");
01619           m_pOpenMapper->PostLoopClosed.Notify(this, eventArguments2);
01620           
01621           m_pOpenMapper->ScansUpdated.Notify(this, karto::EventArguments::Empty());      
01622 
01623           loopClosed = true;
01624         }
01625       }
01626       
01627       candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01628     }
01629     
01630     return loopClosed;
01631   }
01632   
01633   LocalizedLaserScan* MapperGraph::GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const
01634   {
01635     LocalizedLaserScan* pClosestScan = NULL;
01636     kt_double bestSquaredDistance = DBL_MAX;
01637     
01638     karto_const_forEach(LocalizedLaserScanList, &rScans)
01639     {
01640       Pose2 scanPose = (*iter)->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01641       
01642       kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
01643       if (squaredDistance < bestSquaredDistance)
01644       {
01645         bestSquaredDistance = squaredDistance;
01646         pClosestScan = *iter;
01647       }
01648     }
01649     
01650     return pClosestScan;
01651   }
01652     
01653   Edge<LocalizedObjectPtr>* MapperGraph::AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge)
01654   {
01655     // check that vertex exists
01656     assert(pSourceObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
01657     assert(pTargetObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
01658     
01659     Vertex<LocalizedObjectPtr>* v1 = m_Vertices[pSourceObject->GetUniqueId()];
01660     Vertex<LocalizedObjectPtr>* v2 = m_Vertices[pTargetObject->GetUniqueId()];
01661     
01662     // see if edge already exists
01663     karto_const_forEach(List<Edge<LocalizedObjectPtr>*>, &(v1->GetEdges()))
01664     {
01665       Edge<LocalizedObjectPtr>* pEdge = *iter;
01666       
01667       if (pEdge->GetTarget() == v2)
01668       {
01669         rIsNewEdge = false;
01670         return pEdge;
01671       }
01672     }
01673     
01674     Edge<LocalizedObjectPtr>* pEdge = new Edge<LocalizedObjectPtr>(v1, v2);
01675     Graph<LocalizedObjectPtr>::AddEdge(pEdge);
01676     rIsNewEdge = true;
01677     return pEdge;
01678   }
01679   
01680   void MapperGraph::LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance)
01681   {
01682     kt_bool isNewEdge = true;
01683     Edge<LocalizedObjectPtr>* pEdge = AddEdge(pFromObject, pToObject, isNewEdge);
01684     
01685     // only attach link information if the edge is new
01686     if (isNewEdge == true)
01687     {
01688       LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pFromObject);
01689       if (pScan != NULL)
01690       {
01691         pEdge->SetLabel(new LinkInfo(pScan->GetSensorPose(), rMean, rCovariance));
01692       }
01693       else
01694       {
01695         pEdge->SetLabel(new LinkInfo(pFromObject->GetCorrectedPose(), rMean, rCovariance));
01696       }
01697       if (m_pOpenMapper->m_pScanSolver != NULL)
01698       {
01699         m_pOpenMapper->m_pScanSolver->AddConstraint(pEdge);
01700       }
01701     }
01702   }
01703   
01704 #ifdef USE_TBB
01705   class Parallel_LinkNearChains
01706   {
01707   public:
01708     Parallel_LinkNearChains(OpenMapper* pMapper, LocalizedLaserScan* pScan, const List<LocalizedLaserScanList>* pChains,
01709                             kt_bool* pWasChainLinked, Pose2List* pMeans, List<Matrix3>* pCovariances,
01710                             kt_int32u minChainSize, kt_double minResponse)
01711       : m_pOpenMapper(pMapper)
01712       , m_pScan(pScan)
01713       , m_pChains(pChains)
01714       , m_pWasChainLinked(pWasChainLinked)
01715       , m_pMeans(pMeans)
01716       , m_pCovariances(pCovariances)
01717       , m_MinChainSize(minChainSize)
01718       , m_MinResponse(minResponse)
01719     {      
01720     }
01721     
01722     void operator()(const tbb::blocked_range<kt_int32s>& rRange) const
01723     {
01724       for (kt_int32s i = rRange.begin(); i != rRange.end(); i++)
01725       {
01726         m_pWasChainLinked[i] = false;
01727 
01728         const LocalizedLaserScanList& rChain = m_pChains->Get(i);
01729         
01730         if (rChain.Size() < m_MinChainSize)
01731         {
01732           continue;
01733         }
01734 
01735         Pose2 mean;
01736         Matrix3 covariance;
01737         
01738         // match scan against "near" chain
01739         kt_double response = m_pOpenMapper->GetSequentialScanMatcher()->MatchScan(m_pScan, rChain, mean, covariance, false);
01740         if (response > m_MinResponse - KT_TOLERANCE)
01741         {
01742           m_pWasChainLinked[i] = true;
01743           m_pMeans->Set(i, mean);
01744           m_pCovariances->Set(i, covariance);
01745         }
01746       }
01747     }
01748 
01749   private:
01750     OpenMapper* m_pOpenMapper;
01751     LocalizedLaserScan* m_pScan;
01752     const List<LocalizedLaserScanList>* m_pChains;
01753     kt_bool* m_pWasChainLinked;
01754     Pose2List* m_pMeans;
01755     List<Matrix3>* m_pCovariances;
01756     kt_int32u m_MinChainSize;
01757     kt_double m_MinResponse;
01758   };
01759 #endif
01760   
01761   void MapperGraph::LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances)
01762   {
01763     const List<LocalizedLaserScanList> nearChains = FindNearChains(pScan);
01764 
01765     kt_bool gotTbb = false;
01766     if (m_pOpenMapper->IsMultiThreaded())
01767     {
01768 #ifdef USE_TBB
01769       gotTbb = true;
01770       kt_bool* pWasChainLinked = new kt_bool[nearChains.Size()];
01771 
01772       Pose2List means;
01773       means.Resize(nearChains.Size());
01774 
01775       List<Matrix3> covariances;
01776       covariances.Resize(nearChains.Size());
01777 
01778       int grainSize = 100;
01779       Parallel_LinkNearChains myTask(m_pOpenMapper, pScan, &nearChains, pWasChainLinked, &means, &covariances,
01780         m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue(),
01781         m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue());
01782       tbb::parallel_for(tbb::blocked_range<kt_int32s>(0, static_cast<kt_int32s>(nearChains.Size()), grainSize), myTask);
01783 
01784       for (kt_int32u i = 0; i < nearChains.Size(); i++)
01785       {
01786         if (pWasChainLinked[i] == true)
01787         {
01788           rMeans.Add(means[i]);
01789           rCovariances.Add(covariances[i]);
01790           LinkChainToScan(nearChains[i], pScan, means[i], covariances[i]);
01791         }
01792       }
01793 
01794       delete [] pWasChainLinked;
01795 #endif
01796     }
01797 
01798     if (gotTbb == false)
01799     {
01800       karto_const_forEach(List<LocalizedLaserScanList>, &nearChains)
01801       {
01802 #ifdef KARTO_DEBUG2
01803         std::cout << "Near chain for " << pScan->GetStateId() << ": [ ";
01804         karto_const_forEachAs(LocalizedLaserScanList, iter, iter2)
01805         {
01806           std::cout << (*iter2)->GetStateId() << " ";
01807         }
01808         std::cout << "]: ";
01809 #endif
01810 
01811         if (iter->Size() < m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue())
01812         {
01813 #ifdef KARTO_DEBUG2
01814           std::cout << iter->Size() << "(< " << m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue() << ") REJECTED" << std::endl;
01815 #endif
01816           continue;
01817         }
01818 
01819         Pose2 mean;
01820         Matrix3 covariance;
01821         // match scan against "near" chain
01822         kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
01823         if (response > m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE)
01824         {
01825 #ifdef KARTO_DEBUG2
01826           std::cout << " ACCEPTED" << std::endl;
01827 #endif
01828           rMeans.Add(mean);
01829           rCovariances.Add(covariance);
01830           LinkChainToScan(*iter, pScan, mean, covariance);
01831         }
01832         else
01833         {
01834 #ifdef KARTO_DEBUG2
01835           std::cout << response << "(< " << m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue() << ") REJECTED" << std::endl;
01836 #endif        
01837         }
01838       }
01839     }
01840   }
01841   
01842   void MapperGraph::LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance)
01843   {
01844     Pose2 pose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01845 
01846     LocalizedLaserScan* pClosestScan = GetClosestScanToPose(rChain, pose);
01847     assert(pClosestScan != NULL);
01848 
01849     Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01850 
01851     kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
01852     if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01853     {
01854       LinkObjects(pClosestScan, pScan, rMean, rCovariance);
01855 #ifdef KARTO_DEBUG2
01856       std::cout << "Linking scan " << pScan->GetStateId() << " to chain scan " << pClosestScan->GetStateId() << std::endl;
01857 #endif
01858     }
01859   }
01860   
01861   List<LocalizedLaserScanList> MapperGraph::FindNearChains(LocalizedLaserScan* pScan)
01862   {
01863     List<LocalizedLaserScanList> nearChains;
01864     
01865     Pose2 scanPose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01866     
01867     // to keep track of which scans have been added to a chain
01868     LocalizedLaserScanList processed;
01869     
01870     const LocalizedLaserScanList nearLinkedScans = FindNearLinkedScans(pScan, m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue());
01871     karto_const_forEach(LocalizedLaserScanList, &nearLinkedScans)
01872     {
01873       LocalizedLaserScan* pNearScan = *iter;
01874       
01875       if (pNearScan == pScan)
01876       {
01877         continue;
01878       }
01879       
01880       // scan has already been processed, skip
01881       if (processed.Contains(pNearScan) == true)
01882       {
01883         continue;
01884       }
01885       
01886 #ifdef KARTO_DEBUG2
01887       std::cout << "BUILDING CHAIN: Scan " << pScan->GetStateId() << " is near " << pNearScan->GetStateId() << " (< " << m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue() << ")" << std::endl;
01888 #endif
01889       
01890       processed.Add(pNearScan);
01891       
01892       // build up chain
01893       kt_bool isValidChain = true;
01894       LocalizedLaserScanList chain;
01895 
01896       LocalizedLaserScanList scans = m_pOpenMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorIdentifier());
01897       
01898       kt_int32s nearScanIndex = m_pOpenMapper->m_pMapperSensorManager->GetScanIndex(pNearScan);
01899       assert(nearScanIndex >= 0);
01900       
01901       // add scans before current scan being processed
01902       for (kt_int32s candidateScanIndex = nearScanIndex - 1; candidateScanIndex >= 0; candidateScanIndex--)
01903       {
01904         LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
01905         
01906         // chain is invalid--contains scan being added
01907         if (pCandidateScan == pScan)
01908         {
01909 #ifdef KARTO_DEBUG2
01910           std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
01911 #endif
01912           isValidChain = false;
01913         }
01914         
01915         Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01916         kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01917         
01918         if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01919         {
01920           chain.Add(pCandidateScan);
01921           processed.Add(pCandidateScan);
01922           
01923 #ifdef KARTO_DEBUG2
01924           std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
01925           karto_const_forEachAs(LocalizedLaserScanList, &chain, iter2)
01926           {
01927             std::cout << (*iter2)->GetStateId() << " ";
01928           }
01929           std::cout << "]" << std::endl;
01930 #endif          
01931         }
01932         else
01933         {
01934           break;
01935         }
01936       }
01937       
01938       chain.Add(pNearScan);
01939       
01940       // add scans after current scan being processed
01941       kt_size_t end = scans.Size();
01942       for (kt_size_t candidateScanIndex = nearScanIndex + 1; candidateScanIndex < end; candidateScanIndex++)
01943       {
01944         LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
01945         
01946         if (pCandidateScan == pScan)
01947         {
01948 #ifdef KARTO_DEBUG2
01949           std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
01950 #endif          
01951           isValidChain = false;
01952         }
01953         
01954         Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());;
01955         kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01956         
01957         if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01958         {
01959           chain.Add(pCandidateScan);
01960           processed.Add(pCandidateScan);
01961           
01962 #ifdef KARTO_DEBUG2
01963           std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
01964           karto_const_forEachAs(LocalizedLaserScanList, &chain, iter2)
01965           {
01966             std::cout << (*iter2)->GetStateId() << " ";
01967           }
01968           std::cout << "]" << std::endl;
01969 #endif                    
01970         }
01971         else
01972         {
01973           break;
01974         }
01975       }
01976       
01977       if (isValidChain)
01978       {
01979         // add chain to collection
01980         nearChains.Add(chain);
01981       }
01982     }
01983     
01984     return nearChains;
01985   }
01986   
01987   LocalizedLaserScanList MapperGraph::FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance)
01988   {
01989     NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01990     LocalizedObjectList nearLinkedObjects = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
01991     delete pVisitor;
01992     
01993     LocalizedLaserScanList nearLinkedScans;
01994     karto_const_forEach(LocalizedObjectList, &nearLinkedObjects)
01995     {
01996       LocalizedObject* pObject = *iter;
01997       LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01998       if (pScan != NULL)
01999       {
02000         nearLinkedScans.Add(pScan);
02001       }
02002     }
02003     
02004     return nearLinkedScans;
02005   }
02006   
02007   LocalizedLaserScanList MapperGraph::FindOverlappingScans(karto::LocalizedLaserScan *pScan)
02008   {
02009     LocalizedLaserScanList nearScans;
02010 
02011     const BoundingBox2& rBoundingBox = pScan->GetBoundingBox();
02012     
02013     const VertexList& vertices = GetVertices();
02014     karto_const_forEach(VertexList, &vertices)
02015     {
02016       LocalizedObject* pObject = (*iter)->GetVertexObject();
02017       LocalizedLaserScan* pCandidateScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02018       if (pCandidateScan == NULL)
02019       {
02020         continue;
02021       }
02022       
02023       if (rBoundingBox.Intersects(pCandidateScan->GetBoundingBox()) == true)
02024       {
02025         nearScans.Add(pCandidateScan);
02026       }
02027     }
02028     
02029     return nearScans;
02030   }
02031 
02032   
02033   Pose2 MapperGraph::ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const
02034   {
02035     assert(rMeans.Size() == rCovariances.Size());
02036     
02037     // compute sum of inverses and create inverse list
02038     List<Matrix3> inverses;
02039     inverses.EnsureCapacity(rCovariances.Size());
02040     
02041     Matrix3 sumOfInverses;
02042     karto_const_forEach(List<Matrix3>, &rCovariances)
02043     {
02044       Matrix3 inverse = iter->Inverse();
02045       inverses.Add(inverse);
02046       
02047       sumOfInverses += inverse;
02048     }
02049     Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
02050     
02051     // compute weighted mean
02052     Pose2 accumulatedPose;
02053     kt_double thetaX = 0.0;
02054     kt_double thetaY = 0.0;
02055     
02056     Pose2List::ConstIterator meansIter = rMeans.GetConstIterator();
02057     karto_const_forEach(List<Matrix3>, &inverses)
02058     {
02059       Pose2 pose = *meansIter;
02060       kt_double angle = pose.GetHeading();
02061       thetaX += cos(angle);
02062       thetaY += sin(angle);
02063       
02064       Matrix3 weight = inverseOfSumOfInverses * (*iter);
02065       accumulatedPose += weight * pose;
02066       
02067       meansIter++;
02068     }
02069     
02070     thetaX /= rMeans.Size();
02071     thetaY /= rMeans.Size();
02072     accumulatedPose.SetHeading(atan2(thetaY, thetaX));
02073     
02074     return accumulatedPose;
02075   }
02076   
02077   LocalizedLaserScanList MapperGraph::FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex)
02078   {
02079     LocalizedLaserScanList chain; // return value
02080     
02081     Pose2 pose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
02082     
02083     // possible loop closure chain should not include close scans that have a
02084     // path of links to the scan of interest
02085     const LocalizedLaserScanList nearLinkedScans = FindNearLinkedScans(pScan, m_pOpenMapper->m_pLoopSearchMaximumDistance->GetValue());
02086     
02087     LocalizedLaserScanList scans = m_pOpenMapper->m_pMapperSensorManager->GetScans(rSensorName);
02088     kt_size_t nScans = scans.Size();
02089     for (; rStartScanIndex < nScans; rStartScanIndex++)
02090     {
02091       LocalizedLaserScan* pCandidateScan = scans[rStartScanIndex];
02092       
02093       Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
02094       
02095       kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
02096       if (squaredDistance < math::Square(m_pOpenMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
02097       {
02098         // a linked scan cannot be in the chain
02099         if (nearLinkedScans.Contains(pCandidateScan) == true)
02100         {
02101           chain.Clear();
02102         }
02103         else
02104         {
02105           chain.Add(pCandidateScan);
02106         }
02107       }
02108       else
02109       {
02110         // return chain if it is long "enough"
02111         if (chain.Size() >= m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue())
02112         {
02113           return chain;
02114         }
02115         else
02116         {
02117           chain.Clear();
02118         }
02119       }
02120     }
02121     
02122     return chain;
02123   }
02124   
02125   void MapperGraph::CorrectPoses()
02126   {
02127     // optimize scans!
02128     ScanSolver* pSolver = m_pOpenMapper->m_pScanSolver;
02129     if (pSolver != NULL)
02130     {
02131       pSolver->Compute();
02132       
02133       karto_const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
02134       {
02135         LocalizedObject* pObject = m_pOpenMapper->m_pMapperSensorManager->GetLocalizedObject(iter->GetFirst());
02136         LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02137         
02138         if (pScan != NULL)
02139         {
02140           pScan->SetSensorPose(iter->GetSecond());
02141         }
02142         else
02143         {
02144           pObject->SetCorrectedPose(iter->GetSecond());
02145         }
02146       }
02147       
02148       pSolver->Clear();
02149     }
02150   }
02151 
02155 
02159   OpenMapper::OpenMapper(kt_bool multiThreaded)
02160     : Module("OpenMapper")
02161     , m_pScanSolver(NULL)
02162     , m_Initialized(false)
02163     , m_MultiThreaded(multiThreaded)
02164     , m_pSequentialScanMatcher(NULL)
02165     , m_pMapperSensorManager(NULL)
02166     , m_pGraph(NULL)
02167   {
02168     InitializeParameters();
02169   }
02170 
02174   OpenMapper::OpenMapper(const char* pName, kt_bool multiThreaded)
02175     : Module(pName)
02176     , m_pScanSolver(NULL)
02177     , m_Initialized(false)
02178     , m_MultiThreaded(multiThreaded)
02179     , m_pSequentialScanMatcher(NULL)
02180     , m_pMapperSensorManager(NULL)
02181     , m_pGraph(NULL)
02182   {
02183     InitializeParameters();
02184   }
02185 
02189   OpenMapper::~OpenMapper()
02190   {
02191     Reset();
02192 
02193     delete m_pMapperSensorManager;
02194   }
02195 
02196   void OpenMapper::InitializeParameters()
02197   {
02198     m_pUseScanMatching = new Parameter<kt_bool>(GetParameterSet(), "UseScanMatching", "Mapper::Use::Scan Matching", "UseScanMatching", true);
02199     m_pUseScanBarycenter = new Parameter<kt_bool>(GetParameterSet(), "UseScanBarycenter", "Mapper::Use::Scan Barycenter", "UseScanBarycenter", true);
02200     m_pMinimumTravelDistance = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelDistance", "Mapper::Minimum Travel::Distance", "MinimumTravelDistance", 0.2);
02201     m_pMinimumTravelHeading = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelHeading", "Mapper::Minimum Travel::Heading", "MinimumTravelHeading", math::DegreesToRadians(20));
02202 
02203     m_pScanBufferSize = new Parameter<kt_int32u>(GetParameterSet(), "ScanBufferSize", "Mapper::Scan Buffer::Size", "ScanBufferSize", 70);
02204     m_pScanBufferMaximumScanDistance = new Parameter<kt_double>(GetParameterSet(), "ScanBufferMaximumScanDistance", "Mapper::Scan Buffer::Maximum Scan Distance", "ScanBufferMaximumScanDistance", 20);
02205     m_pUseResponseExpansion = new Parameter<kt_bool>(GetParameterSet(), "UseResponseExpansion", "Mapper::Use::Response Expansion", "UseResponseExpansion", false);
02206 
02207     m_pDistanceVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "DistanceVariancePenalty", "Mapper::Scan Matcher::Distance Variance Penalty", "DistanceVariancePenalty",  math::Square(0.3));
02208     m_pMinimumDistancePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumDistancePenalty", "Mapper::Scan Matcher::Minimum Distance Penalty", "MinimumDistancePenalty", 0.5);
02209     m_pAngleVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "AngleVariancePenalty", "Mapper::Scan Matcher::Angle Variance Penalty", "AngleVariancePenalty", math::Square(math::DegreesToRadians(20)));
02210     m_pMinimumAnglePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumAnglePenalty", "Mapper::Scan Matcher::Minimum Angle Penalty", "MinimumAnglePenalty", 0.9);
02211     
02212     m_pLinkMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LinkMatchMinimumResponseFine", "Mapper::Link::Match Minimum Response Fine", "LinkMatchMinimumResponseFine", 0.6);
02213     m_pLinkScanMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LinkScanMaximumDistance", "Mapper::Link::Scan Maximum Distance", "LinkScanMaximumDistance", 5.0);
02214 
02215     m_pCorrelationSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceDimension", "Mapper::Correlation Search Space::Dimension", "CorrelationSearchSpaceDimension", 0.3);
02216     m_pCorrelationSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceResolution", "Mapper::Correlation Search Space::Resolution", "CorrelationSearchSpaceResolution", 0.01);
02217     m_pCorrelationSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceSmearDeviation", "Mapper::Correlation Search Space::Smear Deviation", "CorrelationSearchSpaceSmearDeviation", 0.03);
02218     m_pCoarseSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "CoarseSearchAngleOffset", "Mapper::Scan Matcher::Coarse Search Angle Offset", "CoarseSearchAngleOffset", math::DegreesToRadians(20));
02219     m_pFineSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "FineSearchAngleOffset", "Mapper::Scan Matcher::Fine Search Angle Offset", "FineSearchAngleOffset", math::DegreesToRadians(0.2));
02220     m_pCoarseAngleResolution = new Parameter<kt_double>(GetParameterSet(), "CoarseAngleResolution", "Mapper::Scan Matcher::Coarse Angle Resolution", "CoarseAngleResolution", math::DegreesToRadians(2));
02221     
02222     m_pLoopSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceDimension", "Mapper::Loop Correlation Search Space::Dimension", "LoopSearchSpaceDimension", 8.0);
02223     m_pLoopSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceResolution", "Mapper::Loop Correlation Search Space::Resolution", "LoopSearchSpaceResolution", 0.05);
02224     m_pLoopSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceSmearDeviation", "Mapper::Loop Correlation Search Space::Smear Deviation", "LoopSearchSpaceSmearDeviation", 0.03);
02225 
02226     m_pLoopSearchMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LoopSearchMaximumDistance", "Mapper::Loop::Search Maximum Distance", "LoopSearchMaximumDistance", 4.0);
02227     m_pLoopMatchMinimumChainSize = new Parameter<kt_int32u>(GetParameterSet(), "LoopMatchMinimumChainSize", "Mapper::Loop::Match::Minimum Chain Size", "LoopMatchMinimumChainSize", 10);
02228     m_pLoopMatchMaximumVarianceCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMaximumVarianceCoarse", "Mapper::Loop::Match::Maximum Variance Coarse", "LoopMatchMaximumVarianceCoarse", math::Square(0.4));
02229     m_pLoopMatchMinimumResponseCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseCoarse", "Mapper::Loop::Match::Minimum Response Coarse", "LoopMatchMinimumResponseCoarse", 0.7);
02230     m_pLoopMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseFine", "Mapper::Loop::Match::Minimum Response Fine", "LoopMatchMinimumResponseFine", 0.7);
02231   }
02232 
02233   void OpenMapper::Initialize(kt_double rangeThreshold)
02234   {
02235     if (m_Initialized == false)
02236     {
02237       // create sequential scan and loop matcher
02238       m_pSequentialScanMatcher = ScanMatcher::Create(this, m_pCorrelationSearchSpaceDimension->GetValue(), m_pCorrelationSearchSpaceResolution->GetValue(), m_pCorrelationSearchSpaceSmearDeviation->GetValue(), rangeThreshold);
02239       assert(m_pSequentialScanMatcher);
02240 
02241       m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), m_pScanBufferMaximumScanDistance->GetValue());
02242 
02243       m_pGraph = new MapperGraph(this, rangeThreshold);
02244 
02245       m_Initialized = true;
02246     }
02247     else
02248     {
02249       Log(LOG_WARNING, "Mapper already initialized");
02250     }
02251   }
02252 
02253   void OpenMapper::Reset()
02254   {
02255     Module::Reset();
02256 
02257     delete m_pSequentialScanMatcher;
02258     m_pSequentialScanMatcher = NULL;
02259 
02260     delete m_pGraph;
02261     m_pGraph = NULL;
02262 
02263     delete m_pMapperSensorManager;
02264     m_pMapperSensorManager = NULL;
02265 
02266     m_Initialized = false;
02267   }
02268 
02269   kt_bool OpenMapper::Process(Object* pObject)
02270   {
02271     if (pObject == NULL)
02272     {
02273       return false;
02274     }
02275     
02276     kt_bool isObjectProcessed = Module::Process(pObject);
02277 
02278     if (IsLaserRangeFinder(pObject))
02279     {
02280       LaserRangeFinder* pLaserRangeFinder = dynamic_cast<LaserRangeFinder*>(pObject);
02281  
02282       if (m_Initialized == false)
02283       {
02284         // initialize mapper with range threshold from sensor
02285         Initialize(pLaserRangeFinder->GetRangeThreshold());
02286       }
02287       
02288       // register sensor
02289       m_pMapperSensorManager->RegisterSensor(pLaserRangeFinder->GetIdentifier());
02290       
02291       return true;
02292     }
02293     
02294     LocalizedObject* pLocalizedObject = dynamic_cast<LocalizedObject*>(pObject);
02295     if (pLocalizedObject != NULL)
02296     {
02297       LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02298       if (pScan != NULL)
02299       {
02300         karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
02301         
02302         // validate scan
02303         if (pLaserRangeFinder == NULL)
02304         {
02305           return false;
02306         }
02307         
02308         // validate scan. Throws exception if scan is invalid.
02309         pLaserRangeFinder->Validate(pScan);
02310         
02311         if (m_Initialized == false)
02312         {
02313           // initialize mapper with range threshold from sensor
02314           Initialize(pLaserRangeFinder->GetRangeThreshold());
02315         }
02316       }
02317 
02318       // ensures sensor has been registered with mapper--does nothing if the sensor has already been registered
02319       m_pMapperSensorManager->RegisterSensor(pLocalizedObject->GetSensorIdentifier());
02320 
02321       // get last scan
02322       LocalizedLaserScan* pLastScan = m_pMapperSensorManager->GetLastScan(pLocalizedObject->GetSensorIdentifier());
02323       
02324       // update scans corrected pose based on last correction
02325       if (pLastScan != NULL)
02326       {
02327         Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
02328         pLocalizedObject->SetCorrectedPose(lastTransform.TransformPose(pLocalizedObject->GetOdometricPose()));
02329       }
02330       
02331       // check custom data if object is not a scan or if scan has not moved enough (i.e.,
02332       // scan is outside minimum boundary or if heading is larger then minimum heading)
02333       if (pScan == NULL || (!HasMovedEnough(pScan, pLastScan) && !pScan->IsGpsReadingValid()))
02334       {
02335         if (pLocalizedObject->HasCustomItem() == true)
02336         {
02337           m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
02338           
02339           // add to graph
02340           m_pGraph->AddVertex(pLocalizedObject);
02341           m_pGraph->AddEdges(pLocalizedObject);
02342           
02343           return true;
02344         }
02345         
02346         return false;
02347       }
02348       
02350       // object is a scan
02351       
02352       Matrix3 covariance;
02353       covariance.SetToIdentity();
02354       
02355       // correct scan (if not first scan)
02356       if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
02357       {
02358         Pose2 bestPose;
02359         m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorIdentifier()), bestPose, covariance);
02360         pScan->SetSensorPose(bestPose);
02361       }
02362       
02363       ScanMatched(pScan);
02364       
02365       // add scan to buffer and assign id
02366       m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
02367       
02368       if (m_pUseScanMatching->GetValue())
02369       {
02370         // add to graph
02371         m_pGraph->AddVertex(pScan);
02372         m_pGraph->AddEdges(pScan, covariance);
02373         
02374         m_pMapperSensorManager->AddRunningScan(pScan);
02375         
02376         List<Identifier> sensorNames = m_pMapperSensorManager->GetSensorNames();
02377         karto_const_forEach(List<Identifier>, &sensorNames)
02378         {
02379           m_pGraph->TryCloseLoop(pScan, *iter);
02380         }      
02381       }
02382       
02383       m_pMapperSensorManager->SetLastScan(pScan);
02384       
02385       ScanMatchingEnd(pScan);
02386       
02387       isObjectProcessed = true;
02388     } // if object is LocalizedObject
02389     
02390     return isObjectProcessed;
02391   }
02392 
02393   kt_bool OpenMapper::HasMovedEnough(LocalizedLaserScan* pScan, LocalizedLaserScan* pLastScan) const
02394   {
02395     // test if first scan
02396     if (pLastScan == NULL)
02397     {
02398       return true;
02399     }
02400     
02401     Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
02402     Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
02403 
02404     // test if we have turned enough
02405     kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
02406     if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
02407     {
02408       return true;
02409     }
02410 
02411     // test if we have moved enough
02412     kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
02413     if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
02414     {
02415       return true;
02416     }
02417 
02418     return false;
02419   }
02420 
02421   const LocalizedLaserScanList OpenMapper::GetAllProcessedScans() const
02422   {
02423     LocalizedLaserScanList allScans;
02424 
02425     if (m_pMapperSensorManager != NULL)
02426     {
02427       allScans = m_pMapperSensorManager->GetAllScans();
02428     }
02429 
02430     return allScans;
02431   }
02432 
02433   const LocalizedObjectList OpenMapper::GetAllProcessedObjects() const
02434   {
02435     LocalizedObjectList allObjects;
02436 
02437     if (m_pMapperSensorManager != NULL)
02438     {
02439       // BUGBUG: inefficient?  should return right away?
02440       allObjects = m_pMapperSensorManager->GetAllObjects();
02441     }
02442 
02443     return allObjects;
02444   }
02445 
02446   ScanSolver* OpenMapper::GetScanSolver() const
02447   {
02448     return m_pScanSolver;
02449   }
02450 
02451   void OpenMapper::SetScanSolver(ScanSolver* pScanOptimizer)
02452   {
02453     m_pScanSolver = pScanOptimizer;
02454   }
02455 
02456   MapperGraph* OpenMapper::GetGraph() const
02457   {
02458     return m_pGraph;
02459   }
02460 
02461   ScanMatcher* OpenMapper::GetSequentialScanMatcher() const
02462   {
02463     return m_pSequentialScanMatcher;
02464   }
02465 
02466   ScanMatcher* OpenMapper::GetLoopScanMatcher() const
02467   {
02468     return m_pGraph->GetLoopScanMatcher();
02469   }  
02470 
02471 }


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