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

Removed parameter "OdomF2M/FixedMapPath". GUI: added odometry disabled option. Rtabmap can now localize even if odometry input is null (only in localization mode).

Browse Source
This commit is contained in:
matlabbe
2016-11-27 14:30:11 -05:00
parent 693f623e5e
commit 121446d648
15 changed files with 695 additions and 763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+28 -39
View File
@@ -3456,52 +3456,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(!pose.isNull() &&
data.cameraModels().size() == 1 &&
words.size() &&
words3D.size() == 0)
words3D.size() == 0 &&
_signatures.size() &&
_signatures.rbegin()->second->mapId() == _idMapCount) // same map
{
bool fillWithNaN = true;
if(_signatures.size())
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
fillWithNaN = false;
}
}
if(fillWithNaN)
{
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
}
}