[hector_slam code review] #13. hector_mapping : OccGridMapUtil.h

OccGridMapUtil.h

---

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392

//================================================================================================= // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt // All rights reserved. // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: //     * Redistributions of source code must retain the above copyright //       notice, this list of conditions and the following disclaimer. //     * Redistributions in binary form must reproduce the above copyright //       notice, this list of conditions and the following disclaimer in the //       documentation and/or other materials provided with the distribution. //     * Neither the name of the Simulation, Systems Optimization and Robotics //       group, TU Darmstadt nor the names of its contributors may be used to //       endorse or promote products derived from this software without //       specific prior written permission. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //================================================================================================= #ifndef __OccGridMapUtil_h_ #define __OccGridMapUtil_h_ #include <cmath> #include "../scan/DataPointContainer.h" #include "../util/UtilFunctions.h" namespace hectorslam { template<typename ConcreteOccGridMap, typename ConcreteCacheMethod> class OccGridMapUtil { public:   OccGridMapUtil(const ConcreteOccGridMap* gridMap)     : concreteGridMap(gridMap)     , size(0)   {     mapObstacleThreshold = gridMap->getObstacleThreshold();     cacheMethod.setMapSize(gridMap->getMapDimensions());   }   ~OccGridMapUtil()   {} public:   EIGEN_MAKE_ALIGNED_OPERATOR_NEW   inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const { return concreteGridMap->getWorldCoordsPose(mapPose); };   inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const { return concreteGridMap->getMapCoordsPose(worldPose); };   inline Eigen::Vector2f getWorldCoordsPoint(const Eigen::Vector2f& mapPoint) const { return concreteGridMap->getWorldCoords(mapPoint); };   void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)   {     int size = dataPoints.getSize();     Eigen::Affine2f transform(getTransformForState(pose));     float sinRot = sin(pose[2]);     float cosRot = cos(pose[2]);     H = Eigen::Matrix3f::Zero();     dTr = Eigen::Vector3f::Zero();     for (int i = 0; i < size; ++i) {       const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));       Eigen::Vector3f transformedPointData( interpMapValueWithDerivatives(transform * currPoint));       float funVal = 1.0f - transformedPointData[0];       dTr[0] += transformedPointData[1] * funVal;       dTr[1] += transformedPointData[2] * funVal;       float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);       dTr[2] += rotDeriv * funVal;       H(0, 0) += util::sqr(transformedPointData[1]);       H(1, 1) += util::sqr(transformedPointData[2]);       H(2, 2) += util::sqr(rotDeriv);       H(0, 1) += transformedPointData[1] * transformedPointData[2];       H(0, 2) += transformedPointData[1] * rotDeriv;       H(1, 2) += transformedPointData[2] * rotDeriv;     }     H(1, 0) = H(0, 1);     H(2, 0) = H(0, 2);     H(2, 1) = H(1, 2);   }   Eigen::Matrix3f getCovarianceForPose(const Eigen::Vector3f& mapPose, const DataContainer& dataPoints)   {     float deltaTransX = 1.5f;     float deltaTransY = 1.5f;     float deltaAng = 0.05f;     float x = mapPose[0];     float y = mapPose[1];     float ang = mapPose[2];     Eigen::Matrix<float, 3, 7> sigmaPoints;     sigmaPoints.block<3, 1>(0, 0) = Eigen::Vector3f(x + deltaTransX, y, ang);     sigmaPoints.block<3, 1>(0, 1) = Eigen::Vector3f(x - deltaTransX, y, ang);     sigmaPoints.block<3, 1>(0, 2) = Eigen::Vector3f(x, y + deltaTransY, ang);     sigmaPoints.block<3, 1>(0, 3) = Eigen::Vector3f(x, y - deltaTransY, ang);     sigmaPoints.block<3, 1>(0, 4) = Eigen::Vector3f(x, y, ang + deltaAng);     sigmaPoints.block<3, 1>(0, 5) = Eigen::Vector3f(x, y, ang - deltaAng);     sigmaPoints.block<3, 1>(0, 6) = mapPose;     Eigen::Matrix<float, 7, 1> likelihoods;     likelihoods[0] = getLikelihoodForState(Eigen::Vector3f(x + deltaTransX, y, ang), dataPoints);     likelihoods[1] = getLikelihoodForState(Eigen::Vector3f(x - deltaTransX, y, ang), dataPoints);     likelihoods[2] = getLikelihoodForState(Eigen::Vector3f(x, y + deltaTransY, ang), dataPoints);     likelihoods[3] = getLikelihoodForState(Eigen::Vector3f(x, y - deltaTransY, ang), dataPoints);     likelihoods[4] = getLikelihoodForState(Eigen::Vector3f(x, y, ang + deltaAng), dataPoints);     likelihoods[5] = getLikelihoodForState(Eigen::Vector3f(x, y, ang - deltaAng), dataPoints);     likelihoods[6] = getLikelihoodForState(Eigen::Vector3f(x, y, ang), dataPoints);     float invLhNormalizer = 1 / likelihoods.sum();     std::cout << "\n lhs:\n" << likelihoods;     Eigen::Vector3f mean(Eigen::Vector3f::Zero());     for (int i = 0; i < 7; ++i) {       mean += (sigmaPoints.block<3, 1>(0, i) * likelihoods[i]);     }     mean *= invLhNormalizer;     Eigen::Matrix3f covMatrixMap(Eigen::Matrix3f::Zero());     for (int i = 0; i < 7; ++i) {       Eigen::Vector3f sigPointMinusMean(sigmaPoints.block<3, 1>(0, i) - mean);       covMatrixMap += (likelihoods[i] * invLhNormalizer) * (sigPointMinusMean * (sigPointMinusMean.transpose()));     }     return covMatrixMap;     //covMatrix.cwise() * invLhNormalizer;     //transform = getTransformForState(Eigen::Vector3f(x-deltaTrans, y, ang);   }   Eigen::Matrix3f getCovMatrixWorldCoords(const Eigen::Matrix3f& covMatMap)   {     //std::cout << "\nCovMap:\n" << covMatMap;     Eigen::Matrix3f covMatWorld;     float scaleTrans = concreteGridMap->getCellLength();     float scaleTransSq = util::sqr(scaleTrans);     covMatWorld(0, 0) = covMatMap(0, 0) * scaleTransSq;     covMatWorld(1, 1) = covMatMap(1, 1) * scaleTransSq;     covMatWorld(1, 0) = covMatMap(1, 0) * scaleTransSq;     covMatWorld(0, 1) = covMatWorld(1, 0);     covMatWorld(2, 0) = covMatMap(2, 0) * scaleTrans;     covMatWorld(0, 2) = covMatWorld(2, 0);     covMatWorld(2, 1) = covMatMap(2, 1) * scaleTrans;     covMatWorld(1, 2) = covMatWorld(2, 1);     covMatWorld(2, 2) = covMatMap(2, 2);     return covMatWorld;   }   float getLikelihoodForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)   {     float resid = getResidualForState(state, dataPoints);     return getLikelihoodForResidual(resid, dataPoints.getSize());   }   float getLikelihoodForResidual(float residual, int numDataPoints)   {     float numDataPointsA = static_cast<int>(numDataPoints);     float sizef = static_cast<float>(numDataPointsA);     return 1 - (residual / sizef);   }   float getResidualForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)   {     int size = dataPoints.getSize();     int stepSize = 1;     float residual = 0.0f;     Eigen::Affine2f transform(getTransformForState(state));     for (int i = 0; i < size; i += stepSize) {       float funval = 1.0f - interpMapValue(transform * dataPoints.getVecEntry(i));       residual += funval;     }     return residual;   }   float getUnfilteredGridPoint(Eigen::Vector2i& gridCoords) const   {     return (concreteGridMap->getGridProbabilityMap(gridCoords.x(), gridCoords.y()));   }   float getUnfilteredGridPoint(int index) const   {     return (concreteGridMap->getGridProbabilityMap(index));   }   float interpMapValue(const Eigen::Vector2f& coords)   {     //check if coords are within map limits.     if (concreteGridMap->pointOutOfMapBounds(coords)){       return 0.0f;     }     //map coords are alway positive, floor them by casting to int     Eigen::Vector2i indMin(coords.cast<int>());     //get factors for bilinear interpolation     Eigen::Vector2f factors(coords - indMin.cast<float>());     int sizeX = concreteGridMap->getSizeX();     int index = indMin[1] * sizeX + indMin[0];     // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained     // filter gridPoint with gaussian and store in cache.     if (!cacheMethod.containsCachedData(index, intensities[0])) {       intensities[0] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[0]);     }     ++index;     if (!cacheMethod.containsCachedData(index, intensities[1])) {       intensities[1] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[1]);     }     index += sizeX-1;     if (!cacheMethod.containsCachedData(index, intensities[2])) {       intensities[2] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[2]);     }     ++index;     if (!cacheMethod.containsCachedData(index, intensities[3])) {       intensities[3] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[3]);     }     float xFacInv = (1.0f - factors[0]);     float yFacInv = (1.0f - factors[1]);     return       ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +       ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1]));   }   Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)   {     //check if coords are within map limits.     if (concreteGridMap->pointOutOfMapBounds(coords)){       return Eigen::Vector3f(0.0f, 0.0f, 0.0f);     }     //map coords are always positive, floor them by casting to int     Eigen::Vector2i indMin(coords.cast<int>());     //get factors for bilinear interpolation     Eigen::Vector2f factors(coords - indMin.cast<float>());     int sizeX = concreteGridMap->getSizeX();     int index = indMin[1] * sizeX + indMin[0];     // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained     // filter gridPoint with gaussian and store in cache.     if (!cacheMethod.containsCachedData(index, intensities[0])) {       intensities[0] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[0]);     }     ++index;     if (!cacheMethod.containsCachedData(index, intensities[1])) {       intensities[1] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[1]);     }     index += sizeX-1;     if (!cacheMethod.containsCachedData(index, intensities[2])) {       intensities[2] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[2]);     }     ++index;     if (!cacheMethod.containsCachedData(index, intensities[3])) {       intensities[3] = getUnfilteredGridPoint(index);       cacheMethod.cacheData(index, intensities[3]);     }     float dx1 = intensities[0] - intensities[1];     float dx2 = intensities[2] - intensities[3];     float dy1 = intensities[0] - intensities[2];     float dy2 = intensities[1] - intensities[3];     float xFacInv = (1.0f - factors[0]);     float yFacInv = (1.0f - factors[1]);     return Eigen::Vector3f(       ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +       ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),       -((dx1 * xFacInv) + (dx2 * factors[0])),       -((dy1 * yFacInv) + (dy2 * factors[1]))     );   }   Eigen::Affine2f getTransformForState(const Eigen::Vector3f& transVector) const   {     return Eigen::Translation2f(transVector[0], transVector[1]) * Eigen::Rotation2Df(transVector[2]);   }   Eigen::Translation2f getTranslationForState(const Eigen::Vector3f& transVector) const   {     return Eigen::Translation2f(transVector[0], transVector[1]);   }   void resetCachedData()   {     cacheMethod.resetCache();   }   void resetSamplePoints()   {     samplePoints.clear();   }   const std::vector<Eigen::Vector3f>& getSamplePoints() const   {     return samplePoints;   } protected:   Eigen::Vector4f intensities;   ConcreteCacheMethod cacheMethod;   const ConcreteOccGridMap* concreteGridMap;   std::vector<Eigen::Vector3f> samplePoints;   int size;   float mapObstacleThreshold; }; } #endif Colored by Color Scripter

