Write a CircularBuffer

Write a CircularBuffer

为什么要写一个 CircularBuffer :

  • Template-able CircularBuffer.
  • 可变和不可变容量(recapacity)的 buffer.
  • 可以 forward or move (改变有效数据范围) 的 buffer.
  • 可以 resize 有效数据 size 的 buffer.
  • 可以连续访问未使用区域的 buffer (restBegin and restEnd).

例如可以用于:

  • A copy less ROS/navigation/costmap_2d/ cost map implementation!

实现

The CircularBuffer class brief:

/**
 * Var capacity circular buffer.
 * NOTE:
 * - Like CircularQueue, but if full, then overwrite!
 * - And like CircularQueue, but NOT use front and rear, but use next index
 *   and size(front index just for faster computing).
 * - When use in multi-threads, should add lock or some other to sync.
 * @tparam T data type.
 * @sa CircularQueue
 */
template<typename T>
class CircularBuffer {
public:
    /// The iterator type.
    class Iterator;
    /// The const iterator type.
    class ConstIterator;
    /// Box type: { boxMinXIdx, boxMinYIdx, boxWidth, boxHeight }
    using Box = Eigen::Vector4i;
    /**
     * Ctor.
     * @param capacity the buffer capacity, should >= 1.
     * @throw std::logic_error when capacity invalid.
     */
    CircularBuffer(int32_t capacity = 1, const T& initValue = T{});
    /// Move ctor
    CircularBuffer(CircularBuffer<T>&& rhs) noexcept;
    /// Copy ctor
    CircularBuffer(const CircularBuffer<T>& rhs) noexcept;
    /// Dtor
    ~CircularBuffer() noexcept;
    /// Move assign
    CircularBuffer<T>& operator=(CircularBuffer<T>&& rhs) noexcept;
    /// Copy assign
    CircularBuffer<T>& operator=(const CircularBuffer<T>& rhs) noexcept;
    /// @return internal alloced buffer capacity.
    inline int32_t getAllocedCapacity() const noexcept;
    /// @return buffer capacity.
    inline int32_t capacity() const noexcept;
    /**
     * Set buffer capacity.
     * @param capacity the new capacity to set, should > 0.
     * @param initValue the initValue to set when reset capacity.
     * @return true when success, otherwise false.
     */
    bool recapacity(int32_t capacity, const T& initValue = T{}) noexcept;
    /// Fit internal buffer's capacity to this->capacity().
    void shrink_to_fit() noexcept;
    /**
     * Forward the buffer to frontIdx(real is nextIdx) by step.
     * @param step the steps to forward, if < 0, then backward.
     * @return forwarded buffer.
     */
    inline CircularBuffer<T>& forward(int32_t step) noexcept;
    /**
     * Move box to right and down.
     *
     *  0 1 2 3
     * 0. - - - > right (x, width)
     * 1|
     * 2|
     *  V
     * down (y, height)
     *
     * @param box the box to move.
     * @param width the full(capacity) buffer width (i.e. sizeX).
     * @param height the full(capacity) buffer height (i.e. sizeY).
     * @param xStep x increasing direction steps,
     * if < 0 then to descending direction.
     * @param yStep y increasing direction steps,
     * if < 0 then to descending direction.
     * @param initValue NOTE: if some regions will out of bounds then fill
     * these regions with initValue first, then these regions with initValue
     * will be moved circularly!
     * @return the moved buffer.
     * @sa
     * - fillBox
     * - fillBoxOutside
     */
    CircularBuffer<T>& moveBox(
        const Box& box,
        int32_t width,
        int32_t height,
        int32_t xStep,
        int32_t yStep,
        const T& initValue = T{}) noexcept;
    /**
     * Resize in-use datas count.
     * @note NO re-init.
     * @param newCount the new count to resize, should >= 0.
     * @return resized buffer.
     * @throw std::logic_error when @a newCount invalid.
     */
    CircularBuffer<T>& resize(int32_t newCount);
    /**
     * Resize in-use datas count.
     * @param newCount the new count to resize, should >= 0.
     * @param initValue set new or unused buffer to initValue.
     * @return resized buffer.
     * @throw std::logic_error when @a newCount invalid.
     */
    CircularBuffer<T>& resize(int32_t newCount, const T& initValue);
    /**
     * Push a value.
     * @param value to push.
     */
    void push_back(T&& value) noexcept;
    void push_back(const T& value) noexcept;
    bool pop_back(const bool reset = false) noexcept;
    bool pop_back(T& removed, const bool reset = false) noexcept;
    bool pop_front(const bool reset = false) noexcept;
    bool pop_front(T& removed, const bool reset = false) noexcept;
    /**
     * Get first data when buffer not empty.
     * @return first data when buffer not empty.
     * @throw std::logic_error when buffer empty.
     */
    const T& front() const;
    T& front();
    /**
     * Get last data when buffer not empty.
     * @return last data when buffer not empty.
     * @throw std::logic_error when buffer empty.
     */
    const T& back() const;
    T& back();
    /// @return internal buffer.data().
    inline T* data() noexcept;
    /// @return internal buffer.data().
    inline const T* data() const noexcept;
    /// @return whether buffer empty.
    inline bool empty() const noexcept;
    /// @return whether buffer full.
    inline bool full() const noexcept;
    /// @return data size.
    inline int32_t size() const noexcept;
    /// Clear in-use datas count to 0.
    inline CircularBuffer<T>& clear() noexcept;
    /// Clear buffer and reset to initValue.
    inline CircularBuffer<T>& clear(const T& initValue) noexcept;
    /// Set count to capacity, NOTE: NOT set rest buffer region to a value.
    inline CircularBuffer<T>& beFull() noexcept;
    /// Set count to capacity, and set rest buffer region to a value.
    inline CircularBuffer<T>& beFull(const T& value) noexcept;
    /**
     * Fill all (in-use and rest buffer regions) to gave value, but NOT
     * change buffer's size.
     */
    inline CircularBuffer<T>& fillNoResize(const T& value) noexcept;
    /**
     * Fill to gave starting from frontIdx + offset and count is @a count,
     * but NOT change buffer size.
     * @param offset the offset from frontIdx.
     * @param count the data count to fill.
     * @param value the value to fill.
     */
    inline CircularBuffer<T>& fillNoResize(
        const int64_t offset, const int32_t count, const T& value) noexcept;
    /**
     * Set count to capacity and set in-use and rest buffer regions to
     * gave value.
     */
    inline CircularBuffer<T>& fill(const T& value) noexcept;
    /// Fill in-use buffer region to gave value.
    inline CircularBuffer<T>& fillInUse(const T& value) noexcept;
    /// Fill rest buffer region to gave value.
    CircularBuffer<T>& fillRest(const T& value) noexcept;
    /**
     * Fill out of a inner box (sub-window) with gave value.
     * @param box the box to move.
     * @param width the full(capacity) buffer width (i.e. sizeX).
     * @param height the full(capacity) buffer height (i.e. sizeY).
     * @param value the value to fill.
     * @return filled buffer.
     * @note
     * - Size of buffer should be width * height!
     * - Parameters should valid and range!
     */
    CircularBuffer<T>& fillBoxOutside(
        const Box& box,
        int32_t width,
        int32_t height,
        const T& value) noexcept;
    CircularBuffer<T>& fillBox(
        const Box& box, int32_t width, const T& value) noexcept;
    /// Get buffer begin and end to watch all valid datas.
    Iterator begin() noexcept;
    Iterator end() noexcept;
    ConstIterator begin() const noexcept;
    ConstIterator end() const noexcept;
    /// Get buffer's rest region begin and end.
    Iterator restBegin() noexcept;
    Iterator restEnd() noexcept;
    /**
     * Access buffer from front to last,
     * @param idx data index, [0, size).
     * @return the data of index when success.
     * @throw std::logic_error when buffer empty when @a idx out of range.
     */
    const T& operator[](const int64_t idx) const;
    T& operator[](const int64_t idx);
    /**
     * Get ordered vector of the valid datas.
     * @return ordered vector buffer of all valid datas.
     * @note Result's size maybe < capacity.
     */
    std::vector<T> getOrdered() const noexcept;
    /// Check whether buffer equals @a datas
    bool operator==(const std::initializer_list<T>& datas) const noexcept;
private:
    /**
     * Compute front data index, NOTE: buffer NOT empty, otherwise index
     * invalid.
     */
    inline int64_t computeFrontIdx() const noexcept;
    std::vector<T> buffer;///< The buffer.
    int32_t capa{ 1 };///< The buffer capacity, > 0.
    int64_t nextIdx{ 0 };///< Next buffer data index.
    /**
     * Cache to fast related method, NOTE: sometimes invalid, need external
     * empty check!
     */
    int64_t frontIdx{ 0 };
    int32_t count{ 0 };///< Current valid data count.
};

