SignedDistanceFieldTest.cpp
Go to the documentation of this file.
00001 /*
00002  * SignedDistanceFieldTest.cpp
00003  *
00004  *  Created on: Aug 25, 2017
00005  *      Author: Takahiro Miki, Peter Fankhauser
00006  *       Institute: ETH Zurich, Robotic Systems Lab
00007  */
00008 
00009 #include "grid_map_core/GridMap.hpp"
00010 #include "grid_map_sdf/SignedDistanceField.hpp"
00011 
00012 #include <gtest/gtest.h>
00013 #include <math.h>
00014 
00015 using namespace std;
00016 using namespace grid_map;
00017 
00018 TEST(SignedDistanceField, EmptyMap)
00019 {
00020   GridMap map({"layer"});
00021   map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
00022 
00023   SignedDistanceField sdf;
00024   sdf.calculateSignedDistanceField(map, "layer", 1.0);
00025   Position3 position(0.0, 0.0, 0.0);
00026 
00027   EXPECT_NO_THROW(sdf.getDistanceAt(position));
00028   EXPECT_NO_THROW(sdf.getInterpolatedDistanceAt(position));
00029   EXPECT_NO_THROW(sdf.getDistanceGradientAt(position));
00030 }
00031 
00032 TEST(SignedDistanceField, GetDistanceFlat)
00033 {
00034   GridMap map({"layer"});
00035   map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
00036   map["layer"].setConstant(1.0);
00037   map.at("layer", Index(0,0)) = -1.0;
00038 
00039   SignedDistanceField sdf;
00040   sdf.calculateSignedDistanceField(map, "layer", 2.5);
00041   Position pos;
00042   map.getPosition(Index(9, 9), pos);
00043   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00044   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.1)), -0.9, 0.0001);
00045   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.2)), -0.8, 0.0001);
00046   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.3)), -0.7, 0.0001);
00047   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.4)), -0.6, 0.0001);
00048   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
00049   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.6)), -0.4, 0.0001);
00050   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.7)), -0.3, 0.0001);
00051   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.8)), -0.2, 0.0001);
00052   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.9)), -0.1, 0.0001);
00053   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00054   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00055   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
00056   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
00057   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.4, 0.0001);
00058   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.5, 0.0001);
00059   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.6, 0.0001);
00060   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.7, 0.0001);
00061   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.8, 0.0001);
00062   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.9, 0.0001);
00063   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 1.0, 0.0001);
00064   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 1.1, 0.0001);
00065   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 1.2, 0.0001);
00066   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 1.3, 0.0001);
00067   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 1.4, 0.0001);
00068   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 1.5, 0.0001);
00069   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 2.5, 0.0001);
00070 }
00071 
00072 
00073 TEST(SignedDistanceField, GetDistance)
00074 {
00075   GridMap map({"layer"});
00076   map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00077   map["layer"].setConstant(1.0);
00078 
00079   map.at("layer", Index(3,4)) = 2.0;
00080   map.at("layer", Index(3,5)) = 2.0;
00081   map.at("layer", Index(3,6)) = 2.0;
00082   map.at("layer", Index(4,4)) = 2.0;
00083   map.at("layer", Index(4,5)) = 2.0;
00084   map.at("layer", Index(4,6)) = 2.0;
00085   map.at("layer", Index(5,4)) = 2.0;
00086   map.at("layer", Index(5,5)) = 2.0;
00087   map.at("layer", Index(5,6)) = 2.0;
00088   map.at("layer", Index(6,4)) = 2.0;
00089   map.at("layer", Index(6,5)) = 2.0;
00090   map.at("layer", Index(6,6)) = 2.0;
00091   map.at("layer", Index(7,4)) = 2.0;
00092   map.at("layer", Index(7,5)) = 2.0;
00093   map.at("layer", Index(7,6)) = 2.0;
00094 
00095   SignedDistanceField sdf;
00096   sdf.calculateSignedDistanceField(map, "layer", 1.5);
00097   Position pos;
00098 
00099   map.getPosition(Index(5, 5), pos);
00100   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00101   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -1.0, 0.0001);
00102   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.0, 0.0001);
00103   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.1, 0.0001);
00104   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.1, 0.0001);
00105   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.1, 0.0001);
00106   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.1, 0.0001);
00107   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.1, 0.0001);
00108   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.1, 0.0001);
00109   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.1, 0.0001);
00110   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
00111   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
00112   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
00113   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
00114   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
00115   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
00116   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
00117   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
00118   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.5, 0.0001);
00119 
00120   map.getPosition(Index(5, 2), pos);
00121   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), 0.0, 0.0001);
00122   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), 0.0, 0.0001);
00123   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00124   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00125   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.1, 0.0001);
00126   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.1, 0.0001);
00127   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.1, 0.0001);
00128   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.1, 0.0001);
00129   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.1, 0.0001);
00130   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.1, 0.0001);
00131   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.1, 0.0001);
00132   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.1, 0.0001);
00133   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.2, 0.0001);
00134   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.3, 0.0001);
00135   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.4, 0.0001);
00136   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.5, 0.0001);
00137   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.6, 0.0001);
00138   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.7, 0.0001);
00139   EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.7, 0.0001);
00140 }
00141 
00142 TEST(SignedDistanceField, GetDistanceGradient)
00143 {
00144   GridMap map({"layer"});
00145   map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00146   map["layer"].setConstant(1.0);
00147 
00148   map.at("layer", Index(3,4)) = 2.0;
00149   map.at("layer", Index(3,5)) = 2.0;
00150   map.at("layer", Index(3,6)) = 2.0;
00151   map.at("layer", Index(4,4)) = 2.0;
00152   map.at("layer", Index(4,5)) = 2.0;
00153   map.at("layer", Index(4,6)) = 2.0;
00154   map.at("layer", Index(5,4)) = 2.0;
00155   map.at("layer", Index(5,5)) = 2.0;
00156   map.at("layer", Index(5,6)) = 2.0;
00157   map.at("layer", Index(6,4)) = 2.0;
00158   map.at("layer", Index(6,5)) = 2.0;
00159   map.at("layer", Index(6,6)) = 2.0;
00160   map.at("layer", Index(7,4)) = 2.0;
00161   map.at("layer", Index(7,5)) = 2.0;
00162   map.at("layer", Index(7,6)) = 2.0;
00163 
00164   SignedDistanceField sdf;
00165   sdf.calculateSignedDistanceField(map, "layer", 1.5);
00166   Position pos;
00167   Vector3 gradient;
00168 
00169   map.getPosition(Index(5, 6), pos);
00170   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 0.0));
00171   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00172   EXPECT_NEAR(gradient.y(), -1, 0.0001);
00173   EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00174   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00175   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00176   EXPECT_NEAR(gradient.y(), -1, 0.0001);
00177   EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00178   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
00179   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00180   EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00181   EXPECT_NEAR(gradient.z(), 1, 0.0001);
00182   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
00183   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00184   EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00185   EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
00186   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 10.0));
00187   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00188   EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00189   EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
00190   map.getPosition(Index(2, 2), pos);
00191   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00192   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00193   EXPECT_NEAR(gradient.y(), 1, 0.0001);
00194   EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00195   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
00196   EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
00197   EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
00198   EXPECT_NEAR(gradient.z(), 1, 0.0001);
00199   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
00200   EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
00201   EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
00202   EXPECT_NEAR(gradient.z(), 1, 0.0001);
00203   map.getPosition(Index(12, 22), pos);
00204   gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00205   EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00206   EXPECT_NEAR(gradient.y(), 1.0, 0.0001);
00207   EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00208 }
00209 
00210 TEST(SignedDistanceField, GetInterpolatedDistance)
00211 {
00212   GridMap map({"layer"});
00213   map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00214   map["layer"].setConstant(1.0);
00215 
00216   map.at("layer", Index(3,3)) = 2.0;
00217   map.at("layer", Index(3,4)) = 2.0;
00218   map.at("layer", Index(3,5)) = 2.0;
00219   map.at("layer", Index(3,6)) = 2.0;
00220   map.at("layer", Index(3,7)) = 2.0;
00221   map.at("layer", Index(4,3)) = 2.0;
00222   map.at("layer", Index(4,4)) = 2.0;
00223   map.at("layer", Index(4,5)) = 2.0;
00224   map.at("layer", Index(4,6)) = 2.0;
00225   map.at("layer", Index(4,7)) = 2.0;
00226   map.at("layer", Index(5,3)) = 2.0;
00227   map.at("layer", Index(5,4)) = 2.0;
00228   map.at("layer", Index(5,5)) = 2.0;
00229   map.at("layer", Index(5,6)) = 2.0;
00230   map.at("layer", Index(5,7)) = 2.0;
00231   map.at("layer", Index(6,3)) = 2.0;
00232   map.at("layer", Index(6,4)) = 2.0;
00233   map.at("layer", Index(6,5)) = 2.0;
00234   map.at("layer", Index(6,6)) = 2.0;
00235   map.at("layer", Index(6,7)) = 2.0;
00236   map.at("layer", Index(7,3)) = 2.0;
00237   map.at("layer", Index(7,4)) = 2.0;
00238   map.at("layer", Index(7,5)) = 2.0;
00239   map.at("layer", Index(7,6)) = 2.0;
00240   map.at("layer", Index(7,7)) = 2.0;
00241 
00242   SignedDistanceField sdf;
00243   sdf.calculateSignedDistanceField(map, "layer", 1.5);
00244   Position pos;
00245 
00246   map.getPosition(Index(5, 5), pos);
00247   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -5.05, 0.0001);
00248   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -3.05, 0.0001);
00249   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.05, 0.0001);
00250   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.25, 0.0001);
00251   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.25, 0.0001);
00252   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.25, 0.0001);
00253   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.25, 0.0001);
00254   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.25, 0.0001);
00255   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.25, 0.0001);
00256   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.25, 0.0001);
00257   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
00258   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), -0.1, 0.0001);
00259   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
00260   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
00261   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
00262   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
00263   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
00264   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
00265   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8, 0.0001);
00266 
00267   map.getPosition(Index(5, 10), pos);
00268   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00269   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
00270   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00271   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00272   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
00273   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
00274   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.35, 0.0001);
00275   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.35, 0.0001);
00276   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.35, 0.0001);
00277   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.45, 0.0001);
00278   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.55, 0.0001);
00279   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.65, 0.0001);
00280   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.75, 0.0001);
00281   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.85, 0.0001);
00282   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.95, 0.0001);
00283   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8.45, 0.0001);
00284 
00285   map.getPosition(Index(5, 0), pos);
00286   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
00287   map.getPosition(Index(5, 1), pos);
00288   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.175, 0.0001);
00289   map.getPosition(Index(5, 2), pos);
00290   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.025, 0.0001);
00291   map.getPosition(Index(5, 3), pos);
00292   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.2, 0.0001);
00293   map.getPosition(Index(5, 4), pos);
00294   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.225, 0.0001);
00295   map.getPosition(Index(5, 5), pos);
00296   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
00297   map.getPosition(Index(5, 6), pos);
00298   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.175, 0.0001);
00299   map.getPosition(Index(5, 7), pos);
00300   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.025, 0.0001);
00301   map.getPosition(Index(5, 8), pos);
00302   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.15, 0.0001);
00303   map.getPosition(Index(5, 9), pos);
00304   EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
00305 }

grid_map_sdf
Author(s): Takahiro Miki , Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:34