cs

---

namespace hectorslam { 

namespace는 hectorslam 이다.

---

template<typename ConcreteOccGridMap, typename ConcreteCacheMethod>

class OccGridMapUtil

{

OccGridMap 의 형태와 CacheMethod 방법을 선택하도록 하는 Class Template 이다.
Class 는 OccGridMapUtil이다.

---

protected:

  Eigen::Vector4f intensities;

  ConcreteCacheMethod cacheMethod;

  const ConcreteOccGridMap* concreteGridMap;

  std::vector<Eigen::Vector3f> samplePoints;

  int size;

  float mapObstacleThreshold;

선언된 변수들이다.

---

  OccGridMapUtil(const ConcreteOccGridMap* gridMap)

    : concreteGridMap(gridMap)

    , size(0)

  {

    mapObstacleThreshold = gridMap->getObstacleThreshold();

    cacheMethod.setMapSize(gridMap->getMapDimensions());

  }

생성자이다. gridMap을 받아 생성되며,
클래스의 멤버변수인 concreteGridMap이 gridMap을 파라미터로 받아 리스트초기화된다.

mapObstacleThreshold = gridMap->getObstacleThreshold();

  float getObstacleThreshold() const

  {

    ConcreteCellType temp;

    temp.resetGridCell();

    return concreteGridFunctions.getGridProbability(temp);

  }

