OccGridMapUtil.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef __OccGridMapUtil_h_
00030 #define __OccGridMapUtil_h_
00031 
00032 #include <cmath>
00033 
00034 #include "../scan/DataPointContainer.h"
00035 #include "../util/UtilFunctions.h"
00036 
00037 namespace hectorslam {
00038 
00039 template<typename ConcreteOccGridMap, typename ConcreteCacheMethod>
00040 class OccGridMapUtil
00041 {
00042 public:
00043 
00044   OccGridMapUtil(const ConcreteOccGridMap* gridMap)
00045     : concreteGridMap(gridMap)
00046     , size(0)
00047   {
00048     mapObstacleThreshold = gridMap->getObstacleThreshold();
00049     cacheMethod.setMapSize(gridMap->getMapDimensions());
00050   }
00051 
00052   ~OccGridMapUtil()
00053   {}
00054 
00055 public:
00056 
00057   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00058 
00059   inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const { return concreteGridMap->getWorldCoordsPose(mapPose); };
00060   inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const { return concreteGridMap->getMapCoordsPose(worldPose); };
00061 
00062   inline Eigen::Vector2f getWorldCoordsPoint(const Eigen::Vector2f& mapPoint) const { return concreteGridMap->getWorldCoords(mapPoint); };
00063 
00064   void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)
00065   {
00066     int size = dataPoints.getSize();
00067 
00068     Eigen::Affine2f transform(getTransformForState(pose));
00069 
00070     float sinRot = sin(pose[2]);
00071     float cosRot = cos(pose[2]);
00072 
00073     H = Eigen::Matrix3f::Zero();
00074     dTr = Eigen::Vector3f::Zero();
00075 
00076     for (int i = 0; i < size; ++i) {
00077 
00078       const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));
00079 
00080       Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));
00081 
00082       float funVal = 1.0f - transformedPointData[0];
00083 
00084       dTr[0] += transformedPointData[1] * funVal;
00085       dTr[1] += transformedPointData[2] * funVal;
00086 
00087       float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);
00088 
00089       dTr[2] += rotDeriv * funVal;
00090 
00091       H(0, 0) += util::sqr(transformedPointData[1]);
00092       H(1, 1) += util::sqr(transformedPointData[2]);
00093       H(2, 2) += util::sqr(rotDeriv);
00094 
00095       H(0, 1) += transformedPointData[1] * transformedPointData[2];
00096       H(0, 2) += transformedPointData[1] * rotDeriv;
00097       H(1, 2) += transformedPointData[2] * rotDeriv;
00098     }
00099 
00100     H(1, 0) = H(0, 1);
00101     H(2, 0) = H(0, 2);
00102     H(2, 1) = H(1, 2);
00103 
00104   }
00105 
00106   Eigen::Matrix3f getCovarianceForPose(const Eigen::Vector3f& mapPose, const DataContainer& dataPoints)
00107   {
00108 
00109     float deltaTransX = 1.5f;
00110     float deltaTransY = 1.5f;
00111     float deltaAng = 0.05f;
00112 
00113     float x = mapPose[0];
00114     float y = mapPose[1];
00115     float ang = mapPose[2];
00116 
00117     Eigen::Matrix<float, 3, 7> sigmaPoints;
00118 
00119     sigmaPoints.block<3, 1>(0, 0) = Eigen::Vector3f(x + deltaTransX, y, ang);
00120     sigmaPoints.block<3, 1>(0, 1) = Eigen::Vector3f(x - deltaTransX, y, ang);
00121     sigmaPoints.block<3, 1>(0, 2) = Eigen::Vector3f(x, y + deltaTransY, ang);
00122     sigmaPoints.block<3, 1>(0, 3) = Eigen::Vector3f(x, y - deltaTransY, ang);
00123     sigmaPoints.block<3, 1>(0, 4) = Eigen::Vector3f(x, y, ang + deltaAng);
00124     sigmaPoints.block<3, 1>(0, 5) = Eigen::Vector3f(x, y, ang - deltaAng);
00125     sigmaPoints.block<3, 1>(0, 6) = mapPose;
00126 
00127     Eigen::Matrix<float, 7, 1> likelihoods;
00128 
00129     likelihoods[0] = getLikelihoodForState(Eigen::Vector3f(x + deltaTransX, y, ang), dataPoints);
00130     likelihoods[1] = getLikelihoodForState(Eigen::Vector3f(x - deltaTransX, y, ang), dataPoints);
00131     likelihoods[2] = getLikelihoodForState(Eigen::Vector3f(x, y + deltaTransY, ang), dataPoints);
00132     likelihoods[3] = getLikelihoodForState(Eigen::Vector3f(x, y - deltaTransY, ang), dataPoints);
00133     likelihoods[4] = getLikelihoodForState(Eigen::Vector3f(x, y, ang + deltaAng), dataPoints);
00134     likelihoods[5] = getLikelihoodForState(Eigen::Vector3f(x, y, ang - deltaAng), dataPoints);
00135     likelihoods[6] = getLikelihoodForState(Eigen::Vector3f(x, y, ang), dataPoints);
00136 
00137     float invLhNormalizer = 1 / likelihoods.sum();
00138 
00139     std::cout << "\n lhs:\n" << likelihoods;
00140 
00141     Eigen::Vector3f mean(Eigen::Vector3f::Zero());
00142 
00143     for (int i = 0; i < 7; ++i) {
00144       mean += (sigmaPoints.block<3, 1>(0, i) * likelihoods[i]);
00145     }
00146 
00147     mean *= invLhNormalizer;
00148 
00149     Eigen::Matrix3f covMatrixMap(Eigen::Matrix3f::Zero());
00150 
00151     for (int i = 0; i < 7; ++i) {
00152       Eigen::Vector3f sigPointMinusMean(sigmaPoints.block<3, 1>(0, i) - mean);
00153       covMatrixMap += (likelihoods[i] * invLhNormalizer) * (sigPointMinusMean * (sigPointMinusMean.transpose()));
00154     }
00155 
00156     return covMatrixMap;
00157 
00158     //covMatrix.cwise() * invLhNormalizer;
00159     //transform = getTransformForState(Eigen::Vector3f(x-deltaTrans, y, ang);
00160   }
00161 
00162   Eigen::Matrix3f getCovMatrixWorldCoords(const Eigen::Matrix3f& covMatMap)
00163   {
00164 
00165     //std::cout << "\nCovMap:\n" << covMatMap;
00166 
00167     Eigen::Matrix3f covMatWorld;
00168 
00169     float scaleTrans = concreteGridMap->getCellLength();
00170     float scaleTransSq = util::sqr(scaleTrans);
00171 
00172     covMatWorld(0, 0) = covMatMap(0, 0) * scaleTransSq;
00173     covMatWorld(1, 1) = covMatMap(1, 1) * scaleTransSq;
00174 
00175     covMatWorld(1, 0) = covMatMap(1, 0) * scaleTransSq;
00176     covMatWorld(0, 1) = covMatWorld(1, 0);
00177 
00178     covMatWorld(2, 0) = covMatMap(2, 0) * scaleTrans;
00179     covMatWorld(0, 2) = covMatWorld(2, 0);
00180 
00181     covMatWorld(2, 1) = covMatMap(2, 1) * scaleTrans;
00182     covMatWorld(1, 2) = covMatWorld(2, 1);
00183 
00184     covMatWorld(2, 2) = covMatMap(2, 2);
00185 
00186     return covMatWorld;
00187   }
00188 
00189   float getLikelihoodForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)
00190   {
00191     float resid = getResidualForState(state, dataPoints);
00192 
00193     return getLikelihoodForResidual(resid, dataPoints.getSize());
00194   }
00195 
00196   float getLikelihoodForResidual(float residual, int numDataPoints)
00197   {
00198     float numDataPointsA = static_cast<int>(numDataPoints);
00199     float sizef = static_cast<float>(numDataPointsA);
00200 
00201     return 1 - (residual / sizef);
00202   }
00203 
00204   float getResidualForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)
00205   {
00206     int size = dataPoints.getSize();
00207 
00208     int stepSize = 1;
00209     float residual = 0.0f;
00210 
00211 
00212     Eigen::Affine2f transform(getTransformForState(state));
00213 
00214     for (int i = 0; i < size; i += stepSize) {
00215 
00216       float funval = 1.0f - interpMapValue(transform * dataPoints.getVecEntry(i));
00217       residual += funval;
00218     }
00219 
00220     return residual;
00221   }
00222 
00223   float getUnfilteredGridPoint(Eigen::Vector2i& gridCoords) const
00224   {
00225     return (concreteGridMap->getGridProbabilityMap(gridCoords.x(), gridCoords.y()));
00226   }
00227 
00228   float getUnfilteredGridPoint(int index) const
00229   {
00230     return (concreteGridMap->getGridProbabilityMap(index));
00231   }
00232 
00233   float interpMapValue(const Eigen::Vector2f& coords)
00234   {
00235     //check if coords are within map limits.
00236     if (concreteGridMap->pointOutOfMapBounds(coords)){
00237       return 0.0f;
00238     }
00239 
00240     //map coords are alway positive, floor them by casting to int
00241     Eigen::Vector2i indMin(coords.cast<int>());
00242 
00243     //get factors for bilinear interpolation
00244     Eigen::Vector2f factors(coords - indMin.cast<float>());
00245 
00246     int sizeX = concreteGridMap->getSizeX();
00247 
00248     int index = indMin[1] * sizeX + indMin[0];
00249 
00250     // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
00251     // filter gridPoint with gaussian and store in cache.
00252     if (!cacheMethod.containsCachedData(index, intensities[0])) {
00253       intensities[0] = getUnfilteredGridPoint(index);
00254       cacheMethod.cacheData(index, intensities[0]);
00255     }
00256 
00257     ++index;
00258 
00259     if (!cacheMethod.containsCachedData(index, intensities[1])) {
00260       intensities[1] = getUnfilteredGridPoint(index);
00261       cacheMethod.cacheData(index, intensities[1]);
00262     }
00263 
00264     index += sizeX-1;
00265 
00266     if (!cacheMethod.containsCachedData(index, intensities[2])) {
00267       intensities[2] = getUnfilteredGridPoint(index);
00268       cacheMethod.cacheData(index, intensities[2]);
00269     }
00270 
00271     ++index;
00272 
00273     if (!cacheMethod.containsCachedData(index, intensities[3])) {
00274       intensities[3] = getUnfilteredGridPoint(index);
00275       cacheMethod.cacheData(index, intensities[3]);
00276     }
00277 
00278     float xFacInv = (1.0f - factors[0]);
00279     float yFacInv = (1.0f - factors[1]);
00280 
00281     return
00282       ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
00283       ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1]));
00284 
00285   }
00286 
00287   Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
00288   {
00289     //check if coords are within map limits.
00290     if (concreteGridMap->pointOutOfMapBounds(coords)){
00291       return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
00292     }
00293 
00294     //map coords are always positive, floor them by casting to int
00295     Eigen::Vector2i indMin(coords.cast<int>());
00296 
00297     //get factors for bilinear interpolation
00298     Eigen::Vector2f factors(coords - indMin.cast<float>());
00299 
00300     int sizeX = concreteGridMap->getSizeX();
00301 
00302     int index = indMin[1] * sizeX + indMin[0];
00303 
00304     // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
00305     // filter gridPoint with gaussian and store in cache.
00306     if (!cacheMethod.containsCachedData(index, intensities[0])) {
00307       intensities[0] = getUnfilteredGridPoint(index);
00308       cacheMethod.cacheData(index, intensities[0]);
00309     }
00310 
00311     ++index;
00312 
00313     if (!cacheMethod.containsCachedData(index, intensities[1])) {
00314       intensities[1] = getUnfilteredGridPoint(index);
00315       cacheMethod.cacheData(index, intensities[1]);
00316     }
00317 
00318     index += sizeX-1;
00319 
00320     if (!cacheMethod.containsCachedData(index, intensities[2])) {
00321       intensities[2] = getUnfilteredGridPoint(index);
00322       cacheMethod.cacheData(index, intensities[2]);
00323     }
00324 
00325     ++index;
00326 
00327     if (!cacheMethod.containsCachedData(index, intensities[3])) {
00328       intensities[3] = getUnfilteredGridPoint(index);
00329       cacheMethod.cacheData(index, intensities[3]);
00330     }
00331 
00332     float dx1 = intensities[0] - intensities[1];
00333     float dx2 = intensities[2] - intensities[3];
00334 
00335     float dy1 = intensities[0] - intensities[2];
00336     float dy2 = intensities[1] - intensities[3];
00337 
00338     float xFacInv = (1.0f - factors[0]);
00339     float yFacInv = (1.0f - factors[1]);
00340 
00341     return Eigen::Vector3f(
00342       ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
00343       ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),
00344       -((dx1 * xFacInv) + (dx2 * factors[0])),
00345       -((dy1 * yFacInv) + (dy2 * factors[1]))
00346     );
00347   }
00348 
00349   Eigen::Affine2f getTransformForState(const Eigen::Vector3f& transVector) const
00350   {
00351     return Eigen::Translation2f(transVector[0], transVector[1]) * Eigen::Rotation2Df(transVector[2]);
00352   }
00353 
00354   Eigen::Translation2f getTranslationForState(const Eigen::Vector3f& transVector) const
00355   {
00356     return Eigen::Translation2f(transVector[0], transVector[1]);
00357   }
00358 
00359   void resetCachedData()
00360   {
00361     cacheMethod.resetCache();
00362   }
00363 
00364   void resetSamplePoints()
00365   {
00366     samplePoints.clear();
00367   }
00368 
00369   const std::vector<Eigen::Vector3f>& getSamplePoints() const
00370   {
00371     return samplePoints;
00372   }
00373 
00374 protected:
00375 
00376   Eigen::Vector4f intensities;
00377 
00378   ConcreteCacheMethod cacheMethod;
00379 
00380   const ConcreteOccGridMap* concreteGridMap;
00381 
00382   std::vector<Eigen::Vector3f> samplePoints;
00383 
00384   int size;
00385 
00386   float mapObstacleThreshold;
00387 };
00388 
00389 }
00390 
00391 
00392 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Mon Jun 27 2016 04:57:19