Skip to content

Add OpenCL GPU implementation #1504

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.

Sign up for GitHub

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
sssarana wants to merge 5 commits into
base: humble-devel
Choose a base branch
from
Open

Add OpenCL GPU implementation #1504

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0fd591b
add grid_map_core dependency
sssarana May 12, 2025
cff65db
fix build with NONFREE features on humble-devel
sssarana May 12, 2025
6d9ccfd
add OpenCL GPU implementation
sssarana May 13, 2025
629e442
fix CI errors, remove unnecessary logs
sssarana May 19, 2025
c1d1c87
temporary remove grid_map_core from package.xml
sssarana May 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions corelib/include/rtabmap/core/MarkerDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int, MarkerInfo> detect(const cv::Mat & image,
std::map<int, MarkerInfo> detect(const cv::UMat & image,
const std::vector<CameraModel> & models,
const cv::Mat & depth = cv::Mat(),
const cv::UMat & depth = cv::UMat(),
const std::map<int, float> & markerLengths = std::map<int, float>(),
cv::Mat * imageWithDetections = 0);
cv::UMat * imageWithDetections = 0);

std::map<int, MarkerInfo> detect(const cv::Mat & image,
std::map<int, MarkerInfo> detect(const cv::UMat & image,
const CameraModel & model,
const cv::Mat & depth = cv::Mat(),
const cv::UMat & depth = cv::UMat(),
const std::map<int, float> & markerLengths = std::map<int, float>(),
cv::Mat * imageWithDetections = 0);
cv::UMat * imageWithDetections = 0);

private:
#ifdef HAVE_OPENCV_ARUCO
Expand Down
47 changes: 34 additions & 13 deletions corelib/src/Features2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/version.hpp>
#include <opencv2/opencv_modules.hpp>
#include <opencv2/core/ocl.hpp>

#ifdef RTABMAP_ORB_OCTREE
#include "opencv/ORBextractor.h"
Expand Down Expand Up @@ -769,13 +770,16 @@ std::vector<cv::KeyPoint> 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)
Expand All @@ -795,29 +799,30 @@ std::vector<cv::KeyPoint> 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
{
UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
}
}

UASSERT(mask.empty() || (mask.cols == image.cols && mask.rows == image.rows));
UASSERT(maskU.empty() || (maskU.cols == imageU.cols && maskU.rows == imageU.rows));

