Program Listing for File CLandmarksMap.h

↰ Return to documentation for file (include/mrpt/maps/CLandmarksMap.h)

/*                    _
                     | |    Mobile Robot Programming Toolkit (MRPT)
 _ __ ___  _ __ _ __ | |_
| '_ ` _ \| '__| '_ \| __|          https://www.mrpt.org/
| | | | | | |  | |_) | |_
|_| |_| |_|_|  | .__/ \__|     https://github.com/MRPT/mrpt/
               | |
               |_|

 Copyright (c) 2005-2026, Individual contributors, see AUTHORS file
 See: https://www.mrpt.org/Authors - All rights reserved.
 SPDX-License-Identifier: BSD-3-Clause
*/
#pragma once

#include <mrpt/maps/CLandmark.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/viz/CSetOfObjects.h>

#include <deque>
#include <vector>

namespace mrpt::maps
{
class CLandmarksMap : public mrpt::maps::CMetricMap
{
  DEFINE_SERIALIZABLE(CLandmarksMap, mrpt::maps)

 public:
  using landmark_type = CLandmark;

  // Constructors are defined in the .cpp file: inlining them in user code
  // triggers wrong devirtualization in some GCC versions with LTO.
  CLandmarksMap();

  struct TLandmarksList
  {
    std::deque<CLandmark> m_landmarks;

    void push_back(const CLandmark& lm) { m_landmarks.push_back(lm); }

    [[nodiscard]] CLandmark* get(size_t idx)
    {
      ASSERT_(idx < m_landmarks.size());
      return &m_landmarks[idx];
    }
    [[nodiscard]] const CLandmark* get(size_t idx) const
    {
      ASSERT_(idx < m_landmarks.size());
      return &m_landmarks[idx];
    }

    [[nodiscard]] const CLandmark* getByBeaconID(int64_t beaconID) const
    {
      for (auto& lm : m_landmarks)
        if (lm.ID == beaconID) return &lm;
      return nullptr;
    }

    [[nodiscard]] size_t size() const { return m_landmarks.size(); }
    void clear() { m_landmarks.clear(); }

    // Range-for support:
    auto begin() { return m_landmarks.begin(); }
    auto end() { return m_landmarks.end(); }
    auto begin() const { return m_landmarks.begin(); }
    auto end() const { return m_landmarks.end(); }
  };

  TLandmarksList landmarks;

  void simulateRangeBearingReadings(
      const mrpt::poses::CPose3D& robotPose,
      const mrpt::poses::CPose3D& sensorOnRobot,
      mrpt::obs::CObservationBearingRange& obs,
      bool sensorDistinguishesLandmarks = true,
      double sigmaRange = 0,
      double sigmaYaw = 0,
      double sigmaPitch = 0,
      std::vector<size_t>* out_indices = nullptr,
      double spuriousMean = 0,
      double spuriousStd = 0) const;

  // ---- CMetricMap interface (minimal) ----
  [[nodiscard]] size_t size() const { return landmarks.size(); }

  [[nodiscard]] bool isEmpty() const override { return landmarks.size() == 0; }

  [[nodiscard]] std::string asString() const override { return "CLandmarksMap"; }

  void getVisualizationInto(mrpt::viz::CSetOfObjects&) const override {}

  void saveMetricMapRepresentationToFile(const std::string&) const override {}

 protected:
  void internal_clear() override { landmarks.clear(); }

  bool internal_insertObservation(
      const mrpt::obs::CObservation&, const std::optional<const mrpt::poses::CPose3D>&) override
  {
    return false;
  }

  double internal_computeObservationLikelihood(
      const mrpt::obs::CObservation&, const mrpt::poses::CPose3D&) const override
  {
    return 0;
  }

  bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation&) const override
  {
    return false;
  }
};

}  // namespace mrpt::maps