  float getGridProbabilityMap(int index) const

  {

    return concreteGridFunctions.getGridProbability(this->getCell(index));

  }

  float getGridProbability(const LogOddsCell& cell) const

  {

    float odds = exp(cell.logOddsVal);

    return odds / (odds + 1.0f);

    /*

    float val = cell.logOddsVal;

    //prevent #IND when doing exp(large number).

    if (val > 50.0f) {

      return 1.0f;

    } else {

      float odds = exp(val);

      return odds / (odds + 1.0f);

    }

    */

    //return 0.5f;

  }

  void resetGridCell()

  {

    logOddsVal = 0.0f;

    updateIndex = -1;

  }

    cacheMethod.setMapSize(gridMap->getMapDimensions());

map size를 set한다.

const Eigen::Vector2i& getMapDimensions() const { return mapDimensions; };

  void setMapSize(const Eigen::Vector2i& newDimensions)

  {

    setArraySize(newDimensions);

  }

---

  ~OccGridMapUtil()

  {}

소멸자이다.

---

  inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
{ return concreteGridMap->getWorldCoordsPose(mapPose); };

  inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
{ return concreteGridMap->getMapCoordsPose(worldPose); };

mapPose를 주었을 때, worldPose를
worldPose를 주었을 때, mapPose를 주게 된다.

---

  inline Eigen::Vector2f getWorldCoordsPoint(const Eigen::Vector2f& mapPoint) const 

{ return concreteGridMap->getWorldCoords(mapPoint); };

mapPoint를 주고 worldCoordsPoint를 구한다.

---

  void getCompleteHessianDerivs(const Eigen::Vector3f& pose,
const DataContainer& dataPoints,
Eigen::Matrix3f& H,
Eigen::Vector3f& dTr)

  {

    int size = dataPoints.getSize();

    Eigen::Affine2f transform(getTransformForState(pose));

    float sinRot = sin(pose[2]);

    float cosRot = cos(pose[2]);

    H = Eigen::Matrix3f::Zero();

    dTr = Eigen::Vector3f::Zero();

    for (int i = 0; i < size; ++i) {

      const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));

