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

GUI: Added Multi-Session Localization view. CameraModel: set localTransform to optical rotation in default constructor (fixed matrix invertion errors with code ignoring setting local transform), save/read local transform in/from camera calibration yaml file (so that local transform is also exported when extracting rgb/depth images). Rtabmap: in localization mode, ignore landmarks farther than RGBD/LocalRadius if no global loop closures are already in odometry cache.

Browse Source
This commit is contained in:
matlabbe
2022-02-12 13:35:23 -05:00
parent f584f42ea4
commit 191e165a28
13 changed files with 612 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
corelib/src/CameraModel.cpp
View File

@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
CameraModel::CameraModel()
CameraModel::CameraModel() :
localTransform_(opticalRotation())
{
}
@@ -339,6 +340,25 @@ bool CameraModel::load(const std::string & filePath)
UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["local_transform"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<float> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 4);
localTransform_ = Transform(
data[0], data[1], data[2], data[3],
data[4], data[5], data[6], data[7],
data[8], data[9], data[10], data[11]);
}
else
{
UWARN("Missing \"local_transform\" field in \"%s\"", filePath.c_str());
}
fs.release();
if(isValidForRectification())
@@ -448,6 +468,15 @@ bool CameraModel::save(const std::string & directory) const
fs << "}";
}
if(!localTransform_.isNull())
{
fs << "local_transform" << "{";
fs << "rows" << 3;
fs << "cols" << 4;
fs << "data" << std::vector<float>((float*)localTransform_.data(), ((float*)localTransform_.data())+12);
fs << "}";
}
fs.release();
return true;

54
corelib/src/Rtabmap.cpp
View File

@@ -2348,23 +2348,6 @@ bool Rtabmap::process(
ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
}
//============================================================
// Landmark
//============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksIndex(), iter->first) &&
_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
{
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
}
}
}
//============================================================
// Proximity detections
//============================================================
@@ -2832,6 +2815,41 @@ bool Rtabmap::process(
timeAddLoopClosureLink = timer.ticks();
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
//============================================================
// Landmark
//============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty())
{
bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksIndex(), iter->first) &&
_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
{
if(!_memory->isIncremental() && // In localization mode
!hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks
_localRadius>0.0 &&
iter->second.transform().getNormSquared() > _localRadius*_localRadius)
{
// Ignore landmark detections over local radius
UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
"and odom cache doesn't contain global loop closure(s).",
iter->first,
iter->second.transform().getNorm(),
Parameters::kRGBDLocalRadius().c_str(),
_localRadius);
}
else
{
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
}
}
}
}
//============================================================
// Add virtual links if a path is activated
//============================================================
@@ -3064,7 +3082,7 @@ bool Rtabmap::process(
}
bool hasGlobalLoopClosuresOrLandmarks = false;
if(rejectLocalization)
if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
{
// Let's try again without local loop closures
localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);