fzi-forschungszentrum-informatik / Lanelet2

Map handling framework for automated driving
BSD 3-Clause "New" or "Revised" License
800 stars 327 forks source link

Const Area innerBounds issue #141

Closed aharmat closed 4 years ago

aharmat commented 4 years ago

I recently ran into an issue that I believe is a bug in lanelet2. When accessing the inner bounds of an Area by indexing into innerBounds()[i], the returned result is different than if accessing innerBounds() with a range-based for loop. But this only happens when we're dealing with a const Area! Attached is a minimal example that replicates the issue, it borrows most code from lanelet2_examples:

using namespace lanelet;

inline LineString3d getLineStringAtX(double x) {
  return LineString3d(utils::getId(), {Point3d{utils::getId(), x, 0, 0}, Point3d{utils::getId(), x, 1, 0},
                                       Point3d{utils::getId(), x, 2, 0}});
}

inline LineString3d getLineStringAtY(double y) {
  return LineString3d(utils::getId(), {Point3d{utils::getId(), 0, y, 0}, Point3d{utils::getId(), 1, y, 0},
                                       Point3d{utils::getId(), 2, y, 0}});
}

inline Area getAnArea() {
  LineString3d top = getLineStringAtY(2);
  LineString3d right = getLineStringAtX(2).invert();
  LineString3d bottom = getLineStringAtY(0).invert();
  LineString3d left = getLineStringAtX(0);

  // Adds one inner bounds that is identical to the outer bounds. Silly, but works for this example
  return Area(utils::getId(), {top, right, bottom, left}, {{top, right, bottom, left}});
}

inline LaneletMap getALaneletMap() {
  auto area = getAnArea();
  return std::move(*utils::createMap({area}));
}

void FailExample() {
  LaneletMap map = getALaneletMap();

  // This passes
  for (auto& area : map.areaLayer) {
    std::cout<<"Reading area " << area.id() << std::endl;
    int idx = 0;
    for (const auto& inner_bound : area.innerBounds()) {
      std::cout<<"Inner bound has " << inner_bound.size() << " lines" << std::endl;
      const auto& inner_bound2 = area.innerBounds()[idx];
      if(inner_bound != inner_bound2) {
        std::cerr << "FAIL with non-const area" << std::endl;
      }
      idx++;
    }
  }

  // This fails
  for (const auto& area : map.areaLayer) {
    std::cout<<"Reading area " << area.id() << std::endl;
    int idx = 0;
    for (const auto& inner_bound : area.innerBounds()) {
      std::cout<<"Inner bound has " << inner_bound.size() << " lines" << std::endl;
      const auto& inner_bound2 = area.innerBounds()[idx]; 
      if(inner_bound != inner_bound2) {
        std::cerr << "FAIL with const area" << std::endl;
      }
      idx++;
    }
  }
}

Since this only happens for const Areas, I'm guessing the problem is somewhere in

ConstInnerBounds AreaData::innerBounds() const {
    return utils::transform(innerBounds_, [](const auto& elem) {
      return utils::transform(elem, [](const auto& ls) { return ConstLineString3d(ls); });
    });
  }

but I haven't been able to figure it out.

poggenhans commented 4 years ago

Your code fails because you are accessing temporary objects that have gone out of scope.

Note that the const variant of innerBounds returns not a const reference but a new object. In the line const auto& inner_bound2 = area.innerBounds()[idx]; (where area is const), you are indexing into area.innerBounds() which is a temporary object that goes out of scope at the end of the expression.

The result of your comparison afterwards inner_bound != inner_bound2 is therefore completely random because inner_bound2 no longer exists as an object. If you change your line to auto inner_bound2 = area.innerBounds()[idx];, your code should pass.

aharmat commented 4 years ago

Thank you, that makes sense.