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

Deprecated Rtabmap::get3DMap(), use Rtabmap::getGraph() instead with setting to true the data flags. Rtabmap::getGraph() now ignores children nodes.

Browse Source
This commit is contained in:
matlabbe
2020-05-06 08:04:58 -04:00
parent 7d377d26df
commit 1af139134e
8 changed files with 52 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -206,7 +206,7 @@ public:
void getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const;
std::set<int> getAllSignatureIds() const;
std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
bool isLocalizationDataSaved() const {return _localizationDataSaved;}

17
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -157,16 +157,21 @@ public:
void deleteLastLocation();
void setOptimizedPoses(const std::map<int, Transform> & poses);
Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global) const;
RTABMAP_DEPRECATED(
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.");
void getGraph(std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures = 0);
std::map<int, Signature> * signatures = 0,
bool withImages = false,
bool withScan = false,
bool withUserData = false,
bool withGrid = false) const;
int detectMoreLoopClosures(
float clusterRadius = 0.5f,
float clusterAngle = M_PI/6.0f,

4
corelib/src/Memory.cpp
View File

@@ -1631,12 +1631,12 @@ double Memory::getDbSavingTime() const
return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
}
std::set<int> Memory::getAllSignatureIds() const
std::set<int> Memory::getAllSignatureIds(bool ignoreChildren) const
{
std::set<int> ids;
if(_dbDriver)
{
_dbDriver->getAllNodeIds(ids);
_dbDriver->getAllNodeIds(ids, ignoreChildren);
}
for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
{

73
corelib/src/Rtabmap.cpp
View File

@@ -2761,7 +2761,7 @@ bool Rtabmap::process(
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
u = signature->getPose() * transform;
}
else
@@ -4332,54 +4332,7 @@ void Rtabmap::get3DMap(
bool global) const
{
UDEBUG("");
if(_memory && _memory->getLastWorkingSignature())
{
if(_rgbdSlamMode)
{
if(optimized)
{
poses = _optimizedPoses; // guess
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
}
else
{
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
}
else
{
// no optimization on appearance-only mode
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
// Get data
std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
//remove virtual signature
ids.erase(Memory::kIdVirtual);
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM
if(global)
{
ids = _memory->getAllSignatureIds(); // STM + WM + LTM
}
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
signatures.insert(std::make_pair(*iter, getSignatureCopy(*iter, true, true, true, true)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
{
UERROR("Last working signature is null!?");
}
else if(_memory == 0)
{
UWARN("Memory not initialized...");
}
return getGraph(poses, constraints, optimized, global, &signatures, true, true, true, true);
}
void Rtabmap::getGraph(
@@ -4387,7 +4340,11 @@ void Rtabmap::getGraph(
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures)
std::map<int, Signature> * signatures,
bool withImages,
bool withScan,
bool withUserData,
bool withGrid) const
{
if(_memory && _memory->getLastWorkingSignature())
{
@@ -4414,9 +4371,21 @@ void Rtabmap::getGraph(
if(signatures)
{
for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
// Get data
std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
//remove virtual signature
ids.erase(Memory::kIdVirtual);
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM
if(global)
{
signatures->insert(std::make_pair(iter->first, getSignatureCopy(iter->first, false, false, false, false)));
ids = _memory->getAllSignatureIds(); // STM + WM + LTM, ignoreChildren=true
}
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid)));
}
}
}

26
corelib/src/RtabmapThread.cpp
View File

@@ -138,23 +138,15 @@ void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
if(graphOnly)
{
_rtabmap->getGraph(poses,
constraints,
optimized,
full,
&signatures);
}
else
{
_rtabmap->get3DMap(
signatures,
poses,
constraints,
optimized,
full);
}
_rtabmap->getGraph(poses,
constraints,
optimized,
full,
&signatures,
!graphOnly,
!graphOnly,
!graphOnly,
!graphOnly);
this->post(new RtabmapEvent3DMap(
signatures,