std::vector<cv::KeyPoint> 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
Expand Down Expand Up @@ -854,7 +859,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
{
std::vector<cv::Point2f> 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 ) );
Expand Down Expand Up @@ -1140,6 +1145,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
{
#if CV_MAJOR_VERSION < 3
_surf = cv::Ptr<CV_SURF>(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_));
std::cout << "using cpu" << std::endl;
Comment thread
matlabbe marked this conversation as resolved.
Outdated
#else
_surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
#endif
Expand Down Expand Up @@ -1172,11 +1178,20 @@ std::vector<cv::KeyPoint> 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);
}
Comment thread
matlabbe marked this conversation as resolved.
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif
Expand Down Expand Up @@ -1212,7 +1227,13 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
}
else
{
_surf->compute(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!");
Expand Down
44 changes: 25 additions & 19 deletions corelib/src/MarkerDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
#endif
}

std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
std::map<int, Transform> MarkerDetector::detect(const cv::UMat & image, const CameraModel & model, const cv::UMat & depth, float * markerLengthOut, cv::UMat * imageWithDetections)
{
std::map<int, Transform> detections;
std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
Expand All @@ -141,27 +141,28 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
return detections;
}

std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::UMat & image,
const std::vector<CameraModel> & models,
const cv::Mat & depth,
const cv::UMat & depth,
const std::map<int, float> & 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<int, MarkerInfo> allInfo;
for(size_t i=0; i<models.size(); ++i)
{
cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
cv::Mat subDepth;
cv::UMat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
cv::UMat subDepth;
if(!depth.empty())
subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
subDepth = cv::UMat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
CameraModel model = models[i];
cv::Mat subImageWithDetections;
cv::UMat subImageWithDetections;
std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
if(ULogger::level() >= ULogger::kWarning)
{
Expand All @@ -182,22 +183,22 @@ std::map<int, MarkerInfo> 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<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::UMat & image,
const CameraModel & model,
const cv::Mat & depth,
const cv::UMat & depth,
const std::map<int, float> & markerLengths,
cv::Mat * imageWithDetections)
cv::UMat * imageWithDetections)
{
if(!image.empty() && image.cols != model.imageWidth())
{
Expand Down Expand Up @@ -246,11 +247,13 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
std::map<int, float>::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);
Copy link
Copy Markdown
Member

@matlabbe matlabbe May 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
depth.copyTo(depthMat);
depthMat = depth.getMat(cv::ACCESS_READ);

copy only if needed

Copy link
Copy Markdown
Author

@sssarana sssarana May 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The depthMat = depth.getMat(cv::ACCESS_READ); caused some lifecycle issues for me earlier, so I wouldn't trust it. The reason for that is the original object that we create a shallow copy from gets deleted, so the depthMat gets deleted also.

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)
Expand Down Expand Up @@ -363,9 +366,12 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,

if(imageWithDetections)
{
cv::Mat detections_img;
imageWithDetections->copyTo(detections_img);
UWARN("entering this part");
Comment thread
matlabbe marked this conversation as resolved.
Outdated
if(image.channels()==1)
{
cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR);
cv::cvtColor(image, detections_img, cv::COLOR_GRAY2BGR);
}
else
{
Expand Down
6 changes: 5 additions & 1 deletion corelib/src/Memory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5546,7 +5546,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

if(!models.empty() && models[0].isValidForProjection())
{
std::map<int, MarkerInfo> 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<int, MarkerInfo> markers = _markerDetector->detect(imageRaw, models, depthRaw, _landmarksSize);

for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
{
Expand Down
6 changes: 5 additions & 1 deletion corelib/src/odometry/OdometryF2M.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -876,7 +880,6 @@ Transform OdometryF2M::computeTransform(
++iter;
}
}

if(mapWords.size() != mapPoints.size())
{
UDEBUG("Remove points");
Expand Down Expand Up @@ -1263,6 +1266,7 @@ Transform OdometryF2M::computeTransform(

map_->setWords(words, wordsKpts, transformedPoints, descriptors);
addKeyFrame = true;
descriptors.release();
Comment thread
matlabbe marked this conversation as resolved.
Outdated
}
else
{
Expand Down
14 changes: 7 additions & 7 deletions corelib/src/superpoint_torch/SuperPoint.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ SPDetector::~SPDetector()
{
}

std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat & mask)
std::vector<cv::KeyPoint> 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));
Expand Down Expand Up @@ -206,20 +206,20 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
}
}

cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
cv::UMat SPDetector::compute(const std::vector<cv::KeyPoint> &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
Expand Down Expand Up @@ -249,14 +249,14 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &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<float>());
cv::UMat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data_ptr<float>());

return desc_mat.clone();
}
else
{
UERROR("No model is loaded!");
return cv::Mat();
return cv::UMat();
}
}

Expand Down
12 changes: 9 additions & 3 deletions guilib/src/CameraViewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int, float>(), &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<int, float>(), &imageWithDetections);
imageWithDetections.copyTo(tmp);
imageView_->setImage(uCvMat2QImage(tmp));
}
else
{
Expand Down
3 changes: 1 addition & 2 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
<url type="website">http://introlab.github.io/rtabmap</url>
<url type="bugtracker">https://github.com/introlab/rtabmap/issues</url>
<url type="repository">https://github.com/introlab/rtabmap</url>

<buildtool_depend>cmake</buildtool_depend>

<build_depend>proj</build_depend>
Expand All @@ -22,7 +21,7 @@
<!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
<depend>libsqlite3-dev</depend>
<depend>liboctomap-dev</depend>
<!-- <depend>grid_map_core</depend> --> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
<depend>grid_map_core</depend> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
Copy link
Copy Markdown
Member

@matlabbe matlabbe May 18, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we still need to wait for ANYbotics/grid_map#519 to be released, we may change the comment to

Suggested change
<depend>grid_map_core</depend> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
<!-- <depend>grid_map_core</depend> --> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/519 -->

Note that dependency in package.xml is useful only if you use rosdep to install dependencies, the workaround if you want grid_map_core support locally is to install explicitly grid_map_core and cmake should be able to find it.

Copy link
Copy Markdown
Author

@sssarana sssarana May 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I created a PR to fix grid_map_core here: ANYbotics/grid_map#521, but it didn't get reviewed yet. Though I think you can apply these fixes yourself if you want to test everything properly.

<depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
<depend>zlib</depend>

Expand Down
Loading