      Eigen::Vector3f transformedPointData(
interpMapValueWithDerivatives(transform * currPoint));

      float funVal = 1.0f - transformedPointData[0];

      dTr[0] += transformedPointData[1] * funVal;

      dTr[1] += transformedPointData[2] * funVal;

      float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) *
transformedPointData[1] +
(cosRot * currPoint.x() - sinRot * currPoint.y()) *
transformedPointData[2]);

      dTr[2] += rotDeriv * funVal;

      H(0, 0) += util::sqr(transformedPointData[1]);

      H(1, 1) += util::sqr(transformedPointData[2]);

      H(2, 2) += util::sqr(rotDeriv);

      H(0, 1) += transformedPointData[1] * transformedPointData[2];

      H(0, 2) += transformedPointData[1] * rotDeriv;

      H(1, 2) += transformedPointData[2] * rotDeriv;

    }

    H(1, 0) = H(0, 1);

    H(2, 0) = H(0, 2);

    H(2, 1) = H(1, 2);

  }

Hessian Matrix는 아래 그림과 같은 행렬이다.
( 링크참조 : https://darkpgmr.tistory.com/132 )

위 행렬을 보면 다변수 함수 f에 대해 각 변수에 대한 2차미분을 하는 것을 알 수 있다.

함수 f를 1차미분 한다는 것은 해당 함수에서 어떠한 점 p 근처의 기울기(gradient)를 알 수 있다는 것을 의미한다. (Jacobian 행렬이 그렇다.)

함수 f를 2차미분 한다는 것은 해당 함수에서 어떠한 점 p 근처의 곡률(curvature)를 알 수 있다는 것을 의미한다.

그러므로 Hessian Matrix는 "함수의 곡률(curvature)" 특성을 나타내는 행렬이다.

어떤 (다변수) 함수를 최적화시키기 위해 극점(극대,극소)을 찾기 위해서는 먼저 그 함수의 일차미분인 기울기(gradient)가 0이 되는 지점을 찾는다. 이러한 기울기가 0이 되는 지점이 극대점 혹은 극소점인 경우 critical point라 부르고, 말 안장처럼 방향에 따라서 극대, 극소가 바뀌는 경우 saddle point라 부른다.

이렇게 기울기가 0이 되는 지점이 극대점인지 극소점인지 saddle point인지 구분하기 위해서는 이차미분값을 조사해야 하며, 이를 위해서 Hessian Matrix를 사용하는 것이다.

이 때, 고유벡터와 교유값 개념을 사용하게 된다.
(링크참조 : https://darkpgmr.tistory.com/105)

다크프로그래머님의 말을 빌려오면,
행렬 A를 선형변환으로 봤을 때, 선형변환 A에 의한 변환 결과가 자기 자신의 상수배가 되는 0이 아닌 벡터를 고유벡터(eigenvector)라 하고 이 상수배 값을 고유값(eigenvalue)라 한다.

즉, n x n 정방행렬(고유값, 고유벡터는 정방행렬에 대해서만 정의된다) A에 대해 Av = λv를 만족하는 0이 아닌 열벡터 v를 고유벡터, 상수 λ를 고유값이라 정의한다.

좀더 정확한 용어로는 λ는 '행렬 A의 고유값', v는 '행렬 A의 λ에 대한 고유벡터'이다.

즉, 고유값과 고유벡터는 행렬에 따라 정의되는 값으로서 어떤 행렬은 이러한 고유값-고유벡터가 아에 존재하지 않을수도 있고 어떤 행렬은 하나만 존재하거나 또는 최대 n개까지 존재할 수 있다.

그리고 Hessian Matrix 에도 이러한 고유값과 고유벡터가 존재할 것이다.
이를 이용하여 극대값, 극소값, saddle point 를 판단할 수 있다.

(1) Hessian Matrix의 모든 고유값(eigen value)이 양수인 경우 : 해당 지점에서 함수는 극소

(2) Hessian Matrix의 모든 고유값(eigen value)이 음수인 경우 : 해당 지점에서 함수는 극대

(3) Hessian Matrix의 모든 고유값(eigen value)이 양수와 음수를 모두 갖는 경우 : 해당 지점에서 함수는 saddle point

그리고 이 때, Hessian Matrix의 고유벡터(eigen vector)는 함수의 곡률이 가장 큰 방향 벡터를 나타내고, 고유값(eigen vector)는 해당 고유벡터(eigen vector) 방향으로의 함수의 곡률(curvature, 이차미분값)을 나타낸다.

자 그럼 다시 code로 돌아와보자.
(1) int size = dataPoints.getSize();

먼저 LRF 센서의 data size를 갖고온다.

(2) Eigen::Affine2f transform(getTransformForState(pose));

const Eigen::Vector3f& pose

pose는 robot의 (x,y,theta) 값을 나타내는 벡터이다.

  Eigen::Affine2f getTransformForState(const Eigen::Vector3f& transVector) const

  {

    return Eigen::Translation2f(transVector[0], transVector[1]) *
Eigen::Rotation2Df(transVector[2]);

  }

getTransformForState를 통해 pose값을 넘겨주었고,
함수 내에서는 (x,y) 를 이용해 translation matrix를 만들고, (theta)를 이용해 rotation matrix를 만든다. 그리고 두 matrix를 곱하여 affine matrix를 만들어 반환하게 된다.

최종적으로 transform이란 Affine2f 형태의 matrix에는 이 transform matrix가 들어가는 것이다.

Hector SLAM 에 대한 논문인 A Flexible and Scalable SLAM System with Full
3D Motion Estimation 에서 IV.2D SLAM 에서 아래 수식을 구하는 과정 중,

아래 과정을 진행하기 위해 Affine Matrix를 구해주는 것이다.

(3)

    float sinRot = sin(pose[2]);

    float cosRot = cos(pose[2]);

(theta)값을 파라미터로 하여 sinRot 값과 cosRot 값을 구한다.

(4)

    H = Eigen::Matrix3f::Zero();

    dTr = Eigen::Vector3f::Zero();

이 부분은 Hector SLAM 에 대한 논문인 A Flexible and Scalable SLAM System with Full
3D Motion Estimation 에서 아래 수식들을 표현하기 위한 내용이다.

(5)

for (int i = 0; i < size; ++i) {

      const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));

이제 LRF 센서의 각 data에 대해 연산을 시작한다.
각 data는 currPoint라는 vector에 담긴다.

(6)

Eigen::Vector3f transformedPointData(

interpMapValueWithDerivatives(transform * currPoint));

아래에서 설명된 interMapValueWithDerivatives 함수에 의해 총 3개의 값을 return 받게된다.

1번째는 해당 point의 interpolated 된 값( 여기에서는 양선형 보간법을 이용했다. )
2번째는 해당 point의 y축으로의 편미분 값
3번째는 해당 point의 x축으로의 편미분 값이다.

논문에서는 아래 수식과 같다.

(7)

      float funVal = 1.0f - transformedPointData[0];

이 부분이 기본적으로 우리가 구하려 했던 아래 수식 중 괄호 안의 값이며,

(8)

      dTr[0] += transformedPointData[1] * funVal;

      dTr[1] += transformedPointData[2] * funVal;

transformedPointData[1] = ∂M/∂y(Pm)
transformedPointData[2] = ∂M/∂x(Pm)
funVal = [1 - M(Pm)]
dTr[0] = ∂M/∂y(Pm) * [1 - M(Pm)]
dTr[1] = ∂M/∂x(Pm) * [1 - M(Pm)]

(9)

      float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) *

