diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index aa77ae7e37..4278a82d4b 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -182,6 +182,7 @@ jobs: build-windows-x86_64: needs: build-docstrings runs-on: windows-latest + timeout-minutes: 1440 # Set timeout to 24 hours strategy: matrix: python-version: [3.7, 3.8, 3.9, '3.10', '3.11', '3.12', '3.13'] @@ -336,6 +337,7 @@ jobs: build-linux-x86_64: needs: build-docstrings runs-on: ubuntu-latest + timeout-minutes: 1440 # Set timeout to 24 hours container: image: quay.io/pypa/manylinux_2_28_x86_64:2024.12.05-1 env: @@ -343,6 +345,7 @@ jobs: strategy: matrix: python-set: ["cp37-cp37m", "cp38-cp38", "cp39-cp39", "cp310-cp310", "cp311-cp311", "cp312-cp312", "cp313-cp313"] + fail-fast: false env: DEPTHAI_BUILD_BASALT: ON DEPTHAI_BUILD_PCL: ON @@ -419,6 +422,7 @@ jobs: strategy: matrix: python-set: ["cp37-cp37m", "cp38-cp38", "cp39-cp39", "cp310-cp310", "cp311-cp311", "cp312-cp312", "cp313-cp313"] + fail-fast: false env: # workaround required for cache@v3, https://github.com/actions/cache/issues/1428 VCPKG_FORCE_SYSTEM_BINARIES: "1" # Needed so vpckg can bootstrap itself @@ -443,7 +447,7 @@ jobs: with: submodules: 'recursive' - name: Installing libusb1-devel dependency - run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig + run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig - name: Setup ninja required for arm64 builds run: | git clone https://github.com/ninja-build/ninja.git diff --git a/.gitignore b/.gitignore index 6dca4bdffd..a30a3061b6 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,7 @@ build*/* shared/version.hpp #pybind11 +bindings/python/docstrings/ build/ dist/ _build/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 1adf6e8097..297a3657e0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -303,6 +303,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp src/pipeline/datatype/AprilTags.cpp src/pipeline/datatype/AprilTagConfig.cpp + src/pipeline/datatype/Landmarks.cpp src/pipeline/datatype/Tracklets.cpp src/pipeline/datatype/IMUData.cpp src/pipeline/datatype/StereoDepthConfig.cpp @@ -373,6 +374,7 @@ set(TARGET_OPENCV_SOURCES src/pipeline/node/host/Replay.cpp src/opencv/RecordReplay.cpp src/opencv/HolisticRecordReplay.cpp + src/opencv/Landmarks.cpp ) set(TARGET_PCL_SOURCES src/pcl/PointCloudData.cpp) diff --git a/bindings/js/bindings.cpp b/bindings/js/bindings.cpp index 7c625cbc77..6f2546946a 100644 --- a/bindings/js/bindings.cpp +++ b/bindings/js/bindings.cpp @@ -68,6 +68,7 @@ EMSCRIPTEN_BINDINGS(depthai_js) { .value("EdgeDetectorConfig", dai::DatatypeEnum::EdgeDetectorConfig) .value("AprilTagConfig", dai::DatatypeEnum::AprilTagConfig) .value("AprilTags", dai::DatatypeEnum::AprilTags) + .value("Landmarks", dai::DatatypeEnum::Landmarks) .value("Tracklets", dai::DatatypeEnum::Tracklets) .value("IMUData", dai::DatatypeEnum::IMUData) .value("StereoDepthConfig", dai::DatatypeEnum::StereoDepthConfig) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 3440b99550..7518c98e7c 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -109,6 +109,7 @@ set(SOURCE_LIST src/pipeline/datatype/ADatatypeBindings.cpp src/pipeline/datatype/AprilTagConfigBindings.cpp src/pipeline/datatype/AprilTagsBindings.cpp + src/pipeline/datatype/LandmarksBindings.cpp src/pipeline/datatype/BufferBindings.cpp src/pipeline/datatype/CameraControlBindings.cpp src/pipeline/datatype/EdgeDetectorConfigBindings.cpp diff --git a/bindings/python/src/DatatypeBindings.cpp b/bindings/python/src/DatatypeBindings.cpp index 98ce30eea8..d33b104204 100644 --- a/bindings/python/src/DatatypeBindings.cpp +++ b/bindings/python/src/DatatypeBindings.cpp @@ -31,6 +31,7 @@ void bind_benchmarkreport(pybind11::module& m, void* pCallstack); void bind_pointcloudconfig(pybind11::module& m, void* pCallstack); void bind_pointclouddata(pybind11::module& m, void* pCallstack); void bind_transformdata(pybind11::module& m, void* pCallstack); +void bind_landmarks(pybind11::module& m, void* pCallstack); void bind_rgbddata(pybind11::module& m, void* pCallstack); void bind_imagealignconfig(pybind11::module& m, void* pCallstack); void bind_imageannotations(pybind11::module& m, void* pCallstack); @@ -68,6 +69,7 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_pointcloudconfig); callstack.push_front(bind_pointclouddata); callstack.push_front(bind_transformdata); + callstack.push_front(bind_landmarks); callstack.push_front(bind_imagealignconfig); callstack.push_front(bind_imageannotations); callstack.push_front(bind_rgbddata); @@ -119,6 +121,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack) { .value("BenchmarkReport", DatatypeEnum::BenchmarkReport) .value("MessageGroup", DatatypeEnum::MessageGroup) .value("TransformData", DatatypeEnum::TransformData) + .value("Landmarks", DatatypeEnum::Landmarks) .value("PointCloudConfig", DatatypeEnum::PointCloudConfig) .value("PointCloudData", DatatypeEnum::PointCloudData) .value("ImageAlignConfig", DatatypeEnum::ImageAlignConfig) diff --git a/bindings/python/src/pipeline/datatype/LandmarksBindings.cpp b/bindings/python/src/pipeline/datatype/LandmarksBindings.cpp new file mode 100644 index 0000000000..03de1ec3a0 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/LandmarksBindings.cpp @@ -0,0 +1,54 @@ +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/Landmarks.hpp" + +//pybind +#include +#include + + + +void bind_landmarks(pybind11::module& m, void* pCallstack) { + using namespace dai; + + py::class_ landmark(m, "Landmark", DOC(dai, Landmark)); + py::class_, Buffer, std::shared_ptr> landmarks(m, "Landmarks", DOC(dai, Landmarks)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + landmark.def(py::init<>()) + .def_readwrite("id", &Landmark::id, DOC(dai, Landmark, id)) + .def_readwrite("size", &Landmark::size, DOC(dai, Landmark, size)) + .def_readwrite("translation", &Landmark::translation, DOC(dai, Landmark, translation)) + .def_readwrite("quaternion", &Landmark::quaternion, DOC(dai, Landmark, quaternion)) + .def_readwrite("covariance", &Landmark::covariance, DOC(dai, Landmark, covariance)); + + + // Message + landmarks.def(py::init<>()) + .def("__repr__", &Landmarks::str) + .def_property( + "landmarks", [](Landmarks& det) { return &det.landmarks; }, [](Landmarks& det, std::vector val) { det.landmarks = val; }) + .def("getTimestamp", &Landmarks::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) + .def("getTimestampDevice", &Landmarks::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) + .def("getSequenceNum", &Landmarks::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) + // .def("setTimestamp", &AprilTags::setTimestamp, DOC(dai, Buffer, setTimestamp)) + // .def("setTimestampDevice", &AprilTags::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice)) + // .def("setSequenceNum", &AprilTags::setSequenceNum, DOC(dai, Buffer, setSequenceNum)) + ; + + +} diff --git a/bindings/python/src/pipeline/node/RTABMapSLAMBindings.cpp b/bindings/python/src/pipeline/node/RTABMapSLAMBindings.cpp index bd58ce753c..6d98f201a2 100644 --- a/bindings/python/src/pipeline/node/RTABMapSLAMBindings.cpp +++ b/bindings/python/src/pipeline/node/RTABMapSLAMBindings.cpp @@ -32,8 +32,14 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) { "rect", [](RTABMapSLAM& node) { return &node.rect; }, py::return_value_policy::reference_internal) .def_property_readonly( "depth", [](RTABMapSLAM& node) { return &node.depth; }, py::return_value_policy::reference_internal) - .def_readonly("features", &RTABMapSLAM::features, DOC(dai, node, RTABMapSLAM, features)) + .def_property_readonly( + "features", [](RTABMapSLAM& node) { return &node.features; }, py::return_value_policy::reference_internal) + .def_property_readonly( + "landmarks", [](RTABMapSLAM& node) { return &node.landmarks; }, py::return_value_policy::reference_internal) + // .def_readonly("features", &RTABMapSLAM::features, DOC(dai, node, RTABMapSLAM, features)) + // .def_readonly("landmarks", &RTABMapSLAM::landmarks, DOC(dai, node, RTABMapSLAM, landmarks)) .def_readonly("odom", &RTABMapSLAM::odom, DOC(dai, node, RTABMapSLAM, odom)) + .def_readonly("imu", &RTABMapSLAM::imu, DOC(dai, node, RTABMapSLAM, imu)) .def_readonly("transform", &RTABMapSLAM::transform, DOC(dai, node, RTABMapSLAM, transform)) .def_readonly("odomCorrection", &RTABMapSLAM::odomCorrection, DOC(dai, node, RTABMapSLAM, odomCorrection)) .def_readonly("obstaclePCL", &RTABMapSLAM::obstaclePCL, DOC(dai, node, RTABMapSLAM, obstaclePCL)) @@ -42,6 +48,7 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) { .def_readonly("passthroughRect", &RTABMapSLAM::passthroughRect, DOC(dai, node, RTABMapSLAM, passthroughRect)) .def_readonly("passthroughDepth", &RTABMapSLAM::passthroughDepth, DOC(dai, node, RTABMapSLAM, passthroughDepth)) .def_readonly("passthroughFeatures", &RTABMapSLAM::passthroughFeatures, DOC(dai, node, RTABMapSLAM, passthroughFeatures)) + .def_readonly("passthroughLandmarks", &RTABMapSLAM::passthroughLandmarks, DOC(dai, node, RTABMapSLAM, passthroughLandmarks)) .def_readonly("passthroughOdom", &RTABMapSLAM::passthroughOdom, DOC(dai, node, RTABMapSLAM, passthroughOdom)) .def("setParams", &RTABMapSLAM::setParams, py::arg("params"), DOC(dai, node, RTABMapSLAM, setParams)) .def("setDatabasePath", &RTABMapSLAM::setDatabasePath, py::arg("path"), DOC(dai, node, RTABMapSLAM, setDatabasePath)) @@ -57,7 +64,8 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) { .def("setFreq", &RTABMapSLAM::setFreq, py::arg("f"), DOC(dai, node, RTABMapSLAM, setFreq)) .def("setAlphaScaling", &RTABMapSLAM::setAlphaScaling, py::arg("alpha"), DOC(dai, node, RTABMapSLAM, setAlphaScaling)) .def("setUseFeatures", &RTABMapSLAM::setUseFeatures, py::arg("useFeatures"), DOC(dai, node, RTABMapSLAM, setUseFeatures)) + .def("setUseLandmarks", &RTABMapSLAM::setUseLandmarks, py::arg("useLandmarks"), DOC(dai, node, RTABMapSLAM, setUseLandmarks)) .def("setLocalTransform", &RTABMapSLAM::setLocalTransform, py::arg("transform"), DOC(dai, node, RTABMapSLAM, setLocalTransform)) .def("getLocalTransform", &RTABMapSLAM::getLocalTransform, DOC(dai, node, RTABMapSLAM, getLocalTransform)) .def("triggerNewMap", &RTABMapSLAM::triggerNewMap, DOC(dai, node, RTABMapSLAM, triggerNewMap)); -} \ No newline at end of file +} diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 68da920855..eef32efc28 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -31,12 +31,13 @@ enum class DatatypeEnum : std::int32_t { BenchmarkReport, MessageGroup, TransformData, + Landmarks, PointCloudConfig, PointCloudData, RGBDData, ImageAlignConfig, ImgAnnotations, - ObjectTrackerConfig + ObjectTrackerConfig, }; bool isDatatypeSubclassOf(DatatypeEnum parent, DatatypeEnum children); diff --git a/include/depthai/pipeline/datatype/Landmarks.hpp b/include/depthai/pipeline/datatype/Landmarks.hpp new file mode 100644 index 0000000000..d5e42e727a --- /dev/null +++ b/include/depthai/pipeline/datatype/Landmarks.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +#include "depthai/common/Point3d.hpp" +#include "depthai/common/Quaterniond.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" + +// optional +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + #include + #include +#endif + + + +// #include "utility/Serialization.hpp" + +namespace dai { +struct Landmark { + /** + * The ID of the Landmark + */ + int id; + + /** + * The size of the landmark in meters + */ + double size; + + /** + * The translation of the landmark reletive to base_frame (the camera) + */ + dai::Point3d translation; + + /** + * The orientation of the landmark relative to base_frame (the camera) + */ + dai::Quaterniond quaternion; + + /** + * The covariance of the landmark. If you do not know what this means, you can probably just fill with 0.01. + */ + std::array, 6> covariance; + + #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + cv::Mat getCovarianceAsCvMat(); + #endif +}; + +DEPTHAI_SERIALIZE_EXT(Landmark, id, size, translation, quaternion, covariance); + +class Landmarks : public Buffer { + public: + Landmarks() = default; + virtual ~Landmarks() = default; + + public: + std::vector landmarks; + + DEPTHAI_SERIALIZE(Landmarks, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, landmarks); + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::Landmarks; + } +}; + +} // namespace dai diff --git a/include/depthai/rtabmap/RTABMapSLAM.hpp b/include/depthai/rtabmap/RTABMapSLAM.hpp index 50319ca025..d3f78dda4b 100644 --- a/include/depthai/rtabmap/RTABMapSLAM.hpp +++ b/include/depthai/rtabmap/RTABMapSLAM.hpp @@ -37,6 +37,7 @@ class RTABMapSLAM : public dai::NodeCRTP transform) { localTransform = transform->getRTABMapTransform(); } @@ -202,6 +224,7 @@ class RTABMapSLAM : public dai::NodeCRTP rtabParams; std::string databasePath = ""; diff --git a/src/basalt/BasaltVIO.cpp b/src/basalt/BasaltVIO.cpp index ac2b30c8e8..be1b593c32 100644 --- a/src/basalt/BasaltVIO.cpp +++ b/src/basalt/BasaltVIO.cpp @@ -260,11 +260,11 @@ void BasaltVIO::setDefaultVIOConfig() { vioConfig.vio_min_triangulation_dist = 0.05; vioConfig.vio_max_iterations = 7; vioConfig.vio_enforce_realtime = false; - vioConfig.vio_use_lm = true; + vioConfig.vio_use_lm = false; vioConfig.vio_lm_lambda_initial = 1e-4; vioConfig.vio_lm_lambda_min = 1e-6; vioConfig.vio_lm_lambda_max = 1e2; - vioConfig.vio_scale_jacobian = false; + vioConfig.vio_scale_jacobian = true; vioConfig.vio_init_pose_weight = 1e8; vioConfig.vio_init_ba_weight = 1e1; vioConfig.vio_init_bg_weight = 1e2; diff --git a/src/opencv/Landmarks.cpp b/src/opencv/Landmarks.cpp new file mode 100644 index 0000000000..8342d0f0a4 --- /dev/null +++ b/src/opencv/Landmarks.cpp @@ -0,0 +1,17 @@ +#include "depthai/pipeline/datatype/Landmarks.hpp" +#include "iostream" + +namespace dai { +cv::Mat Landmark::getCovarianceAsCvMat() { + //The matrix will always be 6x6 since that is what rtabmap expects. + cv::Mat cvMat(6, 6, CV_64F); + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + cvMat.at(i, j) = covariance.at(i).at(j); + } + } + + return cvMat; +} +} diff --git a/src/pipeline/datatype/DatatypeEnum.cpp b/src/pipeline/datatype/DatatypeEnum.cpp index f24826ea66..5280d02570 100644 --- a/src/pipeline/datatype/DatatypeEnum.cpp +++ b/src/pipeline/datatype/DatatypeEnum.cpp @@ -33,11 +33,13 @@ const std::unordered_map> hierarchy = { DatatypeEnum::TrackedFeatures, DatatypeEnum::AprilTagConfig, DatatypeEnum::AprilTags, + DatatypeEnum::BenchmarkReport, DatatypeEnum::MessageGroup, DatatypeEnum::PointCloudConfig, DatatypeEnum::PointCloudData, DatatypeEnum::TransformData, + DatatypeEnum::Landmarks, DatatypeEnum::ImgAnnotations}}, {DatatypeEnum::Buffer, {DatatypeEnum::ImgFrame, @@ -67,6 +69,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::PointCloudConfig, DatatypeEnum::PointCloudData, DatatypeEnum::TransformData, + DatatypeEnum::Landmarks, DatatypeEnum::ImgAnnotations}}, {DatatypeEnum::ImgFrame, {}}, {DatatypeEnum::EncodedFrame, {}}, @@ -95,6 +98,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::PointCloudConfig, {}}, {DatatypeEnum::PointCloudData, {}}, {DatatypeEnum::TransformData, {}}, + {DatatypeEnum::Landmarks, {}}, {DatatypeEnum::ImgAnnotations, {}}, }; diff --git a/src/pipeline/datatype/Landmarks.cpp b/src/pipeline/datatype/Landmarks.cpp new file mode 100644 index 0000000000..01e308df4f --- /dev/null +++ b/src/pipeline/datatype/Landmarks.cpp @@ -0,0 +1,5 @@ +#include "depthai/pipeline/datatype/Landmarks.hpp" + +namespace dai { + ; // TODO, no impl needed +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 53d93673c3..f9d6429ad7 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -22,6 +22,7 @@ #include "depthai/pipeline/datatype/ImageAlignConfig.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" #include "depthai/pipeline/datatype/ImgAnnotations.hpp" +#include "depthai/pipeline/datatype/Landmarks.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" @@ -196,6 +197,10 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + // case DatatypeEnum::Landmarks: + // return parseDatatype(metadataStart, serializedObjectSize, data, fd); + // break; + case DatatypeEnum::Tracklets: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; @@ -240,6 +245,9 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::TransformData: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + case DatatypeEnum::Landmarks: + return parseDatatype(metadataStart, serializedObjectSize, data, fd); + break; case DatatypeEnum::ImgAnnotations: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; diff --git a/src/rtabmap/RTABMapSLAM.cpp b/src/rtabmap/RTABMapSLAM.cpp index 65243135b2..e8dfabf44e 100644 --- a/src/rtabmap/RTABMapSLAM.cpp +++ b/src/rtabmap/RTABMapSLAM.cpp @@ -2,19 +2,24 @@ #include #include +#include #include #include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/datatype/Landmarks.hpp" #include "pipeline/ThreadedNodeImpl.hpp" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_mapping.h" +// #include "rtabmap/core/Markers/Landmark.h" +#include +#include namespace dai { namespace node { void RTABMapSLAM::buildInternal() { sync->out.link(inSync); - sync->setRunOnHost(false); + sync->setRunOnHost(true); alphaScaling = -1.0; localTransform = rtabmap::Transform::getIdentity(); rtabmap::Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); @@ -23,12 +28,19 @@ void RTABMapSLAM::buildInternal() { rect.setMaxSize(1); depth.setBlocking(false); depth.setMaxSize(1); + landmarks.setBlocking(false); + landmarks.setMaxSize(1); + features.setBlocking(false); + features.setMaxSize(1); inSync.setMaxSize(1); inSync.setBlocking(false); inSync.addCallback(std::bind(&RTABMapSLAM::syncCB, this, std::placeholders::_1)); odom.setMaxSize(1); odom.setBlocking(false); odom.addCallback(std::bind(&RTABMapSLAM::odomPoseCB, this, std::placeholders::_1)); + imu.setMaxSize(1); + imu.setBlocking(false); + imu.addCallback(std::bind(&RTABMapSLAM::imuCB, this, std::placeholders::_1)); localMaps = std::make_shared(); } @@ -58,24 +70,48 @@ void RTABMapSLAM::saveDatabase() { void RTABMapSLAM::setUseFeatures(bool use) { useFeatures = use; - if(useFeatures) { - features.setBlocking(false); - features.setMaxSize(1); - inputs[featuresInputName] = features; - } + // if(useFeatures) { + // features.setBlocking(false); + // features.setMaxSize(1); + // inputs[featuresInputName] = features; + // } +} + +void RTABMapSLAM::setUseLandmarks(bool use) { + useLandmarks = use; + // if(useLandmarks) { + // landmarks.setBlocking(false); + // landmarks.setMaxSize(1); + // inputs[landmarksInputName] = landmarks; + // } } void RTABMapSLAM::syncCB(std::shared_ptr data) { auto group = std::dynamic_pointer_cast(data); + + // for (auto& i : group->getMessageNames()) { + // std::cout << i << std::endl; + // } + if(group == nullptr) return; std::shared_ptr imgFrame = nullptr; std::shared_ptr depthFrame = nullptr; std::shared_ptr featuresFrame = nullptr; + std::shared_ptr markersFrame = nullptr; imgFrame = group->get(rectInputName); depthFrame = group->get(depthInputName); if(useFeatures) { featuresFrame = group->get(featuresInputName); } + if(useLandmarks) { + // std::cout << "getting landmarks" << std::endl; + markersFrame = group->get(landmarksInputName); + + auto thing = landmarks.get(); + // if (thing != nullptr) { + // std::cout << thing.landmarks. << std::endl; + // } + } if(imgFrame != nullptr && depthFrame != nullptr) { if(!initialized) { auto pipeline = getParentPipeline(); @@ -91,12 +127,28 @@ void RTABMapSLAM::syncCB(std::shared_ptr data) { } sensorData.setFeatures(keypoints, std::vector(), cv::Mat()); } + rtabmap::Landmarks markers; + if(markersFrame != nullptr) { + for(auto& marker : markersFrame->landmarks) { + auto transform = TransformData(marker.translation.x, marker.translation.y, marker.translation.z, + marker.quaternion.qx, marker.quaternion.qy, marker.quaternion.qz, marker.quaternion.qw); + + // auto covariance = cv::Mat::eye(6 ,6, CV_64FC1) * 0.001; + auto covariance = marker.getCovarianceAsCvMat(); + + markers.emplace(std::make_pair(marker.id, rtabmap::Landmark(marker.id, marker.size, transform.getRTABMapTransform(), covariance))); + } + sensorData.setLandmarks(markers); + } } passthroughRect.send(imgFrame); passthroughDepth.send(depthFrame); if(useFeatures) { passthroughFeatures.send(featuresFrame); } + if(useLandmarks) { + passthroughLandmarks.send(markersFrame); + } } } @@ -113,6 +165,21 @@ void RTABMapSLAM::odomPoseCB(std::shared_ptr data) { passthroughOdom.send(odomPose); } +void RTABMapSLAM::imuCB(std::shared_ptr imuData) { + auto imuPackets = std::dynamic_pointer_cast(imuData); + + auto angularCovariance = cv::Mat::eye(3, 3, CV_64FC1) * 0.000034906585; // 0.02 degrees/s in radians/s + auto linearCovariance = cv::Mat::eye(3, 3, CV_64FC1) * 0.002; + + for(auto& imuPacket : imuPackets->packets) { + auto angularVelocity = cv::Vec3d(imuPacket.gyroscope.x, imuPacket.gyroscope.y, imuPacket.gyroscope.z); + auto linearAcceleration = cv::Vec3d(imuPacket.acceleroMeter.x, imuPacket.acceleroMeter.y, imuPacket.acceleroMeter.z); + auto data = rtabmap::IMU(angularVelocity, angularCovariance, linearAcceleration, linearCovariance); + + sensorData.setIMU(data); + } +}; + void RTABMapSLAM::run() { auto& logger = pimpl->logger; while(isRunning()) { diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index f65734119a..7c4436fe6b 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -7,6 +7,7 @@ #include #include "depthai/schemas/PointCloudData.pb.h" +#include "pipeline/datatype/DatatypeEnum.hpp" namespace dai { namespace utility { @@ -158,6 +159,7 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::BenchmarkReport: case DatatypeEnum::MessageGroup: case DatatypeEnum::TransformData: + case DatatypeEnum::Landmarks: case DatatypeEnum::PointCloudConfig: case DatatypeEnum::ImageAlignConfig: case DatatypeEnum::ImgAnnotations: