i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed RegistrationVis feature matching behavior when NaN 3D keypoints are found

Browse Source
This commit is contained in:
matlabbe
2016-02-24 12:45:00 -05:00
parent 061211b1b6
commit 172e9857c7
5 changed files with 231 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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(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, 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, 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."); 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, 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, 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, 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, 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, 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."); RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");

121
corelib/src/OdometryF2M.cpp
View File

@@ -155,7 +155,8 @@ void OdometryF2M::reset(const Transform & initialPose)
if(fixedMapPath_.empty()) if(fixedMapPath_.empty())
{ {
Odometry::reset(initialPose); Odometry::reset(initialPose);
map_->sensorData() = SensorData(); *map_ = Signature(-1);
*lastFrame_ = Signature(1);
} }
else else
{ {
@@ -188,7 +189,8 @@ Transform OdometryF2M::computeTransform(
if(map_->getWords3().size() && lastFrame_->sensorData().isValid()) if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
{ {
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform(); Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Transform transform = regVis_->computeTransformationMod(*map_, *lastFrame_, guess, &regInfo); Signature tmpMap = *map_;
Transform transform = regVis_->computeTransformationMod(tmpMap, *lastFrame_, guess, &regInfo);
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors()); data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
@@ -206,62 +208,66 @@ Transform OdometryF2M::computeTransform(
UWARN("Unknown registration error"); UWARN("Unknown registration error");
} }
if(fixedMapPath_.empty()) if(!transform.isNull())
{ {
output = transform; if(fixedMapPath_.empty())
int added = 0;
int removed = 0;
// update local map
std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size());
std::list<int> newIds = uUniqueKeys(lastFrame_->getWordsDescriptors());
for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter)
{ {
if(mapPoints.find(*iter) == mapPoints.end()) output = transform;
{
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 int added = 0;
if((int)mapPoints.size() > maximumMapSize_) int removed = 0;
{
// remove oldest first, keep matched features // update local map
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end()); *map_ = tmpMap;
std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin(); std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin(); std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();) Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size());
std::list<int> newIds = uUniqueKeys(lastFrame_->getWordsDescriptors());
for(std::list<int>::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); mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t)));
iterMapWords = mapDescriptors.erase(iterMapWords); mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second));
++removed; ++added;
}
else
{
++iter;
++iterMapWords;
} }
} }
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin();
for(std::multimap<int, cv::Point3f>::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 else
@@ -282,13 +288,22 @@ Transform OdometryF2M::computeTransform(
Transform t = this->getPose(); // initial pose may be not identity... Transform t = this->getPose(); // initial pose may be not identity...
std::multimap<int, cv::Point3f> transformedPoints; std::multimap<int, cv::Point3f> transformedPoints;
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter) std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::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_->setWords3(transformedPoints);
map_->setWordsDescriptors(lastFrame_->getWordsDescriptors()); map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels()); map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel()); map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
} }

258
corelib/src/RegistrationVis.cpp
View File