transformedPointData[1] +

(cosRot * currPoint.x() - sinRot * currPoint.y()) *

transformedPointData[2]);

transformedPointData[1] = ∂M/∂y(S i)
transformedPointData[2] = ∂M/∂x(S i)
currPoint.x() = S i,x
currPoint.y() = S i,y

rotDeriv =| ∂M/∂y(Pm) | *  | -sinRot  -cosRot | * | S i,x |
             | ∂M/∂x(Pm) |    | cosRot   -sinRot  |   | S i,y |


| -sinRot  -cosRot | * | S i,x |
| cosRot   -sinRot  |   | S i,y |

는 아래 수식과 같고,

그러므로 rotDeriv는 아래 수식에서 왼쪽 괄호 안의 값과 같다.

(10)
      dTr[2] += rotDeriv * funVal;

funVal = [1-M(Si(ξ))] 이므로

결국에는 아래의 수식의 오른쪽 두 괄호를 의미하게 되는 것이다.

(11)

      H(0, 0) += util::sqr(transformedPointData[1]);

      H(1, 1) += util::sqr(transformedPointData[2]);

      H(2, 2) += util::sqr(rotDeriv);

      H(0, 1) += transformedPointData[1] * transformedPointData[2];

      H(0, 2) += transformedPointData[1] * rotDeriv;

      H(1, 2) += transformedPointData[2] * rotDeriv;

    }

    H(1, 0) = H(0, 1);

    H(2, 0) = H(0, 2);

    H(2, 1) = H(1, 2);

이제 H를 찾아보자.

H의 수식은 다음과 같다.

그리고 ∇M(Si(ξ)) = [ ∂M/∂y(Si)   ∂M/∂x(Si) ] 인 1x2 행렬이다. 

그리고 ∂Si(ξ)/∂ξ 는 아래와 같은 2x3 행렬이다.

그러므로 두 행렬을 곱하면 다음과 같은 1x3 행렬이 만들어진다.

[  ∂M/∂y(Si)   ∂M/∂x(Si)   ∇M(Si(ξ))(∂Si(ξ)/∂ξ) ]

위 행렬을 제곱(행렬에서 제곱이라 표현하기 이상하지만)하면 위 코드가 나오게 된다.

---

 Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)

  {

    //check if coords are within map limits.

    if (concreteGridMap->pointOutOfMapBounds(coords)){

      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);

    }

    //map coords are always positive, floor them by casting to int

    Eigen::Vector2i indMin(coords.cast<int>());

    //get factors for bilinear interpolation

    Eigen::Vector2f factors(coords - indMin.cast<float>());

    int sizeX = concreteGridMap->getSizeX();

    int index = indMin[1] * sizeX + indMin[0];

    // get grid values for the 4 grid points surrounding the current coords. 
