PointCloud.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_POINTCLOUD_H_
#define _MIRA_POINTCLOUD_H_

#include <utils/Buffer.h>
#include <math/Eigen.h>

#include <maps/PointCloudIterator.h>

namespace mira { namespace maps {


class GenericPointCloud
{
public:

    GenericPointCloud() {}

    GenericPointCloud(const PCFormat& format) :
        mFormat(format) {}

    GenericPointCloud(Buffer<uint8> iData, const PCFormat& format) :
        data(std::move(iData)), mFormat(format) {}

public:

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_VERSION(r, 1);
        r.member("Format", mFormat, "The format of the binary point cloud data below");
        r.member("Data", data, "The binary point cloud data in the specified format");
    }

public:

    typedef GenericPointCloudConstIterator const_iterator;
    typedef GenericPointCloudIterator iterator;

    const_iterator begin() const {
        return const_iterator(data.data(), pointStep());
    }

    iterator begin() {
        return iterator(data.data(), pointStep());
    }

    const_iterator end() const {
        return const_iterator(data.data()+data.size(), pointStep());
    }

    iterator end() {
        return iterator(data.data()+data.size(), pointStep());
    }

public:

    template <typename T>
    const T& at(std::size_t idx, const PCDataField& field) const {
        assert(PCDataFieldTypeTrait<T>::type == field.type);
        const uint8* ptr = data.data() + idx*pointStep();
        return *reinterpret_cast<const T*>(ptr + field.offset);
    }

public:

    std::size_t size() const { return data.size() / pointStep(); }

public:

    const PCFormat& getFormat() const { return mFormat; }

    void setFormat(const PCFormat& format) { mFormat = format; }

public:

    std::size_t pointStep() const { return mFormat.size; }

public:

    template<typename TPointCloud>
    TPointCloud& castTo() { return (TPointCloud&)*this; }

    template<typename TPointCloud>
    const TPointCloud& castTo() const { return (const TPointCloud&)*this; }

    template<typename TPointCloud>
    TPointCloud& convertTo() {
        mFormat = pointCloudFormat<typename TPointCloud::value_type>();
        return (TPointCloud&)*this;
    }

public:

    template <typename PointType>
    static GenericPointCloud fromStdVector(std::vector<PointType>& data) {
        Buffer<uint8> buffer((uint8*)data.data(), data.size()*sizeof(PointType));
        return GenericPointCloud(std::move(buffer), pointCloudFormat<PointType>());
    }

    template <typename PointType>
    static const GenericPointCloud fromStdVector(const std::vector<PointType>& data) {
        Buffer<uint8> buffer((uint8*)data.data(), data.size()*sizeof(PointType));
        return GenericPointCloud(std::move(buffer), pointCloudFormat<PointType>());
    }

public:
    Buffer<uint8> data;

private:
    PCFormat mFormat;
};


template<typename PointType>
class PointCloud : public GenericPointCloud
{

public:

    PointCloud() :
        GenericPointCloud(pointCloudFormat<PointType>()) {
        assert(sizeof(PointType)==pointStep());
    }

    PointCloud(Buffer<uint8> iData) :
        GenericPointCloud(iData, pointCloudFormat<PointType>()) {
        assert(sizeof(PointType)==pointStep());
    }

public:
    // stl conform typedefs
    typedef PointType value_type;
    typedef PointCloudConstIterator<PointType> const_iterator;
    typedef PointCloudIterator<PointType> iterator;

    const_iterator begin() const {
        return const_iterator(data.data(), pointStep());
    }

    iterator begin() {
        return iterator(data.data(), pointStep());
    }

    const_iterator end() const {
        return const_iterator(data.data()+data.size(), pointStep());
    }

    iterator end() {
        return iterator(data.data()+data.size(), pointStep());
    }

    const PointType& at(std::size_t idx) const {
        const uint8* ptr = data.data() + idx*pointStep();
        return *reinterpret_cast<const PointType*>(ptr);
    }

    PointType& at(std::size_t idx){
        uint8* ptr = data.data() + idx*pointStep();
        return *reinterpret_cast<PointType*>(ptr);
    }

    PointType& operator[](std::size_t idx)
    {
        return at(idx);
    }

    const PointType& operator[](std::size_t idx) const
    {
        return at(idx);
    }


public:

    PointType* reservePoints(std::size_t count)
    {
        std::size_t oldSize = data.size();
        data.resize(data.size()+pointStep()*count);
        return reinterpret_cast<PointType*>(data.data()+oldSize);
    }

    void push_back(const PointType& point)
    {
        PointType* newPoint = reservePoints(1);
        memcpy(newPoint, &point, sizeof(PointType));
    }

    void resize(std::size_t count) {
        data.resize(count*pointStep());
    }

public:

    void clear() {
        data.resize(0);
    }

};


}} // namespace

#endif /* _MIRA_POINTCLOUD_H_ */