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 && !isnan(currentPoint.GetX()) && !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()))
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_pMinimumTravelDistance = new Parameter<kt_double>(
01680         "MinimumTravelDistance",
01681         "Sets the minimum travel between scans.  If a new scan's position is "
01682         "more than minimumTravelDistance from the previous scan, the mapper "
01683         "will use the data from the new scan. Otherwise, it will discard the "
01684         "new scan if it also does not meet the minimum change in heading "
01685         "requirement. For performance reasons, generally it is a good idea to "
01686         "only process scans if the robot has moved a reasonable amount.",
01687         0.2, GetParameterManager());
01688 
01689     m_pMinimumTravelHeading = new Parameter<kt_double>(
01690         "MinimumTravelHeading",
01691         "Sets the minimum heading change between scans. If a new scan's "
01692         "heading is more than MinimumTravelHeading from the previous scan, the "
01693         "mapper will use the data from the new scan.  Otherwise, it will "
01694         "discard the new scan if it also does not meet the minimum travel "
01695         "distance requirement. For performance reasons, generally it is a good "
01696         "idea to only process scans if the robot has moved a reasonable "
01697         "amount.",
01698         math::DegreesToRadians(10), GetParameterManager());
01699 
01700     m_pScanBufferSize = new Parameter<kt_int32u>(
01701         "ScanBufferSize",
01702         "Scan buffer size is the length of the scan chain stored for scan "
01703         "matching. \"ScanBufferSize\" should be set to approximately "
01704         "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The "
01705         "idea is to get an area approximately 20 meters long for scan "
01706         "matching. For example, if we add scans every MinimumTravelDistance == "
01707         "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)",
01708         70, GetParameterManager());
01709 
01710     m_pScanBufferMaximumScanDistance = new Parameter<kt_double>(
01711         "ScanBufferMaximumScanDistance",
01712         "Scan buffer maximum scan distance is the maximum distance between the "
01713         "first and last scans in the scan chain stored for matching.",
01714         20.0, GetParameterManager());
01715 
01716     m_pLinkMatchMinimumResponseFine = new Parameter<kt_double>(
01717         "LinkMatchMinimumResponseFine",
01718         "Scans are linked only if the correlation response value is greater "
01719         "than this value.",
01720         0.8, GetParameterManager());
01721 
01722     m_pLinkScanMaximumDistance = new Parameter<kt_double>(
01723         "LinkScanMaximumDistance",
01724         "Maximum distance between linked scans.  Scans that are farther apart "
01725         "will not be linked regardless of the correlation response value.",
01726         10.0, GetParameterManager());
01727 
01728     m_pLoopSearchMaximumDistance = new Parameter<kt_double>(
01729         "LoopSearchMaximumDistance",
01730         "Scans less than this distance from the current position will be "
01731         "considered for a match in loop closure.",
01732         4.0, GetParameterManager());
01733 
01734     m_pDoLoopClosing = new Parameter<kt_bool>(
01735         "DoLoopClosing",
01736         "Enable/disable loop closure.",
01737         true, GetParameterManager());
01738 
01739     m_pLoopMatchMinimumChainSize = new Parameter<kt_int32u>(
01740         "LoopMatchMinimumChainSize",
01741         "When the loop closure detection finds a candidate it must be part of "
01742         "a large set of linked scans. If the chain of scans is less than this "
01743         "value we do not attempt to close the loop.",
01744         10, GetParameterManager());
01745 
01746     m_pLoopMatchMaximumVarianceCoarse = new Parameter<kt_double>(
01747         "LoopMatchMaximumVarianceCoarse",
01748         "The co-variance values for a possible loop closure have to be less "
01749         "than this value to consider a viable solution. This applies to the "
01750         "coarse search.",
01751         math::Square(0.4), GetParameterManager());
01752 
01753     m_pLoopMatchMinimumResponseCoarse = new Parameter<kt_double>(
01754         "LoopMatchMinimumResponseCoarse",
01755         "If response is larger then this, then initiate loop closure search at "
01756         "the coarse resolution.",
01757         0.8, GetParameterManager());
01758 
01759     m_pLoopMatchMinimumResponseFine = new Parameter<kt_double>(
01760         "LoopMatchMinimumResponseFine",
01761         "If response is larger then this, then initiate loop closure search at "
01762         "the fine resolution.",
01763         0.8, GetParameterManager());
01764 
01766     //    CorrelationParameters correlationParameters;
01767 
01768     m_pCorrelationSearchSpaceDimension = new Parameter<kt_double>(
01769         "CorrelationSearchSpaceDimension",
01770         "The size of the search grid used by the matcher. The search grid will "
01771         "have the size CorrelationSearchSpaceDimension * "
01772         "CorrelationSearchSpaceDimension",
01773         0.3, GetParameterManager());
01774 
01775     m_pCorrelationSearchSpaceResolution = new Parameter<kt_double>(
01776         "CorrelationSearchSpaceResolution",
01777         "The resolution (size of a grid cell) of the correlation grid.",
01778         0.01, GetParameterManager());
01779 
01780     m_pCorrelationSearchSpaceSmearDeviation = new Parameter<kt_double>(
01781         "CorrelationSearchSpaceSmearDeviation",
01782         "The point readings are smeared by this value in X and Y to create a "
01783         "smoother response.",
01784         0.03, GetParameterManager());
01785 
01786 
01788     //    CorrelationParameters loopCorrelationParameters;
01789 
01790     m_pLoopSearchSpaceDimension = new Parameter<kt_double>(
01791         "LoopSearchSpaceDimension",
01792         "The size of the search grid used by the matcher.",
01793         8.0, GetParameterManager());
01794 
01795     m_pLoopSearchSpaceResolution = new Parameter<kt_double>(
01796         "LoopSearchSpaceResolution",
01797         "The resolution (size of a grid cell) of the correlation grid.",
01798         0.05, GetParameterManager());
01799 
01800     m_pLoopSearchSpaceSmearDeviation = new Parameter<kt_double>(
01801         "LoopSearchSpaceSmearDeviation",
01802         "The point readings are smeared by this value in X and Y to create a "
01803         "smoother response.",
01804         0.03, GetParameterManager());
01805 
01807     // ScanMatcherParameters;
01808 
01809     m_pDistanceVariancePenalty = new Parameter<kt_double>(
01810         "DistanceVariancePenalty",
01811         "Variance of penalty for deviating from odometry when scan-matching. "
01812         "The penalty is a multiplier (less than 1.0) is a function of the "
01813         "delta of the scan position being tested and the odometric pose.",
01814         math::Square(0.3), GetParameterManager());
01815 
01816     m_pAngleVariancePenalty = new Parameter<kt_double>(
01817         "AngleVariancePenalty",
01818         "See DistanceVariancePenalty.",
01819         math::Square(math::DegreesToRadians(20)), GetParameterManager());
01820 
01821     m_pFineSearchAngleOffset = new Parameter<kt_double>(
01822         "FineSearchAngleOffset",
01823         "The range of angles to search during a fine search.",
01824         math::DegreesToRadians(0.2), GetParameterManager());
01825 
01826     m_pCoarseSearchAngleOffset = new Parameter<kt_double>(
01827         "CoarseSearchAngleOffset",
01828         "The range of angles to search during a coarse search.",
01829         math::DegreesToRadians(20), GetParameterManager());
01830 
01831     m_pCoarseAngleResolution = new Parameter<kt_double>(
01832         "CoarseAngleResolution",
01833         "Resolution of angles to search during a coarse search.",
01834         math::DegreesToRadians(2), GetParameterManager());
01835 
01836     m_pMinimumAnglePenalty = new Parameter<kt_double>(
01837         "MinimumAnglePenalty",
01838         "Minimum value of the angle penalty multiplier so scores do not become "
01839         "too small.",
01840         0.9, GetParameterManager());
01841 
01842     m_pMinimumDistancePenalty = new Parameter<kt_double>(
01843         "MinimumDistancePenalty",
01844         "Minimum value of the distance penalty multiplier so scores do not "
01845         "become too small.",
01846         0.5, GetParameterManager());
01847 
01848     m_pUseResponseExpansion = new Parameter<kt_bool>(
01849         "UseResponseExpansion",
01850         "Whether to increase the search space if no good matches are initially "
01851         "found.",
01852         false, GetParameterManager());
01853   }
01854   /* Adding in getters and setters here for easy parameter access */
01855 
01856   // General Parameters
01857 
01858   bool Mapper::getParamUseScanMatching()
01859   {
01860     return static_cast<bool>(m_pUseScanMatching->GetValue());
01861   }
01862 
01863   bool Mapper::getParamUseScanBarycenter()
01864   {
01865     return static_cast<bool>(m_pUseScanBarycenter->GetValue());
01866   }
01867 
01868   double Mapper::getParamMinimumTravelDistance()
01869   {
01870     return static_cast<double>(m_pMinimumTravelDistance->GetValue());
01871   }
01872 
01873   double Mapper::getParamMinimumTravelHeading()
01874   {
01875     return math::RadiansToDegrees(static_cast<double>(m_pMinimumTravelHeading->GetValue()));
01876   }
01877 
01878   int Mapper::getParamScanBufferSize()
01879   {
01880     return static_cast<int>(m_pScanBufferSize->GetValue());
01881   }
01882 
01883   double Mapper::getParamScanBufferMaximumScanDistance()
01884   {
01885     return static_cast<double>(m_pScanBufferMaximumScanDistance->GetValue());
01886   }
01887 
01888   double Mapper::getParamLinkMatchMinimumResponseFine()
01889   {
01890     return static_cast<double>(m_pLinkMatchMinimumResponseFine->GetValue());
01891   }
01892 
01893   double Mapper::getParamLinkScanMaximumDistance()
01894   {
01895     return static_cast<double>(m_pLinkScanMaximumDistance->GetValue());
01896   }
01897 
01898   double Mapper::getParamLoopSearchMaximumDistance()
01899   {
01900     return static_cast<double>(m_pLoopSearchMaximumDistance->GetValue());
01901   }
01902 
01903   bool Mapper::getParamDoLoopClosing()
01904   {
01905     return static_cast<bool>(m_pDoLoopClosing->GetValue());
01906   }
01907 
01908   int Mapper::getParamLoopMatchMinimumChainSize()
01909   {
01910     return static_cast<int>(m_pLoopMatchMinimumChainSize->GetValue());
01911   }
01912 
01913   double Mapper::getParamLoopMatchMaximumVarianceCoarse()
01914   {
01915     return static_cast<double>(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue()));
01916   }
01917 
01918   double Mapper::getParamLoopMatchMinimumResponseCoarse()
01919   {
01920     return static_cast<double>(m_pLoopMatchMinimumResponseCoarse->GetValue());
01921   }
01922 
01923   double Mapper::getParamLoopMatchMinimumResponseFine()
01924   {
01925     return static_cast<double>(m_pLoopMatchMinimumResponseFine->GetValue());
01926   }
01927 
01928   // Correlation Parameters - Correlation Parameters
01929 
01930   double Mapper::getParamCorrelationSearchSpaceDimension()
01931   {
01932     return static_cast<double>(m_pCorrelationSearchSpaceDimension->GetValue());
01933   }
01934 
01935   double Mapper::getParamCorrelationSearchSpaceResolution()
01936   {
01937     return static_cast<double>(m_pCorrelationSearchSpaceResolution->GetValue());
01938   }
01939 
01940   double Mapper::getParamCorrelationSearchSpaceSmearDeviation()
01941   {
01942     return static_cast<double>(m_pCorrelationSearchSpaceSmearDeviation->GetValue());
01943   }
01944 
01945   // Correlation Parameters - Loop Correlation Parameters
01946 
01947   double Mapper::getParamLoopSearchSpaceDimension()
01948   {
01949     return static_cast<double>(m_pLoopSearchSpaceDimension->GetValue());
01950   }
01951 
01952   double Mapper::getParamLoopSearchSpaceResolution()
01953   {
01954     return static_cast<double>(m_pLoopSearchSpaceResolution->GetValue());
01955   }
01956 
01957   double Mapper::getParamLoopSearchSpaceSmearDeviation()
01958   {
01959     return static_cast<double>(m_pLoopSearchSpaceSmearDeviation->GetValue());
01960   }
01961 
01962   // ScanMatcher Parameters
01963 
01964   double Mapper::getParamDistanceVariancePenalty()
01965   {
01966     return std::sqrt(static_cast<double>(m_pDistanceVariancePenalty->GetValue()));
01967   }
01968 
01969   double Mapper::getParamAngleVariancePenalty()
01970   {
01971     return std::sqrt(static_cast<double>(m_pAngleVariancePenalty->GetValue()));
01972   }
01973 
01974   double Mapper::getParamFineSearchAngleOffset()
01975   {
01976     return static_cast<double>(m_pFineSearchAngleOffset->GetValue());
01977   }
01978 
01979   double Mapper::getParamCoarseSearchAngleOffset()
01980   {
01981     return static_cast<double>(m_pCoarseSearchAngleOffset->GetValue());
01982   }
01983 
01984   double Mapper::getParamCoarseAngleResolution()
01985   {
01986     return static_cast<double>(m_pCoarseAngleResolution->GetValue());
01987   }
01988 
01989   double Mapper::getParamMinimumAnglePenalty()
01990   {
01991     return static_cast<double>(m_pMinimumAnglePenalty->GetValue());
01992   }
01993 
01994   double Mapper::getParamMinimumDistancePenalty()
01995   {
01996     return static_cast<double>(m_pMinimumDistancePenalty->GetValue());
01997   }
01998 
01999   bool Mapper::getParamUseResponseExpansion()
02000   {
02001     return static_cast<bool>(m_pUseResponseExpansion->GetValue());
02002   }
02003 
02004   /* Setters for parameters */
02005   // General Parameters
02006   void Mapper::setParamUseScanMatching(bool b)
02007   {
02008     m_pUseScanMatching->SetValue((kt_bool)b);
02009   }
02010 
02011   void Mapper::setParamUseScanBarycenter(bool b)
02012   {
02013     m_pUseScanBarycenter->SetValue((kt_bool)b);
02014   }
02015 
02016   void Mapper::setParamMinimumTravelDistance(double d)
02017   {
02018     m_pMinimumTravelDistance->SetValue((kt_double)d);
02019   }
02020 
02021   void Mapper::setParamMinimumTravelHeading(double d)
02022   {
02023     m_pMinimumTravelHeading->SetValue((kt_double)d);
02024   }
02025 
02026   void Mapper::setParamScanBufferSize(int i)
02027   {
02028     m_pScanBufferSize->SetValue((kt_int32u)i);
02029   }
02030 
02031   void Mapper::setParamScanBufferMaximumScanDistance(double d)
02032   {
02033     m_pScanBufferMaximumScanDistance->SetValue((kt_double)d);
02034   }
02035 
02036   void Mapper::setParamLinkMatchMinimumResponseFine(double d)
02037   {
02038     m_pLinkMatchMinimumResponseFine->SetValue((kt_double)d);
02039   }
02040 
02041   void Mapper::setParamLinkScanMaximumDistance(double d)
02042   {
02043     m_pLinkScanMaximumDistance->SetValue((kt_double)d);
02044   }
02045 
02046   void Mapper::setParamLoopSearchMaximumDistance(double d)
02047   {
02048     m_pLoopSearchMaximumDistance->SetValue((kt_double)d);
02049   }
02050 
02051   void Mapper::setParamDoLoopClosing(bool b)
02052   {
02053     m_pDoLoopClosing->SetValue((kt_bool)b);
02054   }
02055 
02056   void Mapper::setParamLoopMatchMinimumChainSize(int i)
02057   {
02058     m_pLoopMatchMinimumChainSize->SetValue((kt_int32u)i);
02059   }
02060 
02061   void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d)
02062   {
02063     m_pLoopMatchMaximumVarianceCoarse->SetValue((kt_double)math::Square(d));
02064   }
02065 
02066   void Mapper::setParamLoopMatchMinimumResponseCoarse(double d)
02067   {
02068     m_pLoopMatchMinimumResponseCoarse->SetValue((kt_double)d);
02069   }
02070 
02071   void Mapper::setParamLoopMatchMinimumResponseFine(double d)
02072   {
02073     m_pLoopMatchMinimumResponseFine->SetValue((kt_double)d);
02074   }
02075 
02076   // Correlation Parameters - Correlation Parameters
02077   void Mapper::setParamCorrelationSearchSpaceDimension(double d)
02078   {
02079     m_pCorrelationSearchSpaceDimension->SetValue((kt_double)d);
02080   }
02081 
02082   void Mapper::setParamCorrelationSearchSpaceResolution(double d)
02083   {
02084     m_pCorrelationSearchSpaceResolution->SetValue((kt_double)d);
02085   }
02086 
02087   void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d)
02088   {
02089     m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d);
02090   }
02091 
02092 
02093   // Correlation Parameters - Loop Closure Parameters
02094   void Mapper::setParamLoopSearchSpaceDimension(double d)
02095   {
02096     m_pLoopSearchSpaceDimension->SetValue((kt_double)d);
02097   }
02098 
02099   void Mapper::setParamLoopSearchSpaceResolution(double d)
02100   {
02101     m_pLoopSearchSpaceResolution->SetValue((kt_double)d);
02102   }
02103 
02104   void Mapper::setParamLoopSearchSpaceSmearDeviation(double d)
02105   {
02106     m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d);
02107   }
02108 
02109 
02110   // Scan Matcher Parameters
02111   void Mapper::setParamDistanceVariancePenalty(double d)
02112   {
02113     m_pDistanceVariancePenalty->SetValue((kt_double)math::Square(d));
02114   }
02115 
02116   void Mapper::setParamAngleVariancePenalty(double d)
02117   {
02118     m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d));
02119   }
02120 
02121   void Mapper::setParamFineSearchAngleOffset(double d)
02122   {
02123     m_pFineSearchAngleOffset->SetValue((kt_double)d);
02124   }
02125 
02126   void Mapper::setParamCoarseSearchAngleOffset(double d)
02127   {
02128     m_pCoarseSearchAngleOffset->SetValue((kt_double)d);
02129   }
02130 
02131   void Mapper::setParamCoarseAngleResolution(double d)
02132   {
02133     m_pCoarseAngleResolution->SetValue((kt_double)d);
02134   }
02135 
02136   void Mapper::setParamMinimumAnglePenalty(double d)
02137   {
02138     m_pMinimumAnglePenalty->SetValue((kt_double)d);
02139   }
02140 
02141   void Mapper::setParamMinimumDistancePenalty(double d)
02142   {
02143     m_pMinimumDistancePenalty->SetValue((kt_double)d);
02144   }
02145 
02146   void Mapper::setParamUseResponseExpansion(bool b)
02147   {
02148     m_pUseResponseExpansion->SetValue((kt_bool)b);
02149   }
02150 
02151 
02152 
02153   void Mapper::Initialize(kt_double rangeThreshold)
02154   {
02155     if (m_Initialized == false)
02156     {
02157       // create sequential scan and loop matcher
02158       m_pSequentialScanMatcher = ScanMatcher::Create(this,
02159                                                     m_pCorrelationSearchSpaceDimension->GetValue(),
02160                                                     m_pCorrelationSearchSpaceResolution->GetValue(),
02161                                                     m_pCorrelationSearchSpaceSmearDeviation->GetValue(),
02162                                                     rangeThreshold);
02163       assert(m_pSequentialScanMatcher);
02164 
02165       m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(),
02166                                                        m_pScanBufferMaximumScanDistance->GetValue());
02167 
02168       m_pGraph = new MapperGraph(this, rangeThreshold);
02169 
02170       m_Initialized = true;
02171     }
02172   }
02173 
02174   void Mapper::Reset()
02175   {
02176     delete m_pSequentialScanMatcher;
02177     m_pSequentialScanMatcher = NULL;
02178 
02179     delete m_pGraph;
02180     m_pGraph = NULL;
02181 
02182     delete m_pMapperSensorManager;
02183     m_pMapperSensorManager = NULL;
02184 
02185     m_Initialized = false;
02186   }
02187 
02188   kt_bool Mapper::Process(Object*  /*pObject*/)
02189   {
02190     return true;
02191   }
02192 
02193   kt_bool Mapper::Process(LocalizedRangeScan* pScan)
02194   {
02195     if (pScan != NULL)
02196     {
02197       karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
02198 
02199       // validate scan
02200       if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false)
02201       {
02202         return false;
02203       }
02204 
02205       if (m_Initialized == false)
02206       {
02207         // initialize mapper with range threshold from device
02208         Initialize(pLaserRangeFinder->GetRangeThreshold());
02209       }
02210 
02211       // get last scan
02212       LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());
02213 
02214       // update scans corrected pose based on last correction
02215       if (pLastScan != NULL)
02216       {
02217         Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
02218         pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
02219       }
02220 
02221       // test if scan is outside minimum boundary or if heading is larger then minimum heading
02222       if (!HasMovedEnough(pScan, pLastScan))
02223       {
02224         return false;
02225       }
02226 
02227       Matrix3 covariance;
02228       covariance.SetToIdentity();
02229 
02230       // correct scan (if not first scan)
02231       if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
02232       {
02233         Pose2 bestPose;
02234         m_pSequentialScanMatcher->MatchScan(pScan,
02235                                             m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
02236                                                                                     bestPose,
02237                                                                                     covariance);
02238         pScan->SetSensorPose(bestPose);
02239       }
02240 
02241       // add scan to buffer and assign id
02242       m_pMapperSensorManager->AddScan(pScan);
02243 
02244       if (m_pUseScanMatching->GetValue())
02245       {
02246         // add to graph
02247         m_pGraph->AddVertex(pScan);
02248         m_pGraph->AddEdges(pScan, covariance);
02249 
02250         m_pMapperSensorManager->AddRunningScan(pScan);
02251 
02252         if (m_pDoLoopClosing->GetValue())
02253         {
02254           std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
02255           const_forEach(std::vector<Name>, &deviceNames)
02256           {
02257             m_pGraph->TryCloseLoop(pScan, *iter);
02258           }
02259         }
02260       }
02261 
02262       m_pMapperSensorManager->SetLastScan(pScan);
02263 
02264       return true;
02265     }
02266 
02267     return false;
02268   }
02269 
02276   kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const
02277   {
02278     // test if first scan
02279     if (pLastScan == NULL)
02280     {
02281       return true;
02282     }
02283 
02284     Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
02285     Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
02286 
02287     // test if we have turned enough
02288     kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
02289     if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
02290     {
02291       return true;
02292     }
02293 
02294     // test if we have moved enough
02295     kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
02296     if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
02297     {
02298       return true;
02299     }
02300 
02301     return false;
02302   }
02303 
02308   const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const
02309   {
02310     LocalizedRangeScanVector allScans;
02311 
02312     if (m_pMapperSensorManager != NULL)
02313     {
02314       allScans = m_pMapperSensorManager->GetAllScans();
02315     }
02316 
02317     return allScans;
02318   }
02319 
02324   void Mapper::AddListener(MapperListener* pListener)
02325   {
02326     m_Listeners.push_back(pListener);
02327   }
02328 
02333   void Mapper::RemoveListener(MapperListener* pListener)
02334   {
02335     std::vector<MapperListener*>::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener);
02336     if (iter != m_Listeners.end())
02337     {
02338       m_Listeners.erase(iter);
02339     }
02340   }
02341 
02342   void Mapper::FireInfo(const std::string& rInfo) const
02343   {
02344     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02345     {
02346       (*iter)->Info(rInfo);
02347     }
02348   }
02349 
02350   void Mapper::FireDebug(const std::string& rInfo) const
02351   {
02352     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02353     {
02354       MapperDebugListener* pListener = dynamic_cast<MapperDebugListener*>(*iter);
02355 
02356       if (pListener != NULL)
02357       {
02358         pListener->Debug(rInfo);
02359       }
02360     }
02361   }
02362 
02363   void Mapper::FireLoopClosureCheck(const std::string& rInfo) const
02364   {
02365     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02366     {
02367       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02368 
02369       if (pListener != NULL)
02370       {
02371         pListener->LoopClosureCheck(rInfo);
02372       }
02373     }
02374   }
02375 
02376   void Mapper::FireBeginLoopClosure(const std::string& rInfo) const
02377   {
02378     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02379     {
02380       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02381 
02382       if (pListener != NULL)
02383       {
02384         pListener->BeginLoopClosure(rInfo);
02385       }
02386     }
02387   }
02388 
02389   void Mapper::FireEndLoopClosure(const std::string& rInfo) const
02390   {
02391     const_forEach(std::vector<MapperListener*>, &m_Listeners)
02392     {
02393       MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
02394 
02395       if (pListener != NULL)
02396       {
02397         pListener->EndLoopClosure(rInfo);
02398       }
02399     }
02400   }
02401 
02402   void Mapper::SetScanSolver(ScanSolver* pScanOptimizer)
02403   {
02404     m_pScanOptimizer = pScanOptimizer;
02405   }
02406 
02407   MapperGraph* Mapper::GetGraph() const
02408   {
02409     return m_pGraph;
02410   }
02411 
02412   ScanMatcher* Mapper::GetSequentialScanMatcher() const
02413   {
02414     return m_pSequentialScanMatcher;
02415   }
02416 
02417   ScanMatcher* Mapper::GetLoopScanMatcher() const
02418   {
02419     return m_pGraph->GetLoopScanMatcher();
02420   }
02421 }  // namespace karto


open_karto
Author(s):
autogenerated on Thu Aug 27 2015 14:14:06