cartographer: ray_to_pixel_mask

Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
00018 
00019 #include "cartographer/mapping/2d/map_limits.h"
00020 #include "gmock/gmock.h"
00021 #include "gtest/gtest.h"
00022 
00023 namespace cartographer {
00024 namespace mapping {
00025 namespace {
00026 
00027 using ::testing::ElementsAre;
00028 
00029 MATCHER_P(PixelMaskEqual, value, "") {
00030   Eigen::Array2i residual = value - arg;
00031   return residual.matrix().lpNorm<1>() == 0;
00032 }
00033 
00034 TEST(RayToPixelMaskTest, SingleCell) {
00035   const Eigen::Array2i& begin = {1, 1};
00036   const Eigen::Array2i& end = {1, 1};
00037   const int subpixel_scale = 1;
00038   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00039   std::vector<Eigen::Array2i> ray_reference =
00040       std::vector<Eigen::Array2i>{begin};
00041   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin)));
00042 }
00043 
00044 TEST(RayToPixelMaskTest, AxisAlignedX) {
00045   const Eigen::Array2i& begin = {1, 1};
00046   const Eigen::Array2i& end = {3, 1};
00047   const int subpixel_scale = 1;
00048   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00049   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00050                                PixelMaskEqual(Eigen::Array2i({2, 1})),
00051                                PixelMaskEqual(Eigen::Array2i({3, 1}))));
00052   ray = RayToPixelMask(end, begin, subpixel_scale);
00053   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00054                                PixelMaskEqual(Eigen::Array2i({2, 1})),
00055                                PixelMaskEqual(Eigen::Array2i({3, 1}))));
00056 }
00057 
00058 TEST(RayToPixelMaskTest, AxisAlignedY) {
00059   const Eigen::Array2i& begin = {1, 1};
00060   const Eigen::Array2i& end = {1, 3};
00061   const int subpixel_scale = 1;
00062   std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
00063       Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})};
00064   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00065   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00066                                PixelMaskEqual(Eigen::Array2i({1, 2})),
00067                                PixelMaskEqual(Eigen::Array2i({1, 3}))));
00068   ray = RayToPixelMask(end, begin, subpixel_scale);
00069   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00070                                PixelMaskEqual(Eigen::Array2i({1, 2})),
00071                                PixelMaskEqual(Eigen::Array2i({1, 3}))));
00072 }
00073 
00074 TEST(RayToPixelMaskTest, Diagonal) {
00075   Eigen::Array2i begin = {1, 1};
00076   Eigen::Array2i end = {3, 3};
00077   const int subpixel_scale = 1;
00078   std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
00079       Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})};
00080   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00081   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00082                                PixelMaskEqual(Eigen::Array2i({2, 2})),
00083                                PixelMaskEqual(Eigen::Array2i({3, 3}))));
00084   ray = RayToPixelMask(end, begin, subpixel_scale);
00085   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00086                                PixelMaskEqual(Eigen::Array2i({2, 2})),
00087                                PixelMaskEqual(Eigen::Array2i({3, 3}))));
00088   begin = Eigen::Array2i({1, 3});
00089   end = Eigen::Array2i({3, 1});
00090   ray = RayToPixelMask(begin, end, subpixel_scale);
00091   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
00092                                PixelMaskEqual(Eigen::Array2i({2, 2})),
00093                                PixelMaskEqual(Eigen::Array2i({3, 1}))));
00094   ray = RayToPixelMask(end, begin, subpixel_scale);
00095   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
00096                                PixelMaskEqual(Eigen::Array2i({2, 2})),
00097                                PixelMaskEqual(Eigen::Array2i({3, 1}))));
00098 }
00099 
00100 TEST(RayToPixelMaskTest, SteepLine) {
00101   Eigen::Array2i begin = {1, 1};
00102   Eigen::Array2i end = {2, 5};
00103   const int subpixel_scale = 1;
00104   std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
00105       Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}),
00106       Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})};
00107   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00108   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00109                                PixelMaskEqual(Eigen::Array2i({1, 2})),
00110                                PixelMaskEqual(Eigen::Array2i({1, 3})),
00111                                PixelMaskEqual(Eigen::Array2i({2, 3})),
00112                                PixelMaskEqual(Eigen::Array2i({2, 4})),
00113                                PixelMaskEqual(Eigen::Array2i({2, 5}))));
00114   begin = {1, 1};
00115   end = {2, 4};
00116   ray_reference = std::vector<Eigen::Array2i>{
00117       Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}),
00118       Eigen::Array2i({2, 4})};
00119   ray = RayToPixelMask(begin, end, subpixel_scale);
00120   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00121                                PixelMaskEqual(Eigen::Array2i({1, 2})),
00122                                PixelMaskEqual(Eigen::Array2i({2, 3})),
00123                                PixelMaskEqual(Eigen::Array2i({2, 4}))));
00124 }
00125 
00126 TEST(RayToPixelMaskTest, FlatLine) {
00127   Eigen::Array2i begin = {1, 1};
00128   Eigen::Array2i end = {5, 2};
00129   const int subpixel_scale = 1;
00130   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00131   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00132                                PixelMaskEqual(Eigen::Array2i({2, 1})),
00133                                PixelMaskEqual(Eigen::Array2i({3, 1})),
00134                                PixelMaskEqual(Eigen::Array2i({3, 2})),
00135                                PixelMaskEqual(Eigen::Array2i({4, 2})),
00136                                PixelMaskEqual(Eigen::Array2i({5, 2}))));
00137   begin = {1, 1};
00138   end = {4, 2};
00139   ray = RayToPixelMask(begin, end, subpixel_scale);
00140   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
00141                                PixelMaskEqual(Eigen::Array2i({2, 1})),
00142                                PixelMaskEqual(Eigen::Array2i({3, 2})),
00143                                PixelMaskEqual(Eigen::Array2i({4, 2}))));
00144 }
00145 
00146 TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) {
00147   int subpixel_scale;
00148   const int num_cells_x = 20;
00149   const int num_cells_y = 20;
00150   double resolution = 0.1;
00151   Eigen::Vector2d max = {1.0, 1.0};
00152   std::vector<Eigen::Array2i> base_scale_ray;
00153   for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) {
00154     double superscaled_resolution = resolution / subpixel_scale;
00155     MapLimits superscaled_limits(
00156         superscaled_resolution, max,
00157         CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
00158     Eigen::Array2i begin =
00159         superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05}));
00160     Eigen::Array2i end =
00161         superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05}));
00162     std::vector<Eigen::Array2i> ray =
00163         RayToPixelMask(begin, end, subpixel_scale);
00164     EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})),
00165                                  PixelMaskEqual(Eigen::Array2i({9, 7})),
00166                                  PixelMaskEqual(Eigen::Array2i({9, 8})),
00167                                  PixelMaskEqual(Eigen::Array2i({9, 9}))));
00168   }
00169 }
00170 
00171 TEST(RayToPixelMaskTest, MultiScaleSkewedLine) {
00172   int subpixel_scale = 1;
00173   const int num_cells_x = 20;
00174   const int num_cells_y = 20;
00175   double resolution = 0.1;
00176   Eigen::Vector2d max = {1.0, 1.0};
00177   double superscaled_resolution = resolution / subpixel_scale;
00178   MapLimits superscaled_limits(
00179       superscaled_resolution, max,
00180       CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
00181   Eigen::Array2i begin =
00182       superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
00183   Eigen::Array2i end =
00184       superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
00185 
00186   std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
00187   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
00188                                PixelMaskEqual(Eigen::Array2i({8, 8})),
00189                                PixelMaskEqual(Eigen::Array2i({9, 8})),
00190                                PixelMaskEqual(Eigen::Array2i({9, 9}))));
00191   subpixel_scale = 20;
00192   superscaled_resolution = resolution / subpixel_scale;
00193   superscaled_limits = MapLimits(
00194       superscaled_resolution, max,
00195       CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
00196   begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
00197   end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
00198   ray = RayToPixelMask(begin, end, subpixel_scale);
00199   EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
00200                                PixelMaskEqual(Eigen::Array2i({8, 8})),
00201                                PixelMaskEqual(Eigen::Array2i({8, 9})),
00202                                PixelMaskEqual(Eigen::Array2i({9, 9}))));
00203 }
00204 
00205 }  // namespace
00206 }  // namespace mapping
00207 }  // namespace cartographer