OccupancyGrid.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #include <OpenKarto/OccupancyGrid.h>
00019 #include <OpenKarto/OpenMapper.h>
00020 
00021 namespace karto
00022 {
00023 
00027 
00028   OccupancyGrid::OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2d& rOffset, kt_double resolution) 
00029     : Grid<kt_int8u>(width, height)
00030     , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
00031     , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
00032     , m_pCellUpdater(NULL)
00033   {
00034     m_pCellUpdater = new CellUpdater(this);
00035 
00036     if (karto::math::DoubleEqual(resolution, 0.0))
00037     {
00038       throw Exception("Resolution cannot be 0");
00039     }
00040 
00041     m_pMinPassThrough = new Parameter<kt_int32u>(NULL, "MinPassThrough", "", "", 2);
00042     m_pOccupancyThreshold = new Parameter<kt_double>(NULL, "OccupancyThreshold", "", "", 0.1);
00043 
00044     GetCoordinateConverter()->SetScale(1.0 / resolution);
00045     GetCoordinateConverter()->SetOffset(rOffset);
00046   }
00047 
00048   OccupancyGrid::~OccupancyGrid()
00049   {
00050     delete m_pCellUpdater;
00051   }
00052 
00053   OccupancyGrid* OccupancyGrid::Clone() const
00054   {
00055     OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), GetHeight(), GetCoordinateConverter()->GetOffset(), 1.0 / GetCoordinateConverter()->GetScale());
00056     memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
00057 
00058     pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
00059     pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
00060     pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
00061 
00062     return pOccupancyGrid;
00063   }
00064 
00065   // KARTO_DEPRECATED
00066 #ifdef WIN32
00067   OccupancyGrid* OccupancyGrid::CreateFromScans(const std::vector< SmartPointer<LocalizedRangeScan> >& /*rScans*/, kt_double /*resolution*/)
00068   {
00069     throw Exception("OccupancyGrid::CreateFromScans - Not supported in Windows. Please Use CreateFromScans(const LocalizedLaserScanList& rScans, kt_double resolution).");
00070   }
00071 #else
00072   OccupancyGrid* OccupancyGrid::CreateFromScans(const std::vector< SmartPointer<LocalizedRangeScan> >& rScans, kt_double resolution)
00073   {
00074     LocalizedLaserScanList scans;
00075     const_forEach(std::vector< SmartPointer<LocalizedRangeScan> >, &rScans)
00076     {
00077       scans.Add(*iter);
00078     }
00079 
00080     return CreateFromScans(scans, resolution);
00081   }
00082 #endif
00083 
00084   OccupancyGrid* OccupancyGrid::CreateFromScans(const LocalizedLaserScanList& rScans, kt_double resolution)
00085   {
00086     if (rScans.Size() == 0)
00087     {
00088       return NULL;
00089     }
00090 
00091     kt_int32s width, height;
00092     Vector2d offset;
00093     OccupancyGrid::ComputeDimensions(rScans, resolution, width, height, offset);
00094     OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
00095     pOccupancyGrid->CreateFromScans(rScans);
00096 
00097     return pOccupancyGrid;      
00098   }
00099 
00100   OccupancyGrid* OccupancyGrid::CreateFromMapper(OpenMapper* pMapper, kt_double resolution)
00101   {
00102     return karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
00103   }
00104 
00105   void OccupancyGrid::ComputeDimensions(const LocalizedLaserScanList& rScans, kt_double resolution, kt_int32s& rWidth, kt_int32s& rHeight, Vector2d& rOffset)
00106   {
00107     BoundingBox2 boundingBox;
00108     karto_const_forEach(LocalizedLaserScanList, &rScans)
00109     {
00110       LocalizedLaserScan* pLocalizedLaserScan = *iter;
00111 
00112       if (pLocalizedLaserScan != NULL)
00113       {
00114         boundingBox.Add(pLocalizedLaserScan->GetBoundingBox());
00115       }
00116     }
00117 
00118     kt_double scale = 1.0 / resolution;
00119     Size2<kt_double> size = boundingBox.GetSize();
00120 
00121     rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
00122     rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
00123     rOffset = boundingBox.GetMinimum();
00124   }
00125 
00126   void OccupancyGrid::CreateFromScans(const LocalizedLaserScanList& rScans)
00127   {
00128     m_pCellPassCnt->Resize(GetWidth(), GetHeight());
00129     m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
00130 
00131     m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
00132     m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
00133 
00134     karto_const_forEach(LocalizedLaserScanList, &rScans)
00135     {
00136       AddScan(*iter);
00137     }
00138 
00139     UpdateGrid();
00140   }
00141 
00142   kt_bool OccupancyGrid::AddScan(LocalizedLaserScan* pScan, kt_bool doUpdate)
00143   {
00144     kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
00145     kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
00146     kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
00147 
00148     Vector2d scanPosition = pScan->GetSensorPose().GetPosition();
00149 
00150     // get scan point readings
00151     const Vector2dList& rPointReadings = pScan->GetPointReadings(false);
00152 
00153     kt_bool scanInGrid = false;
00154 
00155     // draw lines from scan position to all point readings 
00156     karto_const_forEach(Vector2dList, &rPointReadings)
00157     {
00158       Vector2d point = *iter;
00159       kt_double range = scanPosition.Distance(point);
00160       kt_bool isEndPointValid = range < (rangeThreshold - KT_TOLERANCE);
00161 
00162       if (range >= maxRange || range < minRange)
00163       {
00164         // ignore max range or min range readings
00165         continue;
00166       }
00167       else if (range >= rangeThreshold)
00168       {
00169         // trace up to range reading
00170         kt_double ratio = rangeThreshold / range;
00171         kt_double dx = point.GetX() - scanPosition.GetX();
00172         kt_double dy = point.GetY() - scanPosition.GetY();
00173         point.SetX(scanPosition.GetX() + ratio * dx);
00174         point.SetY(scanPosition.GetY() + ratio * dy);
00175       }
00176 
00177       if (RayTrace(scanPosition, point, isEndPointValid, doUpdate))
00178       {
00179         scanInGrid = true;
00180       }
00181     }
00182 
00183     return scanInGrid;
00184   }
00185 
00186   kt_double OccupancyGrid::RayCast(const Pose2& rPose2, kt_double maxRange) const
00187   {
00188     kt_double scale = GetCoordinateConverter()->GetScale();
00189 
00190     kt_double x = rPose2.GetX();
00191     kt_double y = rPose2.GetY();
00192     kt_double theta = rPose2.GetHeading();
00193 
00194     kt_double sinTheta = sin(theta);
00195     kt_double cosTheta = cos(theta);
00196 
00197     kt_double xStop = x + maxRange * cosTheta;
00198     kt_double xSteps = 1 + fabs(xStop - x) * scale;
00199 
00200     kt_double yStop = y + maxRange * sinTheta;
00201     kt_double ySteps = 1 + fabs(yStop - y) * scale;
00202 
00203     kt_double steps = math::Maximum(xSteps, ySteps);
00204     kt_double delta = maxRange / steps;
00205     kt_double distance = delta;
00206 
00207     for (kt_int32u i = 1; i < steps; i++)
00208     {
00209       kt_double x1 = x + distance * cosTheta;
00210       kt_double y1 = y + distance * sinTheta;
00211 
00212       Vector2i gridIndex = WorldToGrid(Vector2d(x1, y1));
00213       if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
00214       {
00215         distance = (i + 1) * delta;
00216       }
00217       else
00218       {
00219         break;
00220       }
00221     }
00222 
00223     return (distance < maxRange) ? distance : maxRange;
00224   }
00225 
00226   kt_bool OccupancyGrid::RayTrace(const Vector2d& rWorldFrom, const Vector2d& rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate)
00227   {
00228     assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
00229 
00230     Vector2i gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
00231     Vector2i gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
00232 
00233     CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
00234     m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);        
00235 
00236     // for the end point
00237     if (isEndPointValid)
00238     {
00239       if (m_pCellPassCnt->IsValidGridIndex(gridTo))
00240       {
00241         kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
00242 
00243         kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
00244         kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
00245 
00246         // increment cell pass through and hit count
00247         pCellPassCntPtr[index]++;
00248         pCellHitCntPtr[index]++;
00249 
00250         if (doUpdate)
00251         {
00252           (*m_pCellUpdater)(index);
00253         }
00254       }
00255     }      
00256 
00257     return m_pCellPassCnt->IsValidGridIndex(gridTo);
00258   }
00259 
00260   void OccupancyGrid::UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
00261   {
00262     if (cellPassCnt > m_pMinPassThrough->GetValue())
00263     {
00264       kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
00265 
00266       if (hitRatio > m_pOccupancyThreshold->GetValue())
00267       {
00268         *pCell = GridStates_Occupied;
00269       }
00270       else
00271       {
00272         *pCell = GridStates_Free;
00273       }
00274     }      
00275   }
00276 
00277   void OccupancyGrid::UpdateGrid()
00278   {
00279     assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
00280 
00281     // clear grid
00282     Clear();
00283 
00284     // set occupancy status of cells
00285     kt_int8u* pDataPtr = GetDataPointer();
00286     kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
00287     kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
00288 
00289     kt_int32u nBytes = GetDataSize();
00290     for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
00291     {
00292       UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
00293     }
00294   }
00295 
00296   void OccupancyGrid::Resize(kt_int32s width, kt_int32s height)
00297   {
00298     Grid<kt_int8u>::Resize(width, height);
00299 
00300     m_pCellPassCnt->Resize(width, height);
00301     m_pCellHitsCnt->Resize(width, height);
00302   }
00303 
00307 
00308   void CellUpdater::operator() (kt_int32u index)
00309   {
00310     kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
00311     kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
00312     kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
00313 
00314     m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
00315   }
00316 
00317 }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:25