Detailed implementations see:

LRU 实现

LRU 实现

LRU:

Least recently used, 最近最少(least)使用: 对于当前所有缓存, 后续使用时, 缓存满的时候,
最近使用的缓存被移除的概率最小(最后被移除)! .. 即缓存满的时候最先移除最近最少使用的!

(Discards the least recently used items first, i.e. once a cache used, then it least likely to be removed when caches full!!)

参考: https://en.wikipedia.org/wiki/Cache_replacement_policies#LRU


C++ 实现: 使用双链表存储缓存以及使用 hash map 查找缓存!

See also:

template<typename K, typename V>
class LruCache {
public:
    /// Internal cache type.
    using Cache = std::tuple<K, V>;
    /// Cache datas type.
    using Caches = DoublyLinkedList<Cache>;
    /// Key to cache iterator type.
    using KeyCaches = std::unordered_map<K, typename Caches::Iterator>;
    /// Default ctor.
    LruCache() noexcept;
    /// Dtor
    ~LruCache() noexcept;
    /// @return caches capacity.
    inline int32_t getCapacity() const noexcept;
    /// @return current caches size.
    inline int32_t getSize() const noexcept;
    /// Check whether this caches empty.
    inline bool isEmpty() const noexcept;
    /**
     * Set cache capacity.
     * @param capacity the capacity to set, NOTE: should >= current caches
     * and > 0.
     * @return true when set success, otherwise false.
     */
    inline bool setCapacity(const int32_t capacity) noexcept;
    /**
     * Set or update cache of key.
     * @param key the key to set.
     * @param value the value to set.
     */
    void set(K&& key, V&& value) noexcept;
    void set(const K& key, V&& value) noexcept;
    void set(K&& key, const V& value) noexcept;
    void set(const K& key, const V& value) noexcept;
    /// Check whether has cache of key.
    inline bool has(const K& key) const noexcept;
    /**
     * Get cache of key.
     * @param key the key to get cache.
     * @return the cache of key.
     * @return true when success otherwise false.
     */
    bool get(const K& key, V& value) const noexcept;
    /**
     * Get cache of key, if not exists, value is default.
     * @param key the key to get cache.
     * @return the cache of key.
     * @sa
     * - has
     * - get
     * - set
     */
    V& operator[](const K& key) noexcept;
    /// @throw std::logic_error when no such key.
    const V& operator[](const K& key) const;
    /// @return caches.
    inline const Caches& getCaches() const noexcept;
    /// Clear all caches.
    inline void clear() noexcept;
private:
    mutable Caches caches;///< The cache datas.
    KeyCaches keyCaches;///< { key, cacheIterator }.
    int32_t capacity{ 16 };///< The capacity of cache(max caches), default 16.
};
template<typename K, typename V>
LruCache<K, V>::LruCache() noexcept {}
template<typename K, typename V>
LruCache<K, V>::~LruCache() noexcept {}
template<typename K, typename V>
inline int32_t LruCache<K, V>::getCapacity() const noexcept
{
    return this->capacity;
}
template<typename K, typename V>
inline int32_t LruCache<K, V>::getSize() const noexcept
{
    return this->caches.size();
}
template<typename K, typename V>
inline bool LruCache<K, V>::isEmpty() const noexcept
{
    return this->caches.empty();
}
template<typename K, typename V>
bool LruCache<K, V>::setCapacity(const int32_t capacity) noexcept
{
    if (capacity < 1) {
        return false;
    }
    if (uint32_t(capacity) < this->caches.size()) {
        return false;
    }
    this->capacity = capacity;
    return true;
}
template<typename K, typename V>
void LruCache<K, V>::set(K&& key, V&& value) noexcept
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt != this->keyCaches.end()) {
        auto& cacheIt = kCIt->second;
        std::get<1>(*cacheIt) = std::move(value);
        this->caches.rewire_to_front(cacheIt);
    } else {
        if (int32_t(this->keyCaches.size()) >= this->capacity) {
            auto const& back = this->caches.back();
            this->keyCaches.erase(std::get<0>(back));
            this->caches.pop_back();
        }
        this->caches.push_front(Cache{ key, std::move(value) });
        this->keyCaches[std::move(key)] = this->caches.begin();
    }
}
template<typename K, typename V>
void LruCache<K, V>::set(const K& key, V&& value) noexcept
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt != this->keyCaches.end()) {
        auto& cacheIt = kCIt->second;
        std::get<1>(*cacheIt) = std::move(value);
        this->caches.rewire_to_front(cacheIt);
    } else {
        if (int32_t(this->keyCaches.size()) >= this->capacity) {
            auto const& back = this->caches.back();
            this->keyCaches.erase(std::get<0>(back));
            this->caches.pop_back();
        }
        this->caches.push_front(Cache{ key, std::move(value) });
        this->keyCaches[key] = this->caches.begin();
    }
}
template<typename K, typename V>
void LruCache<K, V>::set(K&& key, const V& value) noexcept
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt != this->keyCaches.end()) {
        auto& cacheIt = kCIt->second;
        std::get<1>(*cacheIt) = value;
        this->caches.rewire_to_front(cacheIt);
    } else {
        if (int32_t(this->keyCaches.size()) >= this->capacity) {
            auto const& back = this->caches.back();
            this->keyCaches.erase(std::get<0>(back));
            this->caches.pop_back();
        }
        this->caches.push_front(Cache{ key, value });
        this->keyCaches[std::move(key)] = this->caches.begin();
    }
}
template<typename K, typename V>
void LruCache<K, V>::set(const K& key, const V& value) noexcept
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt != this->keyCaches.end()) {
        auto& cacheIt = kCIt->second;
        std::get<1>(*cacheIt) = value;
        this->caches.rewire_to_front(cacheIt);
    } else {
        if (int32_t(this->keyCaches.size()) >= this->capacity) {
            auto const& back = this->caches.back();
            this->keyCaches.erase(std::get<0>(back));
            this->caches.pop_back();
        }
        this->caches.push_front(Cache{ key, value });
        this->keyCaches[key] = this->caches.begin();
    }
}
template<typename K, typename V>
inline bool LruCache<K, V>::has(const K& key) const noexcept
{
    return this->keyCaches.find(key) != keyCaches.end();
}
template<typename K, typename V>
bool LruCache<K, V>::get(const K& key, V& value) const noexcept
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt == this->keyCaches.end()) {
        return false;
    }
    const auto& cacheIt = kCIt->second;
    this->caches.rewire_to_front(cacheIt);
    value = std::get<1>(*cacheIt);
    return true;
}
template<typename K, typename V>
V& LruCache<K, V>::operator[](const K& key) noexcept
{
    auto kCIt = this->keyCaches.find(key);
    if (kCIt == this->keyCaches.end()) {
        this->set(key, V{});
        kCIt = this->keyCaches.find(key);
    }
    auto& cacheIt = kCIt->second;
    this->caches.rewire_to_front(cacheIt);
    return std::get<1>(*cacheIt);
}
template<typename K, typename V>
const V& LruCache<K, V>::operator[](const K& key) const
{
    const auto kCIt = this->keyCaches.find(key);
    if (kCIt == this->keyCaches.end()) {
        throw std::logic_error("no such key");
    }
    const auto& cacheIt = kCIt->second;
    this->caches.rewire_to_front(cacheIt);
    return std::get<1>(*cacheIt);
}
template<typename K, typename V>
inline const typename LruCache<K, V>::Caches&
LruCache<K, V>::getCaches() const noexcept
{
    return this->caches;
}
template<typename K, typename V>
inline void LruCache<K, V>::clear() noexcept
{
    this->caches.clear();
    this->keyCaches.clear();
}
template<typename K, typename V>
std::ostream& operator<<(
    std::ostream& os, const LruCache<K, V>& lruCache) noexcept
{
    const auto& caches = lruCache.getCaches();
    os << "capacity " << lruCache.getCapacity();
    const int32_t size = lruCache.getSize();
    os << ", size " << size;
    if (size > 0) {
        os << ": ";
    }
    for (const auto& cache : caches) {
        os << '(' << std::get<0>(cache) << ',' << std::get<1>(cache) << ") ";
    }
    return os;
}

