add map publishing

CMakeLists.txt

@@ -781,6 +781,7 @@ if(DEPTHAI_SANITIZE)
     endif()
     if(DEPTHAI_HAVE_RTABMAP_SUPPORT)
         add_sanitizers(${TARGET_RTABMAP_NAME})
+    endif()
     if(DEPTHAI_HAVE_PCL_SUPPORT)
         add_sanitizers(${TARGET_PCL_NAME})
     endif()

cmake/Hunter/config.cmake

@@ -163,7 +163,7 @@ hunter_config(
     rtbmap
     # VERSION "0.21.4"
     URL "https://github.com/introlab/rtabmap/archive/refs/heads/master.tar.gz"
-    SHA1 "99c3b9c680e3d5d95a16f4422d60c03131d81730"
+    SHA1 "b18fd4cf6811c4b5b4e925f9f8f0303ed11c55b1"
     CMAKE_ARGS
         CMAKE_BUILD_TYPE=RelWithDebInfo
         BUILD_APP=OFF

examples/CMakeLists.txt

@@ -292,14 +292,14 @@ dai_add_example(device_information host_side/device_information.cpp OFF OFF)
 dai_add_example(device_logging host_side/device_logging.cpp OFF OFF)
 
 include(FetchContent)
-FetchContent_Declare(rerun_sdk URL https://build.rerun.io/commit/1030934/rerun_cpp_sdk.zip)
+FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/latest/download/rerun_cpp_sdk.zip)
 FetchContent_MakeAvailable(rerun_sdk)
-dai_add_example(rtabmap_vio host_side/rtabmap_vio.cpp OFF)
+dai_add_example(rtabmap_vio host_side/rtabmap_vio.cpp OFF OFF)
 set_property(TARGET rtabmap_vio PROPERTY CXX_STANDARD 17)
 target_link_options(rtabmap_vio PRIVATE "-Wl,--disable-new-dtags")
 target_link_libraries(rtabmap_vio PRIVATE depthai::rtabmap rerun_sdk)
 
-dai_add_example(rtabmap_slam host_side/rtabmap_slam.cpp OFF)
+dai_add_example(rtabmap_slam host_side/rtabmap_slam.cpp OFF ON)
 set_property(TARGET rtabmap_slam PROPERTY CXX_STANDARD 17)
 target_link_options(rtabmap_slam PRIVATE "-Wl,--disable-new-dtags")
 target_link_libraries(rtabmap_slam PRIVATE depthai::rtabmap rerun_sdk)
@@ -311,13 +311,13 @@ find_package(Sophus CONFIG REQUIRED)
 hunter_add_package(magic_enum)
 find_package(magic_enum CONFIG REQUIRED)
 find_package(TBB CONFIG REQUIRED)
-dai_add_example(basalt_dai_vio host_side/basalt_dai_vio.cpp OFF)
+dai_add_example(basalt_dai_vio host_side/basalt_dai_vio.cpp OFF OFF)
 set_property(TARGET basalt_dai_vio PROPERTY CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
 target_link_options(basalt_dai_vio PRIVATE "-Wl,--disable-new-dtags")
 target_link_libraries(basalt_dai_vio PRIVATE basalt rerun_sdk TBB::tbb Eigen3::Eigen Sophus::Sophus magic_enum::magic_enum)
 
-dai_add_example(basalt_dai_vio_slam host_side/basalt_dai_vio_slam.cpp OFF)
+dai_add_example(basalt_dai_vio_slam host_side/basalt_dai_vio_slam.cpp OFF ON)
 set_property(TARGET basalt_dai_vio_slam PROPERTY CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
 target_link_options(basalt_dai_vio_slam PRIVATE "-Wl,--disable-new-dtags")

examples/host_side/basalt_dai_vio_slam.cpp

@@ -182,8 +182,8 @@ class BasaltVIO : public dai::NodeCRTP<dai::ThreadedNode, BasaltVIO> {
 
         Eigen::Quaterniond qR = roll_angle * pitch_angle * yaw_angle;
         // OAK D PRO
-        Eigen::Vector3d transL(0.0, -0.06935, -0.00565);  // y x z for some reason
-        Eigen::Vector3d transR(0.0, 0.0, -0.00565);
+        Eigen::Vector3d transL(0.0, -0.06635, -0.00565);  // y x z for some reason
+        Eigen::Vector3d transR(0.0, 0.00641, -0.00565);
 
         // OAK D PRO W
         // Eigen::Vector3d transL(0.0, -0.075448, -0.0048);  // y x z for some reason
@@ -200,35 +200,55 @@ class BasaltVIO : public dai::NodeCRTP<dai::ThreadedNode, BasaltVIO> {
         Eigen::Vector2i resolution;
         resolution << 640, 400;
         calibInt->resolution.push_back(resolution);
-
         auto leftIntrinsics = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, 640, 400);
         auto rightIntrinsics = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 640, 400);
 
+        auto model = calibHandler.getDistortionModel(dai::CameraBoardSocket::LEFT);
+        auto distCoeffsL = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::LEFT);
+
         basalt::GenericCamera<Scalar> cameraL;
-        basalt::PinholeCamera<Scalar>::VecN paramsL;
+        basalt::PinholeRadtan8Camera<Scalar>::VecN paramsL;
         // fx, fy, cx, cy
         double fxL = leftIntrinsics[0][0];
         double fyL = leftIntrinsics[1][1];
         double cxL = leftIntrinsics[0][2];
         double cyL = leftIntrinsics[1][2];
-        paramsL << fxL, fyL, cxL, cyL;
+        double k1L = distCoeffsL[0];
+        double k2L = distCoeffsL[1];
+        double p1L = distCoeffsL[2];
+        double p2L = distCoeffsL[3];
+        double k3L = distCoeffsL[4];
+        double k4L = distCoeffsL[5];
+        double k5L = distCoeffsL[6];
+        double k6L = distCoeffsL[7];
+
+        paramsL << fxL, fyL, cxL, cyL, k1L, k2L, p1L, p2L, k3L, k4L, k5L, k6L;
         std::cout << "fxL: " << fxL << " fyL: " << fyL << " cxL: " << cxL << " cyL: " << cyL << std::endl;
 
-        basalt::PinholeCamera<Scalar> pinholeL(paramsL);
+        basalt::PinholeRadtan8Camera<Scalar> pinholeL(paramsL);
         cameraL.variant = pinholeL;
 
         calibInt->intrinsics.push_back(cameraL);
 
+        auto distCoeffsR = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::RIGHT);
         basalt::GenericCamera<Scalar> cameraR;
-        basalt::PinholeCamera<Scalar>::VecN paramsR;
+        basalt::PinholeRadtan8Camera<Scalar>::VecN paramsR;
         // fx, fy, cx, cy
         double fxR = rightIntrinsics[0][0];
         double fyR = rightIntrinsics[1][1];
         double cxR = rightIntrinsics[0][2];
         double cyR = rightIntrinsics[1][2];
-        paramsR << fxR, fyR, cxR, cyR;
-
-        basalt::PinholeCamera<Scalar> pinholeR(paramsR);
+        double k1R = distCoeffsR[0];
+        double k2R = distCoeffsR[1];
+        double p1R = distCoeffsR[2];
+        double p2R = distCoeffsR[3];
+        double k3R = distCoeffsR[4];
+        double k4R = distCoeffsR[5];
+        double k5R = distCoeffsR[6];
+        double k6R = distCoeffsR[7];
+        paramsR << fxR, fyR, cxR, cyR, k1R, k2R, p1R, p2R, k3R, k4R, k5R, k6R;
+
+        basalt::PinholeRadtan8Camera<Scalar> pinholeR(paramsR);
         cameraR.variant = pinholeR;
 
         calibInt->intrinsics.push_back(cameraR);
@@ -255,6 +275,8 @@ class RerunStreamer : public dai::NodeCRTP<dai::ThreadedNode, RerunStreamer> {
      */
     Input inputTrans{true, *this, "inTrans", Input::Type::SReceiver, false, 8, true, {{dai::DatatypeEnum::TransformData, true}}};
     Input inputImg{true, *this, "inImg", Input::Type::SReceiver, false, 8, true, {{dai::DatatypeEnum::ImgFrame, true}}};
+    Input inputPCL{true, *this, "inPCL", Input::Type::SReceiver, false, 8, true, {{dai::DatatypeEnum::PointCloudData, true}}};
+    Input inputMap{true, *this, "inMap", Input::Type::SReceiver, false, 8, true, {{dai::DatatypeEnum::ImgFrame, true}}};
     void run() override {
         const auto rec = rerun::RecordingStream("rerun");
         rec.spawn().exit_on_failure();
@@ -264,6 +286,8 @@ class RerunStreamer : public dai::NodeCRTP<dai::ThreadedNode, RerunStreamer> {
         while(isRunning()) {
             std::shared_ptr<dai::TransformData> transData = inputTrans.queue.get<dai::TransformData>();
             auto imgFrame = inputImg.queue.get<dai::ImgFrame>();
+            auto pclData = inputPCL.queue.tryGet<dai::PointCloudData>();
+            auto mapData = inputMap.queue.tryGet<dai::ImgFrame>();
             if(transData != nullptr) {
                 double x, y, z, qx, qy, qz, qw;
                 transData->getTranslation(x, y, z);
@@ -277,6 +301,16 @@ class RerunStreamer : public dai::NodeCRTP<dai::ThreadedNode, RerunStreamer> {
                 rec.log("world/camera/image", rerun::Pinhole::from_focal_length_and_resolution({398.554f, 398.554f}, {640.0f, 400.0f}));
                 rec.log("world/camera/image/rgb",
                         rerun::Image(tensor_shape(imgFrame->getCvFrame()), reinterpret_cast<const uint8_t*>(imgFrame->getCvFrame().data)));
+                if(pclData != nullptr) {
+                    std::vector<rerun::Position3D> points;
+                    for(auto& point : pclData->points) {
+                        points.push_back(rerun::Position3D(point.x, point.y, point.z));
+                    }
+                    rec.log("world/camera/pointcloud", rerun::Points3D(points));
+                }
+                if(mapData != nullptr) {
+                    rec.log("map", rerun::Image(tensor_shape(mapData->getCvFrame()), reinterpret_cast<const uint8_t*>(mapData->getCvFrame().data)));
+                }
             }
         }
     }
@@ -300,13 +334,25 @@ int main() {
     auto sync = pipeline.create<dai::node::Sync>();
     auto odom = pipeline.create<BasaltVIO>();
     auto slam = pipeline.create<dai::node::RTABMapSLAM>();
+    auto params = rtabmap::ParametersMap();
+    params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true"));
+    slam->setParams(params);
     auto rerun = pipeline.create<RerunStreamer>();
     auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
     xoutDepth->setStreamName("depth");
     imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
     imu->setBatchReportThreshold(1);
     imu->setMaxBatchReports(10);
 
+    stereo->setExtendedDisparity(false);
+    stereo->setSubpixel(true);
+    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
+    stereo->setLeftRightCheck(true);
+    stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
+	stereo->enableDistortionCorrection(true);
+    stereo->initialConfig.setLeftRightCheckThreshold(10);
+    stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
+
     left->setCamera("left");
     left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
     left->setFps(fps);
@@ -323,9 +369,11 @@ int main() {
     sync->out.link(odom->inputStereo);
     imu->out.link(odom->inputImu);
     odom->transform.link(slam->inputOdomPose);
-    odom->passthrough.link(slam->inputRect);
+    stereo->rectifiedLeft.link(slam->inputRect);
     slam->transform.link(rerun->inputTrans);
     slam->passthroughRect.link(rerun->inputImg);
+    // slam->pointCloud.link(rerun->inputPCL);
+    slam->occupancyMap.link(rerun->inputMap);
     pipeline.start();
     pipeline.wait();
 }

examples/host_side/rtabmap_slam.cpp

@@ -45,6 +45,7 @@ class RerunStreamer : public dai::NodeCRTP<dai::ThreadedNode, RerunStreamer> {
         const auto rec = rerun::RecordingStream("rerun");
         rec.spawn().exit_on_failure();
         rec.log_timeless("world", rerun::ViewCoordinates::RDF);
+        rec.log("world/ground", rerun::Boxes3D::from_half_sizes({{3.f, 3.f, 0.00001f}}));
 
         while(isRunning()) {
             std::shared_ptr<dai::TransformData> transData = inputTrans.queue.get<dai::TransformData>();

include/depthai/pipeline/datatype/PointCloudData.hpp

@@ -26,6 +26,8 @@ class PointCloudData : public Buffer {
     bool sparse = false;
 
    public:
+    std::vector<Point3f> points;
+    std::vector<Point3f> colors;
     using Buffer::getSequenceNum;
     using Buffer::getTimestamp;
     using Buffer::getTimestampDevice;

include/depthai/rtabmap/RTABMapSLAM.hpp

@@ -9,10 +9,14 @@
 #include "depthai/pipeline/datatype/TransformData.hpp"
 #include "depthai/pipeline/datatype/CameraControl.hpp"
 #include "depthai/pipeline/datatype/TrackedFeatures.hpp"
+#include "depthai/pipeline/datatype/PointCloudData.hpp"
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Transform.h"
+#include "rtabmap/core/LocalGrid.h"
+#include "rtabmap/core/global_map/OccupancyGrid.h"
+
 namespace dai {
 namespace node {
 class RTABMapSLAM : public dai::NodeCRTP<dai::ThreadedNode, RTABMapSLAM> {
@@ -30,6 +34,8 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::ThreadedNode, RTABMapSLAM> {
     Input inputOdomPose{true, *this, "odom_pose", Input::Type::SReceiver, false, 8, false, {{dai::DatatypeEnum::TransformData, true}}};
     Output transform{true, *this, "transform", Output::Type::MSender, {{dai::DatatypeEnum::TransformData, true}}};
     Output passthroughRect{true, *this, "passthrough_rect", Output::Type::MSender, {{dai::DatatypeEnum::ImgFrame, true}}};
+    Output pointCloud{true, *this, "point_cloud", Output::Type::MSender, {{dai::DatatypeEnum::PointCloudData, true}}};
+    Output occupancyMap{true, *this, "map", Output::Type::MSender, {{dai::DatatypeEnum::ImgFrame, true}}};
     void run() override;
     void stop() override;
     void setParams(const rtabmap::ParametersMap& params);
@@ -45,8 +51,11 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::ThreadedNode, RTABMapSLAM> {
     std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
     std::map<double, cv::Vec4f> rotBuffer_;
+    rtabmap::LocalGridCache localMaps_;
+    rtabmap::OccupancyGrid* grid;
     float alphaScaling;
     bool modelSet = false;
+    rtabmap::ParametersMap rtabParams;
 };
 }  // namespace node
 }  // namespace dai

src/rtabmap/RTABMapSLAM.cpp

@@ -1,12 +1,17 @@
 #include "depthai/rtabmap/RTABMapSLAM.hpp"
 
+#include <pcl/point_cloud.h>
+
+#include "rtabmap/core/util3d.h"
+
 namespace dai {
 namespace node {
 void RTABMapSLAM::build() {
     hostNode = true;
     alphaScaling = -1.0;
     reuseFeatures = false;
     rtabmap.init();
+
     // inputRect.queue.setMaxSize(1);
     // inputDepth.queue.setMaxSize(1);
     // inputFeatures.queue.setMaxSize(1);
@@ -22,6 +27,8 @@ void RTABMapSLAM::stop() {
 
 void RTABMapSLAM::setParams(const rtabmap::ParametersMap& params) {
     rtabmap.init(params);
+    rtabParams = params;
+
 }
 
 void RTABMapSLAM::run() {
@@ -39,8 +46,10 @@ void RTABMapSLAM::run() {
             if(!modelSet) {
                 auto pipeline = getParentPipeline();
                 getCalib(pipeline, imgFrame->getInstanceNum(), imgFrame->getWidth(), imgFrame->getHeight());
-                modelSet = true;
                 lastProcessTime = std::chrono::steady_clock::now();
+                grid = new rtabmap::OccupancyGrid(&localMaps_, rtabParams);
+
+                modelSet = true;
             } else {
                 double stamp = std::chrono::duration<double>(imgFrame->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
 
@@ -66,23 +75,86 @@ void RTABMapSLAM::run() {
                     lastProcessTime = now;
                     if(rtabmap.process(data, pose)) {
                         stats = rtabmap.getStatistics();
-                        if(rtabmap.getLoopClosureId() > 0){
+                        if(rtabmap.getLoopClosureId() > 0) {
                             fmt::print("Loop closure detected! last loop closure id = {}\n", rtabmap.getLoopClosureId());
                         }
-                        odomCorrection= stats.mapCorrection();
-
+                        odomCorrection = stats.mapCorrection();
+                        std::map<int, rtabmap::Signature> nodes;
+                        std::map<int, rtabmap::Transform> optimizedPoses;
+                        std::multimap<int, rtabmap::Link> links;
+                        rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+                        for(std::map<int, rtabmap::Transform>::iterator iter = optimizedPoses.begin(); iter != optimizedPoses.end(); ++iter) {
+                            rtabmap::Signature node = nodes.find(iter->first)->second;
+                            if(node.sensorData().gridCellSize() == 0.0f) {
+                                std::cout << "Grid cell size is 0, skipping node " << iter->first << std::endl;
+                                continue;
+                            }
+                            // uncompress grid data
+                            cv::Mat ground, obstacles, empty;
+                            node.sensorData().uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
+                            localMaps_.add(iter->first, ground, obstacles, empty, node.sensorData().gridCellSize(), node.sensorData().gridViewPoint());
+                        }
+                        if(grid->addedNodes().size() || localMaps_.size() > 0) {
+                            grid->update(optimizedPoses);
+                        }
+                        float xMin, yMin;
+                        cv::Mat map = grid->getMap(xMin, yMin);
+                        if(!map.empty()) {
+                            cv::Mat map8U(map.rows, map.cols, CV_8U);
+                            // convert to gray scaled map
+                            for(int i = 0; i < map.rows; ++i) {
+                                for(int j = 0; j < map.cols; ++j) {
+                                    char v = map.at<char>(i, j);
+                                    unsigned char gray;
+                                    if(v == 0) {
+                                        gray = 178;
+                                    } else if(v == 100) {
+                                        gray = 0;
+                                    } else  // -1
+                                    {
+                                        gray = 89;
+                                    }
+                                    map8U.at<unsigned char>(i, j) = gray;
+                                }
+                            }
+                            auto gridMap = std::make_shared<dai::ImgFrame>();
+                            gridMap->setTimestamp(std::chrono::steady_clock::now());
+                            // convert cv mat data to std::vector<uint8_t>
+                            // std::cout << map8U << std::endl;
+                            cv::Mat flat = map8U.reshape(1, map8U.total()*map8U.channels());
+                            std::vector<uchar> vec = map8U.isContinuous()? flat : flat.clone();
+                            gridMap->setData(vec);
+                            gridMap->setType(dai::ImgFrame::Type::GRAY8);
+                            gridMap->setHeight(map8U.rows);
+                            gridMap->setWidth(map8U.cols);
+                            occupancyMap.send(gridMap);
+                        }
                     }
-                } 
-                auto out = std::make_shared<dai::TransformData>(odomCorrection*pose);
+                }
+                auto out = std::make_shared<dai::TransformData>(odomCorrection * pose);
                 transform.send(out);
                 passthroughRect.send(imgFrame);
+                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromSensorData(data,
+                                                                                                 4,  // decimation
+                                                                                                 0.0f);
+                // pcl to dai PointCloudData
+                auto pclData = std::make_shared<dai::PointCloudData>();
 
+                int size = cloud->points.size();
+                pclData->points.resize(size * 3);
+                for(int i = 0; i < size; i++) {
+                    pclData->points[i].x = cloud->points[i].x;
+                    pclData->points[i].y = cloud->points[i].y;
+                    pclData->points[i].z = cloud->points[i].z;
+                }
+                pointCloud.send(pclData);
             }
         }
     }
     fmt::print("Display node stopped\n");
 }
 
+
 void RTABMapSLAM::getCalib(dai::Pipeline& pipeline, int instanceNum, int width, int height) {
     auto device = pipeline.getDevice();
     auto calibHandler = device->readCalibration2();