-
Notifications
You must be signed in to change notification settings - Fork 158
[Draft] adding landmark and imu support to RTABMapSLAM #1398
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
dragonblade316
wants to merge
59
commits into
luxonis:develop
Choose a base branch
from
dragonblade316:develop
base: develop
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Changes from 58 commits
Commits
Show all changes
59 commits
Select commit
Hold shift + click to select a range
ec2af88
Added Landmark data type
dragonblade316 bf974fd
Add landmark input
dragonblade316 620e95f
Add landmarks.cpp to Cmakelists.txt
dragonblade316 3cd47e1
added python bindings for landmarks
dragonblade316 00ba67c
fixed landmark build errors
dragonblade316 b2fd25c
fixed landmark binding build errors
dragonblade316 8bc5bab
temp fixes
dragonblade316 1cc1099
Slight formatting update
dragonblade316 683ea0c
RTABMap Landmark support compiles
dragonblade316 2540c37
Added binding to RTABMapSlam::landmarks
dragonblade316 50a1be2
Added DatatypeEnum::Landmarks to the hiearchy
dragonblade316 f1f17a3
Fixed landmarks datatype in hirearchy
dragonblade316 2af9719
Added landmarks datatype to stream message parser
dragonblade316 13a5bd6
Properly expose landmarks datatype
dragonblade316 5ca3128
Use correct extrisics matrix for Basalt
viggy96 8e8a19d
Use Point3d and Quaternion for Landmarks
viggy96 9548186
Build in debug mode too
e63f161
Fix libusb name
27f77dd
Add support for Debug builds
439047e
Test the Debug builds in CI
863bab0
Add a debug postfix
fcb1ad2
Set the OSX_DEPLOYMENT_TARGET sooner
4c0630e
Add back release only build for VCPKG for performance reasons
902afab
Release build for docstrings
341d046
Build shared libraries in release mode
bf40789
Revert release changes
4c2ffe8
Update README.md
0d92300
[RVC2/4 FW] Bump firmware binaries to develop
c7e16b9
Bump version to v3.0.0-rc.3
94d1417
Error out in case stub generation fails
5da4edc
Missed another step (how many of these are there?) and added Landmark…
dragonblade316 d2bc626
More missing steps for Landmark datatype
viggy96 5c109ab
Fixed includes
dragonblade316 082a580
added setUseFeatures method
dragonblade316 375f55f
Merge branch 'luxonis:develop' into develop
dragonblade316 868860d
Merge pull request #1 from dragonblade316/slam-landmarks
dragonblade316 9a8020b
Reorder Landmark datatype in enum
viggy96 411c110
Added setUseLandmarks(bool) python binding
dragonblade316 39d85f3
fix input thing
dragonblade316 d822f24
Fix landmarks typo
viggy96 b85ac2c
fix input thing (again)
dragonblade316 84fa73a
added passthrough output binding
dragonblade316 36b185d
Add landmarks to input map
viggy96 6d33bcf
Remove docstring file
viggy96 2721612
Ignore docstrings
viggy96 8da3a96
Debugging stuff
dragonblade316 df463e3
Fix landmark covariance
viggy96 98e2e7a
Remove print
viggy96 be9f3c2
Remove duplicate binding
viggy96 b7242bd
Fix features input link on RTABMapSLAM
viggy96 50b2a36
First try at adding covariance
dragonblade316 fb68c4f
covariance fixes
dragonblade316 da49d81
covariance bindings
dragonblade316 61b2292
Added opencv/Landmarks.cpp to cmake
dragonblade316 67530fd
rebase issue??
dragonblade316 aab389e
Add imu link to RTABMapSLAM
viggy96 025e63e
Tune Basalt config
viggy96 77d9907
Extra time for Python to build
viggy96 18ec720
Tighten IMU covariance
viggy96 File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -8,6 +8,7 @@ build*/* | |
| shared/version.hpp | ||
|
|
||
| #pybind11 | ||
| bindings/python/docstrings/ | ||
| build/ | ||
| dist/ | ||
| _build/ | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
54 changes: 54 additions & 0 deletions
54
bindings/python/src/pipeline/datatype/LandmarksBindings.cpp
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| #include "DatatypeBindings.hpp" | ||
| #include "pipeline/CommonBindings.hpp" | ||
|
|
||
| // depthai | ||
| #include "depthai/pipeline/datatype/Landmarks.hpp" | ||
|
|
||
| //pybind | ||
| #include <pybind11/chrono.h> | ||
| #include <pybind11/numpy.h> | ||
|
|
||
|
|
||
|
|
||
| void bind_landmarks(pybind11::module& m, void* pCallstack) { | ||
| using namespace dai; | ||
|
|
||
| py::class_<Landmark> landmark(m, "Landmark", DOC(dai, Landmark)); | ||
| py::class_<Landmarks, Py<Landmarks>, Buffer, std::shared_ptr<Landmarks>> 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<Landmark> 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)) | ||
| ; | ||
|
|
||
|
|
||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,69 @@ | ||
| #pragma once | ||
|
|
||
| #include <array> | ||
| #include <vector> | ||
|
|
||
| #include "depthai/common/Point3d.hpp" | ||
| #include "depthai/common/Quaterniond.hpp" | ||
| #include "depthai/pipeline/datatype/Buffer.hpp" | ||
|
|
||
| // optional | ||
| #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT | ||
| #include <opencv2/core/mat.hpp> | ||
| #include <opencv2/opencv.hpp> | ||
| #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<std::array<double, 6>, 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<Landmark> landmarks; | ||
|
|
||
| DEPTHAI_SERIALIZE(Landmarks, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, landmarks); | ||
| void serialize(std::vector<std::uint8_t>& metadata, DatatypeEnum& datatype) const override { | ||
| metadata = utility::serialize(*this); | ||
| datatype = DatatypeEnum::Landmarks; | ||
| } | ||
| }; | ||
|
|
||
| } // namespace dai |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<double>(i, j) = covariance.at(i).at(j); | ||
| } | ||
| } | ||
|
|
||
| return cvMat; | ||
| } | ||
| } |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Bug: Input Initialization Fails with Default Objects
The
landmarksandfeaturesInputobjects are incorrectly initialized. They are defined as references toinputsmap entries that are created as default-constructed objects upon first access, rather than being properly constructed with required parameters like datatype constraints, blocking behavior, and queue sizes.Locations (1)
include/depthai/rtabmap/RTABMapSLAM.hpp#L50-L53