Draw line primitives

Draw line primitives

在栅格图中找到两点间的栅格(栅格图坐标), 这些点是两个点构成的线段经过的栅格, 使用算法:
Bresenham's_line_algorithm.

See also:


Implementation:

NOTE: Algorithm and implementation for integer arithmetic.

/**
 * Draw the segment with a Brush instance.
 * Implements in Bresenham's line algorithm: is a line drawing algorithm
 * that determines the points of an n-dimensional raster that should be
 * selected in order to form a close approximation to a straight line
 * between two points.
 * It is commonly used to draw line primitives in a bitmap image
 * (e.g. on a computer screen), as it uses only integer addition,
 * subtraction and bit shifting, all of which are very cheap operations
 * in standard computer architectures.
 * @tparam Brush the brush to draw, operator()(x, y) requried.
 * @param brush the brush instance.
 * @note
 * - Scalar should be integer!
 * - n should >= 2, this method for 2D, only use x and y.
 * - operator()(x, y) for Brush requried.
 * @return drawed count.
 * @sa
 * - https://en.wikipedia.org/wiki/Bresenham's_line_algorithm
 * - https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm#All_cases
 * - costmap_2d/include/costmap_2d/costmap_2d.h
 */
template<typename Brush>
uint32_t draw2D(const Brush& brush) const noexcept
{
    // Tparams check
    static_assert(2 == n || 3 == n, "n should be 2 or 3");
    static_assert(
        std::is_integral<typename std::decay<Scalar>::type>::value,
        "Scalar should be integer");
    // Draw
    uint32_t ndrawed = 0;
    // Cover gradients between 0 and -1 by checking whether y needs to
    // increase or decrease (i.e. dy < 0).
    const auto draw2DLow = [&brush, &ndrawed](const P& a, const P& b) {
        Scalar dx = b(0) - a(0);
        Scalar dy = b(1) - a(1);
        Scalar yi;
        if (dy < 0) {
            yi = -1;
            dy = -dy;
        } else {
            yi = 1;
        }
        const Scalar ddx = 2 * dx;
        const Scalar ddy = 2 * dy;
        Scalar D = ddy  - dx;
        Scalar y = a(1);
        for (const Scalar& x : Range<Scalar>{ a(0), b(0) }) {
            brush(x, y);
            ++ndrawed;
            if (D > 0) {
                y += yi;
                D -= ddx;
            }
            D += ddy;
        }
    };
    // By switching the x and y axis an implementation for positive or negative
    // steep gradients can be written as:
    const auto draw2DHigh = [&brush, &ndrawed](const P& a, const P& b) {
        Scalar dx = b(0) - a(0);
        Scalar dy = b(1) - a(1);
        Scalar xi;
        if (dx < 0) {
            xi = -1;
            dx = -dx;
        } else {
            xi = 1;
        }
        const Scalar ddx = 2 * dx;
        const Scalar ddy = 2 * dy;
        Scalar D = ddx  - dy;
        Scalar x = a(0);
        for (const Scalar& y : Range<Scalar>{ a(1), b(1) }) {
            brush(x, y);
            ++ndrawed;
            if (D > 0) {
                x += xi;
                D -= ddy;
            }
            D += ddx;
        }
    };
    // Need to detect whether x1 > x0 or y1 > y0 and reverse the input
    // coordinates before drawing
    if (std::abs(this->b(1) - this->a(1)) < std::abs(this->b(0) - this->a(0))) {
        if (this->a(0) > this->b(0)) {
            draw2DLow(this->b, this->a);
        } else {
            draw2DLow(this->a, this->b);
        }
    } else {
        if (this->a(1) > this->b(1)) {
            draw2DHigh(this->b, this->a);
        } else {
            draw2DHigh(this->a, this->b);
        }
    }
    return ndrawed;
}

Test and example:

TEST(Segment, draw2D)
{
    Segment<int32_t, 2> segment{ Eigen::Vector2i{ 3, 1 }, Eigen::Vector2i{ -6, -4 } };
    const std::vector<Eigen::Vector2i> expect{
        Eigen::Vector2i{ -6, -4 },
        Eigen::Vector2i{ -5, -3 },
        Eigen::Vector2i{ -4, -3 },
        Eigen::Vector2i{ -3, -2 },
        Eigen::Vector2i{ -2, -2 },
        Eigen::Vector2i{ -1, -1 },
        Eigen::Vector2i{ 0,  -1 },
        Eigen::Vector2i{ 1,  0 },
        Eigen::Vector2i{ 2,  0 },
        Eigen::Vector2i{ 3,  1 } };
    std::vector<Eigen::Vector2i> ps;
    const auto brush = [&ps](const int32_t x, const int32_t y) {
        std::cout << x << ' ' << y << '\n';
        ps.emplace_back(Eigen::Vector2i{ x, y });
    };
    ps.clear();
    EXPECT_EQ(expect.size(), segment.draw2D(brush));
    EXPECT_EQ(expect.size(), ps.size());
    if (expect.size() == ps.size()) {
        for (uint32_t i = 0; i < ps.size(); ++i) {
            EXPECT_EQ(expect[i], ps[i]);
        }
    }
    //
    Segment<int32_t, 3> s3;
    ps.clear();
    s3.draw2D(brush);
    EXPECT_EQ(1u, ps.size());
    if (1u == ps.size()) {
        EXPECT_EQ((Eigen::Vector2i{ 0, 0 }), ps[0]);
    }
    // BAD test:
    //Segment<double, 3> badS3;
    //badS3.draw2D(brush);
}

draw2D-example.png