OpenMapper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #ifdef USE_TBB
19 #include <tbb/tbb_thread.h>
20 #include <tbb/concurrent_queue.h>
21 #endif
22 
23 #include <stdexcept>
24 #include <queue>
25 #include <set>
26 #include <iterator>
27 #include <map>
28 #include <iostream>
29 
30 #include <OpenKarto/OpenMapper.h>
31 #include <OpenKarto/Logger.h>
32 
33 namespace karto
34 {
35 
36  // enable this for verbose debug information
37  //#define KARTO_DEBUG
38  //#define KARTO_DEBUG2
39 
40  #define MAX_VARIANCE 500.0
41  #define DISTANCE_PENALTY_GAIN 0.2
42  #define ANGLE_PENALTY_GAIN 0.2
43 
47 
52  {
53  public:
57  SensorDataManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
58  : m_pLastScan(NULL)
59  , m_RunningBufferMaximumSize(runningBufferMaximumSize)
60  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
61  {
62  }
63 
68  {
69  Clear();
70  }
71 
72  public:
79  inline void AddObject(LocalizedObject* pObject, kt_int32s uniqueId)
80  {
81  // assign state id to object
82  pObject->SetStateId(static_cast<kt_int32u>(m_Objects.Size()));
83 
84  // assign unique id to object
85  pObject->SetUniqueId(uniqueId);
86 
87  m_Objects.Add(pObject);
88 
89  // if object is scan and it was scan-matched, add it to scan buffer
90  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
91  if (pScan != NULL)
92  {
93  m_Scans.Add(pScan);
94  }
95  }
96 
102  {
103  return m_pLastScan;
104  }
105 
110  inline void SetLastScan(LocalizedLaserScan* pScan)
111  {
112  m_pLastScan = pScan;
113  }
114 
120  {
121  return m_Objects;
122  }
123 
129  {
130  return m_Scans;
131  }
132 
139  {
140  LocalizedLaserScanPtr pSmartScan(pScan);
141  return m_Scans.BinarySearch(pSmartScan, ScanIndexComparator);
142  }
143 
149  {
150  return m_RunningScans;
151  }
152 
158  {
159  m_RunningScans.Add(pScan);
160 
161  // list has at least one element (first line of this function), so this is valid
162  Pose2 frontScanPose = m_RunningScans.Front()->GetSensorPose();
163  Pose2 backScanPose = m_RunningScans.Back()->GetSensorPose();
164 
165  // cap list size and remove all scans from front of list that are too far from end of list
166  kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
168  {
169  // remove front of running scans
171 
172  // recompute stats of running scans
173  frontScanPose = m_RunningScans.Front()->GetSensorPose();
174  backScanPose = m_RunningScans.Back()->GetSensorPose();
175  squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
176  }
177  }
178 
182  void Clear()
183  {
184  m_Objects.Clear();
185  m_Scans.Clear();
187  m_pLastScan = NULL;
188  }
189 
190  private:
192  {
193  return pScan1->GetStateId() - pScan2->GetStateId();
194  }
195 
196  private:
198 
202 
205  }; // SensorDataManager
206 
210 
211  typedef std::map<Identifier, SensorDataManager*> SensorDataManagerMap;
212 
214  {
215  // map from sensor name to sensor data
216  SensorDataManagerMap m_SensorDataManagers;
217 
220 
222 
224  };
225 
226  MapperSensorManager::MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
227  : m_pMapperSensorManagerPrivate(new MapperSensorManagerPrivate())
228  {
229  m_pMapperSensorManagerPrivate->m_RunningBufferMaximumSize = runningBufferMaximumSize;
230  m_pMapperSensorManagerPrivate->m_RunningBufferMaximumDistance = runningBufferMaximumDistance;
232  }
233 
235  {
236  Clear();
238  }
239 
240 
242  {
243  if (GetSensorDataManager(rSensorName) == NULL)
244  {
246  }
247  }
248 
250  {
251  SensorDataManager* pSensorDataManager = GetSensorDataManager(rSensorName);
252  if (pSensorDataManager != NULL)
253  {
254  return pSensorDataManager->GetObjects().Get(stateId);
255  }
256 
257  assert(false);
258  return NULL;
259  }
260 
261  // for use by scan solver
263  {
265  return m_pMapperSensorManagerPrivate->m_Objects[uniqueId];
266  }
267 
269  {
270  List<Identifier> sensorNames;
272  {
273  sensorNames.Add(iter->first);
274  }
275 
276  return sensorNames;
277  }
278 
280  {
281  return GetSensorDataManager(rSensorName)->GetLastScan();
282  }
283 
285  {
286  GetSensorDataManager(pScan)->SetLastScan(pScan);
287  }
288 
290  {
291  GetSensorDataManager(rSensorName)->SetLastScan(NULL);
292  }
293 
295  {
299  }
300 
302  {
303  GetSensorDataManager(pScan)->AddRunningScan(pScan);
304  }
305 
307  {
308  return GetSensorDataManager(rSensorName)->GetScans();
309  }
310 
312  {
313  return GetSensorDataManager(pScan->GetSensorIdentifier())->GetScanIndex(pScan);
314  }
315 
317  {
318  return GetSensorDataManager(rSensorName)->GetRunningScans();
319  }
320 
322  {
324 
326  {
327  LocalizedLaserScanList& rScans = iter->second->GetScans();
328  scans.Add(rScans);
329  }
330 
331  return scans;
332  }
333 
335  {
336  LocalizedObjectList objects;
337 
339  {
340  LocalizedObjectList& rObjects = iter->second->GetObjects();
341  objects.Add(rObjects);
342  }
343 
344  return objects;
345  }
346 
348  {
350  {
351  delete iter->second;
352  }
353 
355  }
356 
358  {
360  {
362  }
363 
364  return NULL;
365  }
366 
370 
371 #ifdef USE_TBB
373  {
374  public:
375  ScanMatcherGridSetBank(kt_int32u nGrids, kt_int32s corrGridWidth, kt_int32s corrGridHeight, kt_double resolution, kt_double smearDeviation,
376  kt_int32s searchSpaceGridWidth, kt_int32s searchSpaceGridHeight)
377  {
378  if (nGrids <= 0)
379  {
380  throw Exception("ScanMatcherGridSetBank requires at least 1 grid: " + StringHelper::ToString(nGrids));
381  assert(false);
382  }
383 
384  for (kt_int32u i = 0; i < nGrids; i++)
385  {
386  CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(corrGridWidth, corrGridHeight, resolution, smearDeviation);
387  Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceGridWidth, searchSpaceGridHeight, resolution);
388  GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
389 
390  m_ScanMatcherGridSets.push(new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup));
391  }
392  }
393 
394  virtual ~ScanMatcherGridSetBank()
395  {
396  // we add a NULL item on the queue in case we are stuck in CheckOut!
397  m_ScanMatcherGridSets.push(NULL);
398 
399  m_ScanMatcherGridSets.clear();
400  }
401 
402  public:
407  {
408  SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet = NULL;
409 
410  m_ScanMatcherGridSets.pop(pScanMatcherGridSet);
411 
412  return pScanMatcherGridSet;
413  }
414 
418  void Return(SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet)
419  {
420  m_ScanMatcherGridSets.push(pScanMatcherGridSet);
421  }
422 
423  private:
424  tbb::concurrent_bounded_queue<SmartPointer<ScanMatcherGridSet> > m_ScanMatcherGridSets;
425  };
426 #else
428  {
429  };
430 #endif
431 
435 
437  {
438  delete m_pScanMatcherGridSetBank;
439  }
440 
441  ScanMatcher* ScanMatcher::Create(OpenMapper* pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
442  {
443  // invalid parameters
444  if (resolution <= 0)
445  {
446  return NULL;
447  }
448  if (searchSize <= 0)
449  {
450  return NULL;
451  }
452  if (smearDeviation < 0)
453  {
454  return NULL;
455  }
456  if (rangeThreshold <= 0)
457  {
458  return NULL;
459  }
460 
461  assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
462 
463  // calculate search space in grid coordinates
464  kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
465 
466  // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
467  // if a scan is on the border of the search space)
468  kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
469 
470  kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
471 
472  // create correlation grid
473  assert(gridSize % 2 == 1);
474  CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
475 
476  // create search space probabilities
477  Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize, searchSpaceSideSize, resolution);
478 
479  GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
480 
481  ScanMatcher* pScanMatcher = new ScanMatcher(pOpenMapper);
482  pScanMatcher->m_pScanMatcherGridSet = new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup);
483 
484  if (pOpenMapper->IsMultiThreaded())
485  {
486 #ifdef USE_TBB
487  pScanMatcher->m_pScanMatcherGridSetBank = new ScanMatcherGridSetBank(10, gridSize, gridSize, resolution, smearDeviation, searchSpaceSideSize, searchSpaceSideSize);
488 #else
489  pScanMatcher->m_pScanMatcherGridSetBank = NULL;
490 #endif
491  }
492 
493  return pScanMatcher;
494  }
495 
496  kt_double ScanMatcher::MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
497  {
498  SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet;
499 
500  if (m_pOpenMapper->IsMultiThreaded())
501  {
502 #ifdef USE_TBB
503  pScanMatcherGridSet = m_pScanMatcherGridSetBank->CheckOut();
504 #else
505  pScanMatcherGridSet = m_pScanMatcherGridSet;
506 #endif
507  }
508  else
509  {
510  pScanMatcherGridSet = m_pScanMatcherGridSet;
511  }
512 
513  CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
514  Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
515 
517  // set scan pose to be center of grid
518 
519  // 1. get scan position
520  Pose2 scanPose = pScan->GetSensorPose();
521 
522  // scan has no readings; cannot do scan matching
523  // best guess of pose is based off of adjusted odometer reading
524  if (pScan->GetPointReadings(true).Size() == 0)
525  {
526  rMean = scanPose;
527 
528  // maximum covariance
529  rCovariance(0, 0) = MAX_VARIANCE; // XX
530  rCovariance(1, 1) = MAX_VARIANCE; // YY
531  rCovariance(2, 2) = 4 * math::Square(m_pOpenMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH
532 
533  if (m_pOpenMapper->IsMultiThreaded())
534  {
535 #ifdef USE_TBB
536  m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
537 #endif
538  }
539 
540  return 0.0;
541  }
542 
543  // 2. get size of grid
544  Rectangle2<kt_int32s> roi = pCorrelationGrid->GetROI();
545 
546  // 3. compute offset (in meters - lower left corner)
547  Vector2d offset;
548  offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * pCorrelationGrid->GetResolution()));
549  offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * pCorrelationGrid->GetResolution()));
550 
551  // 4. set offset
552  pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
553 
555 
556  // set up correlation grid
557  AddScansNew(pCorrelationGrid, rBaseScans, scanPose.GetPosition());
558 
559  // compute how far to search in each direction
560  Vector2d searchDimensions(pSearchSpaceProbs->GetWidth(), pSearchSpaceProbs->GetHeight());
561  Vector2d coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * pCorrelationGrid->GetResolution());
562 
563  // a coarse search only checks half the cells in each dimension
564  Vector2d coarseSearchResolution(2 * pCorrelationGrid->GetResolution(), 2 * pCorrelationGrid->GetResolution());
565 
566  // actual scan-matching
567  kt_double bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose, coarseSearchOffset, coarseSearchResolution, m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue(), m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
568 
569  if (m_pOpenMapper->m_pUseResponseExpansion->GetValue() == true)
570  {
571  if (math::DoubleEqual(bestResponse, 0.0))
572  {
573 #ifdef KARTO_DEBUG
574  std::cout << "Mapper Info: Expanding response search space!" << std::endl;
575 #endif
576  // try and increase search angle offset with 20 degrees and do another match
577  kt_double newSearchAngleOffset = m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue();
578  for (kt_int32u i = 0; i < 3; i++)
579  {
580  newSearchAngleOffset += math::DegreesToRadians(20);
581 
582  bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose, coarseSearchOffset, coarseSearchResolution, newSearchAngleOffset, m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
583 
584  if (math::DoubleEqual(bestResponse, 0.0) == false)
585  {
586  break;
587  }
588  }
589 
590 #ifdef KARTO_DEBUG
591  if (math::DoubleEqual(bestResponse, 0.0))
592  {
593  std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
594  }
595 #endif
596  }
597  }
598 
599  if (doRefineMatch)
600  {
601  Vector2d fineSearchOffset(coarseSearchResolution * 0.5);
602  Vector2d fineSearchResolution(pCorrelationGrid->GetResolution(), pCorrelationGrid->GetResolution());
603  bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, rMean, fineSearchOffset, fineSearchResolution, 0.5 * m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), m_pOpenMapper->m_pFineSearchAngleOffset->GetValue(), doPenalize, rMean, rCovariance, true);
604  }
605 
606 #ifdef KARTO_DEBUG
607  std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
608 #endif
609 
610  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
611 
612  if (m_pOpenMapper->IsMultiThreaded())
613  {
614 #ifdef USE_TBB
615  m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
616 #endif
617  }
618 
619  return bestResponse;
620  }
621 
622 #ifdef USE_TBB
623  class Parallel_CorrelateScan
624  {
625  public:
626  Parallel_CorrelateScan(std::vector<kt_double>* pNewPositionsY, std::vector<kt_double>* pSquaresY,
627  std::vector<kt_double>* pNewPositionsX, std::vector<kt_double>* pSquaresX,
628  std::vector<kt_double>* pAngles,
629  std::vector<std::pair<kt_double, Pose2> >* pPoseResponses,
630  ScanMatcher* pScanMatcher, kt_bool doPenalize,
631  kt_double distanceVariancePenalty, kt_double minimumDistancePenalty,
632  kt_double angleVariancePenalty, kt_double minimumAnglePenalty,
633  kt_double searchCenterHeading, ScanMatcherGridSet* pScanMatcherGridSet)
634  : m_pNewPositionsY(pNewPositionsY)
635  , m_pSquaresY(pSquaresY)
636  , m_pNewPositionsX(pNewPositionsX)
637  , m_pSquaresX(pSquaresX)
638  , m_pAngles(pAngles)
639  , m_pPoseResponses(pPoseResponses)
640  , m_pScanMatcher(pScanMatcher)
641  , m_DoPenalize(doPenalize)
642  , m_DistanceVariancePenalty(distanceVariancePenalty)
643  , m_MinimumDistancePenalty(minimumDistancePenalty)
644  , m_AngleVariancePenalty(angleVariancePenalty)
645  , m_MinimumAnglePenalty(minimumAnglePenalty)
646  , m_SearchCenterHeading(searchCenterHeading)
647  , m_pScanMatcherGridSet(pScanMatcherGridSet)
648  {
649  m_nX = pNewPositionsX->size();
650  m_nAngles = pAngles->size();
651  }
652 
653  void operator()(const tbb::blocked_range3d<kt_int32s, kt_int32s, kt_int32s>& rRange) const
654  {
655  CorrelationGrid* pCorrelationGrid = m_pScanMatcherGridSet->m_pCorrelationGrid;
656 
657  for (tbb::blocked_range<kt_int32s>::const_iterator yIndex = rRange.pages().begin(); yIndex != rRange.pages().end(); yIndex++)
658  {
659  kt_double newPositionY = m_pNewPositionsY->at(yIndex);
660  kt_double squareY = m_pSquaresY->at(yIndex);
661 
662  for (tbb::blocked_range<kt_int32s>::const_iterator xIndex = rRange.rows().begin(); xIndex != rRange.rows().end(); xIndex++)
663  {
664  kt_double newPositionX = m_pNewPositionsX->at(xIndex);
665  kt_double squareX = m_pSquaresX->at(xIndex);
666 
667  Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
668  kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
669  assert(gridIndex >= 0);
670 
671  kt_double squaredDistance = squareX + squareY;
672 
673  for (tbb::blocked_range<kt_int32s>::const_iterator angleIndex = rRange.cols().begin(); angleIndex != rRange.cols().end(); angleIndex++)
674  {
675  kt_int32u poseResponseIndex = (m_nX * m_nAngles) * yIndex + m_nAngles * xIndex + angleIndex;
676 
677  kt_double angle = m_pAngles->at(angleIndex);
678 
679  kt_double response = m_pScanMatcher->GetResponse(m_pScanMatcherGridSet, angleIndex, gridIndex);
680  if (m_DoPenalize && (math::DoubleEqual(response, 0.0) == false))
681  {
682  // simple model (approximate Gaussian) to take odometry into account
683 
684  kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_DistanceVariancePenalty);
685  distancePenalty = math::Maximum(distancePenalty, m_MinimumDistancePenalty);
686 
687  kt_double squaredAngleDistance = math::Square(angle - m_SearchCenterHeading);
688  kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_AngleVariancePenalty);
689  anglePenalty = math::Maximum(anglePenalty, m_MinimumAnglePenalty);
690 
691  response *= (distancePenalty * anglePenalty);
692  }
693 
694  // store response and pose
695  m_pPoseResponses->at(poseResponseIndex) = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
696  }
697  }
698  }
699  }
700 
701  private:
702  std::vector<kt_double>* m_pNewPositionsY;
703  std::vector<kt_double>* m_pSquaresY;
704  std::vector<kt_double>* m_pNewPositionsX;
705  std::vector<kt_double>* m_pSquaresX;
706  std::vector<kt_double>* m_pAngles;
707  std::vector<std::pair<kt_double, Pose2> >* m_pPoseResponses;
708  ScanMatcher* m_pScanMatcher;
709  kt_bool m_DoPenalize;
710  kt_double m_DistanceVariancePenalty, m_MinimumDistancePenalty;
711  kt_double m_AngleVariancePenalty, m_MinimumAnglePenalty;
712  kt_double m_SearchCenterHeading;
713  kt_int32u m_nX, m_nAngles;
714  ScanMatcherGridSet* m_pScanMatcherGridSet;
715  };
716 #endif
717 
718  kt_double ScanMatcher::CorrelateScan(ScanMatcherGridSet* pScanMatcherGridSet, LocalizedLaserScan* pScan, const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
719  kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
720  {
721  assert(searchAngleResolution != 0.0);
722 
723  CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
724  Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
725  GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
726 
727  // setup lookup arrays
728  pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
729 
730  // only initialize probability grid if computing positional covariance (during coarse match)
731  if (!doingFineMatch)
732  {
733  pSearchSpaceProbs->Clear();
734 
735  // position search grid - finds lower left corner of search grid
736  Vector2d offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
737  pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
738  }
739 
740  // calculate position arrays
741 
742  kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1);
743  std::vector<kt_double> xPoses(nX), newPositionsX(nX), squaresX(nX);
744  kt_double startX = -rSearchSpaceOffset.GetX();
745  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
746  {
747  kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
748  xPoses[xIndex] = x;
749  newPositionsX[xIndex] = rSearchCenter.GetX() + x;
750  squaresX[xIndex] = math::Square(x);
751  }
752  assert(math::DoubleEqual(xPoses.back(), -startX));
753 
754  kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1);
755  std::vector<kt_double> yPoses(nY), newPositionsY(nY), squaresY(nY);
756  kt_double startY = -rSearchSpaceOffset.GetY();
757  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
758  {
759  kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
760  yPoses[yIndex] = y;
761  newPositionsY[yIndex] = rSearchCenter.GetY() + y;
762  squaresY[yIndex] = math::Square(y);
763  }
764  assert(math::DoubleEqual(yPoses.back(), -startY));
765 
766  // calculate pose response array size
767  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
768  std::vector<kt_double> angles(nAngles);
769  kt_double angle = 0.0;
770  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
771  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
772  {
773  angle = startAngle + angleIndex * searchAngleResolution;
774  angles[angleIndex] = angle;
775  }
776  assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
777 
778  // allocate array
779  kt_int32u poseResponseSize = nX * nY * nAngles;
780  std::vector<std::pair<kt_double, Pose2> > poseResponses = std::vector<std::pair<kt_double, Pose2> >(poseResponseSize);
781 
782  Vector2i startGridPoint = pCorrelationGrid->WorldToGrid(Vector2d(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY));
783 
784  // use tbb if enabled and in multi threaded mode!
785  kt_bool gotTbb = false;
786  if (m_pOpenMapper->IsMultiThreaded())
787  {
788 #ifdef USE_TBB
789  gotTbb = true;
790  Parallel_CorrelateScan myTask(&newPositionsY, &squaresY, &newPositionsX, &squaresX, &angles,
791  &poseResponses, this, doPenalize,
792  m_pOpenMapper->m_pDistanceVariancePenalty->GetValue(),
793  m_pOpenMapper->m_pMinimumDistancePenalty->GetValue(),
794  m_pOpenMapper->m_pAngleVariancePenalty->GetValue(),
795  m_pOpenMapper->m_pMinimumAnglePenalty->GetValue(),
796  rSearchCenter.GetHeading(), pScanMatcherGridSet);
797  int grainSizeY = 10;
798  int grainSizeX = 10;
799  int grainSizeAngle = 10;
800  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);
801 #endif
802  }
803 
804  // fallback to single threaded calculation
805  if (gotTbb == false)
806  {
807  kt_int32u poseResponseCounter = 0;
808  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
809  {
810  kt_double newPositionY = newPositionsY[yIndex];
811  kt_double squareY = squaresY[yIndex];
812 
813  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
814  {
815  kt_double newPositionX = newPositionsX[xIndex];
816  kt_double squareX = squaresX[xIndex];
817 
818  Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
819  kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
820  assert(gridIndex >= 0);
821 
822  kt_double squaredDistance = squareX + squareY;
823  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
824  {
825  kt_double angle = angles[angleIndex];
826 
827  kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
828  if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
829  {
830  // simple model (approximate Gaussian) to take odometry into account
831 
832  kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_pOpenMapper->m_pDistanceVariancePenalty->GetValue());
833  distancePenalty = math::Maximum(distancePenalty, m_pOpenMapper->m_pMinimumDistancePenalty->GetValue());
834 
835  kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
836  kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_pOpenMapper->m_pAngleVariancePenalty->GetValue());
837  anglePenalty = math::Maximum(anglePenalty, m_pOpenMapper->m_pMinimumAnglePenalty->GetValue());
838 
839  response *= (distancePenalty * anglePenalty);
840  }
841 
842  // store response and pose
843  poseResponses[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
844  poseResponseCounter++;
845  }
846  }
847  }
848 
849  assert(poseResponseSize == poseResponseCounter);
850  }
851 
852  // find value of best response (in [0; 1])
853  kt_double bestResponse = -1;
854  for (kt_int32u i = 0; i < poseResponseSize; i++)
855  {
856  bestResponse = math::Maximum(bestResponse, poseResponses[i].first);
857 
858  // will compute positional covariance, save best relative probability for each cell
859  if (!doingFineMatch)
860  {
861  const Pose2& rPose = poseResponses[i].second;
862  Vector2i grid = pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
863 
864  kt_double* ptr = (kt_double*)pSearchSpaceProbs->GetDataPointer(grid);
865  if (ptr == NULL)
866  {
867  throw Exception("Mapper FATAL ERROR - Index out of range in probability search!");
868  }
869 
870  *ptr = math::Maximum(poseResponses[i].first, *ptr);
871  }
872  }
873 
874  // average all poses with same highest response
875  Vector2d averagePosition;
876  kt_double thetaX = 0.0;
877  kt_double thetaY = 0.0;
878  kt_int32s averagePoseCount = 0;
879  for (kt_int32u i = 0; i < poseResponseSize; i++)
880  {
881  if (math::DoubleEqual(poseResponses[i].first, bestResponse))
882  {
883  averagePosition += poseResponses[i].second.GetPosition();
884 
885  kt_double heading = poseResponses[i].second.GetHeading();
886  thetaX += cos(heading);
887  thetaY += sin(heading);
888 
889  averagePoseCount++;
890  }
891  }
892 
893  Pose2 averagePose;
894  if (averagePoseCount > 0)
895  {
896  averagePosition /= averagePoseCount;
897 
898  thetaX /= averagePoseCount;
899  thetaY /= averagePoseCount;
900 
901  averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
902  }
903  else
904  {
905  throw Exception("Mapper FATAL ERROR - Unable to find best position");
906  }
907 
908 #ifdef KARTO_DEBUG
909  std::cout << "bestPose: " << averagePose << std::endl;
910  std::cout << "bestResponse: " << bestResponse << std::endl;
911 #endif
912 
913  if (!doingFineMatch)
914  {
915  ComputePositionalCovariance(pSearchSpaceProbs, averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, rSearchSpaceResolution, searchAngleResolution, rCovariance);
916  }
917  else
918  {
919  ComputeAngularCovariance(pScanMatcherGridSet, averagePose, bestResponse, rSearchCenter, searchAngleOffset, searchAngleResolution, rCovariance);
920  }
921 
922  rMean = averagePose;
923 
924 #ifdef KARTO_DEBUG
925  std::cout << "bestPose: " << averagePose << std::endl;
926 #endif
927 
928  if (bestResponse > 1.0)
929  {
930  bestResponse = 1.0;
931  }
932 
933  assert(math::InRange(bestResponse, 0.0, 1.0));
934  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
935 
936  return bestResponse;
937  }
938 
939  void ScanMatcher::ComputePositionalCovariance(Grid<kt_double>* pSearchSpaceProbs, const Pose2& rBestPose, kt_double bestResponse,
940  const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset,
941  const Vector2d& rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3& rCovariance)
942  {
943  // reset covariance to identity matrix
944  rCovariance.SetToIdentity();
945 
946  // if best response is vary small return max variance
947  if (bestResponse < KT_TOLERANCE)
948  {
949  rCovariance(0, 0) = MAX_VARIANCE; // XX
950  rCovariance(1, 1) = MAX_VARIANCE; // YY
951  rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH
952 
953  return;
954  }
955 
956  kt_double accumulatedVarianceXX = 0;
957  kt_double accumulatedVarianceXY = 0;
958  kt_double accumulatedVarianceYY = 0;
959  kt_double norm = 0;
960 
961  kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
962  kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
963 
964  kt_double offsetX = rSearchSpaceOffset.GetX();
965  kt_double offsetY = rSearchSpaceOffset.GetY();
966 
967  kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
968  kt_double startX = -offsetX;
969  assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
970 
971  kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
972  kt_double startY = -offsetY;
973  assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
974 
975  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
976  {
977  kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
978 
979  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
980  {
981  kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
982 
983  Vector2i gridPoint = pSearchSpaceProbs->WorldToGrid(Vector2d(rSearchCenter.GetX() + x, rSearchCenter.GetY() + y));
984  kt_double response = *(pSearchSpaceProbs->GetDataPointer(gridPoint));
985 
986  // response is not a low response
987  if (response >= (bestResponse - 0.1))
988  {
989  norm += response;
990  accumulatedVarianceXX += (math::Square(x - dx) * response);
991  accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
992  accumulatedVarianceYY += (math::Square(y - dy) * response);
993  }
994  }
995  }
996 
997  if (norm > KT_TOLERANCE)
998  {
999  kt_double varianceXX = accumulatedVarianceXX / norm;
1000  kt_double varianceXY = accumulatedVarianceXY / norm;
1001  kt_double varianceYY = accumulatedVarianceYY / norm;
1002  kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
1003 
1004  // lower-bound variances so that they are not too small;
1005  // ensures that links are not too tight
1006  kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
1007  kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
1008  varianceXX = math::Maximum(varianceXX, minVarianceXX);
1009  varianceYY = math::Maximum(varianceYY, minVarianceYY);
1010 
1011  // increase variance for poorer responses
1012  kt_double multiplier = 1.0 / bestResponse;
1013  rCovariance(0, 0) = varianceXX * multiplier;
1014  rCovariance(0, 1) = varianceXY * multiplier;
1015  rCovariance(1, 0) = varianceXY * multiplier;
1016  rCovariance(1, 1) = varianceYY * multiplier;
1017  rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance
1018  }
1019 
1020  // if values are 0, set to MAX_VARIANCE
1021  // values might be 0 if points are too sparse and thus don't hit other points
1022  if (math::DoubleEqual(rCovariance(0, 0), 0.0))
1023  {
1024  rCovariance(0, 0) = MAX_VARIANCE;
1025  }
1026 
1027  if (math::DoubleEqual(rCovariance(1, 1), 0.0))
1028  {
1029  rCovariance(1, 1) = MAX_VARIANCE;
1030  }
1031  }
1032 
1033  void ScanMatcher::ComputeAngularCovariance(ScanMatcherGridSet* pScanMatcherGridSet, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
1034  kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance)
1035  {
1036  // NOTE: do not reset covariance matrix
1037 
1038  CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
1039 
1040  // normalize angle difference
1041  kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
1042 
1043  Vector2i gridPoint = pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
1044  kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
1045 
1046  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
1047 
1048  kt_double angle = 0.0;
1049  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
1050 
1051  kt_double norm = 0.0;
1052  kt_double accumulatedVarianceThTh = 0.0;
1053  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
1054  {
1055  angle = startAngle + angleIndex * searchAngleResolution;
1056  kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
1057 
1058  // response is not a low response
1059  if (response >= (bestResponse - 0.1))
1060  {
1061  norm += response;
1062  accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
1063  }
1064  }
1065  assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
1066 
1067  if (norm > KT_TOLERANCE)
1068  {
1069  if (accumulatedVarianceThTh < KT_TOLERANCE)
1070  {
1071  accumulatedVarianceThTh = math::Square(searchAngleResolution);
1072  }
1073 
1074  accumulatedVarianceThTh /= norm;
1075  }
1076  else
1077  {
1078  accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
1079  }
1080 
1081  rCovariance(2, 2) = accumulatedVarianceThTh;
1082  }
1083 
1084  void ScanMatcher::AddScans(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
1085  {
1086  pCorrelationGrid->Clear();
1087 
1088  // add all scans to grid
1090  {
1091  AddScan(pCorrelationGrid, *iter, rViewPoint);
1092  }
1093  }
1094 
1095  void ScanMatcher::AddScansNew(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
1096  {
1097  pCorrelationGrid->Clear();
1098 
1099  kt_int32s index = 0;
1100  kt_size_t nScans = rScans.Size();
1101  Vector2dList* pValidPoints = new Vector2dList[nScans];
1102 
1103  // first find all valid points
1104 //#pragma omp parallel for
1105 // for (kt_int32s i = 0; i < nScans; i++)
1106 // {
1107 // pValidPoints[i] = FindValidPoints(rScans[i], rViewPoint);
1108 // }
1109 
1111  {
1112  pValidPoints[index++] = FindValidPoints(*iter, rViewPoint);
1113  }
1114 
1115  // then add all valid points to correlation grid
1116  for (kt_size_t i = 0; i < nScans; i++)
1117  {
1118  AddScanNew(pCorrelationGrid, pValidPoints[i]);
1119  }
1120 
1121  delete[] pValidPoints;
1122  }
1123 
1124  void ScanMatcher::AddScan(CorrelationGrid* pCorrelationGrid, LocalizedLaserScan* pScan, const Vector2d& rViewPoint, kt_bool doSmear)
1125  {
1126  Vector2dList validPoints = FindValidPoints(pScan, rViewPoint);
1127 
1128  // put in all valid points
1129  karto_const_forEach(Vector2dList, &validPoints)
1130  {
1131  Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
1132  if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
1133  {
1134  // point not in grid
1135  continue;
1136  }
1137 
1138  int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
1139 
1140  // set grid cell as occupied
1141  if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
1142  {
1143  // value already set
1144  continue;
1145  }
1146 
1147  pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
1148 
1149  // smear grid
1150  if (doSmear == true)
1151  {
1152  pCorrelationGrid->SmearPoint(gridPoint);
1153  }
1154  }
1155  }
1156 
1157  void ScanMatcher::AddScanNew(CorrelationGrid* pCorrelationGrid, const Vector2dList& rValidPoints, kt_bool doSmear)
1158  {
1159  // put in all valid points
1160  karto_const_forEach(Vector2dList, &rValidPoints)
1161  {
1162  Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
1163  if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
1164  {
1165  // point not in grid
1166  continue;
1167  }
1168 
1169  int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
1170 
1171  // set grid cell as occupied
1172  if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
1173  {
1174  // value already set
1175  continue;
1176  }
1177 
1178  pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
1179 
1180  // smear grid
1181  if (doSmear == true)
1182  {
1183  pCorrelationGrid->SmearPoint(gridPoint);
1184  }
1185  }
1186  }
1187 
1189  {
1190  const Vector2dList& rPointReadings = pScan->GetPointReadings(true);
1191 
1192  // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
1193  const kt_double minSquareDistance = math::Square(0.1); // in m^2
1194 
1195  // this iterator lags from the main iterator adding points only when the points are on
1196  // the same side as the viewpoint
1197  Vector2dList::ConstIterator trailingPointIter = rPointReadings.GetConstIterator();
1198  Vector2dList validPoints;
1199 
1200  Vector2d firstPoint;
1201  kt_bool firstTime = true;
1202  karto_const_forEach(Vector2dList, &rPointReadings)
1203  {
1204  Vector2d currentPoint = *iter;
1205 
1206  if (firstTime)
1207  {
1208  firstPoint = currentPoint;
1209  firstTime = false;
1210  }
1211 
1212  Vector2d delta = firstPoint - currentPoint;
1213  if (delta.SquaredLength() > minSquareDistance)
1214  {
1215  // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
1216  // Which computes the direction of rotation, if the rotation is counterclock
1217  // wise then we are looking at data we should keep. If it's negative rotation
1218  // we should not included in in the matching
1219  // have enough distance, check viewpoint
1220  double a = rViewPoint.GetY() - firstPoint.GetY();
1221  double b = firstPoint.GetX() - rViewPoint.GetX();
1222  double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
1223  double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
1224 
1225  // reset beginning point
1226  firstPoint = currentPoint;
1227 
1228  if (ss < 0.0) // wrong side, skip and keep going
1229  {
1230  trailingPointIter = iter;
1231  }
1232  else
1233  {
1234  for (; trailingPointIter != iter; trailingPointIter++)
1235  {
1236  validPoints.Add(*trailingPointIter);
1237  }
1238  }
1239  }
1240  }
1241 
1242  return validPoints;
1243  }
1244 
1245  kt_double ScanMatcher::GetResponse(ScanMatcherGridSet* pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex)
1246  {
1247  CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
1248  GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
1249 
1250  kt_double response = 0.0;
1251 
1252  // add up value for each point
1253  kt_int8u* pByte = pCorrelationGrid->GetDataPointer() + gridPositionIndex;
1254 
1255  const LookupArray* pOffsets = pGridLookup->GetLookupArray(angleIndex);
1256  assert(pOffsets != NULL);
1257 
1258  // get number of points in offset list
1259  kt_int32u nPoints = pOffsets->GetSize();
1260  if (nPoints == 0)
1261  {
1262  return response;
1263  }
1264 
1265  // calculate response
1266  kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
1267  for (kt_int32u i = 0; i < nPoints; i++)
1268  {
1269  // ignore points that fall off the grid
1270  kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
1271  if (!math::IsUpTo(pointGridIndex, pCorrelationGrid->GetDataSize()))
1272  {
1273  continue;
1274  }
1275 
1276  // uses index offsets to efficiently find location of point in the grid
1277  response += pByte[pAngleIndexPointer[i]];
1278  }
1279 
1280  // normalize response
1281  response /= (nPoints * GridStates_Occupied);
1282  assert(fabs(response) <= 1.0);
1283 
1284  return response;
1285  }
1286 
1288  {
1289  if (m_pOpenMapper->IsMultiThreaded())
1290  {
1291  throw Exception("Correlation grid only available in single-threaded mode");
1292  }
1293  else
1294  {
1295  return m_pScanMatcherGridSet->m_pCorrelationGrid;
1296  }
1297  }
1298 
1300  {
1301  if (m_pOpenMapper->IsMultiThreaded())
1302  {
1303  throw Exception("Search grid only available in single-threaded mode");
1304  }
1305  else
1306  {
1307  return m_pScanMatcherGridSet->m_pSearchSpaceProbs;
1308  }
1309  }
1310 
1314 
1315  template<typename T>
1317  {
1318  public:
1323  : GraphTraversal<T>(pGraph)
1324  {
1325  }
1326 
1331  {
1332  }
1333 
1334  public:
1341  virtual List<T> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
1342  {
1343  std::queue<Vertex<T>*> toVisit;
1344  std::set<Vertex<T>*> seenVertices;
1345  std::vector<Vertex<T>*> validVertices;
1346 
1347  toVisit.push(pStartVertex);
1348  seenVertices.insert(pStartVertex);
1349 
1350  do
1351  {
1352  Vertex<T>* pNext = toVisit.front();
1353  toVisit.pop();
1354 
1355  if (pVisitor->Visit(pNext))
1356  {
1357  // vertex is valid, explore neighbors
1358  validVertices.push_back(pNext);
1359 
1360  List<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
1361  karto_const_forEach(typename List<Vertex<T>*>, &adjacentVertices)
1362  {
1363  Vertex<T>* pAdjacent = *iter;
1364 
1365  // adjacent vertex has not yet been seen, add to queue for processing
1366  if (seenVertices.find(pAdjacent) == seenVertices.end())
1367  {
1368  toVisit.push(pAdjacent);
1369  seenVertices.insert(pAdjacent);
1370  }
1371  }
1372  }
1373  } while (toVisit.empty() == false);
1374 
1375  List<T> objects;
1376  forEach(typename std::vector<Vertex<T>*>, &validVertices)
1377  {
1378  objects.Add((*iter)->GetVertexObject());
1379  }
1380 
1381  return objects;
1382  }
1383 
1384  }; // class BreadthFirstTraversal
1385 
1389 
1390  class NearScanVisitor : public Visitor<LocalizedObjectPtr>
1391  {
1392  public:
1393  NearScanVisitor(LocalizedLaserScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
1394  : m_MaxDistanceSquared(math::Square(maxDistance))
1395  , m_UseScanBarycenter(useScanBarycenter)
1396  {
1397  m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
1398  }
1399 
1401  {
1402  LocalizedObject* pObject = pVertex->GetVertexObject();
1403 
1404  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
1405 
1406  // object is not a scan or wasn't scan matched, ignore
1407  if (pScan == NULL)
1408  {
1409  return false;
1410  }
1411 
1412  Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
1413 
1414  kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
1415  return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
1416  }
1417 
1418  protected:
1422 
1423  }; // NearScanVisitor
1424 
1428 
1429  MapperGraph::MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold)
1430  : m_pOpenMapper(pOpenMapper)
1431  {
1433  assert(m_pLoopScanMatcher);
1434 
1436  }
1437 
1439  {
1440  delete m_pLoopScanMatcher;
1441  m_pLoopScanMatcher = NULL;
1442 
1443  delete m_pTraversal;
1444  m_pTraversal = NULL;
1445  }
1446 
1448  {
1449  assert(pObject);
1450 
1451  if (pObject == NULL)
1452  {
1453  return;
1454  }
1455 
1458  if (m_pOpenMapper->m_pScanSolver != NULL)
1459  {
1460  m_pOpenMapper->m_pScanSolver->AddNode(pVertex);
1461  }
1462  }
1463 
1465  {
1466  // loose "spring"
1467  Matrix3 covariance;
1468  covariance(0, 0) = MAX_VARIANCE;
1469  covariance(1, 1) = MAX_VARIANCE;
1470  covariance(2, 2) = MAX_VARIANCE;
1471 
1472  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
1473  if (pScan != NULL)
1474  {
1475  AddEdges(pScan, covariance);
1476  }
1477  else
1478  {
1480  const Identifier& rSensorName = pObject->GetSensorIdentifier();
1481 
1482  LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
1483  if (pLastScan != NULL)
1484  {
1485  LinkObjects(pLastScan, pObject, pObject->GetCorrectedPose(), covariance);
1486  }
1487  }
1488  }
1489 
1490  void MapperGraph::AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance)
1491  {
1493 
1494  const Identifier& rSensorName = pScan->GetSensorIdentifier();
1495 
1496  Pose2List means;
1497  List<Matrix3> covariances;
1498 
1499  LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
1500  if (pLastScan == NULL)
1501  {
1502  // first scan (link to first scan of other robots)
1503 
1504  assert(pSensorManager->GetScans(rSensorName).Size() == 1);
1505 
1506  List<Identifier> sensorNames = pSensorManager->GetSensorNames();
1507  karto_const_forEach(List<Identifier>, &sensorNames)
1508  {
1509  const Identifier& rCandidateSensorName = *iter;
1510 
1511  // skip if candidate sensor is the same or other sensor has no scans
1512  if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).IsEmpty()))
1513  {
1514  continue;
1515  }
1516 
1517  Pose2 bestPose;
1518  Matrix3 covariance;
1519  kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance);
1520  LinkObjects(pSensorManager->GetScans(rCandidateSensorName)[0], pScan, bestPose, covariance);
1521 
1522  // only add to means and covariances if response was high "enough"
1524  {
1525  means.Add(bestPose);
1526  covariances.Add(covariance);
1527  }
1528  }
1529  }
1530  else
1531  {
1532  // link to previous scan
1533  LinkObjects(pLastScan, pScan, pScan->GetSensorPose(), rCovariance);
1534 
1535  // link to running scans
1536  Pose2 scanPose = pScan->GetSensorPose();
1537  means.Add(scanPose);
1538  covariances.Add(rCovariance);
1539  LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
1540  }
1541 
1542  // link to other near chains (chains that include new scan are invalid)
1543  LinkNearChains(pScan, means, covariances);
1544 
1545  if (!means.IsEmpty())
1546  {
1547  pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
1548  }
1549  }
1550 
1552  {
1553  kt_bool loopClosed = false;
1554 
1555  kt_int32u scanIndex = 0;
1556 
1557  LocalizedLaserScanList candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1558 
1559  while (!candidateChain.IsEmpty())
1560  {
1561 #ifdef KARTO_DEBUG2
1562  std::cout << "Candidate chain for " << pScan->GetStateId() << ": [ ";
1564  {
1565  std::cout << (*iter)->GetStateId() << " ";
1566  }
1567  std::cout << "]" << std::endl;
1568 #endif
1569 
1570  Pose2 bestPose;
1571  Matrix3 covariance;
1572  kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false);
1573 
1574  StringBuilder message;
1575  message << "COARSE RESPONSE: " << coarseResponse << " (> " << m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")\n";
1576  message << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
1577 
1578  MapperEventArguments eventArguments(message.ToString());
1579  m_pOpenMapper->Message.Notify(this, eventArguments);
1580 
1581  if (((coarseResponse > m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
1582  (covariance(0, 0) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
1583  (covariance(1, 1) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
1584  ||
1585  // be more lenient if the variance is really small
1586  ((coarseResponse > 0.9 * m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
1587  (covariance(0, 0) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
1588  (covariance(1, 1) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())))
1589  {
1590  // save for reversion
1591  Pose2 oldPose = pScan->GetSensorPose();
1592 
1593  pScan->SetSensorPose(bestPose);
1594  kt_double fineResponse = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false);
1595 
1596  message.Clear();
1597  message << "FINE RESPONSE: " << fineResponse << " (>" << m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")";
1598  MapperEventArguments eventArguments(message.ToString());
1599  m_pOpenMapper->Message.Notify(this, eventArguments);
1600 
1601  if (fineResponse < m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue())
1602  {
1603  // failed verification test, revert
1604  pScan->SetSensorPose(oldPose);
1605 
1606  MapperEventArguments eventArguments("REJECTED!");
1607  m_pOpenMapper->Message.Notify(this, eventArguments);
1608  }
1609  else
1610  {
1611  MapperEventArguments eventArguments1("Closing loop...");
1612  m_pOpenMapper->PreLoopClosed.Notify(this, eventArguments1);
1613 
1614  pScan->SetSensorPose(bestPose);
1615  LinkChainToScan(candidateChain, pScan, bestPose, covariance);
1616  CorrectPoses();
1617 
1618  MapperEventArguments eventArguments2("Loop closed!");
1619  m_pOpenMapper->PostLoopClosed.Notify(this, eventArguments2);
1620 
1622 
1623  loopClosed = true;
1624  }
1625  }
1626 
1627  candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1628  }
1629 
1630  return loopClosed;
1631  }
1632 
1634  {
1635  LocalizedLaserScan* pClosestScan = NULL;
1636  kt_double bestSquaredDistance = DBL_MAX;
1637 
1639  {
1640  Pose2 scanPose = (*iter)->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
1641 
1642  kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
1643  if (squaredDistance < bestSquaredDistance)
1644  {
1645  bestSquaredDistance = squaredDistance;
1646  pClosestScan = *iter;
1647  }
1648  }
1649 
1650  return pClosestScan;
1651  }
1652 
1654  {
1655  // check that vertex exists
1656  assert(pSourceObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
1657  assert(pTargetObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
1658 
1659  Vertex<LocalizedObjectPtr>* v1 = m_Vertices[pSourceObject->GetUniqueId()];
1660  Vertex<LocalizedObjectPtr>* v2 = m_Vertices[pTargetObject->GetUniqueId()];
1661 
1662  // see if edge already exists
1663  karto_const_forEach(List<Edge<LocalizedObjectPtr>*>, &(v1->GetEdges()))
1664  {
1665  Edge<LocalizedObjectPtr>* pEdge = *iter;
1666 
1667  if (pEdge->GetTarget() == v2)
1668  {
1669  rIsNewEdge = false;
1670  return pEdge;
1671  }
1672  }
1673 
1676  rIsNewEdge = true;
1677  return pEdge;
1678  }
1679 
1680  void MapperGraph::LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance)
1681  {
1682  kt_bool isNewEdge = true;
1683  Edge<LocalizedObjectPtr>* pEdge = AddEdge(pFromObject, pToObject, isNewEdge);
1684 
1685  // only attach link information if the edge is new
1686  if (isNewEdge == true)
1687  {
1688  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pFromObject);
1689  if (pScan != NULL)
1690  {
1691  pEdge->SetLabel(new LinkInfo(pScan->GetSensorPose(), rMean, rCovariance));
1692  }
1693  else
1694  {
1695  pEdge->SetLabel(new LinkInfo(pFromObject->GetCorrectedPose(), rMean, rCovariance));
1696  }
1697  if (m_pOpenMapper->m_pScanSolver != NULL)
1698  {
1699  m_pOpenMapper->m_pScanSolver->AddConstraint(pEdge);
1700  }
1701  }
1702  }
1703 
1704 #ifdef USE_TBB
1705  class Parallel_LinkNearChains
1706  {
1707  public:
1708  Parallel_LinkNearChains(OpenMapper* pMapper, LocalizedLaserScan* pScan, const List<LocalizedLaserScanList>* pChains,
1709  kt_bool* pWasChainLinked, Pose2List* pMeans, List<Matrix3>* pCovariances,
1710  kt_int32u minChainSize, kt_double minResponse)
1711  : m_pOpenMapper(pMapper)
1712  , m_pScan(pScan)
1713  , m_pChains(pChains)
1714  , m_pWasChainLinked(pWasChainLinked)
1715  , m_pMeans(pMeans)
1716  , m_pCovariances(pCovariances)
1717  , m_MinChainSize(minChainSize)
1718  , m_MinResponse(minResponse)
1719  {
1720  }
1721 
1722  void operator()(const tbb::blocked_range<kt_int32s>& rRange) const
1723  {
1724  for (kt_int32s i = rRange.begin(); i != rRange.end(); i++)
1725  {
1726  m_pWasChainLinked[i] = false;
1727 
1728  const LocalizedLaserScanList& rChain = m_pChains->Get(i);
1729 
1730  if (rChain.Size() < m_MinChainSize)
1731  {
1732  continue;
1733  }
1734 
1735  Pose2 mean;
1736  Matrix3 covariance;
1737 
1738  // match scan against "near" chain
1739  kt_double response = m_pOpenMapper->GetSequentialScanMatcher()->MatchScan(m_pScan, rChain, mean, covariance, false);
1740  if (response > m_MinResponse - KT_TOLERANCE)
1741  {
1742  m_pWasChainLinked[i] = true;
1743  m_pMeans->Set(i, mean);
1744  m_pCovariances->Set(i, covariance);
1745  }
1746  }
1747  }
1748 
1749  private:
1751  LocalizedLaserScan* m_pScan;
1752  const List<LocalizedLaserScanList>* m_pChains;
1753  kt_bool* m_pWasChainLinked;
1754  Pose2List* m_pMeans;
1755  List<Matrix3>* m_pCovariances;
1756  kt_int32u m_MinChainSize;
1757  kt_double m_MinResponse;
1758  };
1759 #endif
1760 
1762  {
1763  const List<LocalizedLaserScanList> nearChains = FindNearChains(pScan);
1764 
1765  kt_bool gotTbb = false;
1767  {
1768 #ifdef USE_TBB
1769  gotTbb = true;
1770  kt_bool* pWasChainLinked = new kt_bool[nearChains.Size()];
1771 
1772  Pose2List means;
1773  means.Resize(nearChains.Size());
1774 
1775  List<Matrix3> covariances;
1776  covariances.Resize(nearChains.Size());
1777 
1778  int grainSize = 100;
1779  Parallel_LinkNearChains myTask(m_pOpenMapper, pScan, &nearChains, pWasChainLinked, &means, &covariances,
1782  tbb::parallel_for(tbb::blocked_range<kt_int32s>(0, static_cast<kt_int32s>(nearChains.Size()), grainSize), myTask);
1783 
1784  for (kt_int32u i = 0; i < nearChains.Size(); i++)
1785  {
1786  if (pWasChainLinked[i] == true)
1787  {
1788  rMeans.Add(means[i]);
1789  rCovariances.Add(covariances[i]);
1790  LinkChainToScan(nearChains[i], pScan, means[i], covariances[i]);
1791  }
1792  }
1793 
1794  delete [] pWasChainLinked;
1795 #endif
1796  }
1797 
1798  if (gotTbb == false)
1799  {
1801  {
1802 #ifdef KARTO_DEBUG2
1803  std::cout << "Near chain for " << pScan->GetStateId() << ": [ ";
1805  {
1806  std::cout << (*iter2)->GetStateId() << " ";
1807  }
1808  std::cout << "]: ";
1809 #endif
1810 
1811  if (iter->Size() < m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue())
1812  {
1813 #ifdef KARTO_DEBUG2
1814  std::cout << iter->Size() << "(< " << m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue() << ") REJECTED" << std::endl;
1815 #endif
1816  continue;
1817  }
1818 
1819  Pose2 mean;
1820  Matrix3 covariance;
1821  // match scan against "near" chain
1822  kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
1824  {
1825 #ifdef KARTO_DEBUG2
1826  std::cout << " ACCEPTED" << std::endl;
1827 #endif
1828  rMeans.Add(mean);
1829  rCovariances.Add(covariance);
1830  LinkChainToScan(*iter, pScan, mean, covariance);
1831  }
1832  else
1833  {
1834 #ifdef KARTO_DEBUG2
1835  std::cout << response << "(< " << m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue() << ") REJECTED" << std::endl;
1836 #endif
1837  }
1838  }
1839  }
1840  }
1841 
1842  void MapperGraph::LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance)
1843  {
1845 
1846  LocalizedLaserScan* pClosestScan = GetClosestScanToPose(rChain, pose);
1847  assert(pClosestScan != NULL);
1848 
1849  Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
1850 
1851  kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
1853  {
1854  LinkObjects(pClosestScan, pScan, rMean, rCovariance);
1855 #ifdef KARTO_DEBUG2
1856  std::cout << "Linking scan " << pScan->GetStateId() << " to chain scan " << pClosestScan->GetStateId() << std::endl;
1857 #endif
1858  }
1859  }
1860 
1862  {
1863  List<LocalizedLaserScanList> nearChains;
1864 
1866 
1867  // to keep track of which scans have been added to a chain
1868  LocalizedLaserScanList processed;
1869 
1871  karto_const_forEach(LocalizedLaserScanList, &nearLinkedScans)
1872  {
1873  LocalizedLaserScan* pNearScan = *iter;
1874 
1875  if (pNearScan == pScan)
1876  {
1877  continue;
1878  }
1879 
1880  // scan has already been processed, skip
1881  if (processed.Contains(pNearScan) == true)
1882  {
1883  continue;
1884  }
1885 
1886 #ifdef KARTO_DEBUG2
1887  std::cout << "BUILDING CHAIN: Scan " << pScan->GetStateId() << " is near " << pNearScan->GetStateId() << " (< " << m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue() << ")" << std::endl;
1888 #endif
1889 
1890  processed.Add(pNearScan);
1891 
1892  // build up chain
1893  kt_bool isValidChain = true;
1894  LocalizedLaserScanList chain;
1895 
1897 
1898  kt_int32s nearScanIndex = m_pOpenMapper->m_pMapperSensorManager->GetScanIndex(pNearScan);
1899  assert(nearScanIndex >= 0);
1900 
1901  // add scans before current scan being processed
1902  for (kt_int32s candidateScanIndex = nearScanIndex - 1; candidateScanIndex >= 0; candidateScanIndex--)
1903  {
1904  LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
1905 
1906  // chain is invalid--contains scan being added
1907  if (pCandidateScan == pScan)
1908  {
1909 #ifdef KARTO_DEBUG2
1910  std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
1911 #endif
1912  isValidChain = false;
1913  }
1914 
1915  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
1916  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1917 
1919  {
1920  chain.Add(pCandidateScan);
1921  processed.Add(pCandidateScan);
1922 
1923 #ifdef KARTO_DEBUG2
1924  std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
1926  {
1927  std::cout << (*iter2)->GetStateId() << " ";
1928  }
1929  std::cout << "]" << std::endl;
1930 #endif
1931  }
1932  else
1933  {
1934  break;
1935  }
1936  }
1937 
1938  chain.Add(pNearScan);
1939 
1940  // add scans after current scan being processed
1941  kt_size_t end = scans.Size();
1942  for (kt_size_t candidateScanIndex = nearScanIndex + 1; candidateScanIndex < end; candidateScanIndex++)
1943  {
1944  LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
1945 
1946  if (pCandidateScan == pScan)
1947  {
1948 #ifdef KARTO_DEBUG2
1949  std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
1950 #endif
1951  isValidChain = false;
1952  }
1953 
1954  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());;
1955  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1956 
1958  {
1959  chain.Add(pCandidateScan);
1960  processed.Add(pCandidateScan);
1961 
1962 #ifdef KARTO_DEBUG2
1963  std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
1965  {
1966  std::cout << (*iter2)->GetStateId() << " ";
1967  }
1968  std::cout << "]" << std::endl;
1969 #endif
1970  }
1971  else
1972  {
1973  break;
1974  }
1975  }
1976 
1977  if (isValidChain)
1978  {
1979  // add chain to collection
1980  nearChains.Add(chain);
1981  }
1982  }
1983 
1984  return nearChains;
1985  }
1986 
1988  {
1989  NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pOpenMapper->m_pUseScanBarycenter->GetValue());
1990  LocalizedObjectList nearLinkedObjects = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
1991  delete pVisitor;
1992 
1993  LocalizedLaserScanList nearLinkedScans;
1994  karto_const_forEach(LocalizedObjectList, &nearLinkedObjects)
1995  {
1996  LocalizedObject* pObject = *iter;
1997  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
1998  if (pScan != NULL)
1999  {
2000  nearLinkedScans.Add(pScan);
2001  }
2002  }
2003 
2004  return nearLinkedScans;
2005  }
2006 
2008  {
2009  LocalizedLaserScanList nearScans;
2010 
2011  const BoundingBox2& rBoundingBox = pScan->GetBoundingBox();
2012 
2013  const VertexList& vertices = GetVertices();
2014  karto_const_forEach(VertexList, &vertices)
2015  {
2016  LocalizedObject* pObject = (*iter)->GetVertexObject();
2017  LocalizedLaserScan* pCandidateScan = dynamic_cast<LocalizedLaserScan*>(pObject);
2018  if (pCandidateScan == NULL)
2019  {
2020  continue;
2021  }
2022 
2023  if (rBoundingBox.Intersects(pCandidateScan->GetBoundingBox()) == true)
2024  {
2025  nearScans.Add(pCandidateScan);
2026  }
2027  }
2028 
2029  return nearScans;
2030  }
2031 
2032 
2033  Pose2 MapperGraph::ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const
2034  {
2035  assert(rMeans.Size() == rCovariances.Size());
2036 
2037  // compute sum of inverses and create inverse list
2038  List<Matrix3> inverses;
2039  inverses.EnsureCapacity(rCovariances.Size());
2040 
2041  Matrix3 sumOfInverses;
2042  karto_const_forEach(List<Matrix3>, &rCovariances)
2043  {
2044  Matrix3 inverse = iter->Inverse();
2045  inverses.Add(inverse);
2046 
2047  sumOfInverses += inverse;
2048  }
2049  Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
2050 
2051  // compute weighted mean
2052  Pose2 accumulatedPose;
2053  kt_double thetaX = 0.0;
2054  kt_double thetaY = 0.0;
2055 
2056  Pose2List::ConstIterator meansIter = rMeans.GetConstIterator();
2058  {
2059  Pose2 pose = *meansIter;
2060  kt_double angle = pose.GetHeading();
2061  thetaX += cos(angle);
2062  thetaY += sin(angle);
2063 
2064  Matrix3 weight = inverseOfSumOfInverses * (*iter);
2065  accumulatedPose += weight * pose;
2066 
2067  meansIter++;
2068  }
2069 
2070  thetaX /= rMeans.Size();
2071  thetaY /= rMeans.Size();
2072  accumulatedPose.SetHeading(atan2(thetaY, thetaX));
2073 
2074  return accumulatedPose;
2075  }
2076 
2078  {
2079  LocalizedLaserScanList chain; // return value
2080 
2082 
2083  // possible loop closure chain should not include close scans that have a
2084  // path of links to the scan of interest
2086 
2088  kt_size_t nScans = scans.Size();
2089  for (; rStartScanIndex < nScans; rStartScanIndex++)
2090  {
2091  LocalizedLaserScan* pCandidateScan = scans[rStartScanIndex];
2092 
2093  Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
2094 
2095  kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
2097  {
2098  // a linked scan cannot be in the chain
2099  if (nearLinkedScans.Contains(pCandidateScan) == true)
2100  {
2101  chain.Clear();
2102  }
2103  else
2104  {
2105  chain.Add(pCandidateScan);
2106  }
2107  }
2108  else
2109  {
2110  // return chain if it is long "enough"
2112  {
2113  return chain;
2114  }
2115  else
2116  {
2117  chain.Clear();
2118  }
2119  }
2120  }
2121 
2122  return chain;
2123  }
2124 
2126  {
2127  // optimize scans!
2129  if (pSolver != NULL)
2130  {
2131  pSolver->Compute();
2132 
2134  {
2136  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
2137 
2138  if (pScan != NULL)
2139  {
2140  pScan->SetSensorPose(iter->GetSecond());
2141  }
2142  else
2143  {
2144  pObject->SetCorrectedPose(iter->GetSecond());
2145  }
2146  }
2147 
2148  pSolver->Clear();
2149  }
2150  }
2151 
2155 
2160  : Module("OpenMapper")
2161  , m_pScanSolver(NULL)
2162  , m_Initialized(false)
2163  , m_MultiThreaded(multiThreaded)
2164  , m_pSequentialScanMatcher(NULL)
2165  , m_pMapperSensorManager(NULL)
2166  , m_pGraph(NULL)
2167  {
2169  }
2170 
2174  OpenMapper::OpenMapper(const char* pName, kt_bool multiThreaded)
2175  : Module(pName)
2176  , m_pScanSolver(NULL)
2177  , m_Initialized(false)
2178  , m_MultiThreaded(multiThreaded)
2179  , m_pSequentialScanMatcher(NULL)
2180  , m_pMapperSensorManager(NULL)
2181  , m_pGraph(NULL)
2182  {
2184  }
2185 
2189  OpenMapper::~OpenMapper()
2190  {
2191  Reset();
2192 
2193  delete m_pMapperSensorManager;
2194  }
2195 
2197  {
2198  m_pUseScanMatching = new Parameter<kt_bool>(GetParameterSet(), "UseScanMatching", "Mapper::Use::Scan Matching", "UseScanMatching", true);
2199  m_pUseScanBarycenter = new Parameter<kt_bool>(GetParameterSet(), "UseScanBarycenter", "Mapper::Use::Scan Barycenter", "UseScanBarycenter", true);
2200  m_pMinimumTravelDistance = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelDistance", "Mapper::Minimum Travel::Distance", "MinimumTravelDistance", 0.2);
2201  m_pMinimumTravelHeading = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelHeading", "Mapper::Minimum Travel::Heading", "MinimumTravelHeading", math::DegreesToRadians(20));
2202 
2203  m_pScanBufferSize = new Parameter<kt_int32u>(GetParameterSet(), "ScanBufferSize", "Mapper::Scan Buffer::Size", "ScanBufferSize", 70);
2204  m_pScanBufferMaximumScanDistance = new Parameter<kt_double>(GetParameterSet(), "ScanBufferMaximumScanDistance", "Mapper::Scan Buffer::Maximum Scan Distance", "ScanBufferMaximumScanDistance", 20);
2205  m_pUseResponseExpansion = new Parameter<kt_bool>(GetParameterSet(), "UseResponseExpansion", "Mapper::Use::Response Expansion", "UseResponseExpansion", false);
2206 
2207  m_pDistanceVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "DistanceVariancePenalty", "Mapper::Scan Matcher::Distance Variance Penalty", "DistanceVariancePenalty", math::Square(0.3));
2208  m_pMinimumDistancePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumDistancePenalty", "Mapper::Scan Matcher::Minimum Distance Penalty", "MinimumDistancePenalty", 0.5);
2209  m_pAngleVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "AngleVariancePenalty", "Mapper::Scan Matcher::Angle Variance Penalty", "AngleVariancePenalty", math::Square(math::DegreesToRadians(20)));
2210  m_pMinimumAnglePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumAnglePenalty", "Mapper::Scan Matcher::Minimum Angle Penalty", "MinimumAnglePenalty", 0.9);
2211 
2212  m_pLinkMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LinkMatchMinimumResponseFine", "Mapper::Link::Match Minimum Response Fine", "LinkMatchMinimumResponseFine", 0.6);
2213  m_pLinkScanMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LinkScanMaximumDistance", "Mapper::Link::Scan Maximum Distance", "LinkScanMaximumDistance", 5.0);
2214 
2215  m_pCorrelationSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceDimension", "Mapper::Correlation Search Space::Dimension", "CorrelationSearchSpaceDimension", 0.3);
2216  m_pCorrelationSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceResolution", "Mapper::Correlation Search Space::Resolution", "CorrelationSearchSpaceResolution", 0.01);
2217  m_pCorrelationSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceSmearDeviation", "Mapper::Correlation Search Space::Smear Deviation", "CorrelationSearchSpaceSmearDeviation", 0.03);
2218  m_pCoarseSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "CoarseSearchAngleOffset", "Mapper::Scan Matcher::Coarse Search Angle Offset", "CoarseSearchAngleOffset", math::DegreesToRadians(20));
2219  m_pFineSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "FineSearchAngleOffset", "Mapper::Scan Matcher::Fine Search Angle Offset", "FineSearchAngleOffset", math::DegreesToRadians(0.2));
2220  m_pCoarseAngleResolution = new Parameter<kt_double>(GetParameterSet(), "CoarseAngleResolution", "Mapper::Scan Matcher::Coarse Angle Resolution", "CoarseAngleResolution", math::DegreesToRadians(2));
2221 
2222  m_pLoopSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceDimension", "Mapper::Loop Correlation Search Space::Dimension", "LoopSearchSpaceDimension", 8.0);
2223  m_pLoopSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceResolution", "Mapper::Loop Correlation Search Space::Resolution", "LoopSearchSpaceResolution", 0.05);
2224  m_pLoopSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceSmearDeviation", "Mapper::Loop Correlation Search Space::Smear Deviation", "LoopSearchSpaceSmearDeviation", 0.03);
2225 
2226  m_pLoopSearchMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LoopSearchMaximumDistance", "Mapper::Loop::Search Maximum Distance", "LoopSearchMaximumDistance", 4.0);
2227  m_pLoopMatchMinimumChainSize = new Parameter<kt_int32u>(GetParameterSet(), "LoopMatchMinimumChainSize", "Mapper::Loop::Match::Minimum Chain Size", "LoopMatchMinimumChainSize", 10);
2228  m_pLoopMatchMaximumVarianceCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMaximumVarianceCoarse", "Mapper::Loop::Match::Maximum Variance Coarse", "LoopMatchMaximumVarianceCoarse", math::Square(0.4));
2229  m_pLoopMatchMinimumResponseCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseCoarse", "Mapper::Loop::Match::Minimum Response Coarse", "LoopMatchMinimumResponseCoarse", 0.7);
2230  m_pLoopMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseFine", "Mapper::Loop::Match::Minimum Response Fine", "LoopMatchMinimumResponseFine", 0.7);
2231  }
2232 
2233  void OpenMapper::Initialize(kt_double rangeThreshold)
2234  {
2235  if (m_Initialized == false)
2236  {
2237  // create sequential scan and loop matcher
2239  assert(m_pSequentialScanMatcher);
2240 
2242 
2243  m_pGraph = new MapperGraph(this, rangeThreshold);
2244 
2245  m_Initialized = true;
2246  }
2247  else
2248  {
2249  Log(LOG_WARNING, "Mapper already initialized");
2250  }
2251  }
2252 
2254  {
2255  Module::Reset();
2256 
2257  delete m_pSequentialScanMatcher;
2258  m_pSequentialScanMatcher = NULL;
2259 
2260  delete m_pGraph;
2261  m_pGraph = NULL;
2262 
2263  delete m_pMapperSensorManager;
2264  m_pMapperSensorManager = NULL;
2265 
2266  m_Initialized = false;
2267  }
2268 
2270  {
2271  if (pObject == NULL)
2272  {
2273  return false;
2274  }
2275 
2276  kt_bool isObjectProcessed = Module::Process(pObject);
2277 
2278  if (IsLaserRangeFinder(pObject))
2279  {
2280  LaserRangeFinder* pLaserRangeFinder = dynamic_cast<LaserRangeFinder*>(pObject);
2281 
2282  if (m_Initialized == false)
2283  {
2284  // initialize mapper with range threshold from sensor
2285  Initialize(pLaserRangeFinder->GetRangeThreshold());
2286  }
2287 
2288  // register sensor
2289  m_pMapperSensorManager->RegisterSensor(pLaserRangeFinder->GetIdentifier());
2290 
2291  return true;
2292  }
2293 
2294  LocalizedObject* pLocalizedObject = dynamic_cast<LocalizedObject*>(pObject);
2295  if (pLocalizedObject != NULL)
2296  {
2297  LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
2298  if (pScan != NULL)
2299  {
2300  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2301 
2302  // validate scan
2303  if (pLaserRangeFinder == NULL)
2304  {
2305  return false;
2306  }
2307 
2308  // validate scan. Throws exception if scan is invalid.
2309  pLaserRangeFinder->Validate(pScan);
2310 
2311  if (m_Initialized == false)
2312  {
2313  // initialize mapper with range threshold from sensor
2314  Initialize(pLaserRangeFinder->GetRangeThreshold());
2315  }
2316  }
2317 
2318  // ensures sensor has been registered with mapper--does nothing if the sensor has already been registered
2320 
2321  // get last scan
2322  LocalizedLaserScan* pLastScan = m_pMapperSensorManager->GetLastScan(pLocalizedObject->GetSensorIdentifier());
2323 
2324  // update scans corrected pose based on last correction
2325  if (pLastScan != NULL)
2326  {
2327  Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
2328  pLocalizedObject->SetCorrectedPose(lastTransform.TransformPose(pLocalizedObject->GetOdometricPose()));
2329  }
2330 
2331  // check custom data if object is not a scan or if scan has not moved enough (i.e.,
2332  // scan is outside minimum boundary or if heading is larger then minimum heading)
2333  if (pScan == NULL || (!HasMovedEnough(pScan, pLastScan) && !pScan->IsGpsReadingValid()))
2334  {
2335  if (pLocalizedObject->HasCustomItem() == true)
2336  {
2337  m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
2338 
2339  // add to graph
2340  m_pGraph->AddVertex(pLocalizedObject);
2341  m_pGraph->AddEdges(pLocalizedObject);
2342 
2343  return true;
2344  }
2345 
2346  return false;
2347  }
2348 
2350  // object is a scan
2351 
2352  Matrix3 covariance;
2353  covariance.SetToIdentity();
2354 
2355  // correct scan (if not first scan)
2356  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2357  {
2358  Pose2 bestPose;
2360  pScan->SetSensorPose(bestPose);
2361  }
2362 
2363  ScanMatched(pScan);
2364 
2365  // add scan to buffer and assign id
2366  m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
2367 
2369  {
2370  // add to graph
2371  m_pGraph->AddVertex(pScan);
2372  m_pGraph->AddEdges(pScan, covariance);
2373 
2375 
2377  karto_const_forEach(List<Identifier>, &sensorNames)
2378  {
2379  m_pGraph->TryCloseLoop(pScan, *iter);
2380  }
2381  }
2382 
2384 
2385  ScanMatchingEnd(pScan);
2386 
2387  isObjectProcessed = true;
2388  } // if object is LocalizedObject
2389 
2390  return isObjectProcessed;
2391  }
2392 
2394  {
2395  // test if first scan
2396  if (pLastScan == NULL)
2397  {
2398  return true;
2399  }
2400 
2401  Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
2402  Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
2403 
2404  // test if we have turned enough
2405  kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
2406  if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
2407  {
2408  return true;
2409  }
2410 
2411  // test if we have moved enough
2412  kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
2413  if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
2414  {
2415  return true;
2416  }
2417 
2418  return false;
2419  }
2420 
2422  {
2423  LocalizedLaserScanList allScans;
2424 
2425  if (m_pMapperSensorManager != NULL)
2426  {
2427  allScans = m_pMapperSensorManager->GetAllScans();
2428  }
2429 
2430  return allScans;
2431  }
2432 
2434  {
2435  LocalizedObjectList allObjects;
2436 
2437  if (m_pMapperSensorManager != NULL)
2438  {
2439  // BUGBUG: inefficient? should return right away?
2440  allObjects = m_pMapperSensorManager->GetAllObjects();
2441  }
2442 
2443  return allObjects;
2444  }
2445 
2447  {
2448  return m_pScanSolver;
2449  }
2450 
2452  {
2453  m_pScanSolver = pScanOptimizer;
2454  }
2455 
2457  {
2458  return m_pGraph;
2459  }
2460 
2462  {
2463  return m_pSequentialScanMatcher;
2464  }
2465 
2467  {
2468  return m_pGraph->GetLoopScanMatcher();
2469  }
2470 
2471 }
kt_int32s GetHeight() const
Definition: Grid.h:263
T GetHeight() const
Definition: Geometry.h:1911
virtual kt_size_t Size() const
Definition: List.h:214
kt_int32s GetStateId() const
Definition: SensorData.h:87
MapperGraph * m_pGraph
Definition: OpenMapper.h:1738
bool kt_bool
Definition: Types.h:145
kt_int32s GetDataSize() const
Definition: Grid.h:308
virtual const T & Back() const
Definition: List.h:319
LocalizedObject * GetLocalizedObject(const Identifier &rSensorName, kt_int32s stateId)
Definition: OpenMapper.cpp:249
std::size_t kt_size_t
Definition: Types.h:138
kt_double GetHeading() const
Definition: Geometry.h:2274
Grid< kt_double > * GetSearchGrid() const
SensorDataManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: OpenMapper.cpp:57
T Square(T value)
Definition: Math.h:104
const VertexList & GetVertices() const
Definition: OpenMapper.h:536
void SetUniqueId(kt_int32u uniqueId)
Definition: SensorData.h:114
#define forEach(listtype, list)
Definition: Macros.h:88
LocalizedLaserScanList FindOverlappingScans(LocalizedLaserScan *pScan)
void RegisterSensor(const Identifier &rSensorName)
Definition: OpenMapper.cpp:241
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: OpenMapper.h:1779
virtual const T & Front() const
Definition: List.h:303
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: OpenMapper.h:1780
NearScanVisitor(LocalizedLaserScan *pScan, kt_double maxDistance, kt_bool useScanBarycenter)
void AddObject(LocalizedObject *pObject, kt_int32s uniqueId)
Definition: OpenMapper.cpp:79
LocalizedLaserScanList m_RunningScans
Definition: OpenMapper.cpp:200
void AddEdge(Edge< T > *pEdge)
Definition: OpenMapper.h:496
kt_bool HasCustomItem()
Definition: SensorData.cpp:63
CorrelationGrid * GetCorrelationGrid() const
virtual const T & GetValue() const
Definition: Parameter.h:483
virtual T & Get(kt_size_t index)
Definition: List.h:250
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:138
const BoundingBox2 & GetBoundingBox() const
Definition: SensorData.h:666
virtual void Clear()
Definition: OpenMapper.h:806
SmartPointer< CorrelationGrid > m_pCorrelationGrid
Definition: OpenMapper.h:1064
static void ComputeAngularCovariance(ScanMatcherGridSet *pScanMatcherGridSet, const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3 &rCovariance)
kt_int32s * GetArrayPointer()
LocalizedLaserScan * GetLastScan()
Definition: OpenMapper.cpp:101
virtual void Resize(kt_size_t newSize)
Definition: List.h:334
LocalizedObjectList GetAllObjects()
Definition: OpenMapper.cpp:334
const T & GetY() const
Definition: Geometry.h:369
void AddEdges(LocalizedObject *pObject)
static void AddScansNew(CorrelationGrid *pCorrelationGrid, const LocalizedLaserScanList &rScans, const Vector2d &rViewPoint)
static EventArguments & Empty()
Definition: Event.h:1028
List< LocalizedLaserScanList > FindNearChains(LocalizedLaserScan *pScan)
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: OpenMapper.h:1748
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: OpenMapper.h:1789
T GetVertexObject() const
Definition: OpenMapper.h:251
virtual void Reset()
Definition: Module.cpp:74
const Identifier & GetIdentifier() const
Definition: Object.h:83
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: OpenMapper.h:1758
kt_double GetX() const
Definition: Geometry.h:2220
void LinkObjects(LocalizedObject *pFromObject, LocalizedObject *pToObject, const Pose2 &rMean, const Matrix3 &rCovariance)
LocalizedLaserScanList & GetScans(const Identifier &rSensorName)
Definition: OpenMapper.cpp:306
void SetX(const T &x)
Definition: Geometry.h:360
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: OpenMapper.h:1763
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: OpenMapper.h:1759
static void ComputePositionalCovariance(Grid< kt_double > *pSearchSpaceProbs, const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, const Vector2d &rSearchSpaceOffset, const Vector2d &rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3 &rCovariance)
Definition: OpenMapper.cpp:939
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: OpenMapper.h:1787
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:175
SensorDataManager * GetSensorDataManager(LocalizedObject *pObject)
Definition: OpenMapper.h:1359
T GetWidth() const
Definition: Geometry.h:1893
void AddVertex(LocalizedObject *pObject)
virtual void Add(const T &rValue)
Definition: List.h:111
kt_int32s GetWidth() const
Definition: Grid.h:254
static kt_double GetResponse(ScanMatcherGridSet *pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex)
void SetScanSolver(ScanSolver *pSolver)
static ScanMatcher * Create(OpenMapper *pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
Definition: OpenMapper.cpp:441
void SetStateId(kt_int32s stateId)
Definition: SensorData.h:96
kt_double Round(kt_double value)
Definition: Math.h:114
LocalizedLaserScanList & GetRunningScans(const Identifier &rSensorName)
Definition: OpenMapper.cpp:316
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: OpenMapper.h:1747
void SetLastScan(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:110
#define const_forEach(listtype, list)
Definition: Macros.h:100
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: SensorData.h:657
TFSIMD_FORCE_INLINE const tfScalar & y() const
kt_int32u m_RunningBufferMaximumSize
Definition: OpenMapper.cpp:203
Pose2 GetSensorPose() const
Definition: SensorData.h:627
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: OpenMapper.h:1788
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: OpenMapper.h:1754
std::map< Identifier, SensorDataManager * > SensorDataManagerMap
Definition: OpenMapper.cpp:211
void SetOffset(const Vector2d &rOffset)
BasicEvent< EventArguments > ScansUpdated
Definition: OpenMapper.h:1574
void SetToIdentity()
Definition: Geometry.h:2580
virtual kt_bool Process(Object *pObject)
void ComputeOffsets(LocalizedLaserScan *pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
const Rectangle2< kt_int32s > & GetROI() const
Definition: OpenMapper.h:868
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: OpenMapper.h:1746
List< Identifier > GetSensorNames()
Definition: OpenMapper.cpp:268
virtual kt_bool Remove(const T &rValue)
Definition: List.h:148
#define karto_const_forEachAs(listtype, list, iter)
Definition: Macros.h:142
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void ClearLastScan(const Identifier &rSensorName)
Definition: OpenMapper.cpp:289
virtual void ScanMatchingEnd(LocalizedLaserScan *pScan)
Definition: OpenMapper.h:1693
void Clear()
Definition: Grid.h:93
LocalizedObjectList & GetObjects()
Definition: OpenMapper.cpp:119
virtual kt_bool Visit(Vertex< T > *pVertex)=0
BreadthFirstTraversal(Graph< T > *pGraph)
const LocalizedLaserScanList GetAllProcessedScans() const
uint8_t kt_int8u
Definition: Types.h:91
const T & GetX() const
Definition: Geometry.h:351
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: OpenMapper.h:1762
friend class MapperGraph
Definition: OpenMapper.h:1533
void SetSensorPose(const Pose2 &rSensorPose)
Definition: SensorData.h:636
List< Vertex< T > * > GetAdjacentVertices() const
Definition: OpenMapper.h:260
Parameter< kt_int32u > * m_pScanBufferSize
Definition: OpenMapper.h:1753
virtual ConstListIterator< T > GetConstIterator() const
Definition: List.h:494
void Log(LogLevel level, const karto::String &rMessage)
Definition: Logger.cpp:183
MapperSensorManager * m_pMapperSensorManager
Definition: OpenMapper.h:1736
uint32_t kt_int32u
Definition: Types.h:111
LocalizedLaserScanList m_Scans
Definition: OpenMapper.cpp:199
virtual const IdPoseVector & GetCorrections() const =0
virtual kt_bool Process(karto::Object *pObject)
Definition: Module.cpp:36
const Identifier & GetSensorIdentifier() const
Definition: SensorData.h:141
void AddRunningScan(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:301
static Vector2dList FindValidPoints(LocalizedLaserScan *pScan, const Vector2d &rViewPoint)
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: OpenMapper.h:1760
void LinkNearChains(LocalizedLaserScan *pScan, Pose2List &rMeans, List< Matrix3 > &rCovariances)
kt_double MatchScan(LocalizedLaserScan *pScan, const LocalizedLaserScanList &rBaseScans, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doPenalize=true, kt_bool doRefineMatch=true)
Definition: OpenMapper.cpp:496
void EnsureCapacity(kt_size_t newCapacity)
Definition: List.h:361
kt_double GetResolution() const
Definition: Grid.h:348
ScanMatcher * GetLoopScanMatcher() const
Definition: OpenMapper.h:635
virtual void ScanMatched(LocalizedLaserScan *pScan)
Definition: OpenMapper.h:1687
virtual kt_bool IsEmpty() const
Definition: List.h:223
kt_bool TryCloseLoop(LocalizedLaserScan *pScan, const Identifier &rSensorName)
kt_int32s GetUniqueId() const
Definition: SensorData.h:105
const LookupArray * GetLookupArray(kt_int32u index) const
void SmearPoint(const Vector2i &rGridPoint)
Definition: OpenMapper.h:886
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
static Grid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
Definition: Grid.h:68
static kt_int32s ScanIndexComparator(const LocalizedLaserScanPtr &pScan1, const LocalizedLaserScanPtr &pScan2)
Definition: OpenMapper.cpp:191
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: OpenMapper.h:1771
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: OpenMapper.h:1066
kt_int32s GetScanIndex(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:138
static String ToString(const char *value)
ScanSolver * GetScanSolver() const
const kt_double KT_TOLERANCE
Definition: Math.h:71
BasicEvent< MapperEventArguments > PreLoopClosed
Definition: OpenMapper.h:1563
ScanMatcher * GetSequentialScanMatcher() const
ScanMatcher * m_pSequentialScanMatcher
Definition: OpenMapper.h:1734
SensorDataManagerMap m_SensorDataManagers
Definition: OpenMapper.cpp:216
SmartPointer< ScanSolver > m_pScanSolver
Definition: OpenMapper.h:1728
Parameter< kt_bool > * m_pUseScanMatching
Definition: OpenMapper.h:1745
TFSIMD_FORCE_INLINE const tfScalar & x() const
void AddLocalizedObject(LocalizedObject *pObject)
Definition: OpenMapper.cpp:294
virtual kt_bool Contains(const T &rValue) const
Definition: List.h:197
void AddRunningScan(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:157
LocalizedLaserScanList & GetScans()
Definition: OpenMapper.cpp:128
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: OpenMapper.h:1757
BasicEvent< MapperEventArguments > PostLoopClosed
Definition: OpenMapper.h:1568
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Geometry.h:433
LocalizedLaserScanList GetAllScans()
Definition: OpenMapper.cpp:321
const String & ToString() const
MapperSensorManagerPrivate * m_pMapperSensorManagerPrivate
Definition: OpenMapper.h:1372
void SetLastScan(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:284
LocalizedLaserScan * GetClosestScanToPose(const LocalizedLaserScanList &rScans, const Pose2 &rPose) const
virtual kt_int32s GridIndex(const Vector2i &rGrid, kt_bool boundaryCheck=true) const
Definition: OpenMapper.h:856
int32_t kt_int32s
Definition: Types.h:106
void LinkChainToScan(const LocalizedLaserScanList &rChain, LocalizedLaserScan *pScan, const Pose2 &rMean, const Matrix3 &rCovariance)
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
void AddVertex(Vertex< T > *pVertex)
Definition: OpenMapper.h:487
const LocalizedObjectList GetAllProcessedObjects() const
kt_int32u GetSize() const
Pose2 ComputeWeightedMean(const Pose2List &rMeans, const List< Matrix3 > &rCovariances) const
static void AddScans(CorrelationGrid *pCorrelationGrid, const LocalizedLaserScanList &rScans, const Vector2d &rViewPoint)
virtual ~ScanMatcher()
Definition: OpenMapper.cpp:436
kt_int32s BinarySearch(const T &rValue, kt_int32s(*f)(const T &a, const T &b))
Definition: List.h:380
OpenMapper(kt_bool multiThreaded=true)
static CorrelationGrid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
Definition: OpenMapper.h:838
LocalizedLaserScanList FindPossibleLoopClosure(LocalizedLaserScan *pScan, const Identifier &rSensorName, kt_int32u &rStartScanIndex)
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: OpenMapper.h:1769
kt_bool m_Initialized
Definition: OpenMapper.h:1731
kt_bool m_MultiThreaded
Definition: OpenMapper.h:1732
virtual void SetCorrectedPose(const Pose2 &rPose)
Definition: SensorData.h:411
Vector2< kt_double > Vector2d
Definition: Geometry.h:610
virtual kt_bool Visit(Vertex< LocalizedObjectPtr > *pVertex)
kt_bool IsGpsReadingValid() const
Definition: SensorData.h:440
CoordinateConverter * GetCoordinateConverter() const
Definition: Grid.h:339
double kt_double
Definition: Types.h:160
kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
Definition: Math.h:252
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:83
MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: OpenMapper.cpp:226
#define MAX_VARIANCE
Definition: OpenMapper.cpp:40
const kt_double KT_PI
Definition: Math.h:43
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: OpenMapper.h:1773
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:213
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:203
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: OpenMapper.h:1778
LocalizedObjectList m_Objects
Definition: OpenMapper.cpp:197
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: OpenMapper.h:1770
void InitializeParameters()
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:162
virtual MapperGraph * GetGraph() const
kt_double SquaredLength() const
Definition: Geometry.h:414
LocalizedLaserScanPtr m_pLastScan
Definition: OpenMapper.cpp:201
kt_int32s GetScanIndex(LocalizedLaserScan *pScan)
Definition: OpenMapper.cpp:311
ScanMatcher * m_pLoopScanMatcher
Definition: OpenMapper.h:723
ParameterSet * GetParameterSet()
Definition: Object.h:144
void SetLabel(EdgeLabel *pLabel)
Definition: OpenMapper.h:369
virtual void Validate()
Definition: Sensor.cpp:192
Vector2i WorldToGrid(const Vector2d &rWorld, kt_bool flipY=false) const
Definition: Grid.h:212
SmartPointer< Grid< kt_double > > m_pSearchSpaceProbs
Definition: OpenMapper.h:1065
#define ANGLE_PENALTY_GAIN
Definition: OpenMapper.cpp:42
kt_bool IsMultiThreaded()
Definition: OpenMapper.h:1589
kt_double GetY() const
Definition: Geometry.h:2238
void SetHeading(kt_double heading)
Definition: Geometry.h:2283
static void AddScanNew(CorrelationGrid *pCorrelationGrid, const Vector2dList &rValidPoints, kt_bool doSmear=true)
kt_double CorrelateScan(ScanMatcherGridSet *pScanMatcherGridSet, LocalizedLaserScan *pScan, const Pose2 &rSearchCenter, const Vector2d &rSearchSpaceOffset, const Vector2d &rSearchSpaceResolution, kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doingFineMatch)
Definition: OpenMapper.cpp:718
Definition: Any.cpp:20
const Pose2 & GetOdometricPose() const
Definition: SensorData.h:380
virtual void Clear()
Definition: List.h:231
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: OpenMapper.h:1772
Vertex< LocalizedObjectPtr > * GetVertex(LocalizedObject *pObject)
Definition: OpenMapper.h:655
static void AddScan(CorrelationGrid *pCorrelationGrid, LocalizedLaserScan *pScan, const Vector2d &rViewPoint, kt_bool doSmear=true)
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: OpenMapper.h:1755
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: OpenMapper.h:1768
Vertex< T > * GetTarget() const
Definition: OpenMapper.h:351
virtual void Compute()=0
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: OpenMapper.h:1786
OpenMapper * m_pOpenMapper
Definition: OpenMapper.h:718
kt_bool HasMovedEnough(LocalizedLaserScan *pScan, LocalizedLaserScan *pLastScan) const
virtual ~MapperGraph()
kt_bool Intersects(const BoundingBox2 &rOther) const
Definition: Geometry.h:1692
Edge< LocalizedObjectPtr > * AddEdge(LocalizedObject *pSourceObject, LocalizedObject *pTargetObject, kt_bool &rIsNewEdge)
Matrix3 Inverse() const
Definition: Geometry.h:2659
T * GetDataPointer(const Vector2i &rGrid)
Definition: Grid.h:233
MapperGraph(OpenMapper *pOpenMapper, kt_double rangeThreshold)
virtual List< T > Traverse(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)
void SetY(const T &y)
Definition: Geometry.h:378
ScanMatcher * GetLoopScanMatcher() const
kt_double GetRangeThreshold() const
Definition: Sensor.h:304
LocalizedLaserScan * GetLastScan(const Identifier &rSensorName)
Definition: OpenMapper.cpp:279
const Pose2 & GetCorrectedPose() const
Definition: SensorData.h:402
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: OpenMapper.h:1785
GraphTraversal< LocalizedObjectPtr > * m_pTraversal
Definition: OpenMapper.h:728
LocalizedLaserScanList & GetRunningScans()
Definition: OpenMapper.cpp:148
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: SensorData.h:612
const std::string response
#define karto_const_forEach(listtype, list)
Definition: Macros.h:136
LaserRangeFinder * GetLaserRangeFinder() const
Definition: SensorData.h:576
#define DISTANCE_PENALTY_GAIN
Definition: OpenMapper.cpp:41
void Initialize(kt_double rangeThreshold)
kt_double m_RunningBufferMaximumDistance
Definition: OpenMapper.cpp:204
BasicEvent< MapperEventArguments > Message
Definition: OpenMapper.h:1556
kt_double m_MaxDistanceSquared
const Vector2dList & GetPointReadings(kt_bool wantFiltered=false) const
Definition: SensorData.cpp:152
LocalizedLaserScanList FindNearLinkedScans(LocalizedLaserScan *pScan, kt_double maxDistance)


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24