i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

SBA: improved number of duplicated word correspondences (under inlier distance error)

Browse Source
This commit is contained in:
matlabbe
2015-08-13 11:31:19 -04:00
parent 31d374e141
commit 15e75d17cc
2 changed files with 31 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Graph.cpp
+27 -10
View File
@@ -1013,15 +1013,19 @@ std::map<int, Transform> CVSBAOptimizer::optimizeBA(
std::map<int, pcl::PointXYZ> points3DMap; std::map<int, pcl::PointXYZ> points3DMap;
std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint> std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
int genWordId = 1;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
if(uContains(signatures, iter->second.from()) && Link link = iter->second;
uContains(signatures, iter->second.to()) && if(link.to() < link.from())
uContains(frames, iter->second.from()))
{ {
const Signature & sFrom = signatures.at(iter->second.from()); link = link.inverse();
const Signature & sTo = signatures.at(iter->second.to()); }
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(frames, link.from()))
{
const Signature & sFrom = signatures.at(link.from());
const Signature & sTo = signatures.at(link.to());
std::vector<int> inliers; std::vector<int> inliers;
Transform t = util3d::estimateMotion3DTo3D( Transform t = util3d::estimateMotion3DTo3D(
@@ -1040,10 +1044,23 @@ std::map<int, Transform> CVSBAOptimizer::optimizeBA(
for(unsigned int i=0; i<inliers.size(); ++i) for(unsigned int i=0; i<inliers.size(); ++i)
{ {
pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose); pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
points3DMap.insert(std::make_pair(genWordId, p)); std::map<int, pcl::PointXYZ>::iterator jter = points3DMap.find(inliers[i]);
wordReferences.insert(std::make_pair(genWordId, std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); if(jter == points3DMap.end())
wordReferences.insert(std::make_pair(genWordId, std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); {
++genWordId; points3DMap.insert(std::make_pair(inliers[i], p));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
else
{
float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z);
if(dist <= inlierDistance_)
{
// in case of loop closure links
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
}
} }
} }
else else
guilib/src/MainWindow.cpp
+4 -3
View File
@@ -3686,14 +3686,15 @@ void MainWindow::postProcessing()
if(sba) if(sba)
{ {
_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints)...") _initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
.arg(optimizedPoses.size()).arg(linksOut.size())); .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
QApplication::processEvents(); QApplication::processEvents();
uSleep(100);
QApplication::processEvents(); QApplication::processEvents();
ParametersMap parametersSBA = _preferencesDialog->getAllParameters(); ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations))); uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations)));
graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parameters); graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parametersSBA);
cvsba.setInlierDistance(sbaInlierDistance); cvsba.setInlierDistance(sbaInlierDistance);
cvsba.setMinInliers(sbaMinInliers); cvsba.setMinInliers(sbaMinInliers);
std::map<int, Transform> newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap()); std::map<int, Transform> newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());