Add OpenCL GPU implementation

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<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

corelib/src/Features2d.cpp

@@ -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"
@@ -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)
+			maskMat = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
+			for(int i=0; i<(int)maskMat.total(); ++i)
 			{
 				float value = 0.0f;
 				if(maskIn.type()==CV_16UC1)
@@ -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
@@ -830,7 +835,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 		{
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
 			std::vector<cv::KeyPoint> 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());
@@ -848,13 +853,13 @@ std::vector<cv::KeyPoint> 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)
 	{
 		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 ) );
@@ -1172,11 +1177,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);
+    }
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
@@ -1212,7 +1226,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!");

corelib/src/MarkerDetector.cpp

@@ -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);
@@ -141,11 +141,11 @@ 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);
@@ -156,12 +156,12 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 	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)
 		{
@@ -182,22 +182,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())
 	{
@@ -246,11 +246,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);
+                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 +365,11 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 
 	if(imageWithDetections)
 	{
+		cv::Mat detections_img;
+		imageWithDetections->copyTo(detections_img);
 		if(image.channels()==1)
 		{
-			cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR);
+			cv::cvtColor(image, detections_img, cv::COLOR_GRAY2BGR);
 		}
 		else
 		{

corelib/src/Memory.cpp

@@ -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)
 				{

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;
@@ -876,7 +880,6 @@ Transform OdometryF2M::computeTransform(
 								++iter;
 							}
 						}
-
 						if(mapWords.size() != mapPoints.size())
 						{
 							UDEBUG("Remove points");

corelib/src/superpoint_torch/SuperPoint.cc

@@ -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));
@@ -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
@@ -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();
 	}
 }
 

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<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
 		{

package.xml

@@ -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>
@@ -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 -->
   <depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
   <depend>zlib</depend>