@@ -430,13 +430,13 @@ Transform RegistrationVis::computeTransformationImpl(
} }
cv::Mat depthMask; 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 && if(toSignature.sensorData().imageRaw().rows % toSignature.sensorData().depthRaw().rows == 0 &&
fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 && toSignature.sensorData().imageRaw().cols % toSignature.sensorData().depthRaw().cols == 0 &&
fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols) 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 // If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull(); bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet && _guessWinSize > 0) if(guessSet && _guessWinSize > 0 && kptsFrom3D.size())
{ {
UDEBUG(""); UDEBUG("");
UASSERT((int)kptsTo.size() == descriptorsTo.rows); UASSERT((int)kptsTo.size() == descriptorsTo.rows);
UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
// Use guess to project 3D "from" keypoints into "to" image // Use guess to project 3D "from" keypoints into "to" image
std::vector<cv::Point2f> cornersProjected; if(fromSignature.sensorData().cameraModels().size() > 1)
if(kptsFrom3D.size() && (guessSet || kptsFrom.size()==0))
{ {
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_<double>(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_<double>(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<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
/*UDEBUG("guess=%s", guess.prettyPrint().c_str());
std::vector<cv::KeyPoint> 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<cv::Point2f> cornersProjected(projected.size());
std::vector<int> projectedIndexToDescIndex(projected.size());
int oi=0;
for(unsigned int i=0; i<projected.size(); ++i)
{
if(uIsInBounds(projected[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows-1)))
{ {
UFATAL("Radius feature matching is not supported for multiple cameras."); projectedIndexToDescIndex[oi] = i;
cornersProjected[oi++] = projected[i];
} }
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_<double>(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_<double>(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()) projectedIndexToDescIndex.resize(oi);
{ cornersProjected.resize(oi);
cv::KeyPoint::convert(kptsFrom, cornersProjected);
}
UDEBUG("");
UDEBUG("cornersProjected=%d", (int)cornersProjected.size());
// For each projected feature guess of "from" in "to", find its matching feature in // For each projected feature guess of "from" in "to", find its matching feature in
// the radius around the projected guess. // the radius around the projected guess.
// TODO: do cross-check? // TODO: do cross-check?
if(cornersProjected.size()) if(cornersProjected.size())
{ {
// Create kd-tree for keypoints "to"
std::vector<cv::Point2f> pointsTo; // Create kd-tree for projected keypoints
cv::KeyPoint::convert(kptsTo, pointsTo); rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2); rtflann::Index<rtflann::L2<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
rtflann::Index<rtflann::L2<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
index.buildIndex(); index.buildIndex();
std::vector< std::vector<size_t> > indices; std::vector< std::vector<size_t> > indices;
std::vector<std::vector<float> > dists; std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels float radius = (float)_guessWinSize; // pixels
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2); std::vector<cv::Point2f> pointsTo;
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams()); cv::KeyPoint::convert(kptsTo, pointsTo);
rtflann::Matrix<float> 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(descriptorsFrom.cols == descriptorsTo.cols);
UASSERT((int)cornersProjectedMat.rows == descriptorsFrom.rows); UASSERT(kptsFrom.empty() || descriptorsFrom.rows == (int)kptsFrom.size());
UASSERT(kptsFrom.empty() || cornersProjectedMat.rows == kptsFrom.size()); UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
UASSERT(kptsFrom3D.empty() || cornersProjectedMat.rows == kptsFrom3D.size()); UASSERT(pointsToMat.rows == kptsTo.size());
UDEBUG(""); UDEBUG("");
// Process results (Nearest Neighbor Distance Ratio) // Process results (Nearest Neighbor Distance Ratio)
int notMatchedUniqueId = cornersProjectedMat.rows; int notToMatchedUniqueId = descriptorsFrom.rows+descriptorsTo.rows; // make sure new words from "to" are after older one of "from"
std::set<int> addedWordsTo; int notFromMatchedUniqueId = descriptorsTo.rows;
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i) std::map<int,int> addedWordsFrom; //<id, index>
std::set<int> duplicates;
int newWords = 0;
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{ {
int matchedIndex = -1; if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
if(indices[i].size() >= 2)
{ {
cv::Mat descriptors(indices[i].size(), descriptorsTo.cols, descriptorsTo.type()); int matchedIndex = -1;
for(unsigned int j=0; j<indices[i].size(); ++j) if(indices[i].size() >= 2)
{ {
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(j)); cv::Mat descriptors(indices[i].size(), descriptorsFrom.cols, descriptorsFrom.type());
addedWordsTo.insert(indices[i].at(j)); for(unsigned int j=0; j<indices[i].size(); ++j)
{
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(j));
}
std::vector<std::vector<cv::DMatch> > 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<std::vector<cv::DMatch> > matches; if(matchedIndex >= 0)
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)
{ {
matchedIndex = indices[i].at(matches[0].at(0).trainIdx); matchedIndex = projectedIndexToDescIndex[matchedIndex];
} int id = i;
}
else if(indices[i].size() == 1)
{
matchedIndex = indices[i].at(0);
}
if(matchedIndex >= 0) if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{ {
if(kptsFrom.size()) id = addedWordsFrom.at(matchedIndex);
{ duplicates.insert(matchedIndex);
wordsFrom.insert(std::make_pair(i, kptsFrom[i])); }
} else
if(kptsFrom3D.size()) {
{ addedWordsFrom.insert(std::make_pair(matchedIndex, id));
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
}
wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
wordsTo.insert(std::make_pair(i, kptsTo[matchedIndex])); if(kptsFrom.size())
wordsDescTo.insert(std::make_pair(i, descriptorsTo.row(matchedIndex))); {
if(kptsTo3D.size()) wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
{ }
words3To.insert(std::make_pair(i, kptsTo3D[matchedIndex])); words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
} wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(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)));
++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" //remove duplicates
for(unsigned int i=0; i<kptsTo.size(); ++i) for(std::set<int>::iterator iter=duplicates.begin(); iter!=duplicates.end(); ++iter)
{ {
if(addedWordsTo.find(i) == addedWordsTo.end()) wordsFrom.erase(*iter);
{ wordsDescFrom.erase(*iter);
wordsTo.insert(std::make_pair(notMatchedUniqueId, kptsTo[i])); words3From.erase(*iter);
wordsDescTo.insert(std::make_pair(notMatchedUniqueId, descriptorsTo.row(i))); wordsTo.erase(*iter);
if(kptsTo3D.size()) wordsDescTo.erase(*iter);
{ words3To.erase(*iter);
words3To.insert(std::make_pair(notMatchedUniqueId, kptsTo3D[i])); }
}
++notMatchedUniqueId; // create fake ids for not matched words from "from"
int addWordsFromNotMatched = 0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom[i]));
}
wordsDescFrom.insert(std::make_pair(notFromMatchedUniqueId, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom3D[i]));
++notFromMatchedUniqueId;
++addWordsFromNotMatched;
} }
} }
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
} }
UDEBUG(""); UDEBUG("");
} }

10
guilib/src/ui/preferencesDialog.ui
View File

@@ -64,8 +64,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>681</width> <width>686</width>
<height>2010</height> <height>1990</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>23</number> <number>16</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8429,7 +8429,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item row="2" column="1"> <item row="2" column="1">
<widget class="QLabel" name="label_303"> <widget class="QLabel" name="label_303">
<property name="text"> <property name="text">
<string>Matching window size around projected points when a guess transform is provided to find correspondences.</string> <string>Matching window size around projected points when a guess transform is provided to find correspondences. 0 means that global matching will be done.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -8445,7 +8445,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<string> pixels</string> <string> pixels</string>
</property> </property>
<property name="minimum"> <property name="minimum">
<number>3</number> <number>0</number>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>1000</number> <number>1000</number>

2
utilite/include/rtabmap/utilite/UMath.h
View File

@@ -935,7 +935,7 @@ inline std::vector<float> uHamming(unsigned int L)
template <typename T> template <typename T>
bool uIsInBounds(const T& value, const T& low, const T& high) 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 #endif // UMATH_H