// Check cached data first, if not contained

    // filter gridPoint with gaussian and store in cache.

    if (!cacheMethod.containsCachedData(index, intensities[0])) {

      intensities[0] = getUnfilteredGridPoint(index);

      cacheMethod.cacheData(index, intensities[0]);

    }

    ++index;

    if (!cacheMethod.containsCachedData(index, intensities[1])) {

      intensities[1] = getUnfilteredGridPoint(index);

      cacheMethod.cacheData(index, intensities[1]);

    }

    index += sizeX-1;

    if (!cacheMethod.containsCachedData(index, intensities[2])) {

      intensities[2] = getUnfilteredGridPoint(index);

      cacheMethod.cacheData(index, intensities[2]);

    }

    ++index;

    if (!cacheMethod.containsCachedData(index, intensities[3])) {

      intensities[3] = getUnfilteredGridPoint(index);

      cacheMethod.cacheData(index, intensities[3]);

    }

    float dx1 = intensities[0] - intensities[1];

    float dx2 = intensities[2] - intensities[3];

    float dy1 = intensities[0] - intensities[2];

    float dy2 = intensities[1] - intensities[3];

    float xFacInv = (1.0f - factors[0]);

    float yFacInv = (1.0f - factors[1]);

    return Eigen::Vector3f(

      ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +

      ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),

      -((dx1 * xFacInv) + (dx2 * factors[0])),

      -((dy1 * yFacInv) + (dy2 * factors[1]))

    );

  }

(1)
Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)

저 coords 값은 LRF Sensor에 의해 검출된 point 중 하나의 point를 나타낸다.

    //check if coords are within map limits.

    if (concreteGridMap->pointOutOfMapBounds(coords)){

      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);

    }

파라미터로 주어진 포인트가 map limits 안에 존재하는지 확인한다.

(2)

    //map coords are always positive, floor them by casting to int

    Eigen::Vector2i indMin(coords.cast<int>());

파라미터로 주어진 포인트를 int형 vector로 cast한다.

(3)

    //get factors for bilinear interpolation

    Eigen::Vector2f factors(coords - indMin.cast<float>());

bilinear interpolation 은 우리말로 양선형 보간법이다.
(링크참조 : https://darkpgmr.tistory.com/117)

(단, α=h1/(h1+h2), β=h2/(h1+h2), p=w 1/(w 1+w 2), q=w 2/(w 1+w 2))

이를 위한 과정인 것인데, 천천히 살펴보자.

우리는 원래 LRF Sensor의 point를 양의 실수인 coords parameter로 받아왔다.

그리고 이를 int로 cast하고 indMin이란 변수명으로 정의하였다.

그리고 coords 에서 float으로 cast된 indMin을 빼줌으로써 소수점 아래의 값들을 얻은 것이다.

그림으로 표현하면 다음과 같다.

(4)
int sizeX = concreteGridMap->getSizeX();

맵의 x축 사이즈 값을 가져온다.

(5)
int index = indMin[1] * sizeX + indMin[0];

index 값은 당연히 코드의 식과 같다.

(6)

  // get grid values for the 4 grid points surrounding the current coords. 
// Check cached data first, if not contained

    // filter gridPoint with gaussian and store in cache.

    if (!cacheMethod.containsCachedData(index, intensities[0])) {

      intensities[0] = getUnfilteredGridPoint(index);

      cacheMethod.cacheData(index, intensities[0]);

    }

   ++index;

   if (!cacheMethod.containsCachedData(index, intensities[1])) {

     intensities[1] = getUnfilteredGridPoint(index);

     cacheMethod.cacheData(index, intensities[1]);

   }

   index += sizeX-1;

   if (!cacheMethod.containsCachedData(index, intensities[2])) {

     intensities[2] = getUnfilteredGridPoint(index);

     cacheMethod.cacheData(index, intensities[2]);

   }

   ++index;

   if (!cacheMethod.containsCachedData(index, intensities[3])) {

     intensities[3] = getUnfilteredGridPoint(index);

     cacheMethod.cacheData(index, intensities[3]);

   }

  Eigen::Vector4f intensities;

intensities는 클래스의 멤버함수이다.
따로 초기화 되지 않으므로 containsCachedData 함수에서 해당 index(LRF Sensor의 point)에 값과 같은지 다른지 판단한 후 값이 없거나 다르다면 getUnfilteredGridPoint 함수를 통해 intensities[0]값을 정해주고 cacheData 함수를 통해 해당 index에 intensities[0] 값을 넣어준다.

containsCachedData와 cacheData 함수에 대한 설명은 다음과 같다.

  /**

   * Checks wether cached data for coords is available.  * If this is the case, writes data into val.

   * @param coords The coordinates

   * @param val Reference to a float the data is written to if available

   * @return Indicates if cached data is available

   */

  bool containsCachedData(int index, float& val)

  {

    const CachedMapElement& elem (cacheArray[index]);

    if (elem.index == currCacheIndex) {

      val = elem.val;

      return true;

    } else {

      return false;

    }

  }

coordinates를 위한 cached data가 available한지 확인한 후,
available 하다면 val에 data를 써준다.

const CachedMapElement& elem (cacheArray[index]);

먼저 cacheArray[index] 의 데이터를 가져와 CachedMapElement 클래스를 갖는 elem이란 객체를 만들어준다.

    if (elem.index == currCacheIndex) {

      val = elem.val;

      return true;

    } else {

      return false;

    }

만일 elem의 index가 currCacheIndex와 같다면
파라미터로 받은 val의 값에 elem.val 의 값을 넣어준다.
그리고 true를 return한다.

만약 elem의 index가 currCacheIndex와 다르다면 false를 return한다.

  /**

   * Caches float value val for coordinates coords.

   * @param coords The coordinates

   * @param val The value to be cached for coordinates.

   */

  void cacheData(int index, float val)

  {

    CachedMapElement& elem (cacheArray[index]);

    elem.index = currCacheIndex;

    elem.val = val;

  }

이 cacheData란 함수에서는 elem이라는 객체를 cacheArray[index]의 데이터를 받아 생성하고 현재 currCacheIndex 값을 elem.index에 담고, 매개변수로 받은 val를 elem.val에 담은다.

즉
cacheData( 1, 5 )
라고 한다면

1번 인덱스의 cacheArray를 가져와 index값은 현재 인덱스인 currCacheIndex로,
값은 내가 주어준 값인 val로 바꾸는 것이다.





그리고 이러한 과정을 index 값을 변화시켜서 총 4개의 cached points를 얻어
intensities에 담게 되는것이다.

a. index
b. index +1
c. index - size_x
d. index - size_x + 1



결국 위와같은 내용이 된 것이다.

(7)

    float dx1 = intensities[0] - intensities[1];

    float dx2 = intensities[2] - intensities[3];

    float dy1 = intensities[0] - intensities[2];

    float dy2 = intensities[1] - intensities[3];

그리고 x축과 y축의 거리를 각각 구해준다.

(8)

    float xFacInv = (1.0f - factors[0]);

    float yFacInv = (1.0f - factors[1]);

xFacInv와 yFacInv를 (3)에서 만든 그림에 추가하면 다음과 같다.

(9)

    return Eigen::Vector3f(

      ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +

      ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),

      -((dx1 * xFacInv) + (dx2 * factors[0])),

      -((dy1 * yFacInv) + (dy2 * factors[1]))

    );

최종적으로 Vector3f 를 반환하게 되는데,
1번째 파라미터는 interpolation한 point의 interpolation 값이고,
2번째, 3번째 파라미터는

derivates, 즉 미분값이다.

위 식의 파라미터들을 그림의 수식에 맞춰서 변환해보자.

    float dx1 = intensities[0] - intensities[1];

    float dx2 = intensities[2] - intensities[3];

Eigen::Vector2f factors(coords - indMin.cast<float>());
     float xFacInv = (1.0f - factors[0]);

intensities[0] = M(P00)

intensities[1] = M(P10)

intensities[2] = M(P01)

intensities[3] = M(P11)

dx1 = M(P00) - M(P01)

dx2 = M(P10) - M(P11)

factors = ( x-x0 , y-y0 )

xFacInv = x1-x 

yFacInv = y1-y

-((dx1 * xFacInv) + (dx2 * factors[0]))

이제 다시 위 내용을 수식으로 써보자.

- ( ( ( M(P00) - M(P01) ) * (x1-x) ) + ( ( M(P10) - M(P11) ) * (x-x0) ) )

- 를 안에 넣어주면 다음과 같이 변한다.

( ( M(P01) - M(P00) ) * (x1-x) ) + ( ( M(P11) - M(P10) ) * (x-x0) ) 

그리고 grid 평면 상에서 y1과 y0, x1과 x0 사이의 거리는 1이니,

위 사진의 (5), (6) 번에서 분모들이 사라진다고 생각하면

최종적으로 식은 아래와 같아지는 것이다.

∂M/∂y(Pm) = ( ( M(P01) - M(P00) ) * (x1-x) ) + ( ( M(P11) - M(P10) ) * (x-x0) ) 

이는 ∂M/∂x(Pm) 에 대해서도 같다.





솔직히 잘 모르겠는건, 결국에 transformedPointData 의 데이터로 
( 양선형 보간한 값, y에 대해 편미분한 값, x에 대해 편미분한 값 )
을 넘겨준다는 얘기인데... x에 대해 편미분한 값을 1번째 인자로, y에 대해 편미분한 값을 2번째 인자로 넘겨줘야 하는거 아닌가 싶다... 결과에 문제가 없어서 이렇게 진행한거 같긴 한데, 논문의 수식과 반대되지 않나?

--> hector_slam git에 issue를 올린 결과, 내 예상과 같이 코드가 잘못 된 것이며, 내 예상처럼 코드를 수정할 경우 더 잘 된다고 한다.
아래에 링크를 첨부하겠다.

