GridIndexLookup.h
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 #pragma once
00019 
00020 #ifndef __OpenKarto_GridIndexLookup_h__
00021 #define __OpenKarto_GridIndexLookup_h__
00022 
00023 #include <OpenKarto/Grid.h>
00024 #include <OpenKarto/SensorData.h>
00025 
00026 namespace karto
00027 {
00028 
00030 
00031 
00035     
00040   class LookupArray
00041   {
00042   public:
00046     LookupArray();
00047 
00051     virtual ~LookupArray();
00052 
00053   public:
00057     void Clear();
00058 
00063     kt_int32u GetSize() const;
00064 
00069     void SetSize(kt_int32u size);
00070 
00076     inline kt_int32s& operator[](kt_int32u index) 
00077     {
00078       assert(index < m_Size);
00079 
00080       return m_pArray[index]; 
00081     }
00082 
00088     inline kt_int32s operator[](kt_int32u index) const 
00089     {
00090       assert(index < m_Size);
00091 
00092       return m_pArray[index]; 
00093     }
00094 
00099     inline kt_int32s* GetArrayPointer()
00100     {
00101       return m_pArray;
00102     }
00103 
00108     inline kt_int32s* GetArrayPointer() const
00109     {
00110       return m_pArray;
00111     }
00112 
00113   private:
00114     kt_int32s* m_pArray;
00115     kt_int32u m_Capacity;
00116     kt_int32u m_Size;
00117   }; // LookupArray
00118 
00122 
00135   template<typename T>
00136   class GridIndexLookup
00137   {
00138   public:
00143     GridIndexLookup(Grid<T>* pGrid)
00144       : m_pGrid(pGrid)
00145       , m_Capacity(0)
00146       , m_Size(0)
00147       , m_ppLookupArray(NULL)
00148     {
00149     }
00150     
00154     virtual ~GridIndexLookup()
00155     {
00156       DestroyArrays();
00157     }
00158 
00159   public:
00165     const LookupArray* GetLookupArray(kt_int32u index) const
00166     {
00167       assert(math::IsUpTo(index, m_Size));
00168       
00169       return m_ppLookupArray[index];
00170     }
00171     
00176     const List<kt_double>& GetAngles() const
00177     {
00178       return m_Angles;
00179     }
00180     
00188     void ComputeOffsets(LocalizedLaserScan* pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
00189     {
00190       assert(angleOffset != 0.0);
00191       assert(angleResolution != 0.0);
00192  
00193       kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
00194       SetSize(nAngles);
00195 
00197       // convert points into local coordinates of scan pose
00198 
00199       const Vector2dList& rPointReadings = pScan->GetPointReadings();
00200 
00201       // compute transform to scan pose
00202       Transform transform(pScan->GetSensorPose());
00203 
00204       Pose2List localPoints;
00205       karto_const_forEach(Vector2dList, &rPointReadings)
00206       {
00207         // do inverse transform to get points in local coordinates
00208         Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
00209         localPoints.Add(vec);
00210       }
00211 
00213       // create lookup array for different angles
00214       kt_double angle = 0.0;
00215       kt_double startAngle = angleCenter - angleOffset;
00216       for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00217       {
00218         angle = startAngle + angleIndex * angleResolution;
00219         ComputeOffsets(angleIndex, angle, localPoints);
00220       }
00221       //assert(math::DoubleEqual(angle, angleCenter + angleOffset));
00222     }
00223 
00224   private:
00231     void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2List& rLocalPoints)
00232     {
00233       m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.Size()));
00234       m_Angles[angleIndex] = angle;
00235       
00236       // set up point array by computing relative offsets to points readings
00237       // when rotated by given angle
00238       
00239       const Vector2d& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
00240       
00241       kt_double cosine = cos(angle);
00242       kt_double sine = sin(angle);
00243       
00244       kt_int32u readingIndex = 0;
00245 
00246       kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
00247 
00248       karto_const_forEach(Pose2List, &rLocalPoints)
00249       {
00250         const Vector2d& rPosition = iter->GetPosition();
00251         
00252         // counterclockwise rotation and that rotation is about the origin (0, 0).
00253         Vector2d offset;
00254         offset.SetX(cosine * rPosition.GetX() -   sine * rPosition.GetY());
00255         offset.SetY(  sine * rPosition.GetX() + cosine * rPosition.GetY());
00256         
00257         // have to compensate for the grid offset when getting the grid index
00258         Vector2i gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
00259         
00260         // use base GridIndex to ignore ROI 
00261         kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
00262 
00263         pAngleIndexPointer[readingIndex] = lookupIndex;
00264 
00265         readingIndex++;
00266       }
00267       assert(readingIndex == rLocalPoints.Size());
00268     }
00269         
00274     void SetSize(kt_int32u size)
00275     {
00276       assert(size != 0);
00277       
00278       if (size > m_Capacity)
00279       {
00280         if (m_ppLookupArray != NULL)
00281         {
00282           DestroyArrays();
00283         }
00284         
00285         m_Capacity = size;
00286         m_ppLookupArray = new LookupArray*[m_Capacity];
00287         for (kt_int32u i = 0; i < m_Capacity; i++)
00288         {
00289           m_ppLookupArray[i] = new LookupArray();
00290         }        
00291       }
00292       
00293       m_Size = size;
00294       
00295       m_Angles.Resize(size);
00296     }
00297 
00301     void DestroyArrays()
00302     {
00303       for (kt_int32u i = 0; i < m_Capacity; i++)
00304       {
00305         delete m_ppLookupArray[i];
00306       }
00307       
00308       delete[] m_ppLookupArray;
00309       m_ppLookupArray = NULL;      
00310     }
00311     
00312   private:
00313     Grid<T>* m_pGrid; 
00314 
00315     kt_int32u m_Capacity;
00316     kt_int32u m_Size;
00317     
00318     LookupArray **m_ppLookupArray;
00319 
00320     // for sanity check
00321     List<kt_double> m_Angles;
00322   }; // class GridIndexLookup
00323 
00325 
00326 }
00327 
00328 #endif // __OpenKarto_GridIndexLookup_h__


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