From 0fd591ba61644292affa32fea85f8bc8a243335b Mon Sep 17 00:00:00 2001 From: Sofiia Sarana Date: Mon, 12 May 2025 14:25:39 -0400 Subject: [PATCH 1/5] add grid_map_core dependency --- package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 8e5f32aef2..ac9de0bc8c 100644 --- a/package.xml +++ b/package.xml @@ -9,7 +9,6 @@ http://introlab.github.io/rtabmap https://github.com/introlab/rtabmap/issues https://github.com/introlab/rtabmap - cmake proj @@ -22,7 +21,7 @@ libsqlite3-dev liboctomap-dev - + grid_map_core qt_gui_cpp zlib From cff65db9d9b362004f230634f07b8842713b8dac Mon Sep 17 00:00:00 2001 From: Sofiia Sarana Date: Mon, 12 May 2025 14:30:16 -0400 Subject: [PATCH 2/5] fix build with NONFREE features on humble-devel --- corelib/src/Features2d.cpp | 1 + corelib/src/odometry/OdometryF2M.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 5aa67230ef..e26b23df09 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -1140,6 +1140,7 @@ void SURF::parseParameters(const ParametersMap & parameters) { #if CV_MAJOR_VERSION < 3 _surf = cv::Ptr(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_)); + std::cout << "using cpu" << std::endl; #else _surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_); #endif diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 6239a8d0aa..513bb1d8ef 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -876,7 +876,6 @@ Transform OdometryF2M::computeTransform( ++iter; } } - if(mapWords.size() != mapPoints.size()) { UDEBUG("Remove points"); @@ -1263,6 +1262,7 @@ Transform OdometryF2M::computeTransform( map_->setWords(words, wordsKpts, transformedPoints, descriptors); addKeyFrame = true; + descriptors.release(); } else { From 6d9ccfd59d9eddfa7fe8eeadae54836941680832 Mon Sep 17 00:00:00 2001 From: Sofiia Sarana Date: Tue, 13 May 2025 04:18:21 -0400 Subject: [PATCH 3/5] add OpenCL GPU implementation --- corelib/include/rtabmap/core/MarkerDetector.h | 18 +++---- corelib/src/Features2d.cpp | 48 +++++++++++++------ corelib/src/MarkerDetector.cpp | 44 +++++++++-------- corelib/src/Memory.cpp | 6 ++- corelib/src/odometry/OdometryF2M.cpp | 4 ++ corelib/src/superpoint_torch/SuperPoint.cc | 14 +++--- guilib/src/CameraViewer.cpp | 12 +++-- 7 files changed, 93 insertions(+), 53 deletions(-) diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index d628ca022d..7e265af2d7 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -65,23 +65,23 @@ class RTABMAP_CORE_EXPORT MarkerDetector { // Use the other detect(), in which the returned map contains the length of each marker detected. RTABMAP_DEPRECATED - MapIdPose detect(const cv::Mat & image, + MapIdPose detect(const cv::UMat & image, const CameraModel & model, - const cv::Mat & depth = cv::Mat(), + const cv::UMat & depth = cv::UMat(), float * estimatedMarkerLength = 0, - cv::Mat * imageWithDetections = 0); + cv::UMat * imageWithDetections = 0); - std::map detect(const cv::Mat & image, + std::map detect(const cv::UMat & image, const std::vector & models, - const cv::Mat & depth = cv::Mat(), + const cv::UMat & depth = cv::UMat(), const std::map & markerLengths = std::map(), - cv::Mat * imageWithDetections = 0); + cv::UMat * imageWithDetections = 0); - std::map detect(const cv::Mat & image, + std::map detect(const cv::UMat & image, const CameraModel & model, - const cv::Mat & depth = cv::Mat(), + const cv::UMat & depth = cv::UMat(), const std::map & markerLengths = std::map(), - cv::Mat * imageWithDetections = 0); + cv::UMat * imageWithDetections = 0); private: #ifdef HAVE_OPENCV_ARUCO diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index e26b23df09..cb909d11ba 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #ifdef RTABMAP_ORB_OCTREE #include "opencv/ORBextractor.h" @@ -769,13 +770,16 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co UASSERT(!image.empty()); UASSERT(image.type() == CV_8UC1); - cv::Mat mask; + cv::UMat imageU; + image.copyTo(imageU); + cv::UMat maskU; + cv::Mat maskMat; if(!maskIn.empty()) { if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1) { mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1); - for(int i=0; i<(int)mask.total(); ++i) + for(int i=0; i<(int)maskMat.total(); ++i) { float value = 0.0f; if(maskIn.type()==CV_16UC1) @@ -795,14 +799,15 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co (_maxDepth == 0.0f || value <= _maxDepth) && uIsFinite(value)) { - ((unsigned char*)mask.data)[i] = 255; // ORB uses 255 to handle pyramids + maskMat.data[i] = 255; } } + maskMat.copyTo(maskU); } else if(maskIn.type()==CV_8UC1) { // assume a standard mask - mask = maskIn; + maskIn.copyTo(maskU); } else { @@ -810,14 +815,14 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co } } - UASSERT(mask.empty() || (mask.cols == image.cols && mask.rows == image.rows)); + UASSERT(maskU.empty() || (maskU.cols == imageU.cols && maskU.rows == imageU.rows)); std::vector keypoints; UTimer timer; cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios); if(!(globalRoi.width && globalRoi.height)) { - globalRoi = cv::Rect(0,0,image.cols, image.rows); + globalRoi = cv::Rect(0,0,imageU.cols, imageU.rows); } // Get keypoints @@ -854,7 +859,7 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co { std::vector corners; cv::KeyPoint::convert(keypoints, corners); - cv::cornerSubPix( image, corners, + cv::cornerSubPix( imageU, corners, cv::Size( _subPixWinSize, _subPixWinSize ), cv::Size( -1, -1 ), cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) ); @@ -1140,7 +1145,7 @@ void SURF::parseParameters(const ParametersMap & parameters) { #if CV_MAJOR_VERSION < 3 _surf = cv::Ptr(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_)); - std::cout << "using cpu" << std::endl; + std::cout << "using cpu" << std::endl; #else _surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_); #endif @@ -1173,11 +1178,20 @@ std::vector SURF::generateKeypointsImpl(const cv::Mat & image, con cv::cuda::GpuMat maskGpu(maskRoi); (*_gpuSurf.get())(imgGpu, maskGpu, keypoints); #endif - } - else - { - _surf->detect(imgRoi, keypoints, maskRoi); - } + } + else + { + cv::UMat imgRoiU; + imgRoi.copyTo(imgRoiU); + + cv::UMat maskRoiU; + if (!maskRoi.empty()) + { + maskRoi.copyTo(maskRoiU); + } + + _surf->detect(imgRoiU, keypoints, maskRoiU); + } #else UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); #endif @@ -1213,7 +1227,13 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vectorcompute(image, keypoints, descriptors); + cv::UMat imageU; + image.copyTo(imageU); + + cv::UMat descriptorsU; + _surf->compute(imageU, keypoints, descriptorsU); + + descriptorsU.copyTo(descriptors); } #else UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 53446f11f3..7222df7dd2 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -123,7 +123,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) #endif } -std::map MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections) +std::map MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, const cv::UMat & depth, float * markerLengthOut, cv::UMat * imageWithDetections) { std::map detections; std::map infos = detect(image, model, depth, std::map(), imageWithDetections); @@ -141,27 +141,28 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam return detections; } -std::map MarkerDetector::detect(const cv::Mat & image, +std::map MarkerDetector::detect(const cv::UMat & image, const std::vector & models, - const cv::Mat & depth, + const cv::UMat & depth, const std::map & markerLengths, - cv::Mat * imageWithDetections) + cv::UMat * imageWithDetections) { UASSERT(!models.empty() && !image.empty()); UASSERT(int((image.cols/models.size())*models.size()) == image.cols); UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols); + UWARN("entering thisss part"); int subRGBWidth = image.cols/models.size(); int subDepthWidth = depth.cols/models.size(); std::map allInfo; for(size_t i=0; i subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0); if(ULogger::level() >= ULogger::kWarning) { @@ -182,22 +183,22 @@ std::map MarkerDetector::detect(const cv::Mat & image, { if(i==0) { - *imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type()); + *imageWithDetections = cv::UMat(image.size(), subImageWithDetections.type()); } if(!subImageWithDetections.empty()) { - subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); + subImageWithDetections.copyTo(cv::UMat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); } } } return allInfo; } -std::map MarkerDetector::detect(const cv::Mat & image, +std::map MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, - const cv::Mat & depth, + const cv::UMat & depth, const std::map & markerLengths, - cv::Mat * imageWithDetections) + cv::UMat * imageWithDetections) { if(!image.empty() && image.cols != model.imageWidth()) { @@ -246,11 +247,13 @@ std::map MarkerDetector::detect(const cv::Mat & image, std::map::const_iterator findIter = markerLengths.find(ids[i]); if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) { - float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); - float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); - float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); - float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); - float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); + cv::Mat depthMat; + depth.copyTo(depthMat); + float d = util2d::getDepth(depthMat, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); + float d1 = util2d::getDepth(depthMat, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); + float d2 = util2d::getDepth(depthMat, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); + float d3 = util2d::getDepth(depthMat, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); + float d4 = util2d::getDepth(depthMat, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); // Accept measurement only if all 4 depth values are valid and // they are at the same depth (camera should be perpendicular to marker for // best depth estimation) @@ -363,9 +366,12 @@ std::map MarkerDetector::detect(const cv::Mat & image, if(imageWithDetections) { + cv::Mat detections_img; + imageWithDetections->copyTo(detections_img); + UWARN("entering this part"); if(image.channels()==1) { - cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR); + cv::cvtColor(image, detections_img, cv::COLOR_GRAY2BGR); } else { diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 590d4cb00b..9ae4d0f344 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -5546,7 +5546,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(!models.empty() && models[0].isValidForProjection()) { - std::map markers = _markerDetector->detect(data.imageRaw(), models, data.depthRaw(), _landmarksSize); + cv::UMat imageRaw; + cv::UMat depthRaw; + data.depthRaw().copyTo(depthRaw); + data.imageRaw().copyTo(imageRaw); + std::map markers = _markerDetector->detect(imageRaw, models, depthRaw, _landmarksSize); for(std::map::iterator iter=markers.begin(); iter!=markers.end(); ++iter) { diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 513bb1d8ef..e75d314f73 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -656,6 +656,10 @@ Transform OdometryF2M::computeTransform( // add points without depth only if the local map has reached its maximum size bool addPointsWithoutDepth = false; + UDEBUG("visDepthAsMask = %s, validDepthRatio_ = %f, lastFrame_->getWords3().size() = %d", + (visDepthAsMask ? "true" : "false"), + validDepthRatio_, + (int)lastFrame_->getWords3().size()); if(!visDepthAsMask && validDepthRatio_ < 1.0f && !lastFrame_->getWords3().empty()) { int ptsWithDepth = 0; diff --git a/corelib/src/superpoint_torch/SuperPoint.cc b/corelib/src/superpoint_torch/SuperPoint.cc index e7e7f5b177..ba9568f1ab 100644 --- a/corelib/src/superpoint_torch/SuperPoint.cc +++ b/corelib/src/superpoint_torch/SuperPoint.cc @@ -139,7 +139,7 @@ SPDetector::~SPDetector() { } -std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & mask) +std::vector SPDetector::detect(const cv::UMat &img, const cv::UMat & mask) { UASSERT(img.type() == CV_8UC1); UASSERT(mask.empty() || (mask.type() == CV_8UC1 && img.cols == mask.cols && img.rows == mask.rows)); @@ -206,20 +206,20 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & } } -cv::Mat SPDetector::compute(const std::vector &keypoints) +cv::UMat SPDetector::compute(const std::vector &keypoints) { if(!detected_) { UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute()."); - return cv::Mat(); + return cv::UMat(); } if(keypoints.empty()) { - return cv::Mat(); + return cv::UMat(); } if(model_.get()) { - cv::Mat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x) + cv::UMat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x) // Based on sample_descriptors() of SuperPoint implementation in SuperGlue: // https://github.com/magicleap/SuperGluePretrainedNetwork/blob/45a750e5707696da49472f1cad35b0b203325417/models/superpoint.py#L80-L92 @@ -249,14 +249,14 @@ cv::Mat SPDetector::compute(const std::vector &keypoints) if(cuda_) desc = desc.to(torch::kCPU); - cv::Mat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data_ptr()); + cv::UMat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data_ptr()); return desc_mat.clone(); } else { UERROR("No model is loaded!"); - return cv::Mat(); + return cv::UMat(); } } diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 3f21403e0c..e6d0d8ef81 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -151,9 +151,15 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) if(!models.empty() && models[0].isValidForProjection()) { - cv::Mat imageWithDetections; - detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map(), &imageWithDetections); - imageView_->setImage(uCvMat2QImage(imageWithDetections)); + cv::UMat imageWithDetections; + cv::Mat tmp; + cv::UMat raw; + cv::UMat depth_raw; + data.imageRaw().copyTo(raw); + data.depthRaw().copyTo(depth_raw); + detections = markerDetector_->detect(raw, models, depth_raw, std::map(), &imageWithDetections); + imageWithDetections.copyTo(tmp); + imageView_->setImage(uCvMat2QImage(tmp)); } else { From 629e442912a854001cbafe58a96c4cea260e21c7 Mon Sep 17 00:00:00 2001 From: Sofiia Sarana Date: Mon, 19 May 2025 10:11:05 -0400 Subject: [PATCH 4/5] fix CI errors, remove unnecessary logs --- corelib/src/Features2d.cpp | 7 ++-- corelib/src/MarkerDetector.cpp | 48 +++++++++++++--------------- corelib/src/odometry/OdometryF2M.cpp | 1 - package.xml | 2 +- 4 files changed, 27 insertions(+), 31 deletions(-) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index cb909d11ba..ef99551b86 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -778,7 +778,7 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co { if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1) { - mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1); + maskMat = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1); for(int i=0; i<(int)maskMat.total(); ++i) { float value = 0.0f; @@ -835,7 +835,7 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co { cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize); std::vector subKeypoints; - subKeypoints = this->generateKeypointsImpl(image, roi, mask); + subKeypoints = this->generateKeypointsImpl(image, roi, maskMat); if (this->getType() != Feature2D::Type::kFeaturePyDetector) { limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC()); @@ -853,7 +853,7 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co } } UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)", - timer.ticks(), keypoints.size(), gridCols_, gridRows_, mask.empty()?1:0); + timer.ticks(), keypoints.size(), gridCols_, gridRows_, maskMat.empty()?1:0); if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0) { @@ -1145,7 +1145,6 @@ void SURF::parseParameters(const ParametersMap & parameters) { #if CV_MAJOR_VERSION < 3 _surf = cv::Ptr(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_)); - std::cout << "using cpu" << std::endl; #else _surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_); #endif diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 7222df7dd2..3f3802b533 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -123,7 +123,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) #endif } -std::map MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, const cv::UMat & depth, float * markerLengthOut, cv::UMat * imageWithDetections) +std::map MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, const cv::UMat & depth, float * markerLengthOut, cv::UMat * imageWithDetections) { std::map detections; std::map infos = detect(image, model, depth, std::map(), imageWithDetections); @@ -141,28 +141,27 @@ std::map MarkerDetector::detect(const cv::UMat & image, const Ca return detections; } -std::map MarkerDetector::detect(const cv::UMat & image, +std::map MarkerDetector::detect(const cv::UMat & image, const std::vector & models, - const cv::UMat & depth, + const cv::UMat & depth, const std::map & markerLengths, - cv::UMat * imageWithDetections) + cv::UMat * imageWithDetections) { UASSERT(!models.empty() && !image.empty()); UASSERT(int((image.cols/models.size())*models.size()) == image.cols); UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols); - UWARN("entering thisss part"); int subRGBWidth = image.cols/models.size(); int subDepthWidth = depth.cols/models.size(); std::map allInfo; for(size_t i=0; i subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0); if(ULogger::level() >= ULogger::kWarning) { @@ -183,22 +182,22 @@ std::map MarkerDetector::detect(const cv::UMat & image, { if(i==0) { - *imageWithDetections = cv::UMat(image.size(), subImageWithDetections.type()); + *imageWithDetections = cv::UMat(image.size(), subImageWithDetections.type()); } if(!subImageWithDetections.empty()) { - subImageWithDetections.copyTo(cv::UMat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); + subImageWithDetections.copyTo(cv::UMat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); } } } return allInfo; } -std::map MarkerDetector::detect(const cv::UMat & image, +std::map MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, - const cv::UMat & depth, + const cv::UMat & depth, const std::map & markerLengths, - cv::UMat * imageWithDetections) + cv::UMat * imageWithDetections) { if(!image.empty() && image.cols != model.imageWidth()) { @@ -247,13 +246,13 @@ std::map MarkerDetector::detect(const cv::UMat & image, std::map::const_iterator findIter = markerLengths.find(ids[i]); if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) { - cv::Mat depthMat; - depth.copyTo(depthMat); - float d = util2d::getDepth(depthMat, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); - float d1 = util2d::getDepth(depthMat, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); - float d2 = util2d::getDepth(depthMat, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); - float d3 = util2d::getDepth(depthMat, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); - float d4 = util2d::getDepth(depthMat, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); + cv::Mat depthMat; + depth.copyTo(depthMat); + float d = util2d::getDepth(depthMat, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); + float d1 = util2d::getDepth(depthMat, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); + float d2 = util2d::getDepth(depthMat, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); + float d3 = util2d::getDepth(depthMat, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); + float d4 = util2d::getDepth(depthMat, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); // Accept measurement only if all 4 depth values are valid and // they are at the same depth (camera should be perpendicular to marker for // best depth estimation) @@ -366,12 +365,11 @@ std::map MarkerDetector::detect(const cv::UMat & image, if(imageWithDetections) { - cv::Mat detections_img; - imageWithDetections->copyTo(detections_img); - UWARN("entering this part"); + cv::Mat detections_img; + imageWithDetections->copyTo(detections_img); if(image.channels()==1) { - cv::cvtColor(image, detections_img, cv::COLOR_GRAY2BGR); + cv::cvtColor(image, detections_img, cv::COLOR_GRAY2BGR); } else { diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index e75d314f73..e19b8e55aa 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -1266,7 +1266,6 @@ Transform OdometryF2M::computeTransform( map_->setWords(words, wordsKpts, transformedPoints, descriptors); addKeyFrame = true; - descriptors.release(); } else { diff --git a/package.xml b/package.xml index ac9de0bc8c..686465d51d 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,7 @@ libsqlite3-dev liboctomap-dev - grid_map_core + grid_map_core till this PR is released https://github.com/ANYbotics/grid_map/pull/499 qt_gui_cpp zlib From c1d1c8700a07fdfa98d57b868d08d6d6099b6539 Mon Sep 17 00:00:00 2001 From: Sofiia Sarana Date: Mon, 19 May 2025 10:12:53 -0400 Subject: [PATCH 5/5] temporary remove grid_map_core from package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 686465d51d..0d854e3829 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,7 @@ libsqlite3-dev liboctomap-dev - grid_map_core till this PR is released https://github.com/ANYbotics/grid_map/pull/499 + qt_gui_cpp zlib