From 172e9857c7b4907416910ab9aa090027b80f14f8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Feb 2016 12:45:00 -0500 Subject: [PATCH] Fixed RegistrationVis feature matching behavior when NaN 3D keypoints are found --- corelib/include/rtabmap/core/Parameters.h | 4 +- corelib/src/OdometryF2M.cpp | 121 +++++----- corelib/src/RegistrationVis.cpp | 258 +++++++++++++--------- guilib/src/ui/preferencesDialog.ui | 10 +- utilite/include/rtabmap/utilite/UMath.h | 2 +- 5 files changed, 231 insertions(+), 164 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index ba6f6631..dc0a52e1 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -254,7 +254,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes."); - RTABMAP_PARAM(FAST, Threshold, int, 30, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel."); + RTABMAP_PARAM(FAST, Threshold, int, 10, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel."); RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true, "If true, non-maximum suppression is applied to detected corners (keypoints)."); RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected."); RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU."); @@ -391,7 +391,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); RTABMAP_PARAM(Vis, CorNNType, int, 3, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach."); RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach."); - RTABMAP_PARAM(Vis, CorGuessWinSize, int, 16, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences."); + RTABMAP_PARAM(Vis, CorGuessWinSize, int, 0, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled."); RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp index a34e5c2b..77d5a680 100644 --- a/corelib/src/OdometryF2M.cpp +++ b/corelib/src/OdometryF2M.cpp @@ -155,7 +155,8 @@ void OdometryF2M::reset(const Transform & initialPose) if(fixedMapPath_.empty()) { Odometry::reset(initialPose); - map_->sensorData() = SensorData(); + *map_ = Signature(-1); + *lastFrame_ = Signature(1); } else { @@ -188,7 +189,8 @@ Transform OdometryF2M::computeTransform( if(map_->getWords3().size() && lastFrame_->sensorData().isValid()) { Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform(); - Transform transform = regVis_->computeTransformationMod(*map_, *lastFrame_, guess, ®Info); + Signature tmpMap = *map_; + Transform transform = regVis_->computeTransformationMod(tmpMap, *lastFrame_, guess, ®Info); data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors()); @@ -206,62 +208,66 @@ Transform OdometryF2M::computeTransform( UWARN("Unknown registration error"); } - if(fixedMapPath_.empty()) + if(!transform.isNull()) { - output = transform; - - int added = 0; - int removed = 0; - - // update local map - std::multimap mapPoints = map_->getWords3(); - std::multimap mapDescriptors = map_->getWordsDescriptors(); - Transform t = this->getPose()*output; - UASSERT(mapPoints.size() == mapDescriptors.size()); - UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size()); - std::list newIds = uUniqueKeys(lastFrame_->getWordsDescriptors()); - for(std::list::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter) + if(fixedMapPath_.empty()) { - if(mapPoints.find(*iter) == mapPoints.end()) - { - mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t))); - mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second)); - ++added; - } - } + output = transform; - // remove words in map if max size is reached - if((int)mapPoints.size() > maximumMapSize_) - { - // remove oldest first, keep matched features - std::set matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end()); - std::multimap::iterator iterMapWords = mapDescriptors.begin(); - for(std::multimap::iterator iter = mapPoints.begin(); - iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();) + int added = 0; + int removed = 0; + + // update local map + *map_ = tmpMap; + std::multimap mapPoints = map_->getWords3(); + std::multimap mapDescriptors = map_->getWordsDescriptors(); + Transform t = this->getPose()*output; + UASSERT(mapPoints.size() == mapDescriptors.size()); + UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size()); + std::list newIds = uUniqueKeys(lastFrame_->getWordsDescriptors()); + for(std::list::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter) { - if(matches.find(iter->first) == matches.end()) + if(mapPoints.find(*iter) == mapPoints.end() && util3d::isFinite(lastFrame_->getWords3().find(*iter)->second)) { - iter = mapPoints.erase(iter); - iterMapWords = mapDescriptors.erase(iterMapWords); - ++removed; - } - else - { - ++iter; - ++iterMapWords; + mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t))); + mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second)); + ++added; } } + + // remove words in map if max size is reached + if((int)mapPoints.size() > maximumMapSize_) + { + // remove oldest first, keep matched features + std::set matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end()); + std::multimap::iterator iterMapWords = mapDescriptors.begin(); + for(std::multimap::iterator iter = mapPoints.begin(); + iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();) + { + if(matches.find(iter->first) == matches.end()) + { + iter = mapPoints.erase(iter); + iterMapWords = mapDescriptors.erase(iterMapWords); + ++removed; + } + else + { + ++iter; + ++iterMapWords; + } + } + } + + map_->setWords3(mapPoints); + map_->setWordsDescriptors(mapDescriptors); + + UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size()); + } + else + { + // fixed local map, don't update with the new signature + output = transform; } - - map_->setWords3(mapPoints); - map_->setWordsDescriptors(mapDescriptors); - - UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size()); - } - else - { - // fixed local map, don't update with the new signature - output = transform; } } else @@ -282,13 +288,22 @@ Transform OdometryF2M::computeTransform( Transform t = this->getPose(); // initial pose may be not identity... std::multimap transformedPoints; - for(std::multimap::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter) + std::multimap descriptors; + UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size()); + std::multimap::const_iterator descIter = lastFrame_->getWordsDescriptors().begin(); + for(std::multimap::const_iterator iter = lastFrame_->getWords3().begin(); + iter!=lastFrame_->getWords3().end(); + ++iter,++descIter) { - transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t))); + if(util3d::isFinite(iter->second)) + { + transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t))); + descriptors.insert(std::make_pair(iter->first, descIter->second)); + } } map_->setWords3(transformedPoints); - map_->setWordsDescriptors(lastFrame_->getWordsDescriptors()); + map_->setWordsDescriptors(descriptors); map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels()); map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel()); } diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index 80a98994..7dd979e9 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -430,13 +430,13 @@ Transform RegistrationVis::computeTransformationImpl( } cv::Mat depthMask; - if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty()) + if(_useDepthAsMask && !toSignature.sensorData().depthRaw().empty()) { - if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 && - fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 && - fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols) + if(toSignature.sensorData().imageRaw().rows % toSignature.sensorData().depthRaw().rows == 0 && + toSignature.sensorData().imageRaw().cols % toSignature.sensorData().depthRaw().cols == 0 && + toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows == toSignature.sensorData().imageRaw().cols/toSignature.sensorData().depthRaw().cols) { - depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f); + depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows, 0.1f); } } @@ -540,146 +540,198 @@ Transform RegistrationVis::computeTransformationImpl( { // If guess is set, limit the search of matches using optical flow window size bool guessSet = !guess.isIdentity() && !guess.isNull(); - if(guessSet && _guessWinSize > 0) + if(guessSet && _guessWinSize > 0 && kptsFrom3D.size()) { UDEBUG(""); UASSERT((int)kptsTo.size() == descriptorsTo.rows); + UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows); // Use guess to project 3D "from" keypoints into "to" image - std::vector cornersProjected; - if(kptsFrom3D.size() && (guessSet || kptsFrom.size()==0)) + if(fromSignature.sensorData().cameraModels().size() > 1) { - if(fromSignature.sensorData().cameraModels().size() > 1) + UFATAL("Radius feature matching is not supported for multiple cameras."); + } + Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform(); + Transform guessCameraRef = (guess * localTransform).inverse(); + cv::Mat R = (cv::Mat_(3,3) << + (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(), + (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(), + (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33()); + cv::Mat rvec(1,3, CV_64FC1); + cv::Rodrigues(R, rvec); + cv::Mat tvec = (cv::Mat_(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z()); + cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K(); + std::vector projected; + cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected); + + /*UDEBUG("guess=%s", guess.prettyPrint().c_str()); + std::vector projectedKpts; + cv::KeyPoint::convert(projected, projectedKpts); + cv::Mat image = toSignature.sensorData().imageRaw().clone(); + drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0)); + drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255)); + cv::imwrite("projected.bmp", image); + UWARN("saved projected.bmp");*/ + + //remove projected points outside of the image + UASSERT((int)projected.size() == descriptorsFrom.rows); + std::vector cornersProjected(projected.size()); + std::vector projectedIndexToDescIndex(projected.size()); + int oi=0; + for(unsigned int i=0; i(3,3) << - (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(), - (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(), - (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33()); - cv::Mat rvec(1,3, CV_64FC1); - cv::Rodrigues(R, rvec); - cv::Mat tvec = (cv::Mat_(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z()); - cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K(); - cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersProjected); } - else if(kptsFrom.size()) - { - cv::KeyPoint::convert(kptsFrom, cornersProjected); - } - UDEBUG(""); + projectedIndexToDescIndex.resize(oi); + cornersProjected.resize(oi); + + + + UDEBUG("cornersProjected=%d", (int)cornersProjected.size()); // For each projected feature guess of "from" in "to", find its matching feature in // the radius around the projected guess. // TODO: do cross-check? if(cornersProjected.size()) { - // Create kd-tree for keypoints "to" - std::vector pointsTo; - cv::KeyPoint::convert(kptsTo, pointsTo); - rtflann::Matrix pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2); - rtflann::Index > index(pointsToMat, rtflann::KDTreeIndexParams()); + + // Create kd-tree for projected keypoints + rtflann::Matrix cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2); + rtflann::Index > index(cornersProjectedMat, rtflann::KDTreeIndexParams()); index.buildIndex(); std::vector< std::vector > indices; std::vector > dists; float radius = (float)_guessWinSize; // pixels - rtflann::Matrix cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2); - index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams()); + std::vector pointsTo; + cv::KeyPoint::convert(kptsTo, pointsTo); + rtflann::Matrix pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2); + index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams()); - UASSERT(indices.size() == cornersProjectedMat.rows); + UASSERT(indices.size() == pointsToMat.rows); UASSERT(descriptorsFrom.cols == descriptorsTo.cols); - UASSERT((int)cornersProjectedMat.rows == descriptorsFrom.rows); - UASSERT(kptsFrom.empty() || cornersProjectedMat.rows == kptsFrom.size()); - UASSERT(kptsFrom3D.empty() || cornersProjectedMat.rows == kptsFrom3D.size()); - + UASSERT(kptsFrom.empty() || descriptorsFrom.rows == (int)kptsFrom.size()); + UASSERT((int)pointsToMat.rows == descriptorsTo.rows); + UASSERT(pointsToMat.rows == kptsTo.size()); UDEBUG(""); // Process results (Nearest Neighbor Distance Ratio) - int notMatchedUniqueId = cornersProjectedMat.rows; - std::set addedWordsTo; - for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i) + int notToMatchedUniqueId = descriptorsFrom.rows+descriptorsTo.rows; // make sure new words from "to" are after older one of "from" + int notFromMatchedUniqueId = descriptorsTo.rows; + std::map addedWordsFrom; // + std::set duplicates; + int newWords = 0; + for(unsigned int i = 0; i < pointsToMat.rows; ++i) { - int matchedIndex = -1; - if(indices[i].size() >= 2) + if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i])) { - cv::Mat descriptors(indices[i].size(), descriptorsTo.cols, descriptorsTo.type()); - for(unsigned int j=0; j= 2) { - descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(j)); - addedWordsTo.insert(indices[i].at(j)); + cv::Mat descriptors(indices[i].size(), descriptorsFrom.cols, descriptorsFrom.type()); + for(unsigned int j=0; j > matches; + cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2); + UASSERT(matches.size() == 1); + UASSERT(matches[0].size() == 2); + if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance) + { + matchedIndex = indices[i].at(matches[0].at(0).trainIdx); + } + } + else if(indices[i].size() == 1) + { + matchedIndex = indices[i].at(0); } - std::vector > matches; - cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); - matcher.knnMatch(descriptorsFrom.row(i), descriptors, matches, 2); - UASSERT(matches.size() == 1); - UASSERT(matches[0].size() == 2); - if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance) + if(matchedIndex >= 0) { - matchedIndex = indices[i].at(matches[0].at(0).trainIdx); - } - } - else if(indices[i].size() == 1) - { - matchedIndex = indices[i].at(0); - } + matchedIndex = projectedIndexToDescIndex[matchedIndex]; + int id = i; - if(matchedIndex >= 0) - { - if(kptsFrom.size()) - { - wordsFrom.insert(std::make_pair(i, kptsFrom[i])); - } - if(kptsFrom3D.size()) - { - words3From.insert(std::make_pair(i, kptsFrom3D[i])); - } - wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i))); + if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end()) + { + id = addedWordsFrom.at(matchedIndex); + duplicates.insert(matchedIndex); + } + else + { + addedWordsFrom.insert(std::make_pair(matchedIndex, id)); - wordsTo.insert(std::make_pair(i, kptsTo[matchedIndex])); - wordsDescTo.insert(std::make_pair(i, descriptorsTo.row(matchedIndex))); - if(kptsTo3D.size()) - { - words3To.insert(std::make_pair(i, kptsTo3D[matchedIndex])); - } - } - else - { - // gen fake ids - if(kptsFrom.size()) - { - wordsFrom.insert(std::make_pair(notMatchedUniqueId, kptsFrom[i])); - } - if(kptsFrom3D.size()) - { - words3From.insert(std::make_pair(notMatchedUniqueId, kptsFrom3D[i])); - } - wordsDescFrom.insert(std::make_pair(notMatchedUniqueId, descriptorsFrom.row(i))); + if(kptsFrom.size()) + { + wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex])); + } + words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex])); + wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex))); + } - ++notMatchedUniqueId; + wordsTo.insert(std::make_pair(id, kptsTo[i])); + wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i))); + if(kptsTo3D.size()) + { + words3To.insert(std::make_pair(id, kptsTo3D[i])); + } + } + else + { + // gen fake ids + wordsTo.insert(std::make_pair(notToMatchedUniqueId, kptsTo[i])); + wordsDescTo.insert(std::make_pair(notToMatchedUniqueId, descriptorsTo.row(i))); + if(kptsTo3D.size()) + { + words3To.insert(std::make_pair(notToMatchedUniqueId, kptsTo3D[i])); + } + + ++notToMatchedUniqueId; + ++newWords; + } } } - UDEBUG("addedWordsTo=%d, kptsTo=%d, wordsTo=%d", (int)addedWordsTo.size(), (int)kptsTo.size(), (int)wordsTo.size()); + UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d", + (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords, + (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size()); - // create fake ids for not matched words from "to" - for(unsigned int i=0; i::iterator iter=duplicates.begin(); iter!=duplicates.end(); ++iter) { - if(addedWordsTo.find(i) == addedWordsTo.end()) - { - wordsTo.insert(std::make_pair(notMatchedUniqueId, kptsTo[i])); - wordsDescTo.insert(std::make_pair(notMatchedUniqueId, descriptorsTo.row(i))); - if(kptsTo3D.size()) - { - words3To.insert(std::make_pair(notMatchedUniqueId, kptsTo3D[i])); - } + wordsFrom.erase(*iter); + wordsDescFrom.erase(*iter); + words3From.erase(*iter); + wordsTo.erase(*iter); + wordsDescTo.erase(*iter); + words3To.erase(*iter); + } - ++notMatchedUniqueId; + // create fake ids for not matched words from "from" + int addWordsFromNotMatched = 0; + for(unsigned int i=0; i words3From=%d", addWordsFromNotMatched, (int)words3From.size()); } UDEBUG(""); } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f2f16433..cef2e5ac 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,8 +64,8 @@ 0 0 - 681 - 2010 + 686 + 1990 @@ -86,7 +86,7 @@ QFrame::Raised - 23 + 16 @@ -8429,7 +8429,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - Matching window size around projected points when a guess transform is provided to find correspondences. + Matching window size around projected points when a guess transform is provided to find correspondences. 0 means that global matching will be done. true @@ -8445,7 +8445,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare pixels - 3 + 0 1000 diff --git a/utilite/include/rtabmap/utilite/UMath.h b/utilite/include/rtabmap/utilite/UMath.h index db096131..f4c21e36 100644 --- a/utilite/include/rtabmap/utilite/UMath.h +++ b/utilite/include/rtabmap/utilite/UMath.h @@ -935,7 +935,7 @@ inline std::vector uHamming(unsigned int L) template bool uIsInBounds(const T& value, const T& low, const T& high) { - return !(value < low) && !(value >= high); + return uIsFinite(value) && !(value < low) && !(value >= high); } #endif // UMATH_H