Mapper.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #include <sstream>
00019 #include <fstream>
00020 #include <stdexcept>
00021 #include <queue>
00022 #include <set>
00023 #include <list>
00024 #include <iterator>
00025 
00026 #include <math.h>
00027 #include <assert.h>
00028 
00029 #include "open_karto/Mapper.h"
00030 
00031 namespace karto
00032 {
00033 
00034   // enable this for verbose debug information
00035   // #define KARTO_DEBUG
00036 
00037   #define MAX_VARIANCE            500.0
00038   #define DISTANCE_PENALTY_GAIN   0.2
00039   #define ANGLE_PENALTY_GAIN      0.2
00040 
00044 
00048   class ScanManager
00049   {
00050   public:
00054     ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
00055       : m_pLastScan(NULL)
00056       , m_RunningBufferMaximumSize(runningBufferMaximumSize)
00057       , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
00058     {
00059     }
00060 
00064     virtual ~ScanManager()
00065     {
00066       Clear();
00067     }
00068 
00069   public:
00074     inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId)
00075     {
00076       // assign state id to scan
00077       pScan->SetStateId(static_cast<kt_int32u>(m_Scans.size()));
00078 
00079       // assign unique id to scan
00080       pScan->SetUniqueId(uniqueId);
00081 
00082       // add it to scan buffer
00083       m_Scans.push_back(pScan);
00084     }
00085 
00091     inline LocalizedRangeScan* GetLastScan()
00092     {
00093       return m_pLastScan;
00094     }
00095 
00100     inline void SetLastScan(LocalizedRangeScan* pScan)
00101     {
00102       m_pLastScan = pScan;
00103     }
00104 
00109     inline LocalizedRangeScanVector& GetScans()
00110     {
00111       return m_Scans;
00112     }
00113 
00118     inline LocalizedRangeScanVector& GetRunningScans()
00119     {
00120       return m_RunningScans;
00121     }
00122 
00127     void AddRunningScan(LocalizedRangeScan* pScan)
00128     {
00129       m_RunningScans.push_back(pScan);
00130 
00131       // vector has at least one element (first line of this function), so this is valid
00132       Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose();
00133       Pose2 backScanPose = m_RunningScans.back()->GetSensorPose();
00134 
00135       // cap vector size and remove all scans from front of vector that are too far from end of vector
00136       kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00137       while (m_RunningScans.size() > m_RunningBufferMaximumSize ||
00138              squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE)
00139       {
00140         // remove front of running scans
00141         m_RunningScans.erase(m_RunningScans.begin());
00142 
00143         // recompute stats of running scans
00144         frontScanPose = m_RunningScans.front()->GetSensorPose();
00145         backScanPose = m_RunningScans.back()->GetSensorPose();
00146         squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00147       }
00148     }
00149 
00153     void Clear()
00154     {
00155       m_Scans.clear();
00156       m_RunningScans.clear();
00157     }
00158 
00159   private:
00160     LocalizedRangeScanVector m_Scans;
00161     LocalizedRangeScanVector m_RunningScans;
00162     LocalizedRangeScan* m_pLastScan;
00163 
00164     kt_int32u m_RunningBufferMaximumSize;
00165     kt_double m_RunningBufferMaximumDistance;
00166   };  // ScanManager
00167 
00171 
00172   void MapperSensorManager::RegisterSensor(const Name& rSensorName)
00173   {
00174     if (GetScanManager(rSensorName) == NULL)
00175     {
00176       m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance);
00177     }
00178   }
00179 
00180 
00187   LocalizedRangeScan* MapperSensorManager::GetScan(const Name& rSensorName, kt_int32s scanIndex)
00188   {
00189     ScanManager* pScanManager = GetScanManager(rSensorName);
00190     if (pScanManager != NULL)
00191     {
00192       return pScanManager->GetScans().at(scanIndex);
00193     }
00194 
00195     assert(false);
00196     return NULL;
00197   }
00198 
00204   inline LocalizedRangeScan* MapperSensorManager::GetLastScan(const Name& rSensorName)
00205   {
00206     RegisterSensor(rSensorName);
00207 
00208     return GetScanManager(rSensorName)->GetLastScan();
00209   }
00210 
00215   inline void MapperSensorManager::SetLastScan(LocalizedRangeScan* pScan)
00216   {
00217     GetScanManager(pScan)->SetLastScan(pScan);
00218   }
00219 
00224   void MapperSensorManager::AddScan(LocalizedRangeScan* pScan)
00225   {
00226     GetScanManager(pScan)->AddScan(pScan, m_NextScanId);
00227     m_Scans.push_back(pScan);
00228     m_NextScanId++;
00229   }
00230 
00235   inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan* pScan)
00236   {
00237     GetScanManager(pScan)->AddRunningScan(pScan);
00238   }
00239 
00245   inline LocalizedRangeScanVector& MapperSensorManager::GetScans(const Name& rSensorName)
00246   {
00247     return GetScanManager(rSensorName)->GetScans();
00248   }
00249 
00255   inline LocalizedRangeScanVector& MapperSensorManager::GetRunningScans(const Name& rSensorName)
00256   {
00257     return GetScanManager(rSensorName)->GetRunningScans();
00258   }
00259 
00264   LocalizedRangeScanVector MapperSensorManager::GetAllScans()
00265   {
00266     LocalizedRangeScanVector scans;
00267 
00268     forEach(ScanManagerMap, &m_ScanManagers)
00269     {
00270       LocalizedRangeScanVector& rScans = iter->second->GetScans();
00271 
00272       scans.insert(scans.end(), rScans.begin(), rScans.end());
00273     }
00274 
00275     return scans;
00276   }
00277 
00281   void MapperSensorManager::Clear()
00282   {
00283 //    SensorManager::Clear();
00284 
00285     forEach(ScanManagerMap, &m_ScanManagers)
00286     {
00287       delete iter->second;
00288     }
00289 
00290     m_ScanManagers.clear();
00291   }
00292 
00296 
00297   ScanMatcher::~ScanMatcher()
00298   {
00299     delete m_pCorrelationGrid;
00300     delete m_pSearchSpaceProbs;
00301     delete m_pGridLookup;
00302   }
00303 
00304   ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution,
00305                                    kt_double smearDeviation, kt_double rangeThreshold)
00306   {
00307     // invalid parameters
00308     if (resolution <= 0)
00309     {
00310       return NULL;
00311     }
00312     if (searchSize <= 0)
00313     {
00314       return NULL;
00315     }
00316     if (smearDeviation < 0)
00317     {
00318       return NULL;
00319     }
00320     if (rangeThreshold <= 0)
00321     {
00322       return NULL;
00323     }
00324 
00325     assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
00326 
00327     // calculate search space in grid coordinates
00328     kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
00329 
00330     // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
00331     // if a scan is on the border of the search space)
00332     kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
00333 
00334     kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
00335 
00336     // create correlation grid
00337     assert(gridSize % 2 == 1);
00338     CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
00339 
00340     // create search space probabilities
00341     Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize,
00342                                                                      searchSpaceSideSize, resolution);
00343 
00344     ScanMatcher* pScanMatcher = new ScanMatcher(pMapper);
00345     pScanMatcher->m_pCorrelationGrid = pCorrelationGrid;
00346     pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs;
00347     pScanMatcher->m_pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
00348 
00349     return pScanMatcher;
00350   }
00351 
00362   kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean,
00363                                    Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
00364   {
00366     // set scan pose to be center of grid
00367 
00368     // 1. get scan position
00369     Pose2 scanPose = pScan->GetSensorPose();
00370 
00371     // scan has no readings; cannot do scan matching
00372     // best guess of pose is based off of adjusted odometer reading
00373     if (pScan->GetNumberOfRangeReadings() == 0)
00374     {
00375       rMean = scanPose;
00376 
00377       // maximum covariance
00378       rCovariance(0, 0) = MAX_VARIANCE;  // XX
00379       rCovariance(1, 1) = MAX_VARIANCE;  // YY
00380       rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue());  // TH*TH
00381 
00382       return 0.0;
00383     }
00384 
00385     // 2. get size of grid
00386     Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
00387 
00388     // 3. compute offset (in meters - lower left corner)
00389     Vector2<kt_double> offset;
00390     offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
00391     offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
00392 
00393     // 4. set offset
00394     m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
00395 
00397 
00398     // set up correlation grid
00399     AddScans(rBaseScans, scanPose.GetPosition());
00400 
00401     // compute how far to search in each direction
00402     Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
00403     Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
00404                                           0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());
00405 
00406     // a coarse search only checks half the cells in each dimension
00407     Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
00408                                               2 * m_pCorrelationGrid->GetResolution());
00409 
00410     // actual scan-matching
00411     kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
00412                                            m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
00413                                            m_pMapper->m_pCoarseAngleResolution->GetValue(),
00414                                            doPenalize, rMean, rCovariance, false);
00415 
00416     if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
00417     {
00418       if (math::DoubleEqual(bestResponse, 0.0))
00419       {
00420 #ifdef KARTO_DEBUG
00421         std::cout << "Mapper Info: Expanding response search space!" << std::endl;
00422 #endif
00423         // try and increase search angle offset with 20 degrees and do another match
00424         kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
00425         for (kt_int32u i = 0; i < 3; i++)
00426         {
00427           newSearchAngleOffset += math::DegreesToRadians(20);
00428 
00429           bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
00430                                        newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
00431                                        doPenalize, rMean, rCovariance, false);
00432 
00433           if (math::DoubleEqual(bestResponse, 0.0) == false)
00434           {
00435             break;
00436           }
00437         }
00438 
00439 #ifdef KARTO_DEBUG
00440         if (math::DoubleEqual(bestResponse, 0.0))
00441         {
00442           std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
00443         }
00444 #endif
00445       }
00446     }
00447 
00448     if (doRefineMatch)
00449     {
00450       Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
00451       Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
00452       bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
00453                                    0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
00454                                    m_pMapper->m_pFineSearchAngleOffset->GetValue(),
00455                                    doPenalize, rMean, rCovariance, true);
00456     }
00457 
00458 #ifdef KARTO_DEBUG
00459     std::cout << "  BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ",  VARIANCE = "
00460               << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
00461 #endif
00462     assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00463 
00464     return bestResponse;
00465   }
00466 
00483   kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter,
00484                                        const Vector2<kt_double>& rSearchSpaceOffset,
00485                                        const Vector2<kt_double>& rSearchSpaceResolution,
00486                                        kt_double searchAngleOffset, kt_double searchAngleResolution,
00487                                        kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
00488   {
00489     assert(searchAngleResolution != 0.0);
00490 
00491     // setup lookup arrays
00492     m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
00493 
00494     // only initialize probability grid if computing positional covariance (during coarse match)
00495     if (!doingFineMatch)
00496     {
00497       m_pSearchSpaceProbs->Clear();
00498 
00499       // position search grid - finds lower left corner of search grid
00500       Vector2<kt_double> offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
00501       m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
00502     }
00503 
00504     // calculate position arrays
00505 
00506     std::vector<kt_double> xPoses;
00507     kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() *
00508                                           2.0 / rSearchSpaceResolution.GetX()) + 1);
00509     kt_double startX = -rSearchSpaceOffset.GetX();
00510     for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00511     {
00512       xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX());
00513     }
00514     assert(math::DoubleEqual(xPoses.back(), -startX));
00515 
00516     std::vector<kt_double> yPoses;
00517     kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() *
00518                                           2.0 / rSearchSpaceResolution.GetY()) + 1);
00519     kt_double startY = -rSearchSpaceOffset.GetY();
00520     for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00521     {
00522       yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY());
00523     }
00524     assert(math::DoubleEqual(yPoses.back(), -startY));
00525 
00526     // calculate pose response array size
00527     kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
00528 
00529     kt_int32u poseResponseSize = static_cast<kt_int32u>(xPoses.size() * yPoses.size() * nAngles);
00530 
00531     // allocate array
00532     std::pair<kt_double, Pose2>* pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
00533 
00534     Vector2<kt_int32s> startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX()
00535                                                                         + startX, rSearchCenter.GetY() + startY));
00536 
00537     kt_int32u poseResponseCounter = 0;
00538     forEachAs(std::vector<kt_double>, &yPoses, yIter)
00539     {
00540       kt_double y = *yIter;
00541       kt_double newPositionY = rSearchCenter.GetY() + y;
00542       kt_double squareY = math::Square(y);
00543 
00544       forEachAs(std::vector<kt_double>, &xPoses, xIter)
00545       {
00546         kt_double x = *xIter;
00547         kt_double newPositionX = rSearchCenter.GetX() + x;
00548         kt_double squareX = math::Square(x);
00549 
00550         Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(newPositionX, newPositionY));
00551         kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
00552         assert(gridIndex >= 0);
00553 
00554         kt_double angle = 0.0;
00555         kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
00556         for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00557         {
00558           angle = startAngle + angleIndex * searchAngleResolution;
00559 
00560           kt_double response = GetResponse(angleIndex, gridIndex);
00561           if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
00562           {
00563             // simple model (approximate Gaussian) to take odometry into account
00564 
00565             kt_double squaredDistance = squareX + squareY;
00566             kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN *
00567                                                squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue());
00568             distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue());
00569 
00570             kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
00571             kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN *
00572                                             squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue());
00573             anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue());
00574 
00575             response *= (distancePenalty * anglePenalty);
00576           }
00577 
00578           // store response and pose
00579           pPoseResponse[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY,
00580                                                                            math::NormalizeAngle(angle)));
00581           poseResponseCounter++;
00582         }
00583 
00584         assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
00585       }
00586     }
00587 
00588     assert(poseResponseSize == poseResponseCounter);
00589 
00590     // find value of best response (in [0; 1])
00591     kt_double bestResponse = -1;
00592     for (kt_int32u i = 0; i < poseResponseSize; i++)
00593     {
00594       bestResponse = math::Maximum(bestResponse, pPoseResponse[i].first);
00595 
00596       // will compute positional covariance, save best relative probability for each cell
00597       if (!doingFineMatch)
00598       {
00599         const Pose2& rPose = pPoseResponse[i].second;
00600         Vector2<kt_int32s> grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
00601 
00602         // Changed (kt_double*) to the reinterpret_cast - Luc
00603         kt_double* ptr = reinterpret_cast<kt_double*>(m_pSearchSpaceProbs->GetDataPointer(grid));
00604         if (ptr == NULL)
00605         {
00606           throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!");
00607         }
00608 
00609         *ptr = math::Maximum(pPoseResponse[i].first, *ptr);
00610       }
00611     }
00612 
00613     // average all poses with same highest response
00614     Vector2<kt_double> averagePosition;
00615     kt_double thetaX = 0.0;
00616     kt_double thetaY = 0.0;
00617     kt_int32s averagePoseCount = 0;
00618     for (kt_int32u i = 0; i < poseResponseSize; i++)
00619     {
00620       if (math::DoubleEqual(pPoseResponse[i].first, bestResponse))
00621       {
00622         averagePosition += pPoseResponse[i].second.GetPosition();
00623 
00624         kt_double heading = pPoseResponse[i].second.GetHeading();
00625         thetaX += cos(heading);
00626         thetaY += sin(heading);
00627 
00628         averagePoseCount++;
00629       }
00630     }
00631 
00632     Pose2 averagePose;
00633     if (averagePoseCount > 0)
00634     {
00635       averagePosition /= averagePoseCount;
00636 
00637       thetaX /= averagePoseCount;
00638       thetaY /= averagePoseCount;
00639 
00640       averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
00641     }
00642     else
00643     {
00644       throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position");
00645     }
00646 
00647     // delete pose response array
00648     delete [] pPoseResponse;
00649 
00650 #ifdef KARTO_DEBUG
00651     std::cout << "bestPose: " << averagePose << std::endl;
00652     std::cout << "bestResponse: " << bestResponse << std::endl;
00653 #endif
00654 
00655     if (!doingFineMatch)
00656     {
00657       ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset,
00658                                   rSearchSpaceResolution, searchAngleResolution, rCovariance);
00659     }
00660     else
00661     {
00662       ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter,
00663                               searchAngleOffset, searchAngleResolution, rCovariance);
00664     }
00665 
00666     rMean = averagePose;
00667 
00668 #ifdef KARTO_DEBUG
00669     std::cout << "bestPose: " << averagePose << std::endl;
00670 #endif
00671 
00672     if (bestResponse > 1.0)
00673     {
00674       bestResponse = 1.0;
00675     }
00676 
00677     assert(math::InRange(bestResponse, 0.0, 1.0));
00678     assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00679 
00680     return bestResponse;
00681   }
00682 
00693   void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse,
00694                                                 const Pose2& rSearchCenter,
00695                                                 const Vector2<kt_double>& rSearchSpaceOffset,
00696                                                 const Vector2<kt_double>& rSearchSpaceResolution,
00697                                                 kt_double searchAngleResolution, Matrix3& rCovariance)
00698   {
00699     // reset covariance to identity matrix
00700     rCovariance.SetToIdentity();
00701 
00702     // if best response is vary small return max variance
00703     if (bestResponse < KT_TOLERANCE)
00704     {
00705       rCovariance(0, 0) = MAX_VARIANCE;  // XX
00706       rCovariance(1, 1) = MAX_VARIANCE;  // YY
00707       rCovariance(2, 2) = 4 * math::Square(searchAngleResolution);  // TH*TH
00708 
00709       return;
00710     }
00711 
00712     kt_double accumulatedVarianceXX = 0;
00713     kt_double accumulatedVarianceXY = 0;
00714     kt_double accumulatedVarianceYY = 0;
00715     kt_double norm = 0;
00716 
00717     kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
00718     kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
00719 
00720     kt_double offsetX = rSearchSpaceOffset.GetX();
00721     kt_double offsetY = rSearchSpaceOffset.GetY();
00722 
00723     kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
00724     kt_double startX = -offsetX;
00725     assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
00726 
00727     kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
00728     kt_double startY = -offsetY;
00729     assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
00730 
00731     for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00732     {
00733       kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
00734 
00735       for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00736       {
00737         kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
00738 
00739         Vector2<kt_int32s> gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX() + x,
00740                                                                                            rSearchCenter.GetY() + y));
00741         kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint));
00742 
00743         // response is not a low response
00744         if (response >= (bestResponse - 0.1))
00745         {
00746           norm += response;
00747           accumulatedVarianceXX += (math::Square(x - dx) * response);
00748           accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
00749           accumulatedVarianceYY += (math::Square(y - dy) * response);
00750         }
00751       }
00752     }
00753 
00754     if (norm > KT_TOLERANCE)
00755     {
00756       kt_double varianceXX = accumulatedVarianceXX / norm;
00757       kt_double varianceXY = accumulatedVarianceXY / norm;
00758       kt_double varianceYY = accumulatedVarianceYY / norm;
00759       kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
00760 
00761       // lower-bound variances so that they are not too small;
00762       // ensures that links are not too tight
00763       kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
00764       kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
00765       varianceXX = math::Maximum(varianceXX, minVarianceXX);
00766       varianceYY = math::Maximum(varianceYY, minVarianceYY);
00767 
00768       // increase variance for poorer responses
00769       kt_double multiplier = 1.0 / bestResponse;
00770       rCovariance(0, 0) = varianceXX * multiplier;
00771       rCovariance(0, 1) = varianceXY * multiplier;
00772       rCovariance(1, 0) = varianceXY * multiplier;
00773       rCovariance(1, 1) = varianceYY * multiplier;
00774       rCovariance(2, 2) = varianceTHTH;  // this value will be set in ComputeAngularCovariance
00775     }
00776 
00777     // if values are 0, set to MAX_VARIANCE
00778     // values might be 0 if points are too sparse and thus don't hit other points
00779     if (math::DoubleEqual(rCovariance(0, 0), 0.0))
00780     {
00781       rCovariance(0, 0) = MAX_VARIANCE;
00782     }
00783 
00784     if (math::DoubleEqual(rCovariance(1, 1), 0.0))
00785     {
00786       rCovariance(1, 1) = MAX_VARIANCE;
00787     }
00788   }
00789 
00799   void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose,
00800                                              kt_double bestResponse,
00801                                              const Pose2& rSearchCenter,
00802                                              kt_double searchAngleOffset,
00803                                              kt_double searchAngleResolution,
00804                                              Matrix3& rCovariance)
00805   {
00806     // NOTE: do not reset covariance matrix
00807 
00808     // normalize angle difference
00809     kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
00810 
00811     Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
00812     kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
00813 
00814     kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
00815 
00816     kt_double angle = 0.0;
00817     kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
00818 
00819     kt_double norm = 0.0;
00820     kt_double accumulatedVarianceThTh = 0.0;
00821     for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00822     {
00823       angle = startAngle + angleIndex * searchAngleResolution;
00824       kt_double response = GetResponse(angleIndex, gridIndex);
00825 
00826       // response is not a low response
00827       if (response >= (bestResponse - 0.1))
00828       {
00829         norm += response;
00830         accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
00831       }
00832     }
00833     assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
00834 
00835     if (norm > KT_TOLERANCE)
00836     {
00837       if (accumulatedVarianceThTh < KT_TOLERANCE)
00838       {
00839         accumulatedVarianceThTh = math::Square(searchAngleResolution);
00840       }
00841 
00842       accumulatedVarianceThTh /= norm;
00843     }
00844     else
00845     {
00846       accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
00847     }
00848 
00849     rCovariance(2, 2) = accumulatedVarianceThTh;
00850   }
00851 
00857   void ScanMatcher::AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint)
00858   {
00859     m_pCorrelationGrid->Clear();
00860 
00861     // add all scans to grid
00862     const_forEach(LocalizedRangeScanVector, &rScans)
00863     {
00864       AddScan(*iter, viewPoint);
00865     }
00866   }
00867 
00874   void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear)
00875   {
00876     PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint);
00877 
00878     // put in all valid points
00879     const_forEach(PointVectorDouble, &validPoints)
00880     {
00881       Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(*iter);
00882       if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) ||
00883           !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight()))
00884       {
00885         // point not in grid
00886         continue;
00887       }
00888 
00889       int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
00890 
00891       // set grid cell as occupied
00892       if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
00893       {
00894         // value already set
00895         continue;
00896       }
00897 
00898       m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
00899 
00900       // smear grid
00901       if (doSmear == true)
00902       {
00903         m_pCorrelationGrid->SmearPoint(gridPoint);
00904       }
00905     }
00906   }
00907 
00914   PointVectorDouble ScanMatcher::FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const
00915   {
00916     const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
00917 
00918     // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
00919     const kt_double minSquareDistance = math::Square(0.1);  // in m^2
00920 
00921     // this iterator lags from the main iterator adding points only when the points are on
00922     // the same side as the viewpoint
00923     PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin();
00924     PointVectorDouble validPoints;
00925 
00926     Vector2<kt_double> firstPoint;
00927     kt_bool firstTime = true;
00928     const_forEach(PointVectorDouble, &rPointReadings)
00929     {
00930       Vector2<kt_double> currentPoint = *iter;
00931 
00932       if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY()))
00933       {
00934         firstPoint = currentPoint;
00935         firstTime = false;
00936       }
00937 
00938       Vector2<kt_double> delta = firstPoint - currentPoint;
00939       if (delta.SquaredLength() > minSquareDistance)
00940       {
00941         // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
00942         // Which computes the direction of rotation, if the rotation is counterclock
00943         // wise then we are looking at data we should keep. If it's negative rotation
00944         // we should not included in in the matching
00945         // have enough distance, check viewpoint
00946         double a = rViewPoint.GetY() - firstPoint.GetY();
00947         double b = firstPoint.GetX() - rViewPoint.GetX();
00948         double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
00949         double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
00950 
00951         // reset beginning point
00952         firstPoint = currentPoint;
00953 
00954         if (ss < 0.0)  // wrong side, skip and keep going
00955         {
00956           trailingPointIter = iter;
00957         }
00958         else
00959         {
00960           for (; trailingPointIter != iter; ++trailingPointIter)
00961           {
00962             validPoints.push_back(*trailingPointIter);
00963           }
00964         }
00965       }
00966     }
00967 
00968     return validPoints;
00969   }
00970 
00977   kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
00978   {
00979     kt_double response = 0.0;
00980 
00981     // add up value for each point
00982     kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;
00983 
00984     const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
00985     assert(pOffsets != NULL);
00986 
00987     // get number of points in offset list
00988     kt_int32u nPoints = pOffsets->GetSize();
00989     if (nPoints == 0)
00990     {
00991       return response;
00992     }
00993 
00994     // calculate response
00995     kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
00996     for (kt_int32u i = 0; i < nPoints; i++)
00997     {
00998       // ignore points that fall off the grid
00999       kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
01000       if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN)
01001       {
01002         continue;
01003       }
01004 
01005       // uses index offsets to efficiently find location of point in the grid
01006       response += pByte[pAngleIndexPointer[i]];
01007     }
01008 
01009     // normalize response
01010     response /= (nPoints * GridStates_Occupied);
01011     assert(fabs(response) <= 1.0);
01012 
01013     return response;
01014   }
01015 
01016 
01020 
01021   template<typename T>
01022   class BreadthFirstTraversal : public GraphTraversal<T>
01023   {
01024   public:
01028     BreadthFirstTraversal(Graph<T>* pGraph)
01029     : GraphTraversal<T>(pGraph)
01030     {
01031     }
01032 
01036     virtual ~BreadthFirstTraversal()
01037     {
01038     }
01039 
01040   public:
01047     virtual std::vector<T*> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
01048     {
01049       std::queue<Vertex<T>*> toVisit;
01050       std::set<Vertex<T>*> seenVertices;
01051       std::vector<Vertex<T>*> validVertices;
01052 
01053       toVisit.push(pStartVertex);
01054       seenVertices.insert(pStartVertex);
01055 
01056       do
01057       {
01058         Vertex<T>* pNext = toVisit.front();
01059         toVisit.pop();
01060 
01061         if (pVisitor->Visit(pNext))
01062         {
01063           // vertex is valid, explore neighbors
01064           validVertices.push_back(pNext);
01065 
01066           std::vector<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
01067           forEach(typename std::vector<Vertex<T>*>, &adjacentVertices)
01068           {
01069             Vertex<T>* pAdjacent = *iter;
01070 
01071             // adjacent vertex has not yet been seen, add to queue for processing
01072             if (seenVertices.find(pAdjacent) == seenVertices.end())
01073             {
01074               toVisit.push(pAdjacent);
01075               seenVertices.insert(pAdjacent);
01076             }
01077           }
01078         }
01079       } while (toVisit.empty() == false);
01080 
01081       std::vector<T*> objects;
01082       forEach(typename std::vector<Vertex<T>*>, &validVertices)
01083       {
01084         objects.push_back((*iter)->GetObject());
01085       }
01086 
01087       return objects;
01088     }
01089   };  // class BreadthFirstTraversal
01090 
01094 
01095   class NearScanVisitor : public Visitor<LocalizedRangeScan>
01096   {
01097   public:
01098     NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
01099       : m_MaxDistanceSquared(math::Square(maxDistance))
01100       , m_UseScanBarycenter(useScanBarycenter)
01101     {
01102       m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
01103     }
01104 
01105     virtual kt_bool Visit(Vertex<LocalizedRangeScan>* pVertex)
01106     {
01107       LocalizedRangeScan* pScan = pVertex->GetObject();
01108 
01109       Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
01110 
01111       kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
01112       return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
01113     }
01114 
01115   protected:
01116     Pose2 m_CenterPose;
01117     kt_double m_MaxDistanceSquared;
01118     kt_bool m_UseScanBarycenter;
01119   };  // NearScanVisitor
01120 
01124 
01125   MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold)
01126     : m_pMapper(pMapper)
01127   {
01128     m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(),
01129                                              m_pMapper->m_pLoopSearchSpaceResolution->GetValue(),
01130                                              m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold);
01131     assert(m_pLoopScanMatcher);
01132 
01133     m_pTraversal = new BreadthFirstTraversal<LocalizedRangeScan>(this);
01134   }
01135 
01136   MapperGraph::~MapperGraph()
01137   {
01138     delete m_pLoopScanMatcher;
01139     m_pLoopScanMatcher = NULL;
01140 
01141     delete m_pTraversal;
01142     m_pTraversal = NULL;
01143   }
01144 
01145   void MapperGraph::AddVertex(LocalizedRangeScan* pScan)
01146   {
01147     assert(pScan);
01148 
01149     if (pScan != NULL)
01150     {
01151       Vertex<LocalizedRangeScan>* pVertex = new Vertex<LocalizedRangeScan>(pScan);
01152       Graph<LocalizedRangeScan>::AddVertex(pScan->GetSensorName(), pVertex);
01153       if (m_pMapper->m_pScanOptimizer != NULL)
01154       {
01155         m_pMapper->m_pScanOptimizer->AddNode(pVertex);
01156       }
01157     }
01158   }
01159 
01160   void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance)
01161   {
01162     MapperSensorManager* pSensorManager = m_pMapper->m_pMapperSensorManager;
01163 
01164     const Name& rSensorName = pScan->GetSensorName();
01165 
01166     // link to previous scan
01167     kt_int32s previousScanNum = pScan->GetStateId() - 1;
01168     if (pSensorManager->GetLastScan(rSensorName) != NULL)
01169     {
01170       assert(previousScanNum >= 0);
01171       LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), pScan, pScan->GetSensorPose(), rCovariance);
01172     }
01173 
01174     Pose2Vector means;
01175     std::vector<Matrix3> covariances;
01176 
01177     // first scan (link to first scan of other robots)
01178     if (pSensorManager->GetLastScan(rSensorName) == NULL)
01179     {
01180       assert(pSensorManager->GetScans(rSensorName).size() == 1);
01181 
01182       std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
01183       forEach(std::vector<Name>, &deviceNames)
01184       {
01185         const Name& rCandidateSensorName = *iter;
01186 
01187         // skip if candidate device is the same or other device has no scans
01188         if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty()))
01189         {
01190           continue;
01191         }
01192 
01193         Pose2 bestPose;
01194         Matrix3 covariance;
01195         kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan,
01196                                                                   pSensorManager->GetScans(rCandidateSensorName),
01197                                                                   bestPose, covariance);
01198         LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance);
01199 
01200         // only add to means and covariances if response was high "enough"
01201         if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue())
01202         {
01203           means.push_back(bestPose);
01204           covariances.push_back(covariance);
01205         }
01206       }
01207     }
01208     else
01209     {
01210       // link to running scans
01211       Pose2 scanPose = pScan->GetSensorPose();
01212       means.push_back(scanPose);
01213       covariances.push_back(rCovariance);
01214       LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
01215     }
01216 
01217     // link to other near chains (chains that include new scan are invalid)
01218     LinkNearChains(pScan, means, covariances);
01219 
01220     if (!means.empty())
01221     {
01222       pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
01223     }
01224   }
01225 
01226   kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
01227   {
01228     kt_bool loopClosed = false;
01229 
01230     kt_int32u scanIndex = 0;
01231 
01232     LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01233 
01234     while (!candidateChain.empty())
01235     {
01236       Pose2 bestPose;
01237       Matrix3 covariance;
01238       kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,
01239                                                                bestPose, covariance, false, false);
01240 
01241       std::stringstream stream;
01242       stream << "COARSE RESPONSE: " << coarseResponse
01243              << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")"
01244              << std::endl;
01245       stream << "            var: " << covariance(0, 0) << ",  " << covariance(1, 1)
01246              << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
01247 
01248       m_pMapper->FireLoopClosureCheck(stream.str());
01249 
01250       if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
01251           (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
01252           (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
01253       {
01254         LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
01255         tmpScan.SetUniqueId(pScan->GetUniqueId());
01256         tmpScan.SetTime(pScan->GetTime());
01257         tmpScan.SetStateId(pScan->GetStateId());
01258         tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
01259         tmpScan.SetSensorPose(bestPose);  // This also updates OdometricPose.
01260         kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,
01261                                                                                 bestPose, covariance, false);
01262 
01263         std::stringstream stream1;
01264         stream1 << "FINE RESPONSE: " << fineResponse << " (>"
01265                 << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl;
01266         m_pMapper->FireLoopClosureCheck(stream1.str());
01267 
01268         if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
01269         {
01270           m_pMapper->FireLoopClosureCheck("REJECTED!");
01271         }
01272         else
01273         {
01274           m_pMapper->FireBeginLoopClosure("Closing loop...");
01275 
01276           pScan->SetSensorPose(bestPose);
01277           LinkChainToScan(candidateChain, pScan, bestPose, covariance);
01278           CorrectPoses();
01279 
01280           m_pMapper->FireEndLoopClosure("Loop closed!");
01281 
01282           loopClosed = true;
01283         }
01284       }
01285 
01286       candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01287     }
01288 
01289     return loopClosed;
01290   }
01291 
01292   LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans,
01293                                                         const Pose2& rPose) const
01294   {
01295     LocalizedRangeScan* pClosestScan = NULL;
01296     kt_double bestSquaredDistance = DBL_MAX;
01297 
01298     const_forEach(LocalizedRangeScanVector, &rScans)
01299     {
01300       Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01301 
01302       kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
01303       if (squaredDistance < bestSquaredDistance)
01304       {
01305         bestSquaredDistance = squaredDistance;
01306         pClosestScan = *iter;
01307       }
01308     }
01309 
01310     return pClosestScan;
01311   }
01312 
01313   Edge<LocalizedRangeScan>* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan,
01314                                                  LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge)
01315   {
01316     // check that vertex exists
01317     assert(pSourceScan->GetStateId() < (kt_int32s)m_Vertices[pSourceScan->GetSensorName()].size());
01318     assert(pTargetScan->GetStateId() < (kt_int32s)m_Vertices[pTargetScan->GetSensorName()].size());
01319 
01320     Vertex<LocalizedRangeScan>* v1 = m_Vertices[pSourceScan->GetSensorName()][pSourceScan->GetStateId()];
01321     Vertex<LocalizedRangeScan>* v2 = m_Vertices[pTargetScan->GetSensorName()][pTargetScan->GetStateId()];
01322 
01323     // see if edge already exists
01324     const_forEach(std::vector<Edge<LocalizedRangeScan>*>, &(v1->GetEdges()))
01325     {
01326       Edge<LocalizedRangeScan>* pEdge = *iter;
01327 
01328       if (pEdge->GetTarget() == v2)
01329       {
01330         rIsNewEdge = false;
01331         return pEdge;
01332       }
01333     }
01334 
01335     Edge<LocalizedRangeScan>* pEdge = new Edge<LocalizedRangeScan>(v1, v2);
01336     Graph<LocalizedRangeScan>::AddEdge(pEdge);
01337     rIsNewEdge = true;
01338     return pEdge;
01339   }
01340 
01341   void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan,
01342                               const Pose2& rMean, const Matrix3& rCovariance)
01343   {
01344     kt_bool isNewEdge = true;
01345     Edge<LocalizedRangeScan>* pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
01346 
01347     // only attach link information if the edge is new
01348     if (isNewEdge == true)
01349     {
01350       pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
01351       if (m_pMapper->m_pScanOptimizer != NULL)
01352       {
01353         m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
01354       }
01355     }
01356   }
01357 
01358   void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances)
01359   {
01360     const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
01361     const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains)
01362     {
01363       if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
01364       {
01365         continue;
01366       }
01367 
01368       Pose2 mean;
01369       Matrix3 covariance;
01370       // match scan against "near" chain
01371       kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
01372       if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE)
01373       {
01374         rMeans.push_back(mean);
01375         rCovariances.push_back(covariance);
01376         LinkChainToScan(*iter, pScan, mean, covariance);
01377       }
01378     }
01379   }
01380 
01381   void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan,
01382                                     const Pose2& rMean, const Matrix3& rCovariance)
01383   {
01384     Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01385 
01386     LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose);
01387     assert(pClosestScan != NULL);
01388 
01389     Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01390 
01391     kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
01392     if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01393     {
01394       LinkScans(pClosestScan, pScan, rMean, rCovariance);
01395     }
01396   }
01397 
01398   std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan* pScan)
01399   {
01400     std::vector<LocalizedRangeScanVector> nearChains;
01401 
01402     Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01403 
01404     // to keep track of which scans have been added to a chain
01405     LocalizedRangeScanVector processed;
01406 
01407     const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
01408                                                      m_pMapper->m_pLinkScanMaximumDistance->GetValue());
01409     const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
01410     {
01411       LocalizedRangeScan* pNearScan = *iter;
01412 
01413       if (pNearScan == pScan)
01414       {
01415         continue;
01416       }
01417 
01418       // scan has already been processed, skip
01419       if (find(processed.begin(), processed.end(), pNearScan) != processed.end())
01420       {
01421         continue;
01422       }
01423 
01424       processed.push_back(pNearScan);
01425 
01426       // build up chain
01427       kt_bool isValidChain = true;
01428       std::list<LocalizedRangeScan*> chain;
01429 
01430       // add scans before current scan being processed
01431       for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--)
01432       {
01433         LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
01434                                                                                         candidateScanNum);
01435 
01436         // chain is invalid--contains scan being added
01437         if (pCandidateScan == pScan)
01438         {
01439           isValidChain = false;
01440         }
01441 
01442         Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01443         kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01444 
01445         if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01446         {
01447           chain.push_front(pCandidateScan);
01448           processed.push_back(pCandidateScan);
01449         }
01450         else
01451         {
01452           break;
01453         }
01454       }
01455 
01456       chain.push_back(pNearScan);
01457 
01458       // add scans after current scan being processed
01459       kt_int32u end = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size());
01460       for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++)
01461       {
01462         LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
01463                                                                                         candidateScanNum);
01464 
01465         if (pCandidateScan == pScan)
01466         {
01467           isValidChain = false;
01468         }
01469 
01470         Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());;
01471         kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01472 
01473         if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01474         {
01475           chain.push_back(pCandidateScan);
01476           processed.push_back(pCandidateScan);
01477         }
01478         else
01479         {
01480           break;
01481         }
01482       }
01483 
01484       if (isValidChain)
01485       {
01486         // change list to vector
01487         LocalizedRangeScanVector tempChain;
01488         std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));
01489         // add chain to collection
01490         nearChains.push_back(tempChain);
01491       }
01492     }
01493 
01494     return nearChains;
01495   }
01496 
01497   LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance)
01498   {
01499     NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
01500     LocalizedRangeScanVector nearLinkedScans = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
01501     delete pVisitor;
01502 
01503     return nearLinkedScans;
01504   }
01505 
01506   Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const
01507   {
01508     assert(rMeans.size() == rCovariances.size());
01509 
01510     // compute sum of inverses and create inverse list
01511     std::vector<Matrix3> inverses;
01512     inverses.reserve(rCovariances.size());
01513 
01514     Matrix3 sumOfInverses;
01515     const_forEach(std::vector<Matrix3>, &rCovariances)
01516     {
01517       Matrix3 inverse = iter->Inverse();
01518       inverses.push_back(inverse);
01519 
01520       sumOfInverses += inverse;
01521     }
01522     Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
01523 
01524     // compute weighted mean
01525     Pose2 accumulatedPose;
01526     kt_double thetaX = 0.0;
01527     kt_double thetaY = 0.0;
01528 
01529     Pose2Vector::const_iterator meansIter = rMeans.begin();
01530     const_forEach(std::vector<Matrix3>, &inverses)
01531     {
01532       Pose2 pose = *meansIter;
01533       kt_double angle = pose.GetHeading();
01534       thetaX += cos(angle);
01535       thetaY += sin(angle);
01536 
01537       Matrix3 weight = inverseOfSumOfInverses * (*iter);
01538       accumulatedPose += weight * pose;
01539 
01540       ++meansIter;
01541     }
01542 
01543     thetaX /= rMeans.size();
01544     thetaY /= rMeans.size();
01545     accumulatedPose.SetHeading(atan2(thetaY, thetaX));
01546 
01547     return accumulatedPose;
01548   }
01549 
01550   LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan,
01551                                                                 const Name& rSensorName,
01552                                                                 kt_int32u& rStartNum)
01553   {
01554     LocalizedRangeScanVector chain;  // return value
01555 
01556     Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01557 
01558     // possible loop closure chain should not include close scans that have a
01559     // path of links to the scan of interest
01560     const LocalizedRangeScanVector nearLinkedScans =
01561           FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue());
01562 
01563     kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
01564     for (; rStartNum < nScans; rStartNum++)
01565     {
01566       LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
01567 
01568       Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
01569 
01570       kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
01571       if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
01572       {
01573         // a linked scan cannot be in the chain
01574         if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
01575         {
01576           chain.clear();
01577         }
01578         else
01579         {
01580           chain.push_back(pCandidateScan);
01581         }
01582       }
01583       else
01584       {
01585         // return chain if it is long "enough"
01586         if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
01587         {
01588           return chain;
01589         }
01590         else
01591         {
01592           chain.clear();
01593         }
01594       }
01595     }
01596 
01597     return chain;
01598   }
01599 
01600   void MapperGraph::CorrectPoses()
01601   {
01602     // optimize scans!
01603     ScanSolver* pSolver = m_pMapper->m_pScanOptimizer;
01604     if (pSolver != NULL)
01605     {
01606       pSolver->Compute();
01607 
01608       const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
01609       {
01610         m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);
01611       }
01612 
01613       pSolver->Clear();
01614     }
01615   }
01616 
01620 
01624   Mapper::Mapper()
01625     : Module("Mapper")
01626     , m_Initialized(false)
01627     , m_pSequentialScanMatcher(NULL)
01628     , m_pMapperSensorManager(NULL)
01629     , m_pGraph(NULL)
01630     , m_pScanOptimizer(NULL)
01631   {
01632     InitializeParameters();
01633   }
01634 
01638   Mapper::Mapper(const std::string& rName)
01639     : Module(rName)
01640     , m_Initialized(false)
01641     , m_pSequentialScanMatcher(NULL)
01642     , m_pMapperSensorManager(NULL)
01643     , m_pGraph(NULL)
01644     , m_pScanOptimizer(NULL)
01645   {
01646     InitializeParameters();
01647   }
01648 
01652   Mapper::~Mapper()
01653   {
01654     Reset();
01655 
01656     delete m_pMapperSensorManager;
01657   }
01658 
01659   void Mapper::InitializeParameters()
01660   {
01661     m_pUseScanMatching = new Parameter<kt_bool>(
01662         "UseScanMatching",
01663         "When set to true, the mapper will use a scan matching algorithm. "
01664         "In most real-world situations this should be set to true so that the "
01665         "mapper algorithm can correct for noise and errors in odometry and "
01666         "scan data. In some simulator environments where the simulated scan "
01667         "and odometry data are very accurate, the scan matching algorithm can "
01668         "produce worse results. In those cases set this to false to improve "
01669         "results.",
01670         true,
01671         GetParameterManager());
01672 
01673     m_pUseScanBarycenter = new Parameter<kt_bool>(
01674         "UseScanBarycenter",
01675         "Use the barycenter of scan endpoints to define distances between "
01676         "scans.",
01677         true, GetParameterManager());
01678 
01679     m_pMinimumTimeInterval = new Parameter<kt_double>(
01680         "MinimumTimeInterval",
01681         "Sets the minimum time between scans. If a new scan's time stamp is "
01682         "longer than MinimumTimeInterval from the previously processed scan, "
01683         "the mapper will use the data from the new scan. Otherwise, it will "
01684         "discard the new scan if it also does not meet the minimum travel "
01685         "distance and heading requirements. For performance reasons, it is "
01686         "generally it is a good idea to only process scans if a reasonable "
01687         "amount of time has passed. This parameter is particularly useful "
01688         "when there is a need to process scans while the robot is stationary.",
01689         3600, GetParameterManager());
01690 
01691     m_pMinimumTravelDistance = new Parameter<kt_double>(
01692         "MinimumTravelDistance",
01693         "Sets the minimum travel between scans.  If a new scan's position is "
01694         "more than minimumTravelDistance from the previous scan, the mapper "
01695         "will use the data from the new scan. Otherwise, it will discard the "
01696         "new scan if it also does not meet the minimum change in heading "
01697         "requirement. For performance reasons, generally it is a good idea to "
01698         "only process scans if the robot has moved a reasonable amount.",
01699         0.2, GetParameterManager());
01700 
01701     m_pMinimumTravelHeading = new Parameter<kt_double>(
01702         "MinimumTravelHeading",
01703         "Sets the minimum heading change between scans. If a new scan's "
01704         "heading is more than MinimumTravelHeading from the previous scan, the "
01705         "mapper will use the data from the new scan.  Otherwise, it will "
01706         "discard the new scan if it also does not meet the minimum travel "
01707         "distance requirement. For performance reasons, generally it is a good "
01708         "idea to only process scans if the robot has moved a reasonable "
01709         "amount.",
01710         math::DegreesToRadians(10), GetParameterManager());
01711 
01712     m_pScanBufferSize = new Parameter<kt_int32u>(
01713         "ScanBufferSize",
01714         "Scan buffer size is the length of the scan chain stored for scan "
01715         "matching. \"ScanBufferSize\" should be set to approximately "
01716         "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The "
01717         "idea is to get an area approximately 20 meters long for scan "
01718         "matching. For example, if we add scans every MinimumTravelDistance == "
01719         "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)",
01720         70, GetParameterManager());
01721 
01722     m_pScanBufferMaximumScanDistance = new Parameter<kt_double>(
01723         "ScanBufferMaximumScanDistance",
01724         "Scan buffer maximum scan distance is the maximum distance between the "
01725         "first and last scans in the scan chain stored for matching.",
01726         20.0, GetParameterManager());
01727 
01728     m_pLinkMatchMinimumResponseFine = new Parameter<kt_double>(
01729         "LinkMatchMinimumResponseFine",
01730         "Scans are linked only if the correlation response value is greater "
01731         "than this value.",
01732         0.8, GetParameterManager());
01733 
01734     m_pLinkScanMaximumDistance = new Parameter<kt_double>(
01735         "LinkScanMaximumDistance",
01736         "Maximum distance between linked scans.  Scans that are farther apart "
01737         "will not be linked regardless of the correlation response value.",
01738         10.0, GetParameterManager());
01739 
01740     m_pLoopSearchMaximumDistance = new Parameter<kt_double>(
01741         "LoopSearchMaximumDistance",
01742         "Scans less than this distance from the current position will be "
01743         "considered for a match in loop closure.",
01744         4.0, GetParameterManager());
01745 
01746     m_pDoLoopClosing = new Parameter<kt_bool>(
01747         "DoLoopClosing",
01748         "Enable/disable loop closure.",
01749         true, GetParameterManager());
01750 
01751     m_pLoopMatchMinimumChainSize = new Parameter<kt_int32u>(
01752         "LoopMatchMinimumChainSize",
01753         "When the loop closure detection finds a candidate it must be part of "
01754         "a large set of linked scans. If the chain of scans is less than this "
01755         "value we do not attempt to close the loop.",
01756         10, GetParameterManager());
01757 
01758     m_pLoopMatchMaximumVarianceCoarse = new Parameter<kt_double>(
01759         "LoopMatchMaximumVarianceCoarse",
01760         "The co-variance values for a possible loop closure have to be less "
01761         "than this value to consider a viable solution. This applies to the "
01762         "coarse search.",
01763         math::Square(0.4), GetParameterManager());
01764 
01765     m_pLoopMatchMinimumResponseCoarse = new Parameter<kt_double>(
01766         "LoopMatchMinimumResponseCoarse",
01767         "If response is larger then this, then initiate loop closure search at "
01768         "the coarse resolution.",
01769         0.8, GetParameterManager());
01770 
01771     m_pLoopMatchMinimumResponseFine = new Parameter<kt_double>(
01772         "LoopMatchMinimumResponseFine",
01773         "If response is larger then this, then initiate loop closure search at "
01774         "the fine resolution.",
01775         0.8, GetParameterManager());
01776 
01778     //    CorrelationParameters correlationParameters;
01779 
01780     m_pCorrelationSearchSpaceDimension = new Parameter<kt_double>(
01781         "CorrelationSearchSpaceDimension",
01782         "The size of the search grid used by the matcher. The search grid will "
01783         "have the size CorrelationSearchSpaceDimension * "
01784         "CorrelationSearchSpaceDimension",
01785         0.3, GetParameterManager());
01786 
01787     m_pCorrelationSearchSpaceResolution = new Parameter<kt_double>(
01788         "CorrelationSearchSpaceResolution",
01789         "The resolution (size of a grid cell) of the correlation grid.",
01790         0.01, GetParameterManager());
01791 
01792     m_pCorrelationSearchSpaceSmearDeviation = new Parameter<kt_double>(
01793         "CorrelationSearchSpaceSmearDeviation",
01794         "The point readings are smeared by this value in X and Y to create a "
01795         "smoother response.",
01796         0.03, GetParameterManager());
01797 
01798 
01800     //    CorrelationParameters loopCorrelationParameters;
01801 
01802     m_pLoopSearchSpaceDimension = new Parameter<kt_double>(
01803         "LoopSearchSpaceDimension",
01804         "The size of the search grid used by the matcher.",
01805         8.0, GetParameterManager());
01806 
01807     m_pLoopSearchSpaceResolution = new Parameter<kt_double>(
01808         "LoopSearchSpaceResolution",
01809         "The resolution (size of a grid cell) of the correlation grid.",
01810         0.05, GetParameterManager());
01811 
01812     m_pLoopSearchSpaceSmearDeviation = new Parameter<kt_double>(
01813         "LoopSearchSpaceSmearDeviation",
01814         "The point readings are smeared by this value in X and Y to create a "
01815         "smoother response.",
01816         0.03, GetParameterManager());
01817 
01819     // ScanMatcherParameters;
01820 
01821     m_pDistanceVariancePenalty = new Parameter<kt_double>(
01822         "DistanceVariancePenalty",
01823         "Variance of penalty for deviating from odometry when scan-matching. "
01824         "The penalty is a multiplier (less than 1.0) is a function of the "
01825         "delta of the scan position being tested and the odometric pose.",
01826         math::Square(0.3), GetParameterManager());
01827 
01828     m_pAngleVariancePenalty = new Parameter<kt_double>(
01829         "AngleVariancePenalty",
01830         "See DistanceVariancePenalty.",
01831         math::Square(math::DegreesToRadians(20)), GetParameterManager());
01832 
01833     m_pFineSearchAngleOffset = new Parameter<kt_double>(
01834         "FineSearchAngleOffset",
01835         "The range of angles to search during a fine search.",
01836         math::DegreesToRadians(0.2), GetParameterManager());
01837 
01838     m_pCoarseSearchAngleOffset = new Parameter<kt_double>(
01839         "CoarseSearchAngleOffset",
01840         "The range of angles to search during a coarse search.",
01841         math::DegreesToRadians(20), GetParameterManager());
01842 
01843     m_pCoarseAngleResolution = new Parameter<kt_double>(
01844         "CoarseAngleResolution",
01845         "Resolution of angles to search during a coarse search.",
01846         math::DegreesToRadians(2), GetParameterManager());
01847 
01848     m_pMinimumAnglePenalty = new Parameter<kt_double>(
01849         "MinimumAnglePenalty",
01850         "Minimum value of the angle penalty multiplier so scores do not become "
01851         "too small.",
01852         0.9, GetParameterManager());
01853 
01854     m_pMinimumDistancePenalty = new Parameter<kt_double>(
01855         "MinimumDistancePenalty",
01856         "Minimum value of the distance penalty multiplier so scores do not "
01857         "become too small.",
01858         0.5, GetParameterManager());
01859 
01860     m_pUseResponseExpansion = new Parameter<kt_bool>(
01861         "UseResponseExpansion",
01862         "Whether to increase the search space if no good matches are initially "
01863         "found.",
01864         false, GetParameterManager());
01865   }
01866   /* Adding in getters and setters here for easy parameter access */
01867 
01868   // General Parameters
01869 
01870   bool Mapper::getParamUseScanMatching()
01871   {
01872     return static_cast<bool>(m_pUseScanMatching->GetValue());
01873   }
01874 
01875   bool Mapper::getParamUseScanBarycenter()
01876   {
01877     return static_cast<bool>(m_pUseScanBarycenter->GetValue());
01878   }
01879 
01880   double Mapper::getParamMinimumTimeInterval()
01881   {
01882     return static_cast<double>(m_pMinimumTimeInterval->GetValue());
01883   }
01884 
01885   double Mapper::getParamMinimumTravelDistance()
01886   {
01887     return static_cast<double>(m_pMinimumTravelDistance->GetValue());
01888   }
01889 
01890   double Mapper::getParamMinimumTravelHeading()
01891   {
01892     return math::RadiansToDegrees(static_cast<double>(m_pMinimumTravelHeading->GetValue()));
01893   }
01894 
01895   int Mapper::getParamScanBufferSize()
01896   {
01897     return static_cast<int>(m_pScanBufferSize->GetValue());
01898   }
01899 
01900   double Mapper::getParamScanBufferMaximumScanDistance()
01901   {
01902     return static_cast<double>(m_pScanBufferMaximumScanDistance->GetValue());
01903   }
01904 
01905   double Mapper::getParamLinkMatchMinimumResponseFine()
01906   {
01907     return static_cast<double>(m_pLinkMatchMinimumResponseFine->GetValue());
01908   }
01909 
01910   double Mapper::getParamLinkScanMaximumDistance()
01911   {
01912     return static_cast<double>(m_pLinkScanMaximumDistance->GetValue());
01913   }
01914 
01915   double Mapper::getParamLoopSearchMaximumDistance()
01916   {
01917     return static_cast<double>(m_pLoopSearchMaximumDistance->GetValue());
01918   }
01919 
01920   bool Mapper::getParamDoLoopClosing()
01921   {
01922     return static_cast<bool>(m_pDoLoopClosing->GetValue());
01923   }
01924 
01925   int Mapper::getParamLoopMatchMinimumChainSize()
01926   {
01927     return static_cast<int>(m_pLoopMatchMinimumChainSize->GetValue());
01928   }
01929 
01930   double Mapper::getParamLoopMatchMaximumVarianceCoarse()
01931   {
01932     return static_cast<double>(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue()));
01933   }
01934 
01935   double Mapper::getParamLoopMatchMinimumResponseCoarse()
01936   {
01937     return static_cast<double>(m_pLoopMatchMinimumResponseCoarse->GetValue());
01938   }
01939 
01940   double Mapper::getParamLoopMatchMinimumResponseFine()
01941   {
01942     return static_cast<double>(m_pLoopMatchMinimumResponseFine->GetValue());
01943   }
01944 
01945   // Correlation Parameters - Correlation Parameters
01946 
01947   double Mapper::getParamCorrelationSearchSpaceDimension()
01948   {
01949     return static_cast<double>(m_pCorrelationSearchSpaceDimension->GetValue());
01950   }
01951 
01952   double Mapper::getParamCorrelationSearchSpaceResolution()
01953   {
01954     return static_cast<double>(m_pCorrelationSearchSpaceResolution->GetValue());
01955   }
01956 
01957   double Mapper::getParamCorrelationSearchSpaceSmearDeviation()
01958   {
01959     return static_cast<double>(m_pCorrelationSearchSpaceSmearDeviation->GetValue());
01960   }
01961 
01962   // Correlation Parameters - Loop Correlation Parameters
01963 
01964   double Mapper::getParamLoopSearchSpaceDimension()
01965   {
01966     return static_cast<double>(m_pLoopSearchSpaceDimension->GetValue());
01967   }
01968 
01969   double Mapper::getParamLoopSearchSpaceResolution()
01970   {
01971     return static_cast<double>(m_pLoopSearchSpaceResolution->GetValue());
01972   }
01973 
01974   double Mapper::getParamLoopSearchSpaceSmearDeviation()
01975   {
01976     return static_cast<double>(m_pLoopSearchSpaceSmearDeviation->GetValue());
01977   }
01978 
01979   // ScanMatcher Parameters
01980 
01981   double Mapper::getParamDistanceVariancePenalty()
01982   {
01983     return std::sqrt(static_cast<double>(m_pDistanceVariancePenalty->GetValue()));
01984   }
01985 
01986   double Mapper::getParamAngleVariancePenalty()
01987   {
01988     return std::sqrt(static_cast<double>(m_pAngleVariancePenalty->GetValue()));
01989   }
01990 
01991   double Mapper::getParamFineSearchAngleOffset()
01992   {
01993     return static_cast<double>(m_pFineSearchAngleOffset->GetValue());
01994   }
01995 
01996   double Mapper::getParamCoarseSearchAngleOffset()
01997   {
01998     return static_cast<double>(m_pCoarseSearchAngleOffset->GetValue());
01999   }
02000 
02001   double Mapper::getParamCoarseAngleResolution()
02002   {
02003     return static_cast<double>(m_pCoarseAngleResolution->GetValue());
02004   }
02005 
02006   double Mapper::getParamMinimumAnglePenalty()
02007   {
02008     return static_cast<double>(m_pMinimumAnglePenalty->GetValue());
02009   }
02010 
02011   double Mapper::getParamMinimumDistancePenalty()
02012   {
02013     return static_cast<double>(m_pMinimumDistancePenalty->GetValue());
02014   }
02015 
02016   bool Mapper::getParamUseResponseExpansion()
02017   {
02018     return static_cast<bool>(m_pUseResponseExpansion->GetValue());
02019   }
02020 
02021   /* Setters for parameters */
02022   // General Parameters
02023   void Mapper::setParamUseScanMatching(bool b)
02024   {
02025     m_pUseScanMatching->SetValue((kt_bool)b);
02026   }
02027 
02028   void Mapper::setParamUseScanBarycenter(bool b)
02029   {
02030     m_pUseScanBarycenter->SetValue((kt_bool)b);
02031   }
02032 
02033   void Mapper::setParamMinimumTimeInterval(double d)
02034   {
02035     m_pMinimumTimeInterval->SetValue((kt_double)d);
02036   }
02037 
02038   void Mapper::setParamMinimumTravelDistance(double d)
02039   {
02040     m_pMinimumTravelDistance->SetValue((kt_double)d);
02041   }
02042 
02043   void Mapper::setParamMinimumTravelHeading(double d)
02044   {
02045     m_pMinimumTravelHeading->SetValue((kt_double)d);
02046   }
02047 
02048   void Mapper::setParamScanBufferSize(int i)
02049   {
02050     m_pScanBufferSize->SetValue((kt_int32u)i);
02051   }
02052 
02053   void Mapper::setParamScanBufferMaximumScanDistance(double d)
02054   {
02055     m_pScanBufferMaximumScanDistance->SetValue((kt_double)d);
02056   }
02057 
02058   void Mapper::setParamLinkMatchMinimumResponseFine(double d)
02059   {
02060     m_pLinkMatchMinimumResponseFine->SetValue((kt_double)d);
02061   }
02062 
02063   void Mapper::setParamLinkScanMaximumDistance(double d)
02064   {
02065     m_pLinkScanMaximumDistance->SetValue((kt_double)d);
02066   }
02067 
02068   void Mapper::setParamLoopSearchMaximumDistance(double d)
02069   {
02070     m_pLoopSearchMaximumDistance->SetValue((kt_double)d);
02071   }
02072 
02073   void Mapper::setParamDoLoopClosing(bool b)
02074   {
02075     m_pDoLoopClosing->SetValue((kt_bool)b);
02076   }
02077 
02078   void Mapper::setParamLoopMatchMinimumChainSize(int i)
02079   {
02080     m_pLoopMatchMinimumChainSize->SetValue((kt_int32u)i);
02081   }
02082 
02083   void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d)
02084   {
02085     m_pLoopMatchMaximumVarianceCoarse->SetValue((kt_double)math::Square(d));
02086   }
02087 
02088   void Mapper::setParamLoopMatchMinimumResponseCoarse(double d)
02089   {
02090     m_pLoopMatchMinimumResponseCoarse->SetValue((kt_double)d);
02091   }
02092 
02093   void Mapper::setParamLoopMatchMinimumResponseFine(double d)
02094   {
02095     m_pLoopMatchMinimumResponseFine->SetValue((kt_double)d);
02096   }
02097 
02098   // Correlation Parameters - Correlation Parameters
02099   void Mapper::setParamCorrelationSearchSpaceDimension(double d)
02100   {
02101     m_pCorrelationSearchSpaceDimension->SetValue((kt_double)d);
02102   }
02103 
02104   void Mapper::setParamCorrelationSearchSpaceResolution(double d)
02105   {
02106     m_pCorrelationSearchSpaceResolution->SetValue((kt_double)d);
02107   }
02108 
02109   void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d)
02110   {
02111     m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d);
02112   }
02113 
02114 
02115   // Correlation Parameters - Loop Closure Parameters
02116   void Mapper::setParamLoopSearchSpaceDimension(double d)
02117   {
02118     m_pLoopSearchSpaceDimension->SetValue((kt_double)d);
02119   }
02120 
02121   void Mapper::setParamLoopSearchSpaceResolution(double d)
02122   {
02123     m_pLoopSearchSpaceResolution->SetValue((kt_double)d);
02124   }
02125 
02126   void Mapper::setParamLoopSearchSpaceSmearDeviation(double d)
02127   {
02128     m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d);
02129   }
02130 
02131 
02132   // Scan Matcher Parameters
02133   void Mapper::setParamDistanceVariancePenalty(double d)
02134   {
02135     m_pDistanceVariancePenalty->SetValue((kt_double)math::Square(d));
02136   }
02137 
02138   void Mapper::setParamAngleVariancePenalty(double d)
02139   {
02140     m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d));
02141   }
02142 
02143   void Mapper::setParamFineSearchAngleOffset(double d)
02144   {
02145     m_pFineSearchAngleOffset->SetValue((kt_double)d);
02146   }
02147 
02148   void Mapper::setParamCoarseSearchAngleOffset(double d)
02149   {
02150     m_pCoarseSearchAngleOffset->SetValue((kt_double)d);
02151   }
02152 
02153   void Mapper::setParamCoarseAngleResolution(double d)
02154   {
02155     m_pCoarseAngleResolution->SetValue((kt_double)d);
02156   }
02157 
02158   void Mapper::setParamMinimumAnglePenalty(double d)
02159   {
02160     m_pMinimumAnglePenalty->SetValue((kt_double)d);
02161   }
02162 
02163   void Mapper::setParamMinimumDistancePenalty(double d)
02164   {
02165     m_pMinimumDistancePenalty->SetValue((kt_double)d);
02166   }
02167 
02168   void Mapper::setParamUseResponseExpansion(bool b)
02169   {
02170     m_pUseResponseExpansion->SetValue((kt_bool)b);
02171   }
02172 
02173 
02174 
02175   void Mapper::Initialize(kt_double rangeThreshold)
02176   {
02177     if (m_Initialized == false)
02178     {
02179       // create sequential scan and loop matcher
02180       m_pSequentialScanMatcher = ScanMatcher::Create(this,
02181                                                     m_pCorrelationSearchSpaceDimension->GetValue(),
02182                                                     m_pCorrelationSearchSpaceResolution->GetValue(),
02183                                                     m_pCorrelationSearchSpaceSmearDeviation->GetValue(),
02184                                                     rangeThreshold);
02185       assert(m_pSequentialScanMatcher);
02186 
02187       m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(),
02188                                                        m_pScanBufferMaximumScanDistance->GetValue());
02189 
02190       m_pGraph = new MapperGraph(this, rangeThreshold);
02191 
02192       m_Initialized = true;
02193     }
02194   }
02195 
02196   void Mapper::Reset()
02197   {
02198     delete m_pSequentialScanMatcher;
02199     m_pSequentialScanMatcher = NULL;
02200 
02201     delete m_pGraph;
02202     m_pGraph = NULL;
02203 
02204     delete m_pMapperSensorManager;
02205     m_pMapperSensorManager = NULL;
02206 
02207     m_Initialized = false;
02208   }
02209 
02210   kt_bool Mapper::Process(Object*  /*pObject*/)
02211   {
02212     return true;
02213   }
02214 
02215   kt_bool Mapper::Process(LocalizedRangeScan* pScan)
02216   {
02217     if (pScan != NULL)
02218     {
02219       karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
02220 
02221       // validate scan
02222       if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false)
02223       {
02224         return false;
02225       }
02226 
02227       if (m_Initialized == false)
02228       {
02229         // initialize mapper with range threshold from device
02230         Initialize(pLaserRangeFinder->GetRangeThreshold());
02231       }
02232 
02233       // get last scan
02234       LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());
02235 
02236       // update scans corrected pose based on last correction
02237       if (pLastScan != NULL)
02238       {
02239         Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
02240         pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
02241       }
02242 
02243       // test if scan is outside minimum boundary or if heading is larger then minimum heading
02244       if (!HasMovedEnough(pScan, pLastScan))
02245       {
02246         return false;
02247       }
02248 
02249       Matrix3 covariance;
02250       covariance.SetToIdentity();
02251 
02252       // correct scan (if not first scan)
02253       if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
02254       {
02255         Pose2 bestPose;
02256         m_pSequentialScanMatcher->MatchScan(pScan,
02257                                             m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
02258                                                                                     bestPose,
02259                                                                                     covariance);
02260         pScan->SetSensorPose(bestPose);
02261       }
02262 
02263       // add scan to buffer and assign id
02264       m_pMapperSensorManager->AddScan(pScan);
02265 
02266       if (m_pUseScanMatching->GetValue())
02267       {
02268         // add to graph
02269         m_pGraph->AddVertex(pScan);
02270         m_pGraph->AddEdges(pScan, covariance);
02271 
02272         m_pMapperSensorManager->AddRunningScan(pScan);
02273 
02274         if (m_pDoLoopClosing->GetValue())
02275         {
02276           std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
02277           const_forEach(std::vector<Name>, &deviceNames)
02278           {
02279             m_pGraph->TryCloseLoop(pScan, *iter);
02280           }
02281         }
02282       }
02283 
02284       m_pMapperSensorManager->SetLastScan(pScan);
02285 
02286       return true;
02287     }
02288 
02289     return false;
02290   }
02291 
02298   kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const
02299   {
02300     // test if first scan
02301     if (pLastScan == NULL)
02302     {
02303       return true;
02304     }
02305 
02306     // test if enough time has passed
02307     kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
02308     if (timeInterval >= m_pMinimumTimeInterval->GetValue())
02309     {
02310       return true;
02311     }
02312 
02313     Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
02314     Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
02315 
02316     // test if we have turned enough
02317     kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
02318     if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
02319     {
02320       return true;
02321     }
02322 
02323     // test if we have moved enough
02324     kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
02325     if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
02326     {
02327       return true;
02328     }
02329 
02330     return false;
02331   }
02332 
02337   const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const
02338   {
02339     LocalizedRangeScanVector allScans;
02340 
02341     if (m_pMapperSensorManager != NULL)
02342     {
02343       allScans = m_pMapperSensorManager->GetAllScans();
02344     }
02345 
02346     return allScans;
02347   }
02348 
02353   void Mapper::AddListener(MapperListener* pListener)
02354   {
02355     m_Listeners.push_back(pListener);
02356   }
02357 
02362   void Mapper::RemoveListener(MapperListener* pListener)
02363   {
02364     std::vector<MapperListener*>::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener);
02365     if (iter != m_Listeners.end())
02366     {
02367       m_Listeners.erase(iter);
02368     }
02369   }
02370 
02371   void Mapper::FireInfo(const std::string& rInfo) const
02372   {
02373     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02374     {
02375       (*iter)->Info(rInfo);
02376     }
02377   }
02378 
02379   void Mapper::FireDebug(const std::string& rInfo) const
02380   {
02381     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02382     {
02383       MapperDebugListener* pListener = dynamic_cast<MapperDebugListener*>(*iter);
02384 
02385       if (pListener != NULL)
02386       {
02387         pListener->Debug(rInfo);
02388       }
02389     }
02390   }
02391 
02392   void Mapper::FireLoopClosureCheck(const std::string& rInfo) const
02393   {
02394     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02395     {
02396       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02397 
02398       if (pListener != NULL)
02399       {
02400         pListener->LoopClosureCheck(rInfo);
02401       }
02402     }
02403   }
02404 
02405   void Mapper::FireBeginLoopClosure(const std::string& rInfo) const
02406   {
02407     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02408     {
02409       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02410 
02411       if (pListener != NULL)
02412       {
02413         pListener->BeginLoopClosure(rInfo);
02414       }
02415     }
02416   }
02417 
02418   void Mapper::FireEndLoopClosure(const std::string& rInfo) const
02419   {
02420     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02421     {
02422       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02423 
02424       if (pListener != NULL)
02425       {
02426         pListener->EndLoopClosure(rInfo);
02427       }
02428     }
02429   }
02430 
02431   void Mapper::SetScanSolver(ScanSolver* pScanOptimizer)
02432   {
02433     m_pScanOptimizer = pScanOptimizer;
02434   }
02435 
02436   MapperGraph* Mapper::GetGraph() const
02437   {
02438     return m_pGraph;
02439   }
02440 
02441   ScanMatcher* Mapper::GetSequentialScanMatcher() const
02442   {
02443     return m_pSequentialScanMatcher;
02444   }
02445 
02446   ScanMatcher* Mapper::GetLoopScanMatcher() const
02447   {
02448     return m_pGraph->GetLoopScanMatcher();
02449   }
02450 }  // namespace karto


open_karto
Author(s):
autogenerated on Tue May 2 2017 02:41:15