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 5aa67230ef..ef99551b86 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) + 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,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 @@ -830,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()); @@ -848,13 +853,13 @@ 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) { 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 ) ); @@ -1172,11 +1177,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 @@ -1212,7 +1226,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..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::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,11 +141,11 @@ 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); @@ -156,12 +156,12 @@ std::map MarkerDetector::detect(const cv::Mat & image, 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 +182,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 +246,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 +365,11 @@ std::map 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 { 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 6239a8d0aa..e19b8e55aa 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; @@ -876,7 +880,6 @@ Transform OdometryF2M::computeTransform( ++iter; } } - if(mapWords.size() != mapPoints.size()) { UDEBUG("Remove points"); 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 { diff --git a/package.xml b/package.xml index 8e5f32aef2..0d854e3829 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 - + qt_gui_cpp zlib