From 11b8a40bf783a5172704370dfc983ae8b771196b Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 18 Apr 2024 14:51:12 +0000 Subject: [PATCH] add map publishing --- CMakeLists.txt | 1 + cmake/Hunter/config.cmake | 2 +- examples/CMakeLists.txt | 10 +-- examples/host_side/basalt_dai_vio_slam.cpp | 70 +++++++++++++--- examples/host_side/rtabmap_slam.cpp | 1 + .../pipeline/datatype/PointCloudData.hpp | 2 + include/depthai/rtabmap/RTABMapSLAM.hpp | 9 ++ src/rtabmap/RTABMapSLAM.cpp | 84 +++++++++++++++++-- 8 files changed, 156 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3f6da7fe..641f3a8f7 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/cmake/Hunter/config.cmake b/cmake/Hunter/config.cmake index f5ed7fc7c..93ded6b60 100644 --- a/cmake/Hunter/config.cmake +++ b/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 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 43339264c..3cc4bf8c2 100644 --- a/examples/CMakeLists.txt +++ b/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") diff --git a/examples/host_side/basalt_dai_vio_slam.cpp b/examples/host_side/basalt_dai_vio_slam.cpp index 0cbd0e0fc..deba4e2ea 100644 --- a/examples/host_side/basalt_dai_vio_slam.cpp +++ b/examples/host_side/basalt_dai_vio_slam.cpp @@ -182,8 +182,8 @@ class BasaltVIO : public dai::NodeCRTP { 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 { 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 cameraL; - basalt::PinholeCamera::VecN paramsL; + basalt::PinholeRadtan8Camera::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 pinholeL(paramsL); + basalt::PinholeRadtan8Camera pinholeL(paramsL); cameraL.variant = pinholeL; calibInt->intrinsics.push_back(cameraL); + auto distCoeffsR = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::RIGHT); basalt::GenericCamera cameraR; - basalt::PinholeCamera::VecN paramsR; + basalt::PinholeRadtan8Camera::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 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 pinholeR(paramsR); cameraR.variant = pinholeR; calibInt->intrinsics.push_back(cameraR); @@ -255,6 +275,8 @@ class RerunStreamer : public dai::NodeCRTP { */ 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 { while(isRunning()) { std::shared_ptr transData = inputTrans.queue.get(); auto imgFrame = inputImg.queue.get(); + auto pclData = inputPCL.queue.tryGet(); + auto mapData = inputMap.queue.tryGet(); 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 { 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(imgFrame->getCvFrame().data))); + if(pclData != nullptr) { + std::vector 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(mapData->getCvFrame().data))); + } } } } @@ -300,6 +334,9 @@ int main() { auto sync = pipeline.create(); auto odom = pipeline.create(); auto slam = pipeline.create(); + auto params = rtabmap::ParametersMap(); + params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); + slam->setParams(params); auto rerun = pipeline.create(); auto xoutDepth = pipeline.create(); xoutDepth->setStreamName("depth"); @@ -307,6 +344,15 @@ int main() { 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(); } diff --git a/examples/host_side/rtabmap_slam.cpp b/examples/host_side/rtabmap_slam.cpp index a2bf49d74..128702174 100644 --- a/examples/host_side/rtabmap_slam.cpp +++ b/examples/host_side/rtabmap_slam.cpp @@ -45,6 +45,7 @@ class RerunStreamer : public dai::NodeCRTP { 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 transData = inputTrans.queue.get(); diff --git a/include/depthai/pipeline/datatype/PointCloudData.hpp b/include/depthai/pipeline/datatype/PointCloudData.hpp index d6bb34295..a6edb8f25 100644 --- a/include/depthai/pipeline/datatype/PointCloudData.hpp +++ b/include/depthai/pipeline/datatype/PointCloudData.hpp @@ -26,6 +26,8 @@ class PointCloudData : public Buffer { bool sparse = false; public: + std::vector points; + std::vector colors; using Buffer::getSequenceNum; using Buffer::getTimestamp; using Buffer::getTimestampDevice; diff --git a/include/depthai/rtabmap/RTABMapSLAM.hpp b/include/depthai/rtabmap/RTABMapSLAM.hpp index ffb43bbba..0468b8866 100644 --- a/include/depthai/rtabmap/RTABMapSLAM.hpp +++ b/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 { @@ -30,6 +34,8 @@ class RTABMapSLAM : public dai::NodeCRTP { 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 { std::map accBuffer_; std::map gyroBuffer_; std::map rotBuffer_; + rtabmap::LocalGridCache localMaps_; + rtabmap::OccupancyGrid* grid; float alphaScaling; bool modelSet = false; + rtabmap::ParametersMap rtabParams; }; } // namespace node } // namespace dai \ No newline at end of file diff --git a/src/rtabmap/RTABMapSLAM.cpp b/src/rtabmap/RTABMapSLAM.cpp index c112ebe4b..5d023d0df 100644 --- a/src/rtabmap/RTABMapSLAM.cpp +++ b/src/rtabmap/RTABMapSLAM.cpp @@ -1,5 +1,9 @@ #include "depthai/rtabmap/RTABMapSLAM.hpp" +#include + +#include "rtabmap/core/util3d.h" + namespace dai { namespace node { void RTABMapSLAM::build() { @@ -7,6 +11,7 @@ void RTABMapSLAM::build() { 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(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 nodes; + std::map optimizedPoses; + std::multimap links; + rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true); + for(std::map::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(i, j); + unsigned char gray; + if(v == 0) { + gray = 178; + } else if(v == 100) { + gray = 0; + } else // -1 + { + gray = 89; + } + map8U.at(i, j) = gray; + } + } + auto gridMap = std::make_shared(); + gridMap->setTimestamp(std::chrono::steady_clock::now()); + // convert cv mat data to std::vector + // std::cout << map8U << std::endl; + cv::Mat flat = map8U.reshape(1, map8U.total()*map8U.channels()); + std::vector 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(odomCorrection*pose); + } + auto out = std::make_shared(odomCorrection * pose); transform.send(out); passthroughRect.send(imgFrame); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromSensorData(data, + 4, // decimation + 0.0f); + // pcl to dai PointCloudData + auto pclData = std::make_shared(); + 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();