https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/69

---

float getUnfilteredGridPoint(Eigen::Vector2i& gridCoords) const

  {

    return (concreteGridMap->getGridProbabilityMap(gridCoords.x(), gridCoords.y()));

  }

  float getUnfilteredGridPoint(int index) const

  {

    return (concreteGridMap->getGridProbabilityMap(index));

  }

getUnfilteredGridPoint 함수는 파라미터에 따라 두 종류가 있다.

그리고 getGridProbabilityMap 함수는 최종적으로는 아래의 과정을 수행하여 결정된 GridProbability 값을 전달해준다.

  /**

   * Get the probability value represented by the grid cell.

   * @param cell The cell.

   * @return The probability

   */

  float getGridProbability(const LogOddsCell& cell) const

  {

    float odds = exp(cell.logOddsVal);

    return odds / (odds + 1.0f);

    /*

    float val = cell.logOddsVal;

    //prevent #IND when doing exp(large number).

    if (val > 50.0f) {

      return 1.0f;

    } else {

      float odds = exp(val);

      return odds / (odds + 1.0f);

    }

    */

    //return 0.5f;

  }

float odds = exp(cell.logOddsVal);

    return odds / (odds + 1.0f);

probabilistic robotics를 참고하면,
최종적인 probability를 구하기 위한 식은 다음과 같다.

prob = 1 - {1 / (1+exp(log(odds)))}

이 때, 1은 (1+exp(log(odds))) / (1+exp(log(odds))) 이므로
(1+exp(log(odds))) / (1+exp(log(odds))) - {1 / (1+exp(log(odds)))} 는

exp(log(odds)) / (exp(log(odds)) + 1) 이 된다.

이 과정은 sigmoid function을 이용하여 최종 값을 안정적으로 0과 1 사이의 값으로 얻어내는 방법이다.

---

Eigen::Matrix3f getCovarianceForPose(const Eigen::Vector3f& mapPose,
const DataContainer& dataPoints)

  {

    float deltaTransX = 1.5f;

    float deltaTransY = 1.5f;

    float deltaAng = 0.05f;

    float x = mapPose[0];

    float y = mapPose[1];

    float ang = mapPose[2];

    Eigen::Matrix<float, 3, 7> sigmaPoints;

    sigmaPoints.block<3, 1>(0, 0) = Eigen::Vector3f(x + deltaTransX, y, ang);

    sigmaPoints.block<3, 1>(0, 1) = Eigen::Vector3f(x - deltaTransX, y, ang);

    sigmaPoints.block<3, 1>(0, 2) = Eigen::Vector3f(x, y + deltaTransY, ang);

    sigmaPoints.block<3, 1>(0, 3) = Eigen::Vector3f(x, y - deltaTransY, ang);

    sigmaPoints.block<3, 1>(0, 4) = Eigen::Vector3f(x, y, ang + deltaAng);

    sigmaPoints.block<3, 1>(0, 5) = Eigen::Vector3f(x, y, ang - deltaAng);

    sigmaPoints.block<3, 1>(0, 6) = mapPose;

    Eigen::Matrix<float, 7, 1> likelihoods;

    likelihoods[0] = getLikelihoodForState(Eigen::Vector3f(x + deltaTransX, y, ang), dataPoints);

    likelihoods[1] = getLikelihoodForState(Eigen::Vector3f(x - deltaTransX, y, ang), dataPoints);

    likelihoods[2] = getLikelihoodForState(Eigen::Vector3f(x, y + deltaTransY, ang), dataPoints);

    likelihoods[3] = getLikelihoodForState(Eigen::Vector3f(x, y - deltaTransY, ang), dataPoints);

    likelihoods[4] = getLikelihoodForState(Eigen::Vector3f(x, y, ang + deltaAng), dataPoints);

    likelihoods[5] = getLikelihoodForState(Eigen::Vector3f(x, y, ang - deltaAng), dataPoints);

    likelihoods[6] = getLikelihoodForState(Eigen::Vector3f(x, y, ang), dataPoints);

    float invLhNormalizer = 1 / likelihoods.sum();

    std::cout << "\n lhs:\n" << likelihoods;

    Eigen::Vector3f mean(Eigen::Vector3f::Zero());

    for (int i = 0; i < 7; ++i) {

      mean += (sigmaPoints.block<3, 1>(0, i) * likelihoods[i]);

    }

    mean *= invLhNormalizer;

    Eigen::Matrix3f covMatrixMap(Eigen::Matrix3f::Zero());

    for (int i = 0; i < 7; ++i) {

      Eigen::Vector3f sigPointMinusMean(sigmaPoints.block<3, 1>(0, i) - mean);

      covMatrixMap += (likelihoods[i] * invLhNormalizer) * (sigPointMinusMean * (sigPointMinusMean.transpose()));

    }

    return covMatrixMap;

    //covMatrix.cwise() * invLhNormalizer;

    //transform = getTransformForState(Eigen::Vector3f(x-deltaTrans, y, ang);

  }

---

---

---

---

---