Program Listing for File RTABMapVIO.hpp

↰ Return to documentation for file (include/depthai/rtabmap/RTABMapVIO.hpp)

#pragma once

#include "depthai/pipeline/DeviceNode.hpp"
#include "depthai/pipeline/Subnode.hpp"
#include "depthai/pipeline/ThreadedHostNode.hpp"
#include "depthai/pipeline/datatype/Buffer.hpp"
#include "depthai/pipeline/datatype/CameraControl.hpp"
#include "depthai/pipeline/datatype/IMUData.hpp"
#include "depthai/pipeline/datatype/ImgFrame.hpp"
#include "depthai/pipeline/datatype/MessageGroup.hpp"
#include "depthai/pipeline/datatype/TrackedFeatures.hpp"
#include "depthai/pipeline/datatype/TransformData.hpp"
#include "depthai/pipeline/node/Sync.hpp"

namespace dai {
namespace node {

class RTABMapVIO : public NodeCRTP<ThreadedHostNode, RTABMapVIO> {
   public:
    constexpr static const char* NAME = "RTABMapVIO";
    RTABMapVIO();
    ~RTABMapVIO() override;

    std::string rectInputName = "rect";
    std::string depthInputName = "depth";
    std::string featuresInputName = "features";

    Subnode<node::Sync> sync{*this, "sync"};
    InputMap& inputs = sync->inputs;

    Input& rect = inputs[rectInputName];
    Input& depth = inputs[depthInputName];
    Input features{*this, {featuresInputName, DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::TrackedFeatures, true}}}}};
    Input imu{*this, {"imu", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::IMUData, true}}}}};
    Output transform{*this, {"transform", DEFAULT_GROUP, {{{DatatypeEnum::TransformData, true}}}}};
    Output passthroughRect{*this, {"passthroughRect", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, true}}}}};
    Output passthroughDepth{*this, {"passthroughDepth", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, true}}}}};
    Output passthroughFeatures{*this, {"passthroughFeatures", DEFAULT_GROUP, {{{DatatypeEnum::TrackedFeatures, true}}}}};

    void setParams(const std::map<std::string, std::string>& params);
    void setUseFeatures(bool use);

    void setLocalTransform(std::shared_ptr<TransformData> transform);

    void reset(std::shared_ptr<TransformData> transform = nullptr);

    void buildInternal() override;

   private:
    // pimpl
    class Impl;
    Pimpl<Impl> pimplRtabmap;
    void run() override;
    void syncCB(std::shared_ptr<dai::ADatatype> data);
    Input inSync{*this, {"inSync", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::MessageGroup, true}}}}};
    void imuCB(std::shared_ptr<ADatatype> msg);
    void initialize(Pipeline& pipeline, int instanceNum, int width, int height);
    std::mutex imuMtx;
    float alphaScaling = -1.0;
    bool initialized = false;
    bool useFeatures = true;
};
}  // namespace node
}  // namespace dai