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

New parameter: OdomF2M/ValidDepthRatio

Browse Source
This commit is contained in:
matlabbe
2019-03-06 12:35:54 -05:00
parent d85c1c3bcf
commit cf4db63226
8 changed files with 292 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
corelib/src/RegistrationVis.cpp
View File

@@ -790,6 +790,7 @@ Transform RegistrationVis::computeTransformationImpl(
// For each projected feature guess of "from" in "to", find its matching feature in
// the radius around the projected guess.
// TODO: do cross-check?
UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size());
if(cornersProjected.size())
{
if(_guessMatchToProjection)
@@ -823,100 +824,97 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
int octave = kptsTo[i].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
int matchedIndex = -1;
if(indices[i].size() >= 2)
{
// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
int octave = kptsTo[i].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
int matchedIndex = -1;
if(indices[i].size() >= 2)
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
if((int)indices[i].size() > descriptors.rows)
{
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
if((int)indices[i].size() > descriptors.rows)
{
descriptors.resize(indices[i].size());
}
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(octaveFrom==octave)
{
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
}
}
descriptorsIndices.resize(oi);
if(oi >=2)
{
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), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(oi == 1)
{
matchedIndex = descriptorsIndices[0];
}
descriptors.resize(indices[i].size());
}
else if(indices[i].size() == 1)
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave & 255;
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(octaveFrom == octave)
if(octaveFrom==octave)
{
matchedIndex = indices[i].at(0);
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
}
}
if(matchedIndex >= 0)
descriptorsIndices.resize(oi);
if(oi >=2)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
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), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
id = addedWordsFrom.at(matchedIndex);
duplicates.insert(std::make_pair(matchedIndex, id));
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
else
{
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
}
else if(oi == 1)
{
matchedIndex = descriptorsIndices[0];
}
}
else if(indices[i].size() == 1)
{
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(octaveFrom == octave)
{
matchedIndex = indices[i].at(0);
}
}
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)));
}
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
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]));
}
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
id = addedWordsFrom.at(matchedIndex);
duplicates.insert(std::make_pair(matchedIndex, id));
}
else
{
// gen fake ids
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
}
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
++newToId;
++newWords;
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)));
}
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(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
}
++newToId;
++newWords;
}
}
UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
@@ -1048,8 +1046,7 @@ Transform RegistrationVis::computeTransformationImpl(
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
matchedIndexTo >= 0 &&
if( matchedIndexTo >= 0 &&
addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
{
addedWordsTo.insert(matchedIndexTo);
@@ -1353,6 +1350,7 @@ Transform RegistrationVis::computeTransformationImpl(
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
@@ -1394,6 +1392,7 @@ Transform RegistrationVis::computeTransformationImpl(
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
@@ -1460,6 +1459,7 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.sensorData().cameraModels().size() <= 1 &&
toSignature.sensorData().cameraModels().size() <= 1)
{
UDEBUG("Refine with bundle adjustment");
Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
std::map<int, Transform> poses;

145
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -68,6 +68,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
@@ -88,6 +89,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
}
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
UASSERT(bundleMaxFrames_ >= 0);
ParametersMap bundleParameters = parameters;
if(bundleAdjustment_ > 0)
@@ -201,6 +204,8 @@ Transform OdometryF2M::computeTransform(
int totalBundleWordReferencesUsed = 0;
int totalBundleOutliers = 0;
float bundleTime = 0.0f;
bool visDepthAsMask = Parameters::defaultVisDepthAsMask();
Parameters::parse(parameters_, Parameters::kVisDepthAsMask(), visDepthAsMask);
// Generate keypoints from the new data
if(lastFrame_->sensorData().isValid())
@@ -567,11 +572,35 @@ Transform OdometryF2M::computeTransform(
UFATAL("no valid camera model!");
}
}
// add points without depth only if the local map has reached its maximum size
bool addPointsWithoutDepth = false;
if(!visDepthAsMask && validDepthRatio_ < 1.0f)
{
int ptsWithDepth = 0;
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter)
{
if(util3d::isFinite(iter->second))
{
++ptsWithDepth;
}
}
float r = float(ptsWithDepth) / float(lastFrame_->getWords3().size());
addPointsWithoutDepth = r > validDepthRatio_;
if(!addPointsWithoutDepth)
{
UWARN("Not enough points with valid depth in current frame (%d/%d=%f < %s=%f), points without depth are not added to map.",
ptsWithDepth, (int)lastFrame_->getWords3().size(), r, Parameters::kOdomF2MValidDepthRatio().c_str(), validDepthRatio_);
}
}
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
{
if(util3d::isFinite(iter->second))
if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
{
if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
{
newIds.insert(
std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
@@ -579,31 +608,35 @@ Transform OdometryF2M::computeTransform(
std::make_pair(iter2D->second,
std::make_pair(iter->second, iterDesc->second)))));
}
else if(bundleAdjustment_>0)
}
else if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->first) == 1)
{
if(lastFrame_->getWords().count(iter->first) == 1)
std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
if(iterKpts!=mapWords.end())
{
std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
if(iterKpts!=mapWords.end())
{
iterKpts->second.octave = iter2D->second.octave;
}
iterKpts->second.octave = iter2D->second.octave;
}
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
}
//move back point in camera frame (to get depth along z)
float depth = 0.0f;
if(util3d::isFinite(iter->second))
{
depth = util3d::transformPoint(iter->second, invLocalTransform).z;
}
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
}
}
}
@@ -626,22 +659,61 @@ Transform OdometryF2M::computeTransform(
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
float depth = 0.0f;
if(util3d::isFinite(iter->second.second.second.first))
{
depth = util3d::transformPoint(iter->second.second.second.first, invLocalTransform).z;
}
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
}
}
}
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
cv::Point3f pt = iter->second.second.second.first;
if(!util3d::isFinite(pt))
{
// get the ray instead
float x = iter->second.second.first.pt.x;
float y = iter->second.second.first.pt.y;
float subImageWidth = lastFrame_->sensorData().imageRaw().cols;
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() > 1)
{
subImageWidth = lastFrame_->sensorData().imageRaw().cols/lastFrame_->sensorData().cameraModels().size();
int cameraIndex = int(x / subImageWidth);
model = lastFrame_->sensorData().cameraModels()[cameraIndex];
x = x-subImageWidth*cameraIndex;
}
else if(lastFrame_->sensorData().cameraModels().size() == 1)
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else
{
model = lastFrame_->sensorData().stereoCameraModel().left();
}
Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
model.imageSize(),
x,
y,
model.cx(),
model.cy(),
model.fx(),
model.fy());
float scaleInf = (0.05 * model.fx()) / 0.01;
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
}
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
if(lastFrameOldestNewId_ > iter->second.first)
{
@@ -941,7 +1013,18 @@ Transform OdometryF2M::computeTransform(
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{
if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
int ptsWithDepth = 0;
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter)
{
if(util3d::isFinite(iter->second))
{
++ptsWithDepth;
}
}
if (ptsWithDepth >= regPipeline_->getMinVisualCorrespondences())
{
frameValid = true;
// update local map
@@ -994,11 +1077,11 @@ Transform OdometryF2M::computeTransform(
//get depth
float d = 0.0f;
if(lastFrame_->getWords3().count(iter->first) == 1)
if(lastFrame_->getWords3().count(iter->first) == 1 &&
util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
{
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
d = pt3d.z;
d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
}

5
corelib/src/util3d_motion_estimation.cpp
View File

@@ -176,6 +176,11 @@ Transform estimateMotion3DTo2D(
{
UWARN("Not enough close points to compute covariance!");
}
if(float(oi) / float(inliers.size()) < 0.2f)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
}
}
